1107 files changed, 96872 insertions, 23364 deletions

diff --git a/drivers/media/cec/core/cec-adap.c b/drivers/media/cec/core/cec-adap.c
index 559a172ebc6c..ba6828ef540e 100644
--- a/drivers/media/cec/core/cec-adap.c
+++ b/drivers/media/cec/core/cec-adap.c
@@ -7,12 +7,13 @@
 
 #include <linux/errno.h>
 #include <linux/init.h>
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/kmod.h>
 #include <linux/ktime.h>
-#include <linux/slab.h>
 #include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/types.h>
 
@@ -490,6 +491,15 @@ int cec_thread_func(void *_adap)
 			goto unlock;
 		}
 
+		if (adap->transmit_in_progress &&
+		    adap->transmit_in_progress_aborted) {
+			if (adap->transmitting)
+				cec_data_cancel(adap->transmitting,
+						CEC_TX_STATUS_ABORTED, 0);
+			adap->transmit_in_progress = false;
+			adap->transmit_in_progress_aborted = false;
+			goto unlock;
+		}
 		if (adap->transmit_in_progress && timeout) {
 			/*
 			 * If we timeout, then log that. Normally this does
@@ -664,8 +674,9 @@ void cec_transmit_done_ts(struct cec_adapter *adap, u8 status,
 		/* Retry this message */
 		data->attempts -= attempts_made;
 		if (msg->timeout)
-			dprintk(2, "retransmit: %*ph (attempts: %d, wait for 0x%02x)\n",
-				msg->len, msg->msg, data->attempts, msg->reply);
+			dprintk(2, "retransmit: %*ph (attempts: %d, wait for %*ph)\n",
+				msg->len, msg->msg, data->attempts,
+				data->match_len, data->match_reply);
 		else
 			dprintk(2, "retransmit: %*ph (attempts: %d)\n",
 				msg->len, msg->msg, data->attempts);
@@ -771,6 +782,9 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 {
 	struct cec_data *data;
 	bool is_raw = msg_is_raw(msg);
+	bool reply_vendor_id = (msg->flags & CEC_MSG_FL_REPLY_VENDOR_ID) &&
+		msg->len > 1 && msg->msg[1] == CEC_MSG_VENDOR_COMMAND_WITH_ID;
+	int err;
 
 	if (adap->devnode.unregistered)
 		return -ENODEV;
@@ -784,12 +798,13 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 	msg->tx_low_drive_cnt = 0;
 	msg->tx_error_cnt = 0;
 	msg->sequence = 0;
+	msg->flags &= CEC_MSG_FL_REPLY_TO_FOLLOWERS | CEC_MSG_FL_RAW |
+		      (reply_vendor_id ? CEC_MSG_FL_REPLY_VENDOR_ID : 0);
 
-	if (msg->reply && msg->timeout == 0) {
+	if ((reply_vendor_id || msg->reply) && msg->timeout == 0) {
 		/* Make sure the timeout isn't 0. */
 		msg->timeout = 1000;
 	}
-	msg->flags &= CEC_MSG_FL_REPLY_TO_FOLLOWERS | CEC_MSG_FL_RAW;
 
 	if (!msg->timeout)
 		msg->flags &= ~CEC_MSG_FL_REPLY_TO_FOLLOWERS;
@@ -799,6 +814,11 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 		dprintk(1, "%s: invalid length %d\n", __func__, msg->len);
 		return -EINVAL;
 	}
+	if (reply_vendor_id && msg->len < 6) {
+		dprintk(1, "%s: <Vendor Command With ID> message too short\n",
+			__func__);
+		return -EINVAL;
+	}
 
 	memset(msg->msg + msg->len, 0, sizeof(msg->msg) - msg->len);
 
@@ -890,8 +910,9 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 				__func__);
 			return -ENONET;
 		}
-		if (msg->reply) {
-			dprintk(1, "%s: invalid msg->reply\n", __func__);
+		if (reply_vendor_id || msg->reply) {
+			dprintk(1, "%s: adapter is unconfigured so reply is not supported\n",
+				__func__);
 			return -EINVAL;
 		}
 	}
@@ -913,6 +934,14 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 	data->fh = fh;
 	data->adap = adap;
 	data->blocking = block;
+	if (reply_vendor_id) {
+		memcpy(data->match_reply, msg->msg + 1, 4);
+		data->match_reply[4] = msg->reply;
+		data->match_len = 5;
+	} else if (msg->timeout) {
+		data->match_reply[0] = msg->reply;
+		data->match_len = 1;
+	}
 
 	init_completion(&data->c);
 	INIT_DELAYED_WORK(&data->work, cec_wait_timeout);
@@ -935,11 +964,13 @@ int cec_transmit_msg_fh(struct cec_adapter *adap, struct cec_msg *msg,
 	 * Release the lock and wait, retake the lock afterwards.
 	 */
 	mutex_unlock(&adap->lock);
-	wait_for_completion_killable(&data->c);
-	if (!data->completed)
-		cancel_delayed_work_sync(&data->work);
+	err = wait_for_completion_killable(&data->c);
+	cancel_delayed_work_sync(&data->work);
 	mutex_lock(&adap->lock);
 
+	if (err)
+		adap->transmit_in_progress_aborted = true;
+
 	/* Cancel the transmit if it was interrupted */
 	if (!data->completed) {
 		if (data->msg.tx_status & CEC_TX_STATUS_OK)
@@ -1199,13 +1230,15 @@ void cec_received_msg_ts(struct cec_adapter *adap,
 			if (!abort && dst->msg[1] == CEC_MSG_INITIATE_ARC &&
 			    (cmd == CEC_MSG_REPORT_ARC_INITIATED ||
 			     cmd == CEC_MSG_REPORT_ARC_TERMINATED) &&
-			    (dst->reply == CEC_MSG_REPORT_ARC_INITIATED ||
-			     dst->reply == CEC_MSG_REPORT_ARC_TERMINATED))
+			    (data->match_reply[0] == CEC_MSG_REPORT_ARC_INITIATED ||
+			     data->match_reply[0] == CEC_MSG_REPORT_ARC_TERMINATED)) {
 				dst->reply = cmd;
+				data->match_reply[0] = cmd;
+			}
 
 			/* Does the command match? */
 			if ((abort && cmd != dst->msg[1]) ||
-			    (!abort && cmd != dst->reply))
+			    (!abort && memcmp(data->match_reply, msg->msg + 1, data->match_len)))
 				continue;
 
 			/* Does the addressing match? */
@@ -1575,9 +1608,12 @@ unconfigure:
  */
 static void cec_claim_log_addrs(struct cec_adapter *adap, bool block)
 {
-	if (WARN_ON(adap->is_configuring || adap->is_configured))
+	if (WARN_ON(adap->is_claiming_log_addrs ||
+		    adap->is_configuring || adap->is_configured))
 		return;
 
+	adap->is_claiming_log_addrs = true;
+
 	init_completion(&adap->config_completion);
 
 	/* Ready to kick off the thread */
@@ -1592,6 +1628,7 @@ static void cec_claim_log_addrs(struct cec_adapter *adap, bool block)
 		wait_for_completion(&adap->config_completion);
 		mutex_lock(&adap->lock);
 	}
+	adap->is_claiming_log_addrs = false;
 }
 
 /*
@@ -2302,18 +2339,21 @@ int cec_adap_status(struct seq_file *file, void *priv)
 	}
 	data = adap->transmitting;
 	if (data)
-		seq_printf(file, "transmitting message: %*ph (reply: %02x, timeout: %ums)\n",
-			   data->msg.len, data->msg.msg, data->msg.reply,
+		seq_printf(file, "transmitting message: %*ph (reply: %*ph, timeout: %ums)\n",
+			   data->msg.len, data->msg.msg,
+			   data->match_len, data->match_reply,
 			   data->msg.timeout);
 	seq_printf(file, "pending transmits: %u\n", adap->transmit_queue_sz);
 	list_for_each_entry(data, &adap->transmit_queue, list) {
-		seq_printf(file, "queued tx message: %*ph (reply: %02x, timeout: %ums)\n",
-			   data->msg.len, data->msg.msg, data->msg.reply,
+		seq_printf(file, "queued tx message: %*ph (reply: %*ph, timeout: %ums)\n",
+			   data->msg.len, data->msg.msg,
+			   data->match_len, data->match_reply,
 			   data->msg.timeout);
 	}
 	list_for_each_entry(data, &adap->wait_queue, list) {
-		seq_printf(file, "message waiting for reply: %*ph (reply: %02x, timeout: %ums)\n",
-			   data->msg.len, data->msg.msg, data->msg.reply,
+		seq_printf(file, "message waiting for reply: %*ph (reply: %*ph, timeout: %ums)\n",
+			   data->msg.len, data->msg.msg,
+			   data->match_len, data->match_reply,
 			   data->msg.timeout);
 	}
 
diff --git a/drivers/media/cec/core/cec-api.c b/drivers/media/cec/core/cec-api.c
index 67dc79ef1705..2b50578d107e 100644
--- a/drivers/media/cec/core/cec-api.c
+++ b/drivers/media/cec/core/cec-api.c
@@ -178,7 +178,7 @@ static long cec_adap_s_log_addrs(struct cec_adapter *adap, struct cec_fh *fh,
 			   CEC_LOG_ADDRS_FL_ALLOW_RC_PASSTHRU |
 			   CEC_LOG_ADDRS_FL_CDC_ONLY;
 	mutex_lock(&adap->lock);
-	if (!adap->is_configuring &&
+	if (!adap->is_claiming_log_addrs && !adap->is_configuring &&
 	    (!log_addrs.num_log_addrs || !adap->is_configured) &&
 	    !cec_is_busy(adap, fh)) {
 		err = __cec_s_log_addrs(adap, &log_addrs, block);
@@ -222,7 +222,7 @@ static long cec_transmit(struct cec_adapter *adap, struct cec_fh *fh,
 	mutex_lock(&adap->lock);
 	if (adap->log_addrs.num_log_addrs == 0)
 		err = -EPERM;
-	else if (adap->is_configuring)
+	else if (adap->is_configuring && !msg_is_raw(&msg))
 		err = -ENONET;
 	else if (cec_is_busy(adap, fh))
 		err = -EBUSY;
@@ -580,7 +580,7 @@ static int cec_open(struct inode *inode, struct file *filp)
 	fh->mode_initiator = CEC_MODE_INITIATOR;
 	fh->adap = adap;
 
-	err = cec_get_device(devnode);
+	err = cec_get_device(adap);
 	if (err) {
 		kfree(fh);
 		return err;
@@ -664,6 +664,8 @@ static int cec_release(struct inode *inode, struct file *filp)
 		list_del_init(&data->xfer_list);
 	}
 	mutex_unlock(&adap->lock);
+
+	mutex_lock(&fh->lock);
 	while (!list_empty(&fh->msgs)) {
 		struct cec_msg_entry *entry =
 			list_first_entry(&fh->msgs, struct cec_msg_entry, list);
@@ -681,9 +683,10 @@ static int cec_release(struct inode *inode, struct file *filp)
 			kfree(entry);
 		}
 	}
+	mutex_unlock(&fh->lock);
 	kfree(fh);
 
-	cec_put_device(devnode);
+	cec_put_device(adap);
 	filp->private_data = NULL;
 	return 0;
 }
@@ -695,5 +698,4 @@ const struct file_operations cec_devnode_fops = {
 	.compat_ioctl = cec_ioctl,
 	.release = cec_release,
 	.poll = cec_poll,
-	.llseek = no_llseek,
 };
diff --git a/drivers/media/cec/core/cec-core.c b/drivers/media/cec/core/cec-core.c
index 5a54db839e5d..e10bd588a586 100644
--- a/drivers/media/cec/core/cec-core.c
+++ b/drivers/media/cec/core/cec-core.c
@@ -5,13 +5,14 @@
  * Copyright 2016 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
  */
 
+#include <linux/debugfs.h>
 #include <linux/errno.h>
 #include <linux/init.h>
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/kmod.h>
-#include <linux/slab.h>
 #include <linux/mm.h>
+#include <linux/module.h>
+#include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/types.h>
 
@@ -51,35 +52,6 @@ static struct dentry *top_cec_dir;
 /* dev to cec_devnode */
 #define to_cec_devnode(cd) container_of(cd, struct cec_devnode, dev)
 
-int cec_get_device(struct cec_devnode *devnode)
-{
-	/*
-	 * Check if the cec device is available. This needs to be done with
-	 * the devnode->lock held to prevent an open/unregister race:
-	 * without the lock, the device could be unregistered and freed between
-	 * the devnode->registered check and get_device() calls, leading to
-	 * a crash.
-	 */
-	mutex_lock(&devnode->lock);
-	/*
-	 * return ENXIO if the cec device has been removed
-	 * already or if it is not registered anymore.
-	 */
-	if (!devnode->registered) {
-		mutex_unlock(&devnode->lock);
-		return -ENXIO;
-	}
-	/* and increase the device refcount */
-	get_device(&devnode->dev);
-	mutex_unlock(&devnode->lock);
-	return 0;
-}
-
-void cec_put_device(struct cec_devnode *devnode)
-{
-	put_device(&devnode->dev);
-}
-
 /* Called when the last user of the cec device exits. */
 static void cec_devnode_release(struct device *cd)
 {
@@ -273,7 +245,7 @@ struct cec_adapter *cec_allocate_adapter(const struct cec_adap_ops *ops,
 	adap->cec_pin_is_high = true;
 	adap->log_addrs.cec_version = CEC_OP_CEC_VERSION_2_0;
 	adap->log_addrs.vendor_id = CEC_VENDOR_ID_NONE;
-	adap->capabilities = caps;
+	adap->capabilities = caps | CEC_CAP_REPLY_VENDOR_ID;
 	if (debug_phys_addr)
 		adap->capabilities |= CEC_CAP_PHYS_ADDR;
 	adap->needs_hpd = caps & CEC_CAP_NEEDS_HPD;
@@ -467,6 +439,6 @@ static void __exit cec_devnode_exit(void)
 subsys_initcall(cec_devnode_init);
 module_exit(cec_devnode_exit)
 
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
 MODULE_DESCRIPTION("Device node registration for cec drivers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/cec/core/cec-pin-error-inj.c b/drivers/media/cec/core/cec-pin-error-inj.c
index fc0968b9d40e..6e61a04b8168 100644
--- a/drivers/media/cec/core/cec-pin-error-inj.c
+++ b/drivers/media/cec/core/cec-pin-error-inj.c
@@ -4,8 +4,9 @@
  */
 
 #include <linux/delay.h>
-#include <linux/slab.h>
 #include <linux/sched/types.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
 
 #include <media/cec-pin.h>
 #include "cec-pin-priv.h"
diff --git a/drivers/media/cec/core/cec-pin.c b/drivers/media/cec/core/cec-pin.c
index 330d5d5d86ab..59ac12113f3a 100644
--- a/drivers/media/cec/core/cec-pin.c
+++ b/drivers/media/cec/core/cec-pin.c
@@ -4,8 +4,9 @@
  */
 
 #include <linux/delay.h>
-#include <linux/slab.h>
 #include <linux/sched/types.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
 
 #include <media/cec-pin.h>
 #include "cec-pin-priv.h"
@@ -872,19 +873,19 @@ static enum hrtimer_restart cec_pin_timer(struct hrtimer *timer)
 		if (pin->wait_usecs > 150) {
 			pin->wait_usecs -= 100;
 			pin->timer_ts = ktime_add_us(ts, 100);
-			hrtimer_forward_now(timer, ns_to_ktime(100000));
+			hrtimer_forward_now(timer, us_to_ktime(100));
 			return HRTIMER_RESTART;
 		}
 		if (pin->wait_usecs > 100) {
 			pin->wait_usecs /= 2;
 			pin->timer_ts = ktime_add_us(ts, pin->wait_usecs);
 			hrtimer_forward_now(timer,
-					ns_to_ktime(pin->wait_usecs * 1000));
+					us_to_ktime(pin->wait_usecs));
 			return HRTIMER_RESTART;
 		}
 		pin->timer_ts = ktime_add_us(ts, pin->wait_usecs);
 		hrtimer_forward_now(timer,
-				    ns_to_ktime(pin->wait_usecs * 1000));
+				    us_to_ktime(pin->wait_usecs));
 		pin->wait_usecs = 0;
 		return HRTIMER_RESTART;
 	}
@@ -1019,13 +1020,12 @@ static enum hrtimer_restart cec_pin_timer(struct hrtimer *timer)
 	if (!adap->monitor_pin_cnt || usecs <= 150) {
 		pin->wait_usecs = 0;
 		pin->timer_ts = ktime_add_us(ts, usecs);
-		hrtimer_forward_now(timer,
-				ns_to_ktime(usecs * 1000));
+		hrtimer_forward_now(timer, us_to_ktime(usecs));
 		return HRTIMER_RESTART;
 	}
 	pin->wait_usecs = usecs - 100;
 	pin->timer_ts = ktime_add_us(ts, 100);
-	hrtimer_forward_now(timer, ns_to_ktime(100000));
+	hrtimer_forward_now(timer, us_to_ktime(100));
 	return HRTIMER_RESTART;
 }
 
@@ -1345,9 +1345,8 @@ struct cec_adapter *cec_pin_allocate_adapter(const struct cec_pin_ops *pin_ops,
 	if (pin == NULL)
 		return ERR_PTR(-ENOMEM);
 	pin->ops = pin_ops;
-	hrtimer_init(&pin->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 	atomic_set(&pin->work_pin_num_events, 0);
-	pin->timer.function = cec_pin_timer;
+	hrtimer_setup(&pin->timer, cec_pin_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 	init_waitqueue_head(&pin->kthread_waitq);
 	pin->tx_custom_low_usecs = CEC_TIM_CUSTOM_DEFAULT;
 	pin->tx_custom_high_usecs = CEC_TIM_CUSTOM_DEFAULT;
diff --git a/drivers/media/cec/core/cec-priv.h b/drivers/media/cec/core/cec-priv.h
index ed1f8c67626b..ce42a37c4ac0 100644
--- a/drivers/media/cec/core/cec-priv.h
+++ b/drivers/media/cec/core/cec-priv.h
@@ -37,8 +37,6 @@ static inline bool msg_is_raw(const struct cec_msg *msg)
 
 /* cec-core.c */
 extern int cec_debug;
-int cec_get_device(struct cec_devnode *devnode);
-void cec_put_device(struct cec_devnode *devnode);
 
 /* cec-adap.c */
 int cec_monitor_all_cnt_inc(struct cec_adapter *adap);
diff --git a/drivers/media/cec/i2c/Kconfig b/drivers/media/cec/i2c/Kconfig
index d912d143fb31..c31abc26f602 100644
--- a/drivers/media/cec/i2c/Kconfig
+++ b/drivers/media/cec/i2c/Kconfig
@@ -13,3 +13,13 @@ config CEC_CH7322
 	  generic CEC framework interface.
 	  CEC bus is present in the HDMI connector and enables communication
 	  between compatible devices.
+
+config CEC_NXP_TDA9950
+	tristate "NXP Semiconductors TDA9950/TDA998X HDMI CEC"
+	depends on I2C
+	select CEC_NOTIFIER
+	select CEC_CORE
+	default DRM_I2C_NXP_TDA998X
+	help
+	  This is a driver for the NXP TDA9950 CEC controller and for the CEC
+	  controller block integrated into several NXP TDA998x HDMI encoders.
diff --git a/drivers/media/cec/i2c/Makefile b/drivers/media/cec/i2c/Makefile
index d7496dfd0fa4..95c9eda52583 100644
--- a/drivers/media/cec/i2c/Makefile
+++ b/drivers/media/cec/i2c/Makefile
@@ -3,3 +3,4 @@
 # Makefile for the CEC I2C device drivers.
 #
 obj-$(CONFIG_CEC_CH7322) += ch7322.o
+obj-$(CONFIG_CEC_NXP_TDA9950) += tda9950.o
diff --git a/drivers/media/cec/i2c/tda9950.c b/drivers/media/cec/i2c/tda9950.c
new file mode 100644
index 000000000000..cbff851e0c85
--- /dev/null
+++ b/drivers/media/cec/i2c/tda9950.c
@@ -0,0 +1,507 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  TDA9950 Consumer Electronics Control driver
+ *
+ * The NXP TDA9950 implements the HDMI Consumer Electronics Control
+ * interface.  The host interface is similar to a mailbox: the data
+ * registers starting at REG_CDR0 are written to send a command to the
+ * internal CPU, and replies are read from these registers.
+ *
+ * As the data registers represent a mailbox, they must be accessed
+ * as a single I2C transaction.  See the TDA9950 data sheet for details.
+ */
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_data/tda9950.h>
+#include <linux/slab.h>
+#include <drm/drm_edid.h>
+#include <media/cec.h>
+#include <media/cec-notifier.h>
+
+enum {
+	REG_CSR = 0x00,
+	CSR_BUSY = BIT(7),
+	CSR_INT  = BIT(6),
+	CSR_ERR  = BIT(5),
+
+	REG_CER = 0x01,
+
+	REG_CVR = 0x02,
+
+	REG_CCR = 0x03,
+	CCR_RESET = BIT(7),
+	CCR_ON    = BIT(6),
+
+	REG_ACKH = 0x04,
+	REG_ACKL = 0x05,
+
+	REG_CCONR = 0x06,
+	CCONR_ENABLE_ERROR = BIT(4),
+	CCONR_RETRY_MASK = 7,
+
+	REG_CDR0 = 0x07,
+
+	CDR1_REQ = 0x00,
+	CDR1_CNF = 0x01,
+	CDR1_IND = 0x81,
+	CDR1_ERR = 0x82,
+	CDR1_IER = 0x83,
+
+	CDR2_CNF_SUCCESS    = 0x00,
+	CDR2_CNF_OFF_STATE  = 0x80,
+	CDR2_CNF_BAD_REQ    = 0x81,
+	CDR2_CNF_CEC_ACCESS = 0x82,
+	CDR2_CNF_ARB_ERROR  = 0x83,
+	CDR2_CNF_BAD_TIMING = 0x84,
+	CDR2_CNF_NACK_ADDR  = 0x85,
+	CDR2_CNF_NACK_DATA  = 0x86,
+};
+
+struct tda9950_priv {
+	struct i2c_client *client;
+	struct device *hdmi;
+	struct cec_adapter *adap;
+	struct tda9950_glue *glue;
+	u16 addresses;
+	struct cec_msg rx_msg;
+	struct cec_notifier *notify;
+	bool open;
+};
+
+static int tda9950_write_range(struct i2c_client *client, u8 addr, u8 *p, int cnt)
+{
+	struct i2c_msg msg;
+	u8 buf[CEC_MAX_MSG_SIZE + 3];
+	int ret;
+
+	if (WARN_ON(cnt > sizeof(buf) - 1))
+		return -EINVAL;
+
+	buf[0] = addr;
+	memcpy(buf + 1, p, cnt);
+
+	msg.addr = client->addr;
+	msg.flags = 0;
+	msg.len = cnt + 1;
+	msg.buf = buf;
+
+	dev_dbg(&client->dev, "wr 0x%02x: %*ph\n", addr, cnt, p);
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+	if (ret < 0)
+		dev_err(&client->dev, "Error %d writing to cec:0x%x\n", ret, addr);
+	return ret < 0 ? ret : 0;
+}
+
+static void tda9950_write(struct i2c_client *client, u8 addr, u8 val)
+{
+	tda9950_write_range(client, addr, &val, 1);
+}
+
+static int tda9950_read_range(struct i2c_client *client, u8 addr, u8 *p, int cnt)
+{
+	struct i2c_msg msg[2];
+	int ret;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].len = 1;
+	msg[0].buf = &addr;
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].len = cnt;
+	msg[1].buf = p;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+	if (ret < 0)
+		dev_err(&client->dev, "Error %d reading from cec:0x%x\n", ret, addr);
+
+	dev_dbg(&client->dev, "rd 0x%02x: %*ph\n", addr, cnt, p);
+
+	return ret;
+}
+
+static u8 tda9950_read(struct i2c_client *client, u8 addr)
+{
+	int ret;
+	u8 val;
+
+	ret = tda9950_read_range(client, addr, &val, 1);
+	if (ret < 0)
+		val = 0;
+
+	return val;
+}
+
+static irqreturn_t tda9950_irq(int irq, void *data)
+{
+	struct tda9950_priv *priv = data;
+	unsigned int tx_status;
+	u8 csr, cconr, buf[19];
+	u8 arb_lost_cnt, nack_cnt, err_cnt;
+
+	if (!priv->open)
+		return IRQ_NONE;
+
+	csr = tda9950_read(priv->client, REG_CSR);
+	if (!(csr & CSR_INT))
+		return IRQ_NONE;
+
+	cconr = tda9950_read(priv->client, REG_CCONR) & CCONR_RETRY_MASK;
+
+	tda9950_read_range(priv->client, REG_CDR0, buf, sizeof(buf));
+
+	/*
+	 * This should never happen: the data sheet says that there will
+	 * always be a valid message if the interrupt line is asserted.
+	 */
+	if (buf[0] == 0) {
+		dev_warn(&priv->client->dev, "interrupt pending, but no message?\n");
+		return IRQ_NONE;
+	}
+
+	switch (buf[1]) {
+	case CDR1_CNF: /* transmit result */
+		arb_lost_cnt = nack_cnt = err_cnt = 0;
+		switch (buf[2]) {
+		case CDR2_CNF_SUCCESS:
+			tx_status = CEC_TX_STATUS_OK;
+			break;
+
+		case CDR2_CNF_ARB_ERROR:
+			tx_status = CEC_TX_STATUS_ARB_LOST;
+			arb_lost_cnt = cconr;
+			break;
+
+		case CDR2_CNF_NACK_ADDR:
+			tx_status = CEC_TX_STATUS_NACK;
+			nack_cnt = cconr;
+			break;
+
+		default: /* some other error, refer to TDA9950 docs */
+			dev_err(&priv->client->dev, "CNF reply error 0x%02x\n",
+				buf[2]);
+			tx_status = CEC_TX_STATUS_ERROR;
+			err_cnt = cconr;
+			break;
+		}
+		/* TDA9950 executes all retries for us */
+		if (tx_status != CEC_TX_STATUS_OK)
+			tx_status |= CEC_TX_STATUS_MAX_RETRIES;
+		cec_transmit_done(priv->adap, tx_status, arb_lost_cnt,
+				  nack_cnt, 0, err_cnt);
+		break;
+
+	case CDR1_IND:
+		priv->rx_msg.len = buf[0] - 2;
+		if (priv->rx_msg.len > CEC_MAX_MSG_SIZE)
+			priv->rx_msg.len = CEC_MAX_MSG_SIZE;
+
+		memcpy(priv->rx_msg.msg, buf + 2, priv->rx_msg.len);
+		cec_received_msg(priv->adap, &priv->rx_msg);
+		break;
+
+	default: /* unknown */
+		dev_err(&priv->client->dev, "unknown service id 0x%02x\n",
+			buf[1]);
+		break;
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int tda9950_cec_transmit(struct cec_adapter *adap, u8 attempts,
+				u32 signal_free_time, struct cec_msg *msg)
+{
+	struct tda9950_priv *priv = adap->priv;
+	u8 buf[CEC_MAX_MSG_SIZE + 2];
+
+	buf[0] = 2 + msg->len;
+	buf[1] = CDR1_REQ;
+	memcpy(buf + 2, msg->msg, msg->len);
+
+	if (attempts > 5)
+		attempts = 5;
+
+	tda9950_write(priv->client, REG_CCONR, attempts);
+
+	return tda9950_write_range(priv->client, REG_CDR0, buf, 2 + msg->len);
+}
+
+static int tda9950_cec_adap_log_addr(struct cec_adapter *adap, u8 addr)
+{
+	struct tda9950_priv *priv = adap->priv;
+	u16 addresses;
+	u8 buf[2];
+
+	if (addr == CEC_LOG_ADDR_INVALID)
+		addresses = priv->addresses = 0;
+	else
+		addresses = priv->addresses |= BIT(addr);
+
+	/* TDA9950 doesn't want address 15 set */
+	addresses &= 0x7fff;
+	buf[0] = addresses >> 8;
+	buf[1] = addresses;
+
+	return tda9950_write_range(priv->client, REG_ACKH, buf, 2);
+}
+
+/*
+ * When operating as part of the TDA998x, we need additional handling
+ * to initialise and shut down the TDA9950 part of the device.  These
+ * two hooks are provided to allow the TDA998x code to perform those
+ * activities.
+ */
+static int tda9950_glue_open(struct tda9950_priv *priv)
+{
+	int ret = 0;
+
+	if (priv->glue && priv->glue->open)
+		ret = priv->glue->open(priv->glue->data);
+
+	priv->open = true;
+
+	return ret;
+}
+
+static void tda9950_glue_release(struct tda9950_priv *priv)
+{
+	priv->open = false;
+
+	if (priv->glue && priv->glue->release)
+		priv->glue->release(priv->glue->data);
+}
+
+static int tda9950_open(struct tda9950_priv *priv)
+{
+	struct i2c_client *client = priv->client;
+	int ret;
+
+	ret = tda9950_glue_open(priv);
+	if (ret)
+		return ret;
+
+	/* Reset the TDA9950, and wait 250ms for it to recover */
+	tda9950_write(client, REG_CCR, CCR_RESET);
+	msleep(250);
+
+	tda9950_cec_adap_log_addr(priv->adap, CEC_LOG_ADDR_INVALID);
+
+	/* Start the command processor */
+	tda9950_write(client, REG_CCR, CCR_ON);
+
+	return 0;
+}
+
+static void tda9950_release(struct tda9950_priv *priv)
+{
+	struct i2c_client *client = priv->client;
+	int timeout = 50;
+	u8 csr;
+
+	/* Stop the command processor */
+	tda9950_write(client, REG_CCR, 0);
+
+	/* Wait up to .5s for it to signal non-busy */
+	do {
+		csr = tda9950_read(client, REG_CSR);
+		if (!(csr & CSR_BUSY) || !--timeout)
+			break;
+		msleep(10);
+	} while (1);
+
+	/* Warn the user that their IRQ may die if it's shared. */
+	if (csr & CSR_BUSY)
+		dev_warn(&client->dev, "command processor failed to stop, irq%d may die (csr=0x%02x)\n",
+			 client->irq, csr);
+
+	tda9950_glue_release(priv);
+}
+
+static int tda9950_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct tda9950_priv *priv = adap->priv;
+
+	if (!enable) {
+		tda9950_release(priv);
+		return 0;
+	} else {
+		return tda9950_open(priv);
+	}
+}
+
+static const struct cec_adap_ops tda9950_cec_ops = {
+	.adap_enable = tda9950_cec_adap_enable,
+	.adap_log_addr = tda9950_cec_adap_log_addr,
+	.adap_transmit = tda9950_cec_transmit,
+};
+
+/*
+ * When operating as part of the TDA998x, we need to claim additional
+ * resources.  These two hooks permit the management of those resources.
+ */
+static void tda9950_devm_glue_exit(void *data)
+{
+	struct tda9950_glue *glue = data;
+
+	if (glue && glue->exit)
+		glue->exit(glue->data);
+}
+
+static int tda9950_devm_glue_init(struct device *dev, struct tda9950_glue *glue)
+{
+	int ret;
+
+	if (glue && glue->init) {
+		ret = glue->init(glue->data);
+		if (ret)
+			return ret;
+	}
+
+	ret = devm_add_action(dev, tda9950_devm_glue_exit, glue);
+	if (ret)
+		tda9950_devm_glue_exit(glue);
+
+	return ret;
+}
+
+static void tda9950_cec_del(void *data)
+{
+	struct tda9950_priv *priv = data;
+
+	cec_delete_adapter(priv->adap);
+}
+
+static int tda9950_probe(struct i2c_client *client)
+{
+	struct tda9950_glue *glue = client->dev.platform_data;
+	struct device *dev = &client->dev;
+	struct tda9950_priv *priv;
+	unsigned long irqflags;
+	int ret;
+	u8 cvr;
+
+	/*
+	 * We must have I2C functionality: our multi-byte accesses
+	 * must be performed as a single contiguous transaction.
+	 */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev,
+			"adapter does not support I2C functionality\n");
+		return -ENXIO;
+	}
+
+	/* We must have an interrupt to be functional. */
+	if (client->irq <= 0) {
+		dev_err(&client->dev, "driver requires an interrupt\n");
+		return -ENXIO;
+	}
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+	priv->glue = glue;
+
+	i2c_set_clientdata(client, priv);
+
+	/*
+	 * If we're part of a TDA998x, we want the class devices to be
+	 * associated with the HDMI Tx so we have a tight relationship
+	 * between the HDMI interface and the CEC interface.
+	 */
+	priv->hdmi = dev;
+	if (glue && glue->parent)
+		priv->hdmi = glue->parent;
+
+	priv->adap = cec_allocate_adapter(&tda9950_cec_ops, priv, "tda9950",
+					  CEC_CAP_DEFAULTS |
+					  CEC_CAP_CONNECTOR_INFO,
+					  CEC_MAX_LOG_ADDRS);
+	if (IS_ERR(priv->adap))
+		return PTR_ERR(priv->adap);
+
+	ret = devm_add_action(dev, tda9950_cec_del, priv);
+	if (ret) {
+		cec_delete_adapter(priv->adap);
+		return ret;
+	}
+
+	ret = tda9950_devm_glue_init(dev, glue);
+	if (ret)
+		return ret;
+
+	ret = tda9950_glue_open(priv);
+	if (ret)
+		return ret;
+
+	cvr = tda9950_read(client, REG_CVR);
+
+	dev_info(&client->dev,
+		 "TDA9950 CEC interface, hardware version %u.%u\n",
+		 cvr >> 4, cvr & 15);
+
+	tda9950_glue_release(priv);
+
+	irqflags = IRQF_TRIGGER_FALLING;
+	if (glue)
+		irqflags = glue->irq_flags;
+
+	ret = devm_request_threaded_irq(dev, client->irq, NULL, tda9950_irq,
+					irqflags | IRQF_SHARED | IRQF_ONESHOT,
+					dev_name(&client->dev), priv);
+	if (ret < 0)
+		return ret;
+
+	priv->notify = cec_notifier_cec_adap_register(priv->hdmi, NULL,
+						      priv->adap);
+	if (!priv->notify)
+		return -ENOMEM;
+
+	ret = cec_register_adapter(priv->adap, priv->hdmi);
+	if (ret < 0) {
+		cec_notifier_cec_adap_unregister(priv->notify, priv->adap);
+		return ret;
+	}
+
+	/*
+	 * CEC documentation says we must not call cec_delete_adapter
+	 * after a successful call to cec_register_adapter().
+	 */
+	devm_remove_action(dev, tda9950_cec_del, priv);
+
+	return 0;
+}
+
+static void tda9950_remove(struct i2c_client *client)
+{
+	struct tda9950_priv *priv = i2c_get_clientdata(client);
+
+	cec_notifier_cec_adap_unregister(priv->notify, priv->adap);
+	cec_unregister_adapter(priv->adap);
+}
+
+static struct i2c_device_id tda9950_ids[] = {
+	{ "tda9950" },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, tda9950_ids);
+
+static struct i2c_driver tda9950_driver = {
+	.probe = tda9950_probe,
+	.remove = tda9950_remove,
+	.driver = {
+		.name = "tda9950",
+	},
+	.id_table = tda9950_ids,
+};
+
+module_i2c_driver(tda9950_driver);
+
+MODULE_AUTHOR("Russell King <rmk+kernel@armlinux.org.uk>");
+MODULE_DESCRIPTION("TDA9950/TDA998x Consumer Electronics Control Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/cec/platform/Kconfig b/drivers/media/cec/platform/Kconfig
index ede81fe331b0..e40413609f53 100644
--- a/drivers/media/cec/platform/Kconfig
+++ b/drivers/media/cec/platform/Kconfig
@@ -99,7 +99,7 @@ config CEC_TEGRA
 
 config CEC_SECO
 	tristate "SECO Boards HDMI CEC driver"
-	depends on X86 || COMPILE_TEST
+	depends on X86 || (COMPILE_TEST && HAS_IOPORT)
 	depends on PCI && DMI
 	select CEC_CORE
 	select CEC_NOTIFIER
diff --git a/drivers/media/cec/platform/cec-gpio/cec-gpio.c b/drivers/media/cec/platform/cec-gpio/cec-gpio.c
index 98dacb0919b6..50cdc557c943 100644
--- a/drivers/media/cec/platform/cec-gpio/cec-gpio.c
+++ b/drivers/media/cec/platform/cec-gpio/cec-gpio.c
@@ -3,11 +3,12 @@
  * Copyright 2017 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
  */
 
-#include <linux/module.h>
-#include <linux/interrupt.h>
 #include <linux/delay.h>
-#include <linux/platform_device.h>
 #include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
 #include <media/cec-notifier.h>
 #include <media/cec-pin.h>
 
@@ -279,7 +280,7 @@ MODULE_DEVICE_TABLE(of, cec_gpio_match);
 
 static struct platform_driver cec_gpio_pdrv = {
 	.probe	= cec_gpio_probe,
-	.remove_new = cec_gpio_remove,
+	.remove = cec_gpio_remove,
 	.driver = {
 		.name		= "cec-gpio",
 		.of_match_table	= cec_gpio_match,
@@ -288,6 +289,6 @@ static struct platform_driver cec_gpio_pdrv = {
 
 module_platform_driver(cec_gpio_pdrv);
 
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("CEC GPIO driver");
diff --git a/drivers/media/cec/platform/cros-ec/cros-ec-cec.c b/drivers/media/cec/platform/cros-ec/cros-ec-cec.c
index 48ed2993d2f0..419b9a7abcce 100644
--- a/drivers/media/cec/platform/cros-ec/cros-ec-cec.c
+++ b/drivers/media/cec/platform/cros-ec/cros-ec-cec.c
@@ -8,6 +8,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/platform_device.h>
 #include <linux/dmi.h>
 #include <linux/pci.h>
@@ -297,6 +298,7 @@ struct cec_dmi_match {
 static const char *const port_b_conns[] = { "Port B", NULL };
 static const char *const port_db_conns[] = { "Port D", "Port B", NULL };
 static const char *const port_ba_conns[] = { "Port B", "Port A", NULL };
+static const char *const port_ab_conns[] = { "Port A", "Port B", NULL };
 static const char *const port_d_conns[] = { "Port D", NULL };
 
 static const struct cec_dmi_match cec_dmi_match_table[] = {
@@ -328,6 +330,10 @@ static const struct cec_dmi_match cec_dmi_match_table[] = {
 	{ "Google", "Dexi", "0000:00:02.0", port_db_conns },
 	/* Google Dita */
 	{ "Google", "Dita", "0000:00:02.0", port_db_conns },
+	/* Google Dirks */
+	{ "Google", "Dirks", "0000:00:02.0", port_ab_conns },
+	/* Google Moxie */
+	{ "Google", "Moxie", "0000:00:02.0", port_b_conns },
 };
 
 static struct device *cros_ec_cec_find_hdmi_dev(struct device *dev,
@@ -573,13 +579,20 @@ static void cros_ec_cec_remove(struct platform_device *pdev)
 	}
 }
 
+static const struct platform_device_id cros_ec_cec_id[] = {
+	{ DRV_NAME, 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(platform, cros_ec_cec_id);
+
 static struct platform_driver cros_ec_cec_driver = {
 	.probe = cros_ec_cec_probe,
-	.remove_new = cros_ec_cec_remove,
+	.remove = cros_ec_cec_remove,
 	.driver = {
 		.name = DRV_NAME,
 		.pm = &cros_ec_cec_pm_ops,
 	},
+	.id_table = cros_ec_cec_id,
 };
 
 module_platform_driver(cros_ec_cec_driver);
@@ -587,4 +600,3 @@ module_platform_driver(cros_ec_cec_driver);
 MODULE_DESCRIPTION("CEC driver for ChromeOS ECs");
 MODULE_AUTHOR("Neil Armstrong <narmstrong@baylibre.com>");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/media/cec/platform/meson/ao-cec-g12a.c b/drivers/media/cec/platform/meson/ao-cec-g12a.c
index 51294b9b6cd5..41f5b8669cb0 100644
--- a/drivers/media/cec/platform/meson/ao-cec-g12a.c
+++ b/drivers/media/cec/platform/meson/ao-cec-g12a.c
@@ -778,7 +778,7 @@ MODULE_DEVICE_TABLE(of, meson_ao_cec_g12a_of_match);
 
 static struct platform_driver meson_ao_cec_g12a_driver = {
 	.probe   = meson_ao_cec_g12a_probe,
-	.remove_new = meson_ao_cec_g12a_remove,
+	.remove = meson_ao_cec_g12a_remove,
 	.driver  = {
 		.name = "meson-ao-cec-g12a",
 		.of_match_table = of_match_ptr(meson_ao_cec_g12a_of_match),
diff --git a/drivers/media/cec/platform/meson/ao-cec.c b/drivers/media/cec/platform/meson/ao-cec.c
index 494738daf09a..145efd9af6ac 100644
--- a/drivers/media/cec/platform/meson/ao-cec.c
+++ b/drivers/media/cec/platform/meson/ao-cec.c
@@ -714,7 +714,7 @@ MODULE_DEVICE_TABLE(of, meson_ao_cec_of_match);
 
 static struct platform_driver meson_ao_cec_driver = {
 	.probe   = meson_ao_cec_probe,
-	.remove_new = meson_ao_cec_remove,
+	.remove = meson_ao_cec_remove,
 	.driver  = {
 		.name = "meson-ao-cec",
 		.of_match_table = meson_ao_cec_of_match,
diff --git a/drivers/media/cec/platform/s5p/s5p_cec.c b/drivers/media/cec/platform/s5p/s5p_cec.c
index 51ab4a80aafe..4a92d3230f66 100644
--- a/drivers/media/cec/platform/s5p/s5p_cec.c
+++ b/drivers/media/cec/platform/s5p/s5p_cec.c
@@ -294,7 +294,7 @@ MODULE_DEVICE_TABLE(of, s5p_cec_match);
 
 static struct platform_driver s5p_cec_pdrv = {
 	.probe	= s5p_cec_probe,
-	.remove_new = s5p_cec_remove,
+	.remove = s5p_cec_remove,
 	.driver	= {
 		.name		= CEC_NAME,
 		.of_match_table	= s5p_cec_match,
diff --git a/drivers/media/cec/platform/seco/seco-cec.c b/drivers/media/cec/platform/seco/seco-cec.c
index 5d4c5a2cae09..b7bb49f02395 100644
--- a/drivers/media/cec/platform/seco/seco-cec.c
+++ b/drivers/media/cec/platform/seco/seco-cec.c
@@ -778,7 +778,7 @@ static struct platform_driver secocec_driver = {
 		   .pm = SECOCEC_PM_OPS,
 	},
 	.probe = secocec_probe,
-	.remove_new = secocec_remove,
+	.remove = secocec_remove,
 };
 
 module_platform_driver(secocec_driver);
diff --git a/drivers/media/cec/platform/sti/stih-cec.c b/drivers/media/cec/platform/sti/stih-cec.c
index a20fc5c0c88d..49843d576c7c 100644
--- a/drivers/media/cec/platform/sti/stih-cec.c
+++ b/drivers/media/cec/platform/sti/stih-cec.c
@@ -6,6 +6,7 @@
  */
 #include <linux/clk.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/mfd/syscon.h>
 #include <linux/module.h>
@@ -382,7 +383,7 @@ MODULE_DEVICE_TABLE(of, stih_cec_match);
 
 static struct platform_driver stih_cec_pdrv = {
 	.probe	= stih_cec_probe,
-	.remove_new = stih_cec_remove,
+	.remove = stih_cec_remove,
 	.driver = {
 		.name		= CEC_NAME,
 		.of_match_table	= stih_cec_match,
diff --git a/drivers/media/cec/platform/stm32/stm32-cec.c b/drivers/media/cec/platform/stm32/stm32-cec.c
index bda9d254041a..fea2d65acffc 100644
--- a/drivers/media/cec/platform/stm32/stm32-cec.c
+++ b/drivers/media/cec/platform/stm32/stm32-cec.c
@@ -361,7 +361,7 @@ MODULE_DEVICE_TABLE(of, stm32_cec_of_match);
 
 static struct platform_driver stm32_cec_driver = {
 	.probe  = stm32_cec_probe,
-	.remove_new = stm32_cec_remove,
+	.remove = stm32_cec_remove,
 	.driver = {
 		.name		= CEC_NAME,
 		.of_match_table = stm32_cec_of_match,
diff --git a/drivers/media/cec/platform/tegra/tegra_cec.c b/drivers/media/cec/platform/tegra/tegra_cec.c
index 7c1022cee1e8..3ed50097262f 100644
--- a/drivers/media/cec/platform/tegra/tegra_cec.c
+++ b/drivers/media/cec/platform/tegra/tegra_cec.c
@@ -465,7 +465,7 @@ static struct platform_driver tegra_cec_driver = {
 		.of_match_table = tegra_cec_of_match,
 	},
 	.probe = tegra_cec_probe,
-	.remove_new = tegra_cec_remove,
+	.remove = tegra_cec_remove,
 
 #ifdef CONFIG_PM
 	.suspend = tegra_cec_suspend,
diff --git a/drivers/media/cec/usb/Kconfig b/drivers/media/cec/usb/Kconfig
index 3f3a5c75287a..6faf4742981d 100644
--- a/drivers/media/cec/usb/Kconfig
+++ b/drivers/media/cec/usb/Kconfig
@@ -3,6 +3,7 @@
 # USB drivers
 
 if USB_SUPPORT && TTY
+source "drivers/media/cec/usb/extron-da-hd-4k-plus/Kconfig"
 source "drivers/media/cec/usb/pulse8/Kconfig"
 source "drivers/media/cec/usb/rainshadow/Kconfig"
 endif
diff --git a/drivers/media/cec/usb/Makefile b/drivers/media/cec/usb/Makefile
index e4183d1bfa9a..c082679f5318 100644
--- a/drivers/media/cec/usb/Makefile
+++ b/drivers/media/cec/usb/Makefile
@@ -2,5 +2,6 @@
 #
 # Makefile for the CEC USB device drivers.
 #
+obj-$(CONFIG_USB_EXTRON_DA_HD_4K_PLUS_CEC) += extron-da-hd-4k-plus/
 obj-$(CONFIG_USB_PULSE8_CEC) += pulse8/
 obj-$(CONFIG_USB_RAINSHADOW_CEC) += rainshadow/
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/Kconfig b/drivers/media/cec/usb/extron-da-hd-4k-plus/Kconfig
new file mode 100644
index 000000000000..5354f0eebe5c
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/Kconfig
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config USB_EXTRON_DA_HD_4K_PLUS_CEC
+	tristate "Extron DA HD 4K Plus CEC driver"
+	depends on VIDEO_DEV
+	depends on USB
+	depends on USB_ACM
+	select CEC_CORE
+	select SERIO
+	select SERIO_SERPORT
+	help
+	  This is a CEC driver for the Extron DA HD 4K Plus HDMI Splitter.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called extron-da-hd-4k-plus-cec.
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/Makefile b/drivers/media/cec/usb/extron-da-hd-4k-plus/Makefile
new file mode 100644
index 000000000000..2e8f7f60263f
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/Makefile
@@ -0,0 +1,8 @@
+extron-da-hd-4k-plus-cec-objs := extron-da-hd-4k-plus.o cec-splitter.o
+obj-$(CONFIG_USB_EXTRON_DA_HD_4K_PLUS_CEC) := extron-da-hd-4k-plus-cec.o
+
+all:
+	$(MAKE) -C $(KDIR) M=$(shell pwd) modules
+
+install:
+	$(MAKE) -C $(KDIR) M=$(shell pwd) modules_install
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.c b/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.c
new file mode 100644
index 000000000000..73fdec4b791d
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.c
@@ -0,0 +1,657 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
+/*
+ * Copyright 2021-2024 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <media/cec.h>
+
+#include "cec-splitter.h"
+
+/*
+ * Helper function to reply to a received message with a Feature Abort
+ * message.
+ */
+static int cec_feature_abort_reason(struct cec_adapter *adap,
+				    struct cec_msg *msg, u8 reason)
+{
+	struct cec_msg tx_msg = { };
+
+	/*
+	 * Don't reply with CEC_MSG_FEATURE_ABORT to a CEC_MSG_FEATURE_ABORT
+	 * message!
+	 */
+	if (msg->msg[1] == CEC_MSG_FEATURE_ABORT)
+		return 0;
+	/* Don't Feature Abort messages from 'Unregistered' */
+	if (cec_msg_initiator(msg) == CEC_LOG_ADDR_UNREGISTERED)
+		return 0;
+	cec_msg_set_reply_to(&tx_msg, msg);
+	cec_msg_feature_abort(&tx_msg, msg->msg[1], reason);
+	return cec_transmit_msg(adap, &tx_msg, false);
+}
+
+/* Transmit an Active Source message from this output port to a sink */
+static void cec_port_out_active_source(struct cec_splitter_port *p)
+{
+	struct cec_adapter *adap = p->adap;
+	struct cec_msg msg;
+
+	if (!adap->is_configured)
+		return;
+	p->is_active_source = true;
+	cec_msg_init(&msg, adap->log_addrs.log_addr[0], 0);
+	cec_msg_active_source(&msg, adap->phys_addr);
+	cec_transmit_msg(adap, &msg, false);
+}
+
+/* Transmit Active Source messages from all output ports to the sinks */
+static void cec_out_active_source(struct cec_splitter *splitter)
+{
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++)
+		cec_port_out_active_source(splitter->ports[i]);
+}
+
+/* Transmit a Standby message from this output port to a sink */
+static void cec_port_out_standby(struct cec_splitter_port *p)
+{
+	struct cec_adapter *adap = p->adap;
+	struct cec_msg msg;
+
+	if (!adap->is_configured)
+		return;
+	cec_msg_init(&msg, adap->log_addrs.log_addr[0], 0);
+	cec_msg_standby(&msg);
+	cec_transmit_msg(adap, &msg, false);
+}
+
+/* Transmit Standby messages from all output ports to the sinks */
+static void cec_out_standby(struct cec_splitter *splitter)
+{
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++)
+		cec_port_out_standby(splitter->ports[i]);
+}
+
+/* Transmit an Image/Text View On message from this output port to a sink */
+static void cec_port_out_wakeup(struct cec_splitter_port *p, u8 opcode)
+{
+	struct cec_adapter *adap = p->adap;
+	u8 la = adap->log_addrs.log_addr[0];
+	struct cec_msg msg;
+
+	if (la == CEC_LOG_ADDR_INVALID)
+		la = CEC_LOG_ADDR_UNREGISTERED;
+	cec_msg_init(&msg, la, 0);
+	msg.len = 2;
+	msg.msg[1] = opcode;
+	cec_transmit_msg(adap, &msg, false);
+}
+
+/* Transmit Image/Text View On messages from all output ports to the sinks */
+static void cec_out_wakeup(struct cec_splitter *splitter, u8 opcode)
+{
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++)
+		cec_port_out_wakeup(splitter->ports[i], opcode);
+}
+
+/*
+ * Update the power state of the unconfigured CEC device to either
+ * Off or On depending on the current state of the splitter.
+ * This keeps the outputs in a consistent state.
+ */
+void cec_splitter_unconfigured_output(struct cec_splitter_port *p)
+{
+	p->video_latency = 1;
+	p->power_status = p->splitter->is_standby ?
+		CEC_OP_POWER_STATUS_TO_STANDBY : CEC_OP_POWER_STATUS_TO_ON;
+
+	/* The adapter was unconfigured, so clear the sequence and ts values */
+	p->out_give_device_power_status_seq = 0;
+	p->out_give_device_power_status_ts = ktime_set(0, 0);
+	p->out_request_current_latency_seq = 0;
+	p->out_request_current_latency_ts = ktime_set(0, 0);
+}
+
+/*
+ * Update the power state of the newly configured CEC device to either
+ * Off or On depending on the current state of the splitter.
+ * This keeps the outputs in a consistent state.
+ */
+void cec_splitter_configured_output(struct cec_splitter_port *p)
+{
+	p->video_latency = 1;
+	p->power_status = p->splitter->is_standby ?
+		CEC_OP_POWER_STATUS_TO_STANDBY : CEC_OP_POWER_STATUS_TO_ON;
+
+	if (p->splitter->is_standby) {
+		/*
+		 * Some sinks only obey Standby if it comes from the
+		 * active source.
+		 */
+		cec_port_out_active_source(p);
+		cec_port_out_standby(p);
+	} else {
+		cec_port_out_wakeup(p, CEC_MSG_IMAGE_VIEW_ON);
+	}
+}
+
+/* Pass the in_msg on to all output ports */
+static void cec_out_passthrough(struct cec_splitter *splitter,
+				const struct cec_msg *in_msg)
+{
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+		struct cec_msg msg;
+
+		if (!adap->is_configured)
+			continue;
+		cec_msg_init(&msg, adap->log_addrs.log_addr[0], 0);
+		msg.len = in_msg->len;
+		memcpy(msg.msg + 1, in_msg->msg + 1, msg.len - 1);
+		cec_transmit_msg(adap, &msg, false);
+	}
+}
+
+/*
+ * See if all output ports received the Report Current Latency message,
+ * and if so, transmit the result from the input port to the video source.
+ */
+static void cec_out_report_current_latency(struct cec_splitter *splitter,
+					   struct cec_adapter *input_adap)
+{
+	struct cec_msg reply = {};
+	unsigned int reply_lat = 0;
+	unsigned int cnt = 0;
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+
+		/* Skip unconfigured ports */
+		if (!adap->is_configured)
+			continue;
+		/* Return if a port is still waiting for a reply */
+		if (p->out_request_current_latency_seq)
+			return;
+		reply_lat += p->video_latency - 1;
+		cnt++;
+	}
+
+	/*
+	 * All ports that can reply, replied, so clear the sequence
+	 * and timestamp values.
+	 */
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+
+		p->out_request_current_latency_seq = 0;
+		p->out_request_current_latency_ts = ktime_set(0, 0);
+	}
+
+	/*
+	 * Return if there were no replies or the input port is no longer
+	 * configured.
+	 */
+	if (!cnt || !input_adap->is_configured)
+		return;
+
+	/* Reply with the average latency */
+	reply_lat = 1 + reply_lat / cnt;
+	cec_msg_init(&reply, input_adap->log_addrs.log_addr[0],
+		     splitter->request_current_latency_dest);
+	cec_msg_report_current_latency(&reply, input_adap->phys_addr,
+				       reply_lat, 1, 1, 1);
+	cec_transmit_msg(input_adap, &reply, false);
+}
+
+/* Transmit Request Current Latency to all output ports */
+static int cec_out_request_current_latency(struct cec_splitter *splitter)
+{
+	ktime_t now = ktime_get();
+	bool error = true;
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+
+		if (!adap->is_configured) {
+			/* Clear if not configured */
+			p->out_request_current_latency_seq = 0;
+			p->out_request_current_latency_ts = ktime_set(0, 0);
+		} else if (!p->out_request_current_latency_seq) {
+			/*
+			 * Keep the old ts if an earlier request is still
+			 * pending. This ensures that the request will
+			 * eventually time out based on the timestamp of
+			 * the first request if the sink is unresponsive.
+			 */
+			p->out_request_current_latency_ts = now;
+		}
+	}
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+		struct cec_msg msg;
+
+		if (!adap->is_configured)
+			continue;
+		cec_msg_init(&msg, adap->log_addrs.log_addr[0], 0);
+		cec_msg_request_current_latency(&msg, true, adap->phys_addr);
+		if (cec_transmit_msg(adap, &msg, false))
+			continue;
+		p->out_request_current_latency_seq = msg.sequence | (1U << 31);
+		error = false;
+	}
+	return error ? -ENODEV : 0;
+}
+
+/*
+ * See if all output ports received the Report Power Status message,
+ * and if so, transmit the result from the input port to the video source.
+ */
+static void cec_out_report_power_status(struct cec_splitter *splitter,
+					struct cec_adapter *input_adap)
+{
+	struct cec_msg reply = {};
+	/* The target power status of the splitter itself */
+	u8 splitter_pwr = splitter->is_standby ?
+		CEC_OP_POWER_STATUS_STANDBY : CEC_OP_POWER_STATUS_ON;
+	/*
+	 * The transient power status of the splitter, used if not all
+	 * output report the target power status.
+	 */
+	u8 splitter_transient_pwr = splitter->is_standby ?
+		CEC_OP_POWER_STATUS_TO_STANDBY : CEC_OP_POWER_STATUS_TO_ON;
+	u8 reply_pwr = splitter_pwr;
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+
+		/* Skip if no sink was found (HPD was low for more than 5s) */
+		if (!p->found_sink)
+			continue;
+
+		/* Return if a port is still waiting for a reply */
+		if (p->out_give_device_power_status_seq)
+			return;
+		if (p->power_status != splitter_pwr)
+			reply_pwr = splitter_transient_pwr;
+	}
+
+	/*
+	 * All ports that can reply, replied, so clear the sequence
+	 * and timestamp values.
+	 */
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+
+		p->out_give_device_power_status_seq = 0;
+		p->out_give_device_power_status_ts = ktime_set(0, 0);
+	}
+
+	/* Return if the input port is no longer configured. */
+	if (!input_adap->is_configured)
+		return;
+
+	/* Reply with the new power status */
+	cec_msg_init(&reply, input_adap->log_addrs.log_addr[0],
+		     splitter->give_device_power_status_dest);
+	cec_msg_report_power_status(&reply, reply_pwr);
+	cec_transmit_msg(input_adap, &reply, false);
+}
+
+/* Transmit Give Device Power Status to all output ports */
+static int cec_out_give_device_power_status(struct cec_splitter *splitter)
+{
+	ktime_t now = ktime_get();
+	bool error = true;
+	unsigned int i;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+
+		/*
+		 * Keep the old ts if an earlier request is still
+		 * pending. This ensures that the request will
+		 * eventually time out based on the timestamp of
+		 * the first request if the sink is unresponsive.
+		 */
+		if (adap->is_configured && !p->out_give_device_power_status_seq)
+			p->out_give_device_power_status_ts = now;
+	}
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		struct cec_adapter *adap = p->adap;
+		struct cec_msg msg;
+
+		if (!adap->is_configured)
+			continue;
+
+		cec_msg_init(&msg, adap->log_addrs.log_addr[0], 0);
+		cec_msg_give_device_power_status(&msg, true);
+		if (cec_transmit_msg(adap, &msg, false))
+			continue;
+		p->out_give_device_power_status_seq = msg.sequence | (1U << 31);
+		error = false;
+	}
+	return error ? -ENODEV : 0;
+}
+
+/*
+ * CEC messages received on the HDMI input of the splitter are
+ * forwarded (if relevant) to the HDMI outputs of the splitter.
+ */
+int cec_splitter_received_input(struct cec_splitter_port *p, struct cec_msg *msg)
+{
+	if (!cec_msg_status_is_ok(msg))
+		return 0;
+
+	if (msg->len < 2)
+		return -ENOMSG;
+
+	switch (msg->msg[1]) {
+	case CEC_MSG_DEVICE_VENDOR_ID:
+	case CEC_MSG_REPORT_POWER_STATUS:
+	case CEC_MSG_SET_STREAM_PATH:
+	case CEC_MSG_ROUTING_CHANGE:
+	case CEC_MSG_REQUEST_ACTIVE_SOURCE:
+	case CEC_MSG_SYSTEM_AUDIO_MODE_STATUS:
+		return 0;
+
+	case CEC_MSG_STANDBY:
+		p->splitter->is_standby = true;
+		cec_out_standby(p->splitter);
+		return 0;
+
+	case CEC_MSG_IMAGE_VIEW_ON:
+	case CEC_MSG_TEXT_VIEW_ON:
+		p->splitter->is_standby = false;
+		cec_out_wakeup(p->splitter, msg->msg[1]);
+		return 0;
+
+	case CEC_MSG_ACTIVE_SOURCE:
+		cec_out_active_source(p->splitter);
+		return 0;
+
+	case CEC_MSG_SET_SYSTEM_AUDIO_MODE:
+		cec_out_passthrough(p->splitter, msg);
+		return 0;
+
+	case CEC_MSG_GIVE_DEVICE_POWER_STATUS:
+		p->splitter->give_device_power_status_dest =
+			cec_msg_initiator(msg);
+		if (cec_out_give_device_power_status(p->splitter))
+			cec_feature_abort_reason(p->adap, msg,
+						 CEC_OP_ABORT_INCORRECT_MODE);
+		return 0;
+
+	case CEC_MSG_REQUEST_CURRENT_LATENCY: {
+		u16 pa;
+
+		p->splitter->request_current_latency_dest =
+			cec_msg_initiator(msg);
+		cec_ops_request_current_latency(msg, &pa);
+		if (pa == p->adap->phys_addr &&
+		    cec_out_request_current_latency(p->splitter))
+			cec_feature_abort_reason(p->adap, msg,
+						 CEC_OP_ABORT_INCORRECT_MODE);
+		return 0;
+	}
+
+	default:
+		return -ENOMSG;
+	}
+	return -ENOMSG;
+}
+
+void cec_splitter_nb_transmit_canceled_output(struct cec_splitter_port *p,
+					      const struct cec_msg *msg,
+					      struct cec_adapter *input_adap)
+{
+	struct cec_splitter *splitter = p->splitter;
+	u32 seq = msg->sequence | (1U << 31);
+
+	/*
+	 * If this is the result of a failed non-blocking transmit, or it is
+	 * the result of the failed reply to a non-blocking transmit, then
+	 * check if the original transmit was to get the current power status
+	 * or latency and, if so, assume that the remove device is for one
+	 * reason or another unavailable and assume that it is in the same
+	 * power status as the splitter, or has no video latency.
+	 */
+	if ((cec_msg_recv_is_tx_result(msg) && !(msg->tx_status & CEC_TX_STATUS_OK)) ||
+	    (cec_msg_recv_is_rx_result(msg) && !(msg->rx_status & CEC_RX_STATUS_OK))) {
+		u8 tx_op = msg->msg[1];
+
+		if (msg->len < 2)
+			return;
+		if (cec_msg_recv_is_rx_result(msg) &&
+		    (msg->rx_status & CEC_RX_STATUS_FEATURE_ABORT))
+			tx_op = msg->msg[2];
+		switch (tx_op) {
+		case CEC_MSG_GIVE_DEVICE_POWER_STATUS:
+			if (p->out_give_device_power_status_seq != seq)
+				break;
+			p->out_give_device_power_status_seq = 0;
+			p->out_give_device_power_status_ts = ktime_set(0, 0);
+			p->power_status = splitter->is_standby ?
+					  CEC_OP_POWER_STATUS_STANDBY :
+					  CEC_OP_POWER_STATUS_ON;
+			cec_out_report_power_status(splitter, input_adap);
+			break;
+		case CEC_MSG_REQUEST_CURRENT_LATENCY:
+			if (p->out_request_current_latency_seq != seq)
+				break;
+			p->video_latency = 1;
+			p->out_request_current_latency_seq = 0;
+			p->out_request_current_latency_ts = ktime_set(0, 0);
+			cec_out_report_current_latency(splitter, input_adap);
+			break;
+		}
+		return;
+	}
+
+	if (cec_msg_recv_is_tx_result(msg)) {
+		if (p->out_request_current_latency_seq != seq)
+			return;
+		p->out_request_current_latency_ts = ns_to_ktime(msg->tx_ts);
+		return;
+	}
+}
+
+/*
+ * CEC messages received on an HDMI output of the splitter
+ * are processed here.
+ */
+int cec_splitter_received_output(struct cec_splitter_port *p, struct cec_msg *msg,
+				 struct cec_adapter *input_adap)
+{
+	struct cec_adapter *adap = p->adap;
+	struct cec_splitter *splitter = p->splitter;
+	u32 seq = msg->sequence | (1U << 31);
+	struct cec_msg reply = {};
+	u16 pa;
+
+	if (!adap->is_configured || msg->len < 2)
+		return -ENOMSG;
+
+	switch (msg->msg[1]) {
+	case CEC_MSG_REPORT_POWER_STATUS: {
+		u8 pwr;
+
+		cec_ops_report_power_status(msg, &pwr);
+		if (pwr > CEC_OP_POWER_STATUS_TO_STANDBY)
+			pwr = splitter->is_standby ?
+				CEC_OP_POWER_STATUS_TO_STANDBY :
+				CEC_OP_POWER_STATUS_TO_ON;
+		p->power_status = pwr;
+		if (p->out_give_device_power_status_seq == seq) {
+			p->out_give_device_power_status_seq = 0;
+			p->out_give_device_power_status_ts = ktime_set(0, 0);
+		}
+		cec_out_report_power_status(splitter, input_adap);
+		return 0;
+	}
+
+	case CEC_MSG_REPORT_CURRENT_LATENCY: {
+		u8 video_lat;
+		u8 low_lat_mode;
+		u8 audio_out_comp;
+		u8 audio_out_delay;
+
+		cec_ops_report_current_latency(msg, &pa,
+					       &video_lat, &low_lat_mode,
+					       &audio_out_comp, &audio_out_delay);
+		if (!video_lat || video_lat >= 252)
+			video_lat = 1;
+		p->video_latency = video_lat;
+		if (p->out_request_current_latency_seq == seq) {
+			p->out_request_current_latency_seq = 0;
+			p->out_request_current_latency_ts = ktime_set(0, 0);
+		}
+		cec_out_report_current_latency(splitter, input_adap);
+		return 0;
+	}
+
+	case CEC_MSG_STANDBY:
+	case CEC_MSG_ROUTING_CHANGE:
+	case CEC_MSG_GIVE_SYSTEM_AUDIO_MODE_STATUS:
+		return 0;
+
+	case CEC_MSG_ACTIVE_SOURCE:
+		cec_ops_active_source(msg, &pa);
+		if (pa == 0)
+			p->is_active_source = false;
+		return 0;
+
+	case CEC_MSG_REQUEST_ACTIVE_SOURCE:
+		if (!p->is_active_source)
+			return 0;
+		cec_msg_set_reply_to(&reply, msg);
+		cec_msg_active_source(&reply, adap->phys_addr);
+		cec_transmit_msg(adap, &reply, false);
+		return 0;
+
+	case CEC_MSG_GIVE_DEVICE_POWER_STATUS:
+		cec_msg_set_reply_to(&reply, msg);
+		cec_msg_report_power_status(&reply, splitter->is_standby ?
+					    CEC_OP_POWER_STATUS_STANDBY :
+					    CEC_OP_POWER_STATUS_ON);
+		cec_transmit_msg(adap, &reply, false);
+		return 0;
+
+	case CEC_MSG_SET_STREAM_PATH:
+		cec_ops_set_stream_path(msg, &pa);
+		if (pa == adap->phys_addr) {
+			cec_msg_set_reply_to(&reply, msg);
+			cec_msg_active_source(&reply, pa);
+			cec_transmit_msg(adap, &reply, false);
+		}
+		return 0;
+
+	default:
+		return -ENOMSG;
+	}
+	return -ENOMSG;
+}
+
+/*
+ * Called every second to check for timed out messages and whether there
+ * still is a video sink connected or not.
+ *
+ * Returns true if sinks were lost.
+ */
+bool cec_splitter_poll(struct cec_splitter *splitter,
+		       struct cec_adapter *input_adap, bool debug)
+{
+	ktime_t now = ktime_get();
+	u8 pwr = splitter->is_standby ?
+		 CEC_OP_POWER_STATUS_STANDBY : CEC_OP_POWER_STATUS_ON;
+	unsigned int max_delay_ms = input_adap->xfer_timeout_ms + 2000;
+	unsigned int i;
+	bool res = false;
+
+	for (i = 0; i < splitter->num_out_ports; i++) {
+		struct cec_splitter_port *p = splitter->ports[i];
+		s64 pwr_delta, lat_delta;
+		bool pwr_timeout, lat_timeout;
+
+		if (!p)
+			continue;
+
+		pwr_delta = ktime_ms_delta(now, p->out_give_device_power_status_ts);
+		pwr_timeout = p->out_give_device_power_status_seq &&
+			      pwr_delta >= max_delay_ms;
+		lat_delta = ktime_ms_delta(now, p->out_request_current_latency_ts);
+		lat_timeout = p->out_request_current_latency_seq &&
+			      lat_delta >= max_delay_ms;
+
+		/*
+		 * If the HPD is low for more than 5 seconds, then assume no display
+		 * is connected.
+		 */
+		if (p->found_sink && ktime_to_ns(p->lost_sink_ts) &&
+		    ktime_ms_delta(now, p->lost_sink_ts) > 5000) {
+			if (debug)
+				dev_info(splitter->dev,
+					 "port %u: HPD low for more than 5s, assume no sink is connected.\n",
+					 p->port);
+			p->found_sink = false;
+			p->lost_sink_ts = ktime_set(0, 0);
+			res = true;
+		}
+
+		/*
+		 * If the power status request timed out, then set the port's
+		 * power status to that of the splitter, ensuring a consistent
+		 * power state.
+		 */
+		if (pwr_timeout) {
+			mutex_lock(&p->adap->lock);
+			if (debug)
+				dev_info(splitter->dev,
+					 "port %u: give up on power status for seq %u\n",
+					 p->port,
+					 p->out_give_device_power_status_seq & ~(1 << 31));
+			p->power_status = pwr;
+			p->out_give_device_power_status_seq = 0;
+			p->out_give_device_power_status_ts = ktime_set(0, 0);
+			mutex_unlock(&p->adap->lock);
+			cec_out_report_power_status(splitter, input_adap);
+		}
+
+		/*
+		 * If the current latency request timed out, then set the port's
+		 * latency to 1.
+		 */
+		if (lat_timeout) {
+			mutex_lock(&p->adap->lock);
+			if (debug)
+				dev_info(splitter->dev,
+					 "port %u: give up on latency for seq %u\n",
+					 p->port,
+					 p->out_request_current_latency_seq & ~(1 << 31));
+			p->video_latency = 1;
+			p->out_request_current_latency_seq = 0;
+			p->out_request_current_latency_ts = ktime_set(0, 0);
+			mutex_unlock(&p->adap->lock);
+			cec_out_report_current_latency(splitter, input_adap);
+		}
+	}
+	return res;
+}
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.h b/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.h
new file mode 100644
index 000000000000..7422f7c5719e
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/cec-splitter.h
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/*
+ * Copyright 2021-2024 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _CEC_SPLITTER_H_
+#define _CEC_SPLITTER_H_
+
+struct cec_splitter;
+
+#define STATE_CHANGE_MAX_REPEATS 2
+
+struct cec_splitter_port {
+	struct cec_splitter *splitter;
+	struct cec_adapter *adap;
+	unsigned int port;
+	bool is_active_source;
+	bool found_sink;
+	ktime_t lost_sink_ts;
+	u32 out_request_current_latency_seq;
+	ktime_t out_request_current_latency_ts;
+	u8 video_latency;
+	u32 out_give_device_power_status_seq;
+	ktime_t out_give_device_power_status_ts;
+	u8 power_status;
+};
+
+struct cec_splitter {
+	struct device *dev;
+	unsigned int num_out_ports;
+	struct cec_splitter_port **ports;
+
+	/* High-level splitter state */
+	u8 request_current_latency_dest;
+	u8 give_device_power_status_dest;
+	bool is_standby;
+};
+
+void cec_splitter_unconfigured_output(struct cec_splitter_port *port);
+void cec_splitter_configured_output(struct cec_splitter_port *port);
+int cec_splitter_received_input(struct cec_splitter_port *port, struct cec_msg *msg);
+int cec_splitter_received_output(struct cec_splitter_port *port, struct cec_msg *msg,
+				 struct cec_adapter *input_adap);
+void cec_splitter_nb_transmit_canceled_output(struct cec_splitter_port *port,
+					      const struct cec_msg *msg,
+					      struct cec_adapter *input_adap);
+bool cec_splitter_poll(struct cec_splitter *splitter,
+		       struct cec_adapter *input_adap, bool debug);
+
+#endif
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.c b/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.c
new file mode 100644
index 000000000000..41d019b01ec0
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.c
@@ -0,0 +1,1836 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright 2021-2024 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+/*
+ * Currently this driver does not fully support the serial port of the
+ * Extron, only the USB port is fully supported.
+ *
+ * Issues specific to using the serial port instead of the USB since the
+ * serial port doesn't detect if the device is powered off:
+ *
+ * - Some periodic ping mechanism is needed to detect when the Extron is
+ *   powered off and when it is powered on again.
+ * - What to do when it is powered off and the driver is modprobed? Keep
+ *   trying to contact the Extron indefinitely?
+ */
+
+#include <linux/completion.h>
+#include <linux/ctype.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+
+#include "extron-da-hd-4k-plus.h"
+
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
+MODULE_DESCRIPTION("Extron DA HD 4K PLUS HDMI CEC driver");
+MODULE_LICENSE("GPL");
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-1)");
+
+static unsigned int vendor_id;
+module_param(vendor_id, uint, 0444);
+MODULE_PARM_DESC(vendor_id, "CEC Vendor ID");
+
+static char manufacturer_name[4];
+module_param_string(manufacturer_name, manufacturer_name,
+		    sizeof(manufacturer_name), 0644);
+MODULE_PARM_DESC(manufacturer_name,
+		 "EDID Vendor String (3 uppercase characters)");
+
+static bool hpd_never_low;
+module_param(hpd_never_low, bool, 0644);
+MODULE_PARM_DESC(hpd_never_low, "Input HPD will never go low (1), or go low if all output HPDs are low (0, default)");
+
+#define EXTRON_TIMEOUT_SECS 6
+
+static const u8 hdmi_edid[256] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x31, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x20, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 0x20, 0x00, 0x00, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x3a,
+	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
+	0x45, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x87, 0x11, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x68,
+	0x64, 0x6d, 0x69, 0x2d, 0x31, 0x30, 0x38, 0x30,
+	0x70, 0x36, 0x30, 0x0a, 0x00, 0x00, 0x00, 0xfe,
+	0x00, 0x73, 0x65, 0x72, 0x69, 0x6f, 0x0a, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x01, 0x95,
+
+	0x02, 0x03, 0x1b, 0xf1, 0x42, 0x10, 0x01, 0x23,
+	0x09, 0x07, 0x07, 0x83, 0x01, 0x00, 0x00, 0x68,
+	0x03, 0x0c, 0x00, 0x10, 0x00, 0x00, 0x21, 0x01,
+	0xe2, 0x00, 0xca, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x89,
+};
+
+static const u8 hdmi_edid_4k_300[256] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x31, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x20, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 0x20, 0x00, 0x00, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x3a,
+	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
+	0x45, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x87, 0x3c, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x68,
+	0x64, 0x6d, 0x69, 0x2d, 0x34, 0x6b, 0x2d, 0x36,
+	0x30, 0x30, 0x0a, 0x20, 0x00, 0x00, 0x00, 0xfe,
+	0x00, 0x73, 0x65, 0x72, 0x69, 0x6f, 0x0a, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x01, 0x87,
+
+	0x02, 0x03, 0x1f, 0xf1, 0x43, 0x10, 0x5f, 0x01,
+	0x23, 0x09, 0x07, 0x07, 0x83, 0x01, 0x00, 0x00,
+	0x6b, 0x03, 0x0c, 0x00, 0x10, 0x00, 0x00, 0x3c,
+	0x21, 0x00, 0x20, 0x01, 0xe2, 0x00, 0xca, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc6,
+};
+
+static const u8 hdmi_edid_4k_600[256] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x31, 0xd8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x01, 0x20, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 0x20, 0x00, 0x00, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x08, 0xe8,
+	0x00, 0x30, 0xf2, 0x70, 0x5a, 0x80, 0xb0, 0x58,
+	0x8a, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x87, 0x3c, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x68,
+	0x64, 0x6d, 0x69, 0x2d, 0x34, 0x6b, 0x2d, 0x36,
+	0x30, 0x30, 0x0a, 0x20, 0x00, 0x00, 0x00, 0xfe,
+	0x00, 0x73, 0x65, 0x72, 0x69, 0x6f, 0x0a, 0x20,
+	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x01, 0x4c,
+
+	0x02, 0x03, 0x28, 0xf1, 0x44, 0x61, 0x5f, 0x10,
+	0x01, 0x23, 0x09, 0x07, 0x07, 0x83, 0x01, 0x00,
+	0x00, 0x6b, 0x03, 0x0c, 0x00, 0x10, 0x00, 0x00,
+	0x3c, 0x21, 0x00, 0x20, 0x01, 0x67, 0xd8, 0x5d,
+	0xc4, 0x01, 0x78, 0x00, 0x00, 0xe2, 0x00, 0xca,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82,
+};
+
+static int extron_send_byte(struct serio *serio, char byte)
+{
+	int err, i;
+
+	for (i = 0; i < 100; i++) {
+		err = serio_write(serio, byte);
+		if (!err)
+			break;
+		usleep_range(80, 120);
+	}
+	if (err)
+		dev_warn(&serio->dev, "unable to write byte after 100 attempts\n");
+	return err ? -EIO : 0;
+}
+
+static int extron_send_len(struct serio *serio, const char *command,
+			   const unsigned char *bin, unsigned int len)
+{
+	int err = 0;
+
+	for (; !err && *command; command++)
+		err = extron_send_byte(serio, *command);
+	if (!err)
+		err = extron_send_byte(serio, '\r');
+	if (bin)
+		for (; !err && len; len--)
+			err = extron_send_byte(serio, *bin++);
+	return err;
+}
+
+static int extron_send_and_wait_len(struct extron *extron, struct extron_port *port,
+				    const char *cmd, const unsigned char *bin,
+				    unsigned int len, const char *response)
+{
+	int timeout = EXTRON_TIMEOUT_SECS * HZ;
+	int err;
+
+	if (debug) {
+		if (response)
+			dev_info(extron->dev, "transmit %s (response: %s)\n",
+				 cmd, response);
+		else
+			dev_info(extron->dev, "transmit %s\n", cmd);
+	}
+
+	mutex_lock(&extron->serio_lock);
+	if (port) {
+		init_completion(&port->cmd_done);
+		port->cmd_error = 0;
+		port->response = response;
+	} else {
+		init_completion(&extron->cmd_done);
+		extron->cmd_error = 0;
+		extron->response = response;
+	}
+	err = extron_send_len(extron->serio, cmd, bin, len);
+
+	if (!err && response &&
+	    !wait_for_completion_timeout(port ? &port->cmd_done : &extron->cmd_done, timeout)) {
+		dev_info(extron->dev, "transmit %s failed with %s (expected: %s)\n",
+			 cmd, extron->reply, response);
+		err = -ETIMEDOUT;
+	}
+
+	if (!err && response && (port ? port->cmd_error : extron->cmd_error)) {
+		dev_info(extron->dev, "transmit %s failed with E%02u (expected: %s)\n",
+			 cmd, port ? port->cmd_error : extron->cmd_error, response);
+		if (port)
+			port->cmd_error = 0;
+		else
+			extron->cmd_error = 0;
+		err = -EPROTO;
+	}
+	if (port)
+		port->response = NULL;
+	else
+		extron->response = NULL;
+	mutex_unlock(&extron->serio_lock);
+	return err;
+}
+
+static int extron_send_and_wait(struct extron *extron, struct extron_port *port,
+				const char *cmd, const char *response)
+{
+	return extron_send_and_wait_len(extron, port, cmd, NULL, 0, response);
+}
+
+static void extron_parse_edid(struct extron_port *port)
+{
+	const u8 *edid = port->edid;
+	unsigned int i, end;
+	u8 d;
+
+	port->has_4kp30 = false;
+	port->has_4kp60 = false;
+	port->has_qy = false;
+	port->has_qs = false;
+	/* Store Established Timings 1 and 2 */
+	port->est_i = edid[0x23];
+	port->est_ii = edid[0x24];
+
+	// Check DTDs in base block
+	for (i = 0; i < 4; i++) {
+		const u8 *dtd = edid + 0x36 + i * 18;
+		unsigned int w, h;
+		unsigned int mhz;
+		u64 pclk;
+
+		if (!dtd[0] && !dtd[1])
+			continue;
+		w = dtd[2] + ((dtd[4] & 0xf0) << 4);
+		h = dtd[5] + ((dtd[7] & 0xf0) << 4);
+		if (w != 3840 || h != 2160)
+			continue;
+
+		w += dtd[3] + ((dtd[4] & 0x0f) << 8);
+		h += dtd[6] + ((dtd[7] & 0x0f) << 8);
+		pclk = dtd[0] + (dtd[1] << 8);
+		pclk *= 100000;
+		mhz = div_u64(pclk, w * h);
+		if (mhz >= 297)
+			port->has_4kp30 = true;
+		if (mhz >= 594)
+			port->has_4kp60 = true;
+	}
+
+	if (port->edid_blocks == 1)
+		return;
+
+	edid += 128;
+
+	/* Return if not a CTA-861 extension block */
+	if (edid[0] != 0x02 || edid[1] != 0x03)
+		return;
+
+	/* search Video Data Block (tag 2) */
+	d = edid[2] & 0x7f;
+	/* Check if there are Data Blocks */
+	if (d <= 4)
+		return;
+
+	i = 4;
+	end = d;
+
+	do {
+		u8 tag = edid[i] >> 5;
+		u8 len = edid[i] & 0x1f;
+
+		/* Avoid buffer overrun in case the EDID is malformed */
+		if (i + len + 1 > 0x7f)
+			return;
+
+		switch (tag) {
+		case 2: /* Video Data Block */
+			/* Search for VIC 97 */
+			if (memchr(edid + i + 1, 97, len))
+				port->has_4kp60 = true;
+			/* Search for VIC 95 */
+			if (memchr(edid + i + 1, 95, len))
+				port->has_4kp30 = true;
+			break;
+
+		case 7: /* Use Extended Tag */
+			switch (edid[i + 1]) {
+			case 0: /* Video Capability Data Block */
+				if (edid[i + 2] & 0x80)
+					port->has_qy = true;
+				if (edid[i + 2] & 0x40)
+					port->has_qs = true;
+				break;
+			}
+			break;
+		}
+		i += len + 1;
+	} while (i < end);
+}
+
+static int get_edid_tag_location(const u8 *edid, unsigned int size,
+				 u8 want_tag, u8 ext_tag)
+{
+	unsigned int offset = 128;
+	int i, end;
+	u8 d;
+
+	edid += offset;
+
+	/* Return if not a CTA-861 extension block */
+	if (size < 256 || edid[0] != 0x02 || edid[1] != 0x03)
+		return -ENOENT;
+
+	/* search tag */
+	d = edid[0x02] & 0x7f;
+	if (d <= 4)
+		return -ENOENT;
+
+	i = 0x04;
+	end = 0x00 + d;
+
+	do {
+		unsigned char tag = edid[i] >> 5;
+		unsigned char len = edid[i] & 0x1f;
+
+		if (tag != want_tag || i + len > end) {
+			i += len + 1;
+			continue;
+		}
+
+		if (tag < 7 || (len >= 1 && edid[i + 1] == ext_tag))
+			return offset + i;
+		i += len + 1;
+	} while (i < end);
+	return -ENOENT;
+}
+
+static void extron_edid_crc(u8 *edid)
+{
+	u8 sum = 0;
+	int offset;
+
+	/* Update CRC */
+	for (offset = 0; offset < 127; offset++)
+		sum += edid[offset];
+	edid[127] = 256 - sum;
+}
+
+/*
+ * Fill in EDID string. As per VESA EDID-1.3, strings are at most 13 chars
+ * long. If shorter then add a 0x0a character after the string and pad the
+ * remainder with spaces.
+ */
+static void extron_set_edid_string(u8 *start, const char *s)
+{
+	const unsigned int max_len = 13;
+	int len = strlen(s);
+
+	memset(start, ' ', max_len);
+	if (len > max_len)
+		len = max_len;
+	memcpy(start, s, len);
+	if (len < max_len)
+		start[len] = 0x0a;
+}
+
+static void extron_update_edid(struct extron_port *port, unsigned int blocks)
+{
+	int offset;
+	u8 c1, c2;
+
+	c1 = ((manufacturer_name[0] - '@') << 2) |
+		(((manufacturer_name[1] - '@') >> 3) & 0x03);
+	c2 = (((manufacturer_name[1] - '@') & 0x07) << 5) |
+		((manufacturer_name[2] - '@') & 0x1f);
+
+	port->edid_tmp[8] = c1;
+	port->edid_tmp[9] = c2;
+
+	/* Set Established Timings, but always enable VGA */
+	port->edid_tmp[0x23] = port->est_i | 0x20;
+	port->edid_tmp[0x24] = port->est_ii;
+
+	/* Set the Monitor Name to the unit name */
+	extron_set_edid_string(port->edid_tmp + 0x5f, port->extron->unit_name);
+	/* Set the ASCII String to the CEC adapter name */
+	extron_set_edid_string(port->edid_tmp + 0x71, port->adap->name);
+
+	extron_edid_crc(port->edid_tmp);
+
+	/* Find Video Capability Data Block */
+	offset = get_edid_tag_location(port->edid_tmp, blocks * 128, 7, 0);
+	if (offset > 0) {
+		port->edid_tmp[offset + 2] &= ~0xc0;
+		if (port->has_qy)
+			port->edid_tmp[offset + 2] |= 0x80;
+		if (port->has_qs)
+			port->edid_tmp[offset + 2] |= 0x40;
+	}
+
+	extron_edid_crc(port->edid_tmp + 128);
+}
+
+static int extron_write_edid(struct extron_port *port,
+			     const u8 *edid, unsigned int blocks)
+{
+	struct extron *extron = port->extron;
+	u16 phys_addr = CEC_PHYS_ADDR_INVALID;
+	int ret;
+
+	if (cec_get_edid_spa_location(edid, blocks * 128))
+		phys_addr = 0;
+
+	if (mutex_lock_interruptible(&extron->edid_lock))
+		return -EINTR;
+
+	memcpy(port->edid_tmp, edid, blocks * 128);
+
+	if (manufacturer_name[0])
+		extron_update_edid(port, blocks);
+
+	ret = extron_send_and_wait_len(port->extron, port, "W+UF256,in.bin",
+				       port->edid_tmp, sizeof(port->edid_tmp),
+				       "Upl");
+	if (ret)
+		goto unlock;
+	ret = extron_send_and_wait(port->extron, port, "WI1,in.binEDID",
+				   "EdidI01");
+	if (ret)
+		goto unlock;
+
+	port->edid_blocks = blocks;
+	memcpy(port->edid, port->edid_tmp, blocks * 128);
+	port->read_edid = true;
+	mutex_unlock(&extron->edid_lock);
+
+	cec_s_phys_addr(port->adap, phys_addr, false);
+	return 0;
+
+unlock:
+	mutex_unlock(&extron->edid_lock);
+	return ret;
+}
+
+static void update_edid_work(struct work_struct *w)
+{
+	struct extron *extron = container_of(w, struct extron,
+					     work_update_edid.work);
+	struct extron_port *in = extron->ports[extron->num_out_ports];
+	struct extron_port *p;
+	bool has_edid = false;
+	bool has_4kp30 = true;
+	bool has_4kp60 = true;
+	bool has_qy = true;
+	bool has_qs = true;
+	u8 est_i = 0xff;
+	u8 est_ii = 0xff;
+	unsigned int out;
+
+	for (out = 0; has_4kp60 && out < extron->num_out_ports; out++) {
+		p = extron->ports[out];
+		if (p->read_edid) {
+			has_4kp60 = p->has_4kp60;
+			est_i &= p->est_i;
+			est_ii &= p->est_ii;
+			has_edid = true;
+		}
+	}
+	for (out = 0; has_4kp30 && out < extron->num_out_ports; out++)
+		if (extron->ports[out]->read_edid)
+			has_4kp30 = extron->ports[out]->has_4kp30;
+
+	for (out = 0; has_qy && out < extron->num_out_ports; out++)
+		if (extron->ports[out]->read_edid)
+			has_qy = extron->ports[out]->has_qy;
+
+	for (out = 0; has_qs && out < extron->num_out_ports; out++)
+		if (extron->ports[out]->read_edid)
+			has_qs = extron->ports[out]->has_qs;
+
+	/* exit if no output port had an EDID */
+	if (!has_edid)
+		return;
+
+	/* exit if the input EDID properties remained unchanged */
+	if (has_4kp60 == in->has_4kp60 && has_4kp30 == in->has_4kp30 &&
+	    has_qy == in->has_qy && has_qs == in->has_qs &&
+	    est_i == in->est_i && est_ii == in->est_ii)
+		return;
+
+	in->has_4kp60 = has_4kp60;
+	in->has_4kp30 = has_4kp30;
+	in->has_qy = has_qy;
+	in->has_qs = has_qs;
+	in->est_i = est_i;
+	in->est_ii = est_ii;
+	extron_write_edid(extron->ports[extron->num_out_ports],
+			  has_4kp60 ? hdmi_edid_4k_600 :
+			  (has_4kp30 ? hdmi_edid_4k_300 : hdmi_edid), 2);
+}
+
+static void extron_read_edid(struct extron_port *port)
+{
+	struct extron *extron = port->extron;
+	char cmd[10], reply[10];
+	unsigned int idx;
+
+	idx = port->port.port + (port->is_input ? 0 : extron->num_in_ports);
+	snprintf(cmd, sizeof(cmd), "WR%uEDID", idx);
+	snprintf(reply, sizeof(reply), "EdidR%u", idx);
+	if (mutex_lock_interruptible(&extron->edid_lock))
+		return;
+	if (port->read_edid)
+		goto unlock;
+	extron->edid_bytes_read = 0;
+	extron->edid_port = port;
+	port->edid_blocks = 0;
+	if (!port->has_edid)
+		goto no_edid;
+
+	extron->edid_reading = true;
+
+	if (!extron_send_and_wait(extron, port, cmd, reply))
+		wait_for_completion_killable_timeout(&extron->edid_completion,
+						     msecs_to_jiffies(1000));
+	if (port->edid_blocks) {
+		extron_parse_edid(port);
+		port->read_edid = true;
+		if (!port->is_input)
+			v4l2_ctrl_s_ctrl(port->ctrl_tx_edid_present, 1);
+	}
+no_edid:
+	extron->edid_reading = false;
+unlock:
+	mutex_unlock(&extron->edid_lock);
+	cancel_delayed_work_sync(&extron->work_update_edid);
+	if (manufacturer_name[0])
+		schedule_delayed_work(&extron->work_update_edid,
+				      msecs_to_jiffies(1000));
+}
+
+static void extron_irq_work_handler(struct work_struct *work)
+{
+	struct extron_port *port =
+		container_of(work, struct extron_port, irq_work);
+	struct extron *extron = port->extron;
+	unsigned long flags;
+	bool update_pa;
+	u16 pa;
+	bool update_has_signal;
+	bool has_signal;
+	bool update_has_edid;
+	bool has_edid;
+	u32 status;
+
+	spin_lock_irqsave(&port->msg_lock, flags);
+	while (port->rx_msg_num) {
+		spin_unlock_irqrestore(&port->msg_lock, flags);
+		cec_received_msg(port->adap,
+				 &port->rx_msg[port->rx_msg_cur_idx]);
+		spin_lock_irqsave(&port->msg_lock, flags);
+		if (port->rx_msg_num)
+			port->rx_msg_num--;
+		port->rx_msg_cur_idx =
+			(port->rx_msg_cur_idx + 1) % NUM_MSGS;
+	}
+	update_pa = port->update_phys_addr;
+	pa = port->phys_addr;
+	port->update_phys_addr = false;
+	update_has_signal = port->update_has_signal;
+	has_signal = port->has_signal;
+	port->update_has_signal = false;
+	update_has_edid = port->update_has_edid;
+	has_edid = port->has_edid;
+	port->update_has_edid = false;
+	status = port->tx_done_status;
+	port->tx_done_status = 0;
+	spin_unlock_irqrestore(&port->msg_lock, flags);
+
+	if (status)
+		cec_transmit_done(port->adap, status, 0, 0, 0, 0);
+
+	if (update_has_signal && port->is_input)
+		v4l2_ctrl_s_ctrl(port->ctrl_rx_power_present, has_signal);
+
+	if (update_has_edid && !port->is_input) {
+		v4l2_ctrl_s_ctrl(port->ctrl_tx_hotplug,
+				 port->has_edid);
+		if (port->has_edid) {
+			port->port.found_sink = true;
+			port->port.lost_sink_ts = ktime_set(0, 0);
+		} else {
+			port->port.lost_sink_ts = ktime_get();
+		}
+		if (!has_edid) {
+			port->edid_blocks = 0;
+			port->read_edid = false;
+			if (extron->edid_reading && !has_edid &&
+			    extron->edid_port == port)
+				extron->edid_reading = false;
+			v4l2_ctrl_s_ctrl(port->ctrl_tx_edid_present, 0);
+		} else if (!extron->edid_reading || extron->edid_port != port) {
+			extron_read_edid(port);
+		}
+	}
+	if (update_pa)
+		cec_s_phys_addr(port->adap, pa, false);
+}
+
+static void extron_process_received(struct extron_port *port, const char *data)
+{
+	struct cec_msg msg = {};
+	unsigned int len = strlen(data);
+	unsigned long irq_flags;
+	unsigned int idx;
+
+	if (!port || port->disconnected)
+		return;
+
+	if (len < 5 || (len - 2) % 3 || data[len - 2] != '*')
+		goto malformed;
+
+	while (*data != '*') {
+		int v = hex2bin(&msg.msg[msg.len], data + 1, 1);
+
+		if (*data != '%' || v)
+			goto malformed;
+		msg.len++;
+		data += 3;
+	}
+
+	spin_lock_irqsave(&port->msg_lock, irq_flags);
+	idx = (port->rx_msg_cur_idx + port->rx_msg_num) %
+		NUM_MSGS;
+	if (port->rx_msg_num == NUM_MSGS) {
+		dev_warn(port->dev,
+			 "message queue is full, dropping %*ph\n",
+			 msg.len, msg.msg);
+		spin_unlock_irqrestore(&port->msg_lock,
+				       irq_flags);
+		return;
+	}
+	port->rx_msg_num++;
+	port->rx_msg[idx] = msg;
+	spin_unlock_irqrestore(&port->msg_lock, irq_flags);
+	if (!port->disconnected)
+		schedule_work(&port->irq_work);
+	return;
+
+malformed:
+	dev_info(port->extron->dev, "malformed msg received: '%s'\n", data);
+}
+
+static void extron_port_signal_change(struct extron_port *port, bool has_sig)
+{
+	unsigned long irq_flags;
+	bool update = false;
+
+	if (!port)
+		return;
+
+	spin_lock_irqsave(&port->msg_lock, irq_flags);
+	if (!port->update_has_signal && port->has_signal != has_sig) {
+		port->update_has_signal = true;
+		update = true;
+	}
+	port->has_signal = has_sig;
+	spin_unlock_irqrestore(&port->msg_lock, irq_flags);
+	if (update && !port->disconnected)
+		schedule_work(&port->irq_work);
+}
+
+static void extron_process_signal_change(struct extron *extron, const char *data)
+{
+	unsigned int i;
+
+	extron_port_signal_change(extron->ports[extron->num_out_ports],
+				  data[0] == '1');
+	for (i = 0; i < extron->num_out_ports; i++)
+		extron_port_signal_change(extron->ports[i],
+					  data[2 + 2 * i] != '0');
+}
+
+static void extron_port_edid_change(struct extron_port *port, bool has_edid)
+{
+	unsigned long irq_flags;
+	bool update = false;
+
+	if (!port)
+		return;
+
+	spin_lock_irqsave(&port->msg_lock, irq_flags);
+	if (!port->update_has_edid && port->has_edid != has_edid) {
+		port->update_has_edid = true;
+		update = true;
+	}
+	port->has_edid = has_edid;
+	spin_unlock_irqrestore(&port->msg_lock, irq_flags);
+	if (update && !port->disconnected)
+		schedule_work(&port->irq_work);
+}
+
+static void extron_process_edid_change(struct extron *extron, const char *data)
+{
+	unsigned int i;
+
+	/*
+	 * Do nothing if the Extron isn't ready yet. Trying to do this
+	 * while the Extron firmware is still settling will fail.
+	 */
+	if (!extron->is_ready)
+		return;
+
+	for (i = 0; i < extron->num_out_ports; i++)
+		extron_port_edid_change(extron->ports[i],
+					data[2 + 2 * i] != '0');
+}
+
+static void extron_phys_addr_change(struct extron_port *port, u16 pa)
+{
+	unsigned long irq_flags;
+	bool update = false;
+
+	if (!port)
+		return;
+
+	spin_lock_irqsave(&port->msg_lock, irq_flags);
+	if (!port->update_phys_addr && port->phys_addr != pa) {
+		update = true;
+		port->update_phys_addr = true;
+	}
+	port->phys_addr = pa;
+	spin_unlock_irqrestore(&port->msg_lock, irq_flags);
+	if (update && !port->disconnected)
+		schedule_work(&port->irq_work);
+}
+
+static void extron_process_tx_done(struct extron_port *port, char status)
+{
+	unsigned long irq_flags;
+	unsigned int tx_status;
+
+	if (!port)
+		return;
+
+	switch (status) {
+	case '0':
+		tx_status = CEC_TX_STATUS_NACK | CEC_TX_STATUS_MAX_RETRIES;
+		break;
+	case '1':
+		tx_status = CEC_TX_STATUS_OK;
+		break;
+	default:
+		tx_status = CEC_TX_STATUS_ERROR;
+		break;
+	}
+	spin_lock_irqsave(&port->msg_lock, irq_flags);
+	port->tx_done_status = tx_status;
+	spin_unlock_irqrestore(&port->msg_lock, irq_flags);
+	if (!port->disconnected)
+		schedule_work(&port->irq_work);
+}
+
+static void extron_add_edid(struct extron_port *port, const char *hex)
+{
+	struct extron *extron = port ? port->extron : NULL;
+
+	if (!port || port != extron->edid_port)
+		return;
+	while (extron->edid_bytes_read < sizeof(port->edid) && *hex) {
+		int err = hex2bin(&port->edid[extron->edid_bytes_read], hex, 1);
+
+		if (err) {
+			extron->edid_reading = false;
+			complete(&extron->edid_completion);
+			break;
+		}
+		extron->edid_bytes_read++;
+		hex += 2;
+	}
+	if (extron->edid_bytes_read == 128 &&
+	    port->edid[126] == 0) {
+		/* There are no extension blocks, we're done */
+		port->edid_blocks = 1;
+		extron->edid_reading = false;
+		complete(&extron->edid_completion);
+	}
+	if (extron->edid_bytes_read < sizeof(port->edid))
+		return;
+	if (!*hex)
+		port->edid_blocks = 2;
+	extron->edid_reading = false;
+	complete(&extron->edid_completion);
+}
+
+static irqreturn_t extron_interrupt(struct serio *serio, unsigned char data,
+				    unsigned int flags)
+{
+	struct extron *extron = serio_get_drvdata(serio);
+	struct extron_port *port = NULL;
+	bool found_response;
+	unsigned int p;
+
+	if (data == '\r' || data == '\n') {
+		if (extron->idx == 0)
+			return IRQ_HANDLED;
+		memcpy(extron->data, extron->buf, extron->idx);
+		extron->len = extron->idx;
+		extron->data[extron->len] = 0;
+		if (debug)
+			dev_info(extron->dev, "received %s\n", extron->data);
+		extron->idx = 0;
+		if (!memcmp(extron->data, "Sig", 3) &&
+		    extron->data[4] == '*') {
+			extron_process_signal_change(extron, extron->data + 3);
+		} else if (!memcmp(extron->data, "Hdcp", 4) &&
+			   extron->data[5] == '*') {
+			extron_process_edid_change(extron, extron->data + 4);
+		} else if (!memcmp(extron->data, "DcecI", 5) &&
+			   extron->data[5] >= '1' &&
+			   extron->data[5] < '1' + extron->num_in_ports) {
+			unsigned int p = extron->data[5] - '1';
+
+			p += extron->num_out_ports;
+			extron_process_tx_done(extron->ports[p],
+					       extron->data[extron->len - 1]);
+		} else if (!memcmp(extron->data, "Ceci", 4) &&
+			   extron->data[4] >= '1' &&
+			   extron->data[4] < '1' + extron->num_in_ports &&
+			   extron->data[5] == '*') {
+			unsigned int p = extron->data[4] - '1';
+
+			p += extron->num_out_ports;
+			extron_process_received(extron->ports[p],
+						extron->data + 6);
+		} else if (!memcmp(extron->data, "DcecO", 5) &&
+			   extron->data[5] >= '1' &&
+			   extron->data[5] < '1' + extron->num_out_ports) {
+			unsigned int p = extron->data[5] - '1';
+
+			extron_process_tx_done(extron->ports[p],
+					       extron->data[extron->len - 1]);
+		} else if (!memcmp(extron->data, "Ceco", 4) &&
+			   extron->data[4] >= '1' &&
+			   extron->data[4] < '1' + extron->num_out_ports &&
+			   extron->data[5] == '*') {
+			unsigned int p = extron->data[4] - '1';
+
+			extron_process_received(extron->ports[p],
+						extron->data + 6);
+		} else if (!memcmp(extron->data, "Pceco", 5) &&
+			   extron->data[5] >= '1' &&
+			   extron->data[5] < '1' + extron->num_out_ports) {
+			unsigned int p = extron->data[5] - '1';
+			unsigned int tmp_pa[2] = { 0xff, 0xff };
+
+			if (sscanf(extron->data + 7, "%%%02x%%%02x",
+				   &tmp_pa[0], &tmp_pa[1]) == 2)
+				extron_phys_addr_change(extron->ports[p],
+							tmp_pa[0] << 8 | tmp_pa[1]);
+		} else if (!memcmp(extron->data, "Pceci", 5) &&
+			   extron->data[5] >= '1' &&
+			   extron->data[5] < '1' + extron->num_in_ports) {
+			unsigned int p = extron->data[5] - '1';
+			unsigned int tmp_pa[2] = { 0xff, 0xff };
+
+			p += extron->num_out_ports;
+			if (sscanf(extron->data + 7, "%%%02x%%%02x",
+				   &tmp_pa[0], &tmp_pa[1]) == 2)
+				extron_phys_addr_change(extron->ports[p],
+							tmp_pa[0] << 8 | tmp_pa[1]);
+		} else if (!memcmp(extron->data, "EdidR", 5) &&
+			   extron->data[5] >= '1' &&
+			   extron->data[5] < '1' + extron->num_ports &&
+			   extron->data[6] == '*') {
+			unsigned int p = extron->data[5] - '1';
+
+			if (p)
+				p--;
+			else
+				p = extron->num_out_ports;
+			extron_add_edid(extron->ports[p], extron->data + 7);
+		} else if (extron->edid_reading && extron->len == 32 &&
+			   extron->edid_port) {
+			extron_add_edid(extron->edid_port, extron->data);
+		}
+
+		found_response = false;
+		if (extron->response &&
+		    !strncmp(extron->response, extron->data,
+			     strlen(extron->response)))
+			found_response = true;
+
+		for (p = 0; !found_response && p < extron->num_ports; p++) {
+			port = extron->ports[p];
+			if (port && port->response &&
+			    !strncmp(port->response, extron->data,
+				     strlen(port->response)))
+				found_response = true;
+		}
+
+		if (!found_response && extron->response &&
+		    extron->data[0] == 'E' &&
+		    isdigit(extron->data[1]) &&
+		    isdigit(extron->data[2]) &&
+		    !extron->data[3]) {
+			extron->cmd_error = (extron->data[1] - '0') * 10 +
+				extron->data[2] - '0';
+			extron->response = NULL;
+			complete(&extron->cmd_done);
+		}
+
+		if (!found_response)
+			return IRQ_HANDLED;
+
+		memcpy(extron->reply, extron->data, extron->len);
+		extron->reply[extron->len] = 0;
+		if (!port) {
+			extron->response = NULL;
+			complete(&extron->cmd_done);
+		} else {
+			port->response = NULL;
+			complete(&port->cmd_done);
+		}
+		return IRQ_HANDLED;
+	}
+
+	if (extron->idx >= DATA_SIZE - 1) {
+		dev_info(extron->dev,
+			 "throwing away %d bytes of garbage\n", extron->idx);
+		extron->idx = 0;
+	}
+	extron->buf[extron->idx++] = (char)data;
+	return IRQ_HANDLED;
+}
+
+static int extron_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+
+	return (port->disconnected && enable) ? -ENODEV : 0;
+}
+
+static int extron_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+	char cmd[26];
+	char resp[25];
+	u8 la = log_addr == CEC_LOG_ADDR_INVALID ? 15 : log_addr;
+	int err;
+
+	if (port->disconnected)
+		return -ENODEV;
+	snprintf(cmd, sizeof(cmd), "W%c%u*%uLCEC",
+		 port->direction, port->port.port, la);
+	snprintf(resp, sizeof(resp), "Lcec%c%u*%u",
+		 port->direction, port->port.port, la);
+	err = extron_send_and_wait(port->extron, port, cmd, resp);
+	return log_addr != CEC_LOG_ADDR_INVALID && err ? err : 0;
+}
+
+static int extron_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+				    u32 signal_free_time, struct cec_msg *msg)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+	char buf[(CEC_MAX_MSG_SIZE - 1) * 3 + 1];
+	char cmd[sizeof(buf) + 14];
+	unsigned int i;
+
+	if (port->disconnected)
+		return -ENODEV;
+	buf[0] = 0;
+	for (i = 0; i < msg->len - 1; i++)
+		sprintf(buf + i * 3, "%%%02X", msg->msg[i + 1]);
+	snprintf(cmd, sizeof(cmd), "W%c%u*%u*%u*%sDCEC",
+		 port->direction, port->port.port,
+		 cec_msg_initiator(msg), cec_msg_destination(msg), buf);
+	return extron_send_and_wait(port->extron, port, cmd, NULL);
+}
+
+static void extron_cec_adap_unconfigured(struct cec_adapter *adap)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+
+	if (port->disconnected)
+		return;
+	if (debug)
+		dev_info(port->extron->dev, "unconfigured port %d (%s)\n",
+			 port->port.port,
+			 port->extron->splitter.is_standby ? "Off" : "On");
+	if (!port->is_input)
+		cec_splitter_unconfigured_output(&port->port);
+}
+
+static void extron_cec_configured(struct cec_adapter *adap)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+
+	if (port->disconnected)
+		return;
+	if (debug)
+		dev_info(port->extron->dev, "configured port %d (%s)\n",
+			 port->port.port,
+			 port->extron->splitter.is_standby ? "Off" : "On");
+	if (!port->is_input)
+		cec_splitter_configured_output(&port->port);
+}
+
+static void extron_cec_adap_nb_transmit_canceled(struct cec_adapter *adap,
+						 const struct cec_msg *msg)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+	struct cec_adapter *input_adap;
+
+	if (!vendor_id)
+		return;
+	if (port->disconnected || port->is_input)
+		return;
+	input_adap = port->extron->ports[port->extron->num_out_ports]->adap;
+	cec_splitter_nb_transmit_canceled_output(&port->port, msg, input_adap);
+}
+
+static int extron_received(struct cec_adapter *adap, struct cec_msg *msg)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+
+	if (!vendor_id)
+		return -ENOMSG;
+	if (port->disconnected)
+		return -ENOMSG;
+	if (port->is_input)
+		return cec_splitter_received_input(&port->port, msg);
+	return cec_splitter_received_output(&port->port, msg,
+					    port->extron->ports[port->extron->num_out_ports]->adap);
+}
+
+#define log_printf(adap, file, fmt, arg...) \
+	do { \
+		if (file) \
+			seq_printf((file), fmt, ## arg); \
+		else \
+			pr_info("cec-%s: " fmt, (adap)->name, ## arg);	\
+	} while (0)
+
+static const char * const pwr_state[] = {
+	"on",
+	"standby",
+	"to on",
+	"to standby",
+};
+
+static void extron_adap_status_port(struct extron_port *port, struct seq_file *file)
+{
+	struct cec_adapter *adap = port->adap;
+
+	if (port->disconnected) {
+		log_printf(adap, file,
+			   "\tport %u: disconnected\n", port->port.port);
+		return;
+	}
+	if (port->is_input)
+		log_printf(adap, file,
+			   "\tport %u: %s signal, %s edid, %s 4kp30, %s 4kp60, %sQS/%sQY, is %s\n",
+			   port->port.port,
+			   port->has_signal ? "has" : "no",
+			   port->has_edid ? "has" : "no",
+			   port->has_4kp30 ? "has" : "no",
+			   port->has_4kp60 ? "has" : "no",
+			   port->has_qs ? "" : "no ",
+			   port->has_qy ? "" : "no ",
+			   !port->port.adap->is_configured ? "not configured" :
+				pwr_state[port->extron->splitter.is_standby]);
+	else
+		log_printf(adap, file,
+			   "\tport %u: %s sink, %s signal, %s edid, %s 4kp30, %s 4kp60, %sQS/%sQY, is %sactive source, is %s\n",
+			   port->port.port,
+			   port->port.found_sink ? "found" : "no",
+			   port->has_signal ? "has" : "no",
+			   port->has_edid ? "has" : "no",
+			   port->has_4kp30 ? "has" : "no",
+			   port->has_4kp60 ? "has" : "no",
+			   port->has_qs ? "" : "no ",
+			   port->has_qy ? "" : "no ",
+			   port->port.is_active_source ? "" : "not ",
+			   !port->port.adap->is_configured ? "not configured" :
+			   pwr_state[port->port.power_status & 3]);
+	if (port->port.out_give_device_power_status_seq)
+		log_printf(adap, file,
+			   "\tport %u: querying power status (%u, %lldms)\n",
+			   port->port.port,
+			   port->port.out_give_device_power_status_seq & ~(1 << 31),
+			   ktime_ms_delta(ktime_get(),
+					  port->port.out_give_device_power_status_ts));
+	if (port->port.out_request_current_latency_seq)
+		log_printf(adap, file,
+			   "\tport %u: querying latency (%u, %lldms)\n",
+			   port->port.port,
+			   port->port.out_request_current_latency_seq & ~(1 << 31),
+			   ktime_ms_delta(ktime_get(),
+					  port->port.out_request_current_latency_ts));
+}
+
+static void extron_adap_status(struct cec_adapter *adap, struct seq_file *file)
+{
+	struct extron_port *port = cec_get_drvdata(adap);
+	struct extron *extron = port->extron;
+	unsigned int i;
+
+	log_printf(adap, file, "name: %s type: %s\n",
+		   extron->unit_name, extron->unit_type);
+	log_printf(adap, file, "model: 60-160%c-01 (1 input, %u outputs)\n",
+		   '6' + extron->num_out_ports / 2, extron->num_out_ports);
+	log_printf(adap, file, "firmware version: %s CEC engine version: %s\n",
+		   extron->unit_fw_version, extron->unit_cec_engine_version);
+	if (extron->hpd_never_low)
+		log_printf(adap, file, "always keep input HPD high\n");
+	else
+		log_printf(adap, file,
+			   "pull input HPD low if all output HPDs are low\n");
+	if (vendor_id)
+		log_printf(adap, file,
+			   "splitter vendor ID: 0x%06x\n", vendor_id);
+	if (manufacturer_name[0])
+		log_printf(adap, file, "splitter manufacturer name: %s\n",
+			   manufacturer_name);
+	log_printf(adap, file, "splitter power status: %s\n",
+		   pwr_state[extron->splitter.is_standby]);
+	log_printf(adap, file, "%s port: %d (%s)\n",
+		   port->is_input ? "input" : "output",
+		   port->port.port, port->name);
+	log_printf(adap, file, "splitter input port:\n");
+	extron_adap_status_port(extron->ports[extron->num_out_ports], file);
+
+	log_printf(adap, file, "splitter output ports:\n");
+	for (i = 0; i < extron->num_out_ports; i++)
+		extron_adap_status_port(extron->ports[i], file);
+
+	if (!port->has_edid || !port->read_edid)
+		return;
+
+	for (i = 0; i < port->edid_blocks * 128; i += 16) {
+		if (i % 128 == 0)
+			log_printf(adap, file, "\n");
+		log_printf(adap, file, "EDID: %*ph\n", 16, port->edid + i);
+	}
+}
+
+static const struct cec_adap_ops extron_cec_adap_ops = {
+	.adap_enable = extron_cec_adap_enable,
+	.adap_log_addr = extron_cec_adap_log_addr,
+	.adap_transmit = extron_cec_adap_transmit,
+	.adap_nb_transmit_canceled = extron_cec_adap_nb_transmit_canceled,
+	.adap_unconfigured = extron_cec_adap_unconfigured,
+	.adap_status = extron_adap_status,
+	.configured = extron_cec_configured,
+	.received = extron_received,
+};
+
+static int extron_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	strscpy(cap->driver, "extron-da-hd-4k-plus-cec", sizeof(cap->driver));
+	strscpy(cap->card, cap->driver, sizeof(cap->card));
+	snprintf(cap->bus_info, sizeof(cap->bus_info), "serio:%s", port->name);
+	return 0;
+}
+
+static int extron_enum_input(struct file *file, void *priv, struct v4l2_input *inp)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	if (inp->index)
+		return -EINVAL;
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	snprintf(inp->name, sizeof(inp->name), "HDMI IN %u", port->port.port);
+	inp->status = v4l2_ctrl_g_ctrl(port->ctrl_rx_power_present) ?
+		0 : V4L2_IN_ST_NO_SIGNAL;
+	return 0;
+}
+
+static int extron_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int extron_s_input(struct file *file, void *priv, unsigned int i)
+{
+	return i ? -EINVAL : 0;
+}
+
+static int extron_enum_output(struct file *file, void *priv, struct v4l2_output *out)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	if (out->index)
+		return -EINVAL;
+	out->type = V4L2_OUTPUT_TYPE_ANALOG;
+	snprintf(out->name, sizeof(out->name), "HDMI OUT %u", port->port.port);
+	return 0;
+}
+
+static int extron_g_output(struct file *file, void *priv, unsigned int *o)
+{
+	*o = 0;
+	return 0;
+}
+
+static int extron_s_output(struct file *file, void *priv, unsigned int o)
+{
+	return o ? -EINVAL : 0;
+}
+
+static int extron_g_edid(struct file *file, void *_fh,
+			 struct v4l2_edid *edid)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (port->disconnected)
+		return -ENODEV;
+	if (edid->pad)
+		return -EINVAL;
+	if (!port->has_edid)
+		return -ENODATA;
+	if (!port->read_edid)
+		extron_read_edid(port);
+	if (!port->read_edid)
+		return -ENODATA;
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = port->edid_blocks;
+		return 0;
+	}
+	if (edid->start_block >= port->edid_blocks)
+		return -EINVAL;
+	if (edid->blocks > port->edid_blocks - edid->start_block)
+		edid->blocks = port->edid_blocks - edid->start_block;
+	memcpy(edid->edid, port->edid + edid->start_block * 128, edid->blocks * 128);
+	return 0;
+}
+
+static int extron_s_edid(struct file *file, void *_fh, struct v4l2_edid *edid)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (port->disconnected)
+		return -ENODEV;
+	if (edid->pad)
+		return -EINVAL;
+
+	/* Unfortunately it is not possible to clear the EDID */
+	if (edid->blocks == 0)
+		return -EINVAL;
+
+	if (edid->blocks > MAX_EDID_BLOCKS) {
+		edid->blocks = MAX_EDID_BLOCKS;
+		return -E2BIG;
+	}
+
+	if (cec_get_edid_spa_location(edid->edid, edid->blocks * 128))
+		v4l2_set_edid_phys_addr(edid->edid, edid->blocks * 128, 0);
+	extron_parse_edid(port);
+	return extron_write_edid(port, edid->edid, edid->blocks);
+}
+
+static int extron_log_status(struct file *file, void *priv)
+{
+	struct extron_port *port = video_drvdata(file);
+
+	extron_adap_status(port->adap, NULL);
+	return v4l2_ctrl_log_status(file, priv);
+}
+
+static const struct v4l2_ioctl_ops extron_ioctl_ops = {
+	.vidioc_querycap		= extron_querycap,
+	.vidioc_enum_input		= extron_enum_input,
+	.vidioc_g_input			= extron_g_input,
+	.vidioc_s_input			= extron_s_input,
+	.vidioc_enum_output		= extron_enum_output,
+	.vidioc_g_output		= extron_g_output,
+	.vidioc_s_output		= extron_s_output,
+	.vidioc_g_edid			= extron_g_edid,
+	.vidioc_s_edid			= extron_s_edid,
+	.vidioc_log_status		= extron_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_file_operations extron_fops = {
+	.owner		= THIS_MODULE,
+	.open		= v4l2_fh_open,
+	.release	= v4l2_fh_release,
+	.poll		= v4l2_ctrl_poll,
+	.unlocked_ioctl	= video_ioctl2,
+};
+
+static const struct video_device extron_videodev = {
+	.name		= "extron-da-hd-4k-plus-cec",
+	.vfl_dir	= VFL_DIR_RX,
+	.fops		= &extron_fops,
+	.ioctl_ops	= &extron_ioctl_ops,
+	.minor		= -1,
+	.release	= video_device_release_empty,
+};
+
+static void extron_disconnect(struct serio *serio)
+{
+	struct extron *extron = serio_get_drvdata(serio);
+	unsigned int p;
+
+	kthread_stop(extron->kthread_setup);
+
+	for (p = 0; p < extron->num_ports; p++) {
+		struct extron_port *port = extron->ports[p];
+
+		if (!port)
+			continue;
+		port->disconnected = true;
+		cancel_work_sync(&port->irq_work);
+	}
+	cancel_delayed_work_sync(&extron->work_update_edid);
+	for (p = 0; p < extron->num_ports; p++) {
+		struct extron_port *port = extron->ports[p];
+
+		if (!port)
+			continue;
+
+		if (port->cec_was_registered) {
+			if (cec_is_registered(port->adap))
+				cec_unregister_adapter(port->adap);
+			/*
+			 * After registering the adapter, the
+			 * extron_setup_thread() function took an extra
+			 * reference to the device. We call the corresponding
+			 * put here.
+			 */
+			cec_put_device(port->adap);
+		} else {
+			cec_delete_adapter(port->adap);
+		}
+		video_unregister_device(&port->vdev);
+	}
+
+	complete(&extron->edid_completion);
+
+	for (p = 0; p < extron->num_ports; p++) {
+		struct extron_port *port = extron->ports[p];
+
+		if (!port)
+			continue;
+		v4l2_ctrl_handler_free(&port->hdl);
+		mutex_destroy(&port->video_lock);
+		kfree(port);
+	}
+	mutex_destroy(&extron->edid_lock);
+	mutex_destroy(&extron->serio_lock);
+	extron->serio = NULL;
+	serio_set_drvdata(serio, NULL);
+	serio_close(serio);
+}
+
+static int extron_setup(struct extron *extron)
+{
+	struct serio *serio = extron->serio;
+	struct extron_port *port;
+	u8 *reply = extron->reply;
+	unsigned int p;
+	unsigned int major, minor;
+	int err;
+
+	/*
+	 * Attempt to disable CEC: avoid received CEC messages
+	 * from interfering with the other serial port traffic.
+	 */
+	extron_send_and_wait(extron, NULL, "WI1*0CCEC", NULL);
+	extron_send_and_wait(extron, NULL, "WO0*CCEC", NULL);
+
+	/* Obtain unit part number */
+	err = extron_send_and_wait(extron, NULL, "N", "Pno");
+	if (err)
+		return err;
+	dev_info(extron->dev, "Unit part number: %s\n", reply + 3);
+	if (strcmp(reply + 3, "60-1607-01") &&
+	    strcmp(reply + 3, "60-1608-01") &&
+	    strcmp(reply + 3, "60-1609-01")) {
+		dev_err(extron->dev, "Unsupported model\n");
+		return -ENODEV;
+	}
+	/* Up to 6 output ports and one input port */
+	extron->num_out_ports = 2 * (reply[9] - '6');
+	extron->splitter.num_out_ports = extron->num_out_ports;
+	extron->splitter.ports = extron->splitter_ports;
+	extron->splitter.dev = extron->dev;
+	extron->num_in_ports = 1;
+	extron->num_ports = extron->num_out_ports + extron->num_in_ports;
+	dev_info(extron->dev, "Unit output ports: %d\n", extron->num_out_ports);
+	dev_info(extron->dev, "Unit input ports: %d\n", extron->num_in_ports);
+
+	err = extron_send_and_wait(extron, NULL, "W CN", "Ipn ");
+	if (err)
+		return err;
+	dev_info(extron->dev, "Unit name: %s\n", reply + 4);
+	strscpy(extron->unit_name, reply + 4, sizeof(extron->unit_name));
+
+	err = extron_send_and_wait(extron, NULL, "*Q", "Bld");
+	if (err)
+		return err;
+	dev_info(extron->dev, "Unit FW Version: %s\n", reply + 3);
+	strscpy(extron->unit_fw_version, reply + 3,
+		sizeof(extron->unit_fw_version));
+	if (sscanf(reply + 3, "%u.%u.", &major, &minor) < 2 ||
+	    major < 1 || minor < 2) {
+		dev_err(extron->dev,
+			"Unsupported FW version (only 1.02 or up is supported)\n");
+		return -ENODEV;
+	}
+
+	err = extron_send_and_wait(extron, NULL, "2i", "Inf02*");
+	if (err)
+		return err;
+	dev_info(extron->dev, "Unit Type: %s\n", reply + 6);
+	strscpy(extron->unit_type, reply + 6, sizeof(extron->unit_type));
+
+	err = extron_send_and_wait(extron, NULL, "39Q", "Ver39*");
+	if (err)
+		return err;
+	dev_info(extron->dev, "CEC Engine Version: %s\n", reply + 6);
+	strscpy(extron->unit_cec_engine_version, reply + 6,
+		sizeof(extron->unit_cec_engine_version));
+
+	/* Disable CEC */
+	err = extron_send_and_wait(extron, NULL, "WI1*0CCEC", "CcecI1*");
+	if (err)
+		return err;
+	err = extron_send_and_wait(extron, NULL, "WO0*CCEC", "CcecO0");
+	if (err)
+		return err;
+
+	extron->hpd_never_low = hpd_never_low;
+
+	/* Pull input port HPD low if all output ports also have a low HPD */
+	if (hpd_never_low) {
+		dev_info(extron->dev, "Always keep input HPD high\n");
+	} else {
+		dev_info(extron->dev, "Pull input HPD low if all output HPDs are low\n");
+		extron_send_and_wait(extron, NULL, "W1ihpd", "Ihpd1");
+	}
+
+	for (p = 0; p < extron->num_ports; p++) {
+		u32 caps = CEC_CAP_DEFAULTS | CEC_CAP_MONITOR_ALL;
+
+		if (vendor_id)
+			caps &= ~CEC_CAP_LOG_ADDRS;
+		port = kzalloc(sizeof(*port), GFP_KERNEL);
+		if (!port)
+			return -ENOMEM;
+
+		INIT_WORK(&port->irq_work, extron_irq_work_handler);
+		spin_lock_init(&port->msg_lock);
+		mutex_init(&port->video_lock);
+		port->extron = extron;
+		port->is_input = p >= extron->num_out_ports;
+		port->direction = port->is_input ? 'I' : 'O';
+		port->port.port = 1 + (port->is_input ? p - extron->num_out_ports : p);
+		port->port.splitter = &extron->splitter;
+		port->phys_addr = CEC_PHYS_ADDR_INVALID;
+		snprintf(port->name, sizeof(port->name), "%s-%s-%u",
+			 dev_name(&serio->dev), port->is_input ? "in" : "out",
+			 port->port.port);
+
+		port->dev = extron->dev;
+		port->adap = cec_allocate_adapter(&extron_cec_adap_ops, port,
+						  port->name, caps, 1);
+		err = PTR_ERR_OR_ZERO(port->adap);
+		if (err < 0) {
+			kfree(port);
+			return err;
+		}
+
+		port->adap->xfer_timeout_ms = EXTRON_TIMEOUT_SECS * 1000;
+		port->port.adap = port->adap;
+		port->vdev = extron_videodev;
+		port->vdev.lock = &port->video_lock;
+		port->vdev.v4l2_dev = &extron->v4l2_dev;
+		port->vdev.ctrl_handler = &port->hdl;
+		port->vdev.device_caps = V4L2_CAP_EDID;
+		video_set_drvdata(&port->vdev, port);
+
+		v4l2_ctrl_handler_init(&port->hdl, 2);
+
+		if (port->is_input) {
+			port->vdev.vfl_dir = VFL_DIR_RX;
+			port->ctrl_rx_power_present =
+				v4l2_ctrl_new_std(&port->hdl, NULL,
+						  V4L2_CID_DV_RX_POWER_PRESENT,
+						  0, 1, 0, 0);
+			port->has_edid = true;
+		} else {
+			port->vdev.vfl_dir = VFL_DIR_TX;
+			port->ctrl_tx_hotplug =
+				v4l2_ctrl_new_std(&port->hdl, NULL,
+						  V4L2_CID_DV_TX_HOTPLUG,
+						  0, 1, 0, 0);
+			port->ctrl_tx_edid_present =
+				v4l2_ctrl_new_std(&port->hdl, NULL,
+						  V4L2_CID_DV_TX_EDID_PRESENT,
+						  0, 1, 0, 0);
+		}
+
+		err = port->hdl.error;
+		if (err < 0) {
+			cec_delete_adapter(port->adap);
+			kfree(port);
+			return err;
+		}
+		extron->ports[p] = port;
+		extron->splitter_ports[p] = &port->port;
+		if (port->is_input && manufacturer_name[0])
+			extron_write_edid(port, hdmi_edid, 2);
+	}
+
+	/* Enable CEC (manual mode, i.e. controlled by the driver) */
+	err = extron_send_and_wait(extron, NULL, "WI1*20CCEC", "CcecI1*");
+	if (err)
+		return err;
+
+	err = extron_send_and_wait(extron, NULL, "WO20*CCEC", "CcecO20");
+	if (err)
+		return err;
+
+	/* Set logical addresses to 15 */
+	err = extron_send_and_wait(extron, NULL, "WI1*15LCEC", "LcecI1*15");
+	if (err)
+		return err;
+
+	for (p = 0; p < extron->num_out_ports; p++) {
+		char cmd[20];
+		char resp[20];
+
+		snprintf(cmd, sizeof(cmd), "WO%u*15LCEC", p + 1);
+		snprintf(resp, sizeof(resp), "LcecO%u*15", p + 1);
+		err = extron_send_and_wait(extron, extron->ports[p], cmd, resp);
+		if (err)
+			return err;
+	}
+
+	/*
+	 * The Extron is now ready for operation. Specifically it is now
+	 * possible to retrieve EDIDs.
+	 */
+	extron->is_ready = true;
+
+	/* Query HDCP and Signal states, used to update the initial state */
+	err = extron_send_and_wait(extron, NULL, "WHDCP", "Hdcp");
+	if (err)
+		return err;
+
+	return extron_send_and_wait(extron, NULL, "WLS", "Sig");
+}
+
+static int extron_setup_thread(void *_extron)
+{
+	struct extron *extron = _extron;
+	struct extron_port *port;
+	unsigned int p;
+	bool poll_splitter = false;
+	bool was_connected = true;
+	int err;
+
+	while (1) {
+		if (kthread_should_stop())
+			return 0;
+		err = extron_send_and_wait(extron, NULL, "W3CV", "Vrb3");
+		// that should make it possible to detect a serio disconnect
+		// here by stopping the workqueue
+		if (err >= 0)
+			break;
+		was_connected = false;
+		ssleep(1);
+	}
+
+	/*
+	 * If the Extron was not connected at probe() time, i.e. it just got
+	 * powered up and while the serial port is working, the firmware is
+	 * still booting up, then wait 10 seconds for the firmware to settle.
+	 *
+	 * Trying to continue too soon means that some commands will not
+	 * work yet.
+	 */
+	if (!was_connected)
+		ssleep(10);
+
+	err = extron_setup(extron);
+	if (err)
+		goto disable_ports;
+
+	for (p = 0; p < extron->num_ports; p++) {
+		struct cec_log_addrs log_addrs = {};
+
+		port = extron->ports[p];
+		if (port->is_input && manufacturer_name[0])
+			v4l2_disable_ioctl(&port->vdev, VIDIOC_S_EDID);
+		err = video_register_device(&port->vdev, VFL_TYPE_VIDEO, -1);
+		if (err) {
+			v4l2_err(&extron->v4l2_dev, "Failed to register video device\n");
+			goto disable_ports;
+		}
+
+		err = cec_register_adapter(port->adap, extron->dev);
+		if (err < 0)
+			goto disable_ports;
+		port->dev = &port->adap->devnode.dev;
+		port->cec_was_registered = true;
+		/*
+		 * This driver is unusual in that the whole setup takes place
+		 * in a thread since it can take such a long time before the
+		 * Extron Splitter boots up, and you do not want to block the
+		 * probe function on this driver. In addition, as soon as
+		 * CEC adapters come online, they can be used, and you cannot
+		 * just unregister them again if an error occurs, since that
+		 * can delete the underlying CEC adapter, which might already
+		 * be in use.
+		 *
+		 * So we take an additional reference to the adapter. This
+		 * allows us to unregister the device node if needed, without
+		 * deleting the actual adapter.
+		 *
+		 * In the disconnect function we will do the corresponding
+		 * put call to ensure the adapter is deleted.
+		 */
+		cec_get_device(port->adap);
+
+		/*
+		 * If vendor_id wasn't set, then userspace configures the
+		 * CEC devices. Otherwise the driver configures the CEC
+		 * devices as TV (input) and Playback (outputs) devices
+		 * and the driver processes all CEC messages.
+		 */
+		if (!vendor_id)
+			continue;
+
+		log_addrs.cec_version = CEC_OP_CEC_VERSION_2_0;
+		log_addrs.num_log_addrs = 1;
+		log_addrs.vendor_id = vendor_id;
+		if (port->is_input) {
+			strscpy(log_addrs.osd_name, "Splitter In",
+				sizeof(log_addrs.osd_name));
+			log_addrs.log_addr_type[0] = CEC_LOG_ADDR_TYPE_TV;
+			log_addrs.primary_device_type[0] = CEC_OP_PRIM_DEVTYPE_TV;
+			log_addrs.all_device_types[0] = CEC_OP_ALL_DEVTYPE_TV;
+		} else {
+			snprintf(log_addrs.osd_name, sizeof(log_addrs.osd_name),
+				 "Splitter Out%u", port->port.port);
+			log_addrs.log_addr_type[0] = CEC_LOG_ADDR_TYPE_PLAYBACK;
+			log_addrs.primary_device_type[0] = CEC_OP_PRIM_DEVTYPE_PLAYBACK;
+			log_addrs.all_device_types[0] = CEC_OP_ALL_DEVTYPE_PLAYBACK;
+		}
+		err = cec_s_log_addrs(port->adap, &log_addrs, false);
+		if (err < 0)
+			goto disable_ports;
+	}
+	poll_splitter = true;
+
+	port = extron->ports[extron->num_out_ports];
+	while (!kthread_should_stop()) {
+		ssleep(1);
+		if (hpd_never_low != extron->hpd_never_low) {
+			/*
+			 * Keep input port HPD high at all times, or pull it low
+			 * if all output ports also have a low HPD
+			 */
+			if (hpd_never_low) {
+				dev_info(extron->dev, "Always keep input HPD high\n");
+				extron_send_and_wait(extron, NULL, "W0ihpd", "Ihpd0");
+			} else {
+				dev_info(extron->dev, "Pull input HPD low if all output HPDs are low\n");
+				extron_send_and_wait(extron, NULL, "W1ihpd", "Ihpd1");
+			}
+			extron->hpd_never_low = hpd_never_low;
+		}
+		if (poll_splitter &&
+		    cec_splitter_poll(&extron->splitter, port->adap, debug) &&
+		    manufacturer_name[0]) {
+			/*
+			 * Sinks were lost, so see if the input edid needs to
+			 * be updated.
+			 */
+			cancel_delayed_work_sync(&extron->work_update_edid);
+			schedule_delayed_work(&extron->work_update_edid,
+					      msecs_to_jiffies(1000));
+		}
+	}
+	return 0;
+
+disable_ports:
+	extron->is_ready = false;
+	for (p = 0; p < extron->num_ports; p++) {
+		struct extron_port *port = extron->ports[p];
+
+		if (!port)
+			continue;
+		port->disconnected = true;
+		cancel_work_sync(&port->irq_work);
+		video_unregister_device(&port->vdev);
+		if (port->cec_was_registered)
+			cec_unregister_adapter(port->adap);
+	}
+	cancel_delayed_work_sync(&extron->work_update_edid);
+	complete(&extron->edid_completion);
+	dev_err(extron->dev, "Setup failed with error %d\n", err);
+	while (!kthread_should_stop())
+		ssleep(1);
+	return err;
+}
+
+static int extron_connect(struct serio *serio, struct serio_driver *drv)
+{
+	struct extron *extron;
+	int err = -ENOMEM;
+
+	if (manufacturer_name[0] &&
+	    (!isupper(manufacturer_name[0]) ||
+	     !isupper(manufacturer_name[1]) ||
+	     !isupper(manufacturer_name[2]))) {
+		dev_warn(&serio->dev, "ignoring invalid manufacturer name\n");
+		manufacturer_name[0] = 0;
+	}
+
+	extron = kzalloc(sizeof(*extron), GFP_KERNEL);
+
+	if (!extron)
+		return -ENOMEM;
+
+	extron->serio = serio;
+	extron->dev = &serio->dev;
+	mutex_init(&extron->serio_lock);
+	mutex_init(&extron->edid_lock);
+	INIT_DELAYED_WORK(&extron->work_update_edid, update_edid_work);
+
+	err = v4l2_device_register(extron->dev, &extron->v4l2_dev);
+	if (err)
+		goto free_device;
+
+	err = serio_open(serio, drv);
+	if (err)
+		goto unreg_v4l2_dev;
+
+	serio_set_drvdata(serio, extron);
+	init_completion(&extron->edid_completion);
+
+	extron->kthread_setup = kthread_run(extron_setup_thread, extron,
+					    "extron-da-hd-4k-plus-cec-%s", dev_name(&serio->dev));
+	if (!IS_ERR(extron->kthread_setup))
+		return 0;
+
+	dev_err(extron->dev, "kthread_run() failed\n");
+	err = PTR_ERR(extron->kthread_setup);
+
+	extron->serio = NULL;
+	serio_set_drvdata(serio, NULL);
+	serio_close(serio);
+unreg_v4l2_dev:
+	v4l2_device_unregister(&extron->v4l2_dev);
+free_device:
+	mutex_destroy(&extron->edid_lock);
+	mutex_destroy(&extron->serio_lock);
+	kfree(extron);
+	return err;
+}
+
+static const struct serio_device_id extron_serio_ids[] = {
+	{
+		.type	= SERIO_RS232,
+		.proto	= SERIO_EXTRON_DA_HD_4K_PLUS,
+		.id	= SERIO_ANY,
+		.extra	= SERIO_ANY,
+	},
+	{ 0 }
+};
+
+MODULE_DEVICE_TABLE(serio, extron_serio_ids);
+
+static struct serio_driver extron_drv = {
+	.driver		= {
+		.name	= "extron-da-hd-4k-plus-cec",
+	},
+	.description	= "Extron DA HD 4K PLUS HDMI CEC driver",
+	.id_table	= extron_serio_ids,
+	.interrupt	= extron_interrupt,
+	.connect	= extron_connect,
+	.disconnect	= extron_disconnect,
+};
+
+module_serio_driver(extron_drv);
diff --git a/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.h b/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.h
new file mode 100644
index 000000000000..b79f1253ab5d
--- /dev/null
+++ b/drivers/media/cec/usb/extron-da-hd-4k-plus/extron-da-hd-4k-plus.h
@@ -0,0 +1,118 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/*
+ * Copyright 2021-2024 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _EXTRON_DA_HD_4K_PLUS_H_
+#define _EXTRON_DA_HD_4K_PLUS_H_
+
+#include <linux/kthread.h>
+#include <linux/serio.h>
+#include <linux/workqueue.h>
+#include <media/cec.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+
+#include "cec-splitter.h"
+
+#define DATA_SIZE 256
+
+#define PING_PERIOD	(15 * HZ)
+
+#define NUM_MSGS CEC_MAX_MSG_RX_QUEUE_SZ
+
+#define MAX_PORTS (1 + 6)
+
+#define MAX_EDID_BLOCKS 2
+
+struct extron;
+
+struct extron_port {
+	struct cec_splitter_port port;
+	struct device *dev;
+	struct cec_adapter *adap;
+	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *ctrl_rx_power_present;
+	struct v4l2_ctrl *ctrl_tx_hotplug;
+	struct v4l2_ctrl *ctrl_tx_edid_present;
+	bool is_input;
+	char direction;
+	char name[26];
+	unsigned char edid[MAX_EDID_BLOCKS * 128];
+	unsigned char edid_tmp[MAX_EDID_BLOCKS * 128];
+	unsigned int edid_blocks;
+	bool read_edid;
+	struct extron *extron;
+	struct work_struct irq_work;
+	struct completion cmd_done;
+	const char *response;
+	unsigned int cmd_error;
+	struct cec_msg rx_msg[NUM_MSGS];
+	unsigned int rx_msg_cur_idx, rx_msg_num;
+	/* protect rx_msg_cur_idx and rx_msg_num */
+	spinlock_t msg_lock;
+	u32 tx_done_status;
+	bool update_phys_addr;
+	u16 phys_addr;
+	bool cec_was_registered;
+	bool disconnected;
+	bool update_has_signal;
+	bool has_signal;
+	bool update_has_edid;
+	bool has_edid;
+	bool has_4kp30;
+	bool has_4kp60;
+	bool has_qy;
+	bool has_qs;
+	u8 est_i, est_ii;
+
+	/* locks access to the video_device */
+	struct mutex video_lock;
+};
+
+struct extron {
+	struct cec_splitter splitter;
+	struct device *dev;
+	struct serio *serio;
+	/* locks access to serio */
+	struct mutex serio_lock;
+	unsigned int num_ports;
+	unsigned int num_in_ports;
+	unsigned int num_out_ports;
+	char unit_name[32];
+	char unit_type[64];
+	char unit_fw_version[32];
+	char unit_cec_engine_version[32];
+	struct extron_port *ports[MAX_PORTS];
+	struct cec_splitter_port *splitter_ports[MAX_PORTS];
+	struct v4l2_device v4l2_dev;
+	bool hpd_never_low;
+	struct task_struct *kthread_setup;
+	struct delayed_work work_update_edid;
+
+	/* serializes EDID reading */
+	struct mutex edid_lock;
+	unsigned int edid_bytes_read;
+	struct extron_port *edid_port;
+	struct completion edid_completion;
+	bool edid_reading;
+	bool is_ready;
+
+	struct completion cmd_done;
+	const char *response;
+	unsigned int cmd_error;
+	char data[DATA_SIZE];
+	unsigned int len;
+	char reply[DATA_SIZE];
+	char buf[DATA_SIZE];
+	unsigned int idx;
+};
+
+#endif
diff --git a/drivers/media/cec/usb/pulse8/pulse8-cec.c b/drivers/media/cec/usb/pulse8/pulse8-cec.c
index ba67587bd43e..171366fe3544 100644
--- a/drivers/media/cec/usb/pulse8/pulse8-cec.c
+++ b/drivers/media/cec/usb/pulse8/pulse8-cec.c
@@ -685,7 +685,7 @@ static int pulse8_setup(struct pulse8 *pulse8, struct serio *serio,
 	err = pulse8_send_and_wait(pulse8, cmd, 1, cmd[0], 4);
 	if (err)
 		return err;
-	date = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
+	date = ((unsigned)data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
 	dev_info(pulse8->dev, "Firmware build date %ptT\n", &date);
 
 	dev_dbg(pulse8->dev, "Persistent config:\n");
diff --git a/drivers/media/common/b2c2/flexcop-common.h b/drivers/media/common/b2c2/flexcop-common.h
index f944c59cf495..a468ea7e77a1 100644
--- a/drivers/media/common/b2c2/flexcop-common.h
+++ b/drivers/media/common/b2c2/flexcop-common.h
@@ -125,8 +125,6 @@ void flexcop_dma_free(struct flexcop_dma *dma);
 
 int flexcop_dma_control_timer_irq(struct flexcop_device *fc,
 		flexcop_dma_index_t no, int onoff);
-int flexcop_dma_control_size_irq(struct flexcop_device *fc,
-		flexcop_dma_index_t no, int onoff);
 int flexcop_dma_config(struct flexcop_device *fc, struct flexcop_dma *dma,
 		flexcop_dma_index_t dma_idx);
 int flexcop_dma_xfer_control(struct flexcop_device *fc,
@@ -170,8 +168,6 @@ int flexcop_sram_init(struct flexcop_device *fc);
 void flexcop_determine_revision(struct flexcop_device *fc);
 void flexcop_device_name(struct flexcop_device *fc,
 		const char *prefix, const char *suffix);
-void flexcop_dump_reg(struct flexcop_device *fc,
-		flexcop_ibi_register reg, int num);
 
 /* from flexcop-hw-filter.c */
 int flexcop_pid_feed_control(struct flexcop_device *fc,
diff --git a/drivers/media/common/b2c2/flexcop-misc.c b/drivers/media/common/b2c2/flexcop-misc.c
index 83d01d3a81cc..251c4f731ed1 100644
--- a/drivers/media/common/b2c2/flexcop-misc.c
+++ b/drivers/media/common/b2c2/flexcop-misc.c
@@ -70,16 +70,3 @@ void flexcop_device_name(struct flexcop_device *fc,
 			flexcop_bus_names[fc->bus_type],
 			flexcop_revision_names[fc->rev], suffix);
 }
-
-void flexcop_dump_reg(struct flexcop_device *fc,
-		flexcop_ibi_register reg, int num)
-{
-	flexcop_ibi_value v;
-	int i;
-	for (i = 0; i < num; i++) {
-		v = fc->read_ibi_reg(fc, reg+4*i);
-		deb_rdump("0x%03x: %08x, ", reg+4*i, v.raw);
-	}
-	deb_rdump("\n");
-}
-EXPORT_SYMBOL(flexcop_dump_reg);
diff --git a/drivers/media/common/saa7146/saa7146_fops.c b/drivers/media/common/saa7146/saa7146_fops.c
index a7047e548245..9d0362a75ecd 100644
--- a/drivers/media/common/saa7146/saa7146_fops.c
+++ b/drivers/media/common/saa7146/saa7146_fops.c
@@ -147,13 +147,13 @@ void saa7146_buffer_next(struct saa7146_dev *dev,
 			printk("vdma%d.num_line_byte: 0x%08x\n", 1,saa7146_read(dev,NUM_LINE_BYTE1));
 */
 		}
-		del_timer(&q->timeout);
+		timer_delete(&q->timeout);
 	}
 }
 
 void saa7146_buffer_timeout(struct timer_list *t)
 {
-	struct saa7146_dmaqueue *q = from_timer(q, t, timeout);
+	struct saa7146_dmaqueue *q = timer_container_of(q, t, timeout);
 	struct saa7146_dev *dev = q->dev;
 	unsigned long flags;
 
diff --git a/drivers/media/common/saa7146/saa7146_hlp.c b/drivers/media/common/saa7146/saa7146_hlp.c
index 7569d8cdd4d8..fe3348af543e 100644
--- a/drivers/media/common/saa7146/saa7146_hlp.c
+++ b/drivers/media/common/saa7146/saa7146_hlp.c
@@ -122,7 +122,7 @@ static int calculate_h_scale_registers(struct saa7146_dev *dev,
 	xacm = 0;
 
 	/* set horizontal filter parameters (CXY = CXUV) */
-	cxy = hps_h_coeff_tab[( (xpsc - 1) < 63 ? (xpsc - 1) : 63 )].hps_coeff;
+	cxy = hps_h_coeff_tab[min(xpsc - 1, 63)].hps_coeff;
 	cxuv = cxy;
 
 	/* calculate and set horizontal fine scale (xsci) */
@@ -151,7 +151,7 @@ static int calculate_h_scale_registers(struct saa7146_dev *dev,
 		xacm = 0;
 		/* get best match in the table of attenuations
 		   for horizontal scaling */
-		h_atten = hps_h_coeff_tab[( (xpsc - 1) < 63 ? (xpsc - 1) : 63 )].weight_sum;
+		h_atten = hps_h_coeff_tab[min(xpsc - 1, 63)].weight_sum;
 
 		for (i = 0; h_attenuation[i] != 0; i++) {
 			if (h_attenuation[i] >= h_atten)
@@ -283,10 +283,10 @@ static int calculate_v_scale_registers(struct saa7146_dev *dev, enum v4l2_field
 		}
 
 		/* get filter coefficients for cya, cyb from table hps_v_coeff_tab */
-		cya_cyb = hps_v_coeff_tab[ (yacl < 63 ? yacl : 63 ) ].hps_coeff;
+		cya_cyb = hps_v_coeff_tab[min(yacl, 63)].hps_coeff;
 
 		/* get best match in the table of attenuations for vertical scaling */
-		v_atten = hps_v_coeff_tab[ (yacl < 63 ? yacl : 63 ) ].weight_sum;
+		v_atten = hps_v_coeff_tab[min(yacl, 63)].weight_sum;
 
 		for (i = 0; v_attenuation[i] != 0; i++) {
 			if (v_attenuation[i] >= v_atten)
diff --git a/drivers/media/common/saa7146/saa7146_vbi.c b/drivers/media/common/saa7146/saa7146_vbi.c
index bb7d81f7eba6..1ffcc025d1a4 100644
--- a/drivers/media/common/saa7146/saa7146_vbi.c
+++ b/drivers/media/common/saa7146/saa7146_vbi.c
@@ -322,15 +322,15 @@ static void vbi_stop(struct saa7146_dev *dev)
 	/* shut down dma 3 transfers */
 	saa7146_write(dev, MC1, MASK_20);
 
-	del_timer(&vv->vbi_dmaq.timeout);
-	del_timer(&vv->vbi_read_timeout);
+	timer_delete(&vv->vbi_dmaq.timeout);
+	timer_delete(&vv->vbi_read_timeout);
 
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
 static void vbi_read_timeout(struct timer_list *t)
 {
-	struct saa7146_vv *vv = from_timer(vv, t, vbi_read_timeout);
+	struct saa7146_vv *vv = timer_container_of(vv, t, vbi_read_timeout);
 	struct saa7146_dev *dev = vv->vbi_dmaq.dev;
 
 	DEB_VBI("dev:%p\n", dev);
@@ -407,8 +407,6 @@ const struct vb2_ops vbi_qops = {
 	.buf_cleanup	= buf_cleanup,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------ */
diff --git a/drivers/media/common/saa7146/saa7146_video.c b/drivers/media/common/saa7146/saa7146_video.c
index 040489e15ea0..733e18001d0d 100644
--- a/drivers/media/common/saa7146/saa7146_video.c
+++ b/drivers/media/common/saa7146/saa7146_video.c
@@ -668,7 +668,7 @@ static void stop_streaming(struct vb2_queue *q)
 	struct saa7146_dev *dev = vb2_get_drv_priv(q);
 	struct saa7146_dmaqueue *dq = &dev->vv_data->video_dmaq;
 
-	del_timer(&dq->timeout);
+	timer_delete(&dq->timeout);
 	video_end(dev);
 	return_buffers(q, VB2_BUF_STATE_ERROR);
 }
@@ -681,8 +681,6 @@ const struct vb2_ops video_qops = {
 	.buf_cleanup	= buf_cleanup,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 /********************************************************************************/
diff --git a/drivers/media/common/siano/smscoreapi.c b/drivers/media/common/siano/smscoreapi.c
index 7ebcb10126c9..3732367e0c62 100644
--- a/drivers/media/common/siano/smscoreapi.c
+++ b/drivers/media/common/siano/smscoreapi.c
@@ -839,7 +839,7 @@ static int smscore_configure_board(struct smscore_device_t *coredev)
 		mtu_msg.x_msg_header.msg_flags = 0;
 		mtu_msg.x_msg_header.msg_type = MSG_SMS_SET_MAX_TX_MSG_LEN_REQ;
 		mtu_msg.x_msg_header.msg_length = sizeof(mtu_msg);
-		mtu_msg.msg_data[0] = board->mtu;
+		mtu_msg.msg_data = board->mtu;
 
 		coredev->sendrequest_handler(coredev->context, &mtu_msg,
 					     sizeof(mtu_msg));
@@ -852,7 +852,7 @@ static int smscore_configure_board(struct smscore_device_t *coredev)
 		SMS_INIT_MSG(&crys_msg.x_msg_header,
 				MSG_SMS_NEW_CRYSTAL_REQ,
 				sizeof(crys_msg));
-		crys_msg.msg_data[0] = board->crystal;
+		crys_msg.msg_data = board->crystal;
 
 		coredev->sendrequest_handler(coredev->context, &crys_msg,
 					     sizeof(crys_msg));
@@ -1132,8 +1132,7 @@ static char *smscore_get_fw_filename(struct smscore_device_t *coredev,
  * return: 0 on success, <0 on error.
  */
 static int smscore_load_firmware_from_file(struct smscore_device_t *coredev,
-					   int mode,
-					   loadfirmware_t loadfirmware_handler)
+					   int mode)
 {
 	int rc = -ENOENT;
 	u8 *fw_buf;
@@ -1147,8 +1146,7 @@ static int smscore_load_firmware_from_file(struct smscore_device_t *coredev,
 	}
 	pr_debug("Firmware name: %s\n", fw_filename);
 
-	if (!loadfirmware_handler &&
-	    !(coredev->device_flags & SMS_DEVICE_FAMILY2))
+	if (!(coredev->device_flags & SMS_DEVICE_FAMILY2))
 		return -EINVAL;
 
 	rc = request_firmware(&fw, fw_filename, coredev->device);
@@ -1166,10 +1164,8 @@ static int smscore_load_firmware_from_file(struct smscore_device_t *coredev,
 		memcpy(fw_buf, fw->data, fw->size);
 		fw_buf_size = fw->size;
 
-		rc = (coredev->device_flags & SMS_DEVICE_FAMILY2) ?
-			smscore_load_firmware_family2(coredev, fw_buf, fw_buf_size)
-			: loadfirmware_handler(coredev->context, fw_buf,
-			fw_buf_size);
+		rc = smscore_load_firmware_family2(coredev, fw_buf,
+						   fw_buf_size);
 	}
 
 	kfree(fw_buf);
@@ -1306,7 +1302,7 @@ static int smscore_init_device(struct smscore_device_t *coredev, int mode)
 	msg = (struct sms_msg_data *)SMS_ALIGN_ADDRESS(buffer);
 	SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
 			sizeof(struct sms_msg_data));
-	msg->msg_data[0] = mode;
+	msg->msg_data = mode;
 
 	rc = smscore_sendrequest_and_wait(coredev, msg,
 			msg->x_msg_header. msg_length,
@@ -1353,8 +1349,7 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 		}
 
 		if (!(coredev->modes_supported & (1 << mode))) {
-			rc = smscore_load_firmware_from_file(coredev,
-							     mode, NULL);
+			rc = smscore_load_firmware_from_file(coredev, mode);
 			if (rc >= 0)
 				pr_debug("firmware download success\n");
 		} else {
@@ -1394,7 +1389,7 @@ int smscore_set_device_mode(struct smscore_device_t *coredev, int mode)
 
 			SMS_INIT_MSG(&msg->x_msg_header, MSG_SMS_INIT_DEVICE_REQ,
 				     sizeof(struct sms_msg_data));
-			msg->msg_data[0] = mode;
+			msg->msg_data = mode;
 
 			rc = smscore_sendrequest_and_wait(
 				coredev, msg, msg->x_msg_header.msg_length,
@@ -1554,7 +1549,7 @@ void smscore_onresponse(struct smscore_device_t *coredev,
 			struct sms_msg_data *validity = (struct sms_msg_data *) phdr;
 
 			pr_debug("MSG_SMS_DATA_VALIDITY_RES, checksum = 0x%x\n",
-				validity->msg_data[0]);
+				validity->msg_data);
 			complete(&coredev->data_validity_done);
 			break;
 		}
diff --git a/drivers/media/common/siano/smscoreapi.h b/drivers/media/common/siano/smscoreapi.h
index f8789ee0d554..d945a2d6d624 100644
--- a/drivers/media/common/siano/smscoreapi.h
+++ b/drivers/media/common/siano/smscoreapi.h
@@ -97,7 +97,6 @@ typedef int (*hotplug_t)(struct smscore_device_t *coredev,
 typedef int (*setmode_t)(void *context, int mode);
 typedef void (*detectmode_t)(void *context, int *mode);
 typedef int (*sendrequest_t)(void *context, void *buffer, size_t size);
-typedef int (*loadfirmware_t)(void *context, void *buffer, size_t size);
 typedef int (*preload_t)(void *context);
 typedef int (*postload_t)(void *context);
 
@@ -616,7 +615,7 @@ struct sms_msg_hdr {
 
 struct sms_msg_data {
 	struct sms_msg_hdr x_msg_header;
-	u32 msg_data[1];
+	u32 msg_data;
 };
 
 struct sms_msg_data2 {
@@ -666,7 +665,7 @@ struct sms_firmware {
 	u32			check_sum;
 	u32			length;
 	u32			start_address;
-	u8			payload[1];
+	u8			payload[];
 };
 
 /* statistics information returned as response for
@@ -1042,20 +1041,6 @@ struct sms_srvm_signal_status {
 	u32 request_id;
 };
 
-struct sms_i2c_req {
-	u32	device_address; /* I2c device address */
-	u32	write_count; /* number of bytes to write */
-	u32	read_count; /* number of bytes to read */
-	u8	Data[1];
-};
-
-struct sms_i2c_res {
-	u32	status; /* non-zero value in case of failure */
-	u32	read_count; /* number of bytes read */
-	u8	Data[1];
-};
-
-
 struct smscore_config_gpio {
 #define SMS_GPIO_DIRECTION_INPUT  0
 #define SMS_GPIO_DIRECTION_OUTPUT 1
@@ -1116,9 +1101,6 @@ extern int smscore_register_device(struct smsdevice_params_t *params,
 extern void smscore_unregister_device(struct smscore_device_t *coredev);
 
 extern int smscore_start_device(struct smscore_device_t *coredev);
-extern int smscore_load_firmware(struct smscore_device_t *coredev,
-				 char *filename,
-				 loadfirmware_t loadfirmware_handler);
 
 extern int smscore_set_device_mode(struct smscore_device_t *coredev, int mode);
 extern int smscore_get_device_mode(struct smscore_device_t *coredev);
@@ -1133,12 +1115,6 @@ extern int smsclient_sendrequest(struct smscore_client_t *client,
 extern void smscore_onresponse(struct smscore_device_t *coredev,
 			       struct smscore_buffer_t *cb);
 
-extern int smscore_get_common_buffer_size(struct smscore_device_t *coredev);
-extern int smscore_map_common_buffer(struct smscore_device_t *coredev,
-				      struct vm_area_struct *vma);
-extern int smscore_send_fw_file(struct smscore_device_t *coredev,
-				u8 *ufwbuf, int size);
-
 extern
 struct smscore_buffer_t *smscore_getbuffer(struct smscore_device_t *coredev);
 extern void smscore_putbuffer(struct smscore_device_t *coredev,
diff --git a/drivers/media/common/siano/smsdvb-debugfs.c b/drivers/media/common/siano/smsdvb-debugfs.c
index 73990e469df9..d14ba271db50 100644
--- a/drivers/media/common/siano/smsdvb-debugfs.c
+++ b/drivers/media/common/siano/smsdvb-debugfs.c
@@ -398,8 +398,6 @@ void smsdvb_debugfs_release(struct smsdvb_client_t *client)
 
 void smsdvb_debugfs_register(void)
 {
-	struct dentry *d;
-
 	/*
 	 * FIXME: This was written to debug Siano USB devices. So, it creates
 	 * the debugfs node under <debugfs>/usb.
@@ -410,12 +408,7 @@ void smsdvb_debugfs_register(void)
 	 * node for sdio-based boards, but this may need some logic at sdio
 	 * subsystem.
 	 */
-	d = debugfs_create_dir("smsdvb", usb_debug_root);
-	if (IS_ERR_OR_NULL(d)) {
-		pr_err("Couldn't create sysfs node for smsdvb\n");
-		return;
-	}
-	smsdvb_debugfs_usb_root = d;
+	smsdvb_debugfs_usb_root = debugfs_create_dir("smsdvb", usb_debug_root);
 }
 
 void smsdvb_debugfs_unregister(void)
diff --git a/drivers/media/common/siano/smsdvb-main.c b/drivers/media/common/siano/smsdvb-main.c
index d893a0e4672b..9b1a650ed055 100644
--- a/drivers/media/common/siano/smsdvb-main.c
+++ b/drivers/media/common/siano/smsdvb-main.c
@@ -689,7 +689,7 @@ static int smsdvb_start_feed(struct dvb_demux_feed *feed)
 	pid_msg.x_msg_header.msg_flags = 0;
 	pid_msg.x_msg_header.msg_type  = MSG_SMS_ADD_PID_FILTER_REQ;
 	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
-	pid_msg.msg_data[0] = feed->pid;
+	pid_msg.msg_data = feed->pid;
 
 	return smsclient_sendrequest(client->smsclient,
 				     &pid_msg, sizeof(pid_msg));
@@ -711,7 +711,7 @@ static int smsdvb_stop_feed(struct dvb_demux_feed *feed)
 	pid_msg.x_msg_header.msg_flags = 0;
 	pid_msg.x_msg_header.msg_type  = MSG_SMS_REMOVE_PID_FILTER_REQ;
 	pid_msg.x_msg_header.msg_length = sizeof(pid_msg);
-	pid_msg.msg_data[0] = feed->pid;
+	pid_msg.msg_data = feed->pid;
 
 	return smsclient_sendrequest(client->smsclient,
 				     &pid_msg, sizeof(pid_msg));
@@ -1243,6 +1243,8 @@ static int __init smsdvb_module_init(void)
 	smsdvb_debugfs_register();
 
 	rc = smscore_register_hotplug(smsdvb_hotplug);
+	if (rc)
+		smsdvb_debugfs_unregister();
 
 	pr_debug("\n");
 
diff --git a/drivers/media/common/siano/smsendian.c b/drivers/media/common/siano/smsendian.c
index a3573814919b..b957970c7d97 100644
--- a/drivers/media/common/siano/smsendian.c
+++ b/drivers/media/common/siano/smsendian.c
@@ -20,11 +20,12 @@ void smsendian_handle_tx_message(void *buffer)
 	struct sms_msg_data *msg = buffer;
 	int i;
 	int msg_words;
+	u32 *msg_data = &msg->msg_data;
 
 	switch (msg->x_msg_header.msg_type) {
 	case MSG_SMS_DATA_DOWNLOAD_REQ:
 	{
-		msg->msg_data[0] = le32_to_cpu((__force __le32)(msg->msg_data[0]));
+		msg->msg_data = le32_to_cpu((__force __le32)(msg->msg_data));
 		break;
 	}
 
@@ -33,7 +34,7 @@ void smsendian_handle_tx_message(void *buffer)
 				sizeof(struct sms_msg_hdr))/4;
 
 		for (i = 0; i < msg_words; i++)
-			msg->msg_data[i] = le32_to_cpu((__force __le32)msg->msg_data[i]);
+			msg_data[i] = le32_to_cpu((__force __le32)msg_data[i]);
 
 		break;
 	}
@@ -66,11 +67,12 @@ void smsendian_handle_rx_message(void *buffer)
 
 	default:
 	{
+		u32 *msg_data = &msg->msg_data;
 		msg_words = (msg->x_msg_header.msg_length -
 				sizeof(struct sms_msg_hdr))/4;
 
 		for (i = 0; i < msg_words; i++)
-			msg->msg_data[i] = le32_to_cpu((__force __le32)msg->msg_data[i]);
+			msg_data[i] = le32_to_cpu((__force __le32)msg_data[i]);
 
 		break;
 	}
diff --git a/drivers/media/common/uvc.c b/drivers/media/common/uvc.c
index 9c0ba7a6c185..1ad4604474ac 100644
--- a/drivers/media/common/uvc.c
+++ b/drivers/media/common/uvc.c
@@ -97,6 +97,10 @@ static const struct uvc_format_desc uvc_fmts[] = {
 		.fcc		= V4L2_PIX_FMT_RGB565,
 	},
 	{
+		.guid		= UVC_GUID_FORMAT_D3DFMT_R5G6B5,
+		.fcc		= V4L2_PIX_FMT_RGB565,
+	},
+	{
 		.guid		= UVC_GUID_FORMAT_BGR3,
 		.fcc		= V4L2_PIX_FMT_BGR24,
 	},
@@ -121,6 +125,10 @@ static const struct uvc_format_desc uvc_fmts[] = {
 		.fcc		= V4L2_PIX_FMT_Y12I,
 	},
 	{
+		.guid		= UVC_GUID_FORMAT_Y16I,
+		.fcc		= V4L2_PIX_FMT_Y16I,
+	},
+	{
 		.guid		= UVC_GUID_FORMAT_Z16,
 		.fcc		= V4L2_PIX_FMT_Z16,
 	},
@@ -180,4 +188,5 @@ const struct uvc_format_desc *uvc_format_by_guid(const u8 guid[16])
 }
 EXPORT_SYMBOL_GPL(uvc_format_by_guid);
 
+MODULE_DESCRIPTION("USB Video Class common code");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
index 642c48e8c1f5..931e5dc453b9 100644
--- a/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
+++ b/drivers/media/common/v4l2-tpg/v4l2-tpg-core.c
@@ -1795,6 +1795,9 @@ static void tpg_precalculate_line(struct tpg_data *tpg)
 	unsigned p;
 	unsigned x;
 
+	if (WARN_ON_ONCE(!tpg->src_width || !tpg->scaled_width))
+		return;
+
 	switch (tpg->pattern) {
 	case TPG_PAT_GREEN:
 		contrast = TPG_COLOR_100_RED;
@@ -2246,10 +2249,10 @@ void tpg_log_status(struct tpg_data *tpg)
 		tpg->src_width, tpg->src_height,
 		tpg_color_enc_str(tpg->color_enc));
 	pr_info("tpg field: %u\n", tpg->field);
-	pr_info("tpg crop: %ux%u@%dx%d\n", tpg->crop.width, tpg->crop.height,
-			tpg->crop.left, tpg->crop.top);
-	pr_info("tpg compose: %ux%u@%dx%d\n", tpg->compose.width, tpg->compose.height,
-			tpg->compose.left, tpg->compose.top);
+	pr_info("tpg crop: (%d,%d)/%ux%u\n", tpg->crop.left, tpg->crop.top,
+		tpg->crop.width, tpg->crop.height);
+	pr_info("tpg compose: (%d,%d)/%ux%u\n", tpg->compose.left, tpg->compose.top,
+		tpg->compose.width, tpg->compose.height);
 	pr_info("tpg colorspace: %d\n", tpg->colorspace);
 	pr_info("tpg transfer function: %d/%d\n", tpg->xfer_func, tpg->real_xfer_func);
 	if (tpg->color_enc == TGP_COLOR_ENC_HSV)
diff --git a/drivers/media/common/videobuf2/videobuf2-core.c b/drivers/media/common/videobuf2/videobuf2-core.c
index b6bf8f232f48..2df566f409b6 100644
--- a/drivers/media/common/videobuf2/videobuf2-core.c
+++ b/drivers/media/common/videobuf2/videobuf2-core.c
@@ -199,7 +199,6 @@ module_param(debug, int, 0644);
 })
 
 static void __vb2_queue_cancel(struct vb2_queue *q);
-static void __enqueue_in_driver(struct vb2_buffer *vb);
 
 static const char *vb2_state_name(enum vb2_buffer_state s)
 {
@@ -304,14 +303,22 @@ static void __vb2_plane_dmabuf_put(struct vb2_buffer *vb, struct vb2_plane *p)
 	if (!p->mem_priv)
 		return;
 
-	if (p->dbuf_mapped)
-		call_void_memop(vb, unmap_dmabuf, p->mem_priv);
+	if (!p->dbuf_duplicated) {
+		if (p->dbuf_mapped)
+			call_void_memop(vb, unmap_dmabuf, p->mem_priv);
+
+		call_void_memop(vb, detach_dmabuf, p->mem_priv);
+	}
 
-	call_void_memop(vb, detach_dmabuf, p->mem_priv);
 	dma_buf_put(p->dbuf);
 	p->mem_priv = NULL;
 	p->dbuf = NULL;
 	p->dbuf_mapped = 0;
+	p->bytesused = 0;
+	p->length = 0;
+	p->m.fd = 0;
+	p->data_offset = 0;
+	p->dbuf_duplicated = false;
 }
 
 /*
@@ -320,9 +327,15 @@ static void __vb2_plane_dmabuf_put(struct vb2_buffer *vb, struct vb2_plane *p)
  */
 static void __vb2_buf_dmabuf_put(struct vb2_buffer *vb)
 {
-	unsigned int plane;
+	int plane;
 
-	for (plane = 0; plane < vb->num_planes; ++plane)
+	/*
+	 * When multiple planes share the same DMA buffer attachment, the plane
+	 * with the lowest index owns the mem_priv.
+	 * Put planes in the reversed order so that we don't leave invalid
+	 * mem_priv behind.
+	 */
+	for (plane = vb->num_planes - 1; plane >= 0; --plane)
 		__vb2_plane_dmabuf_put(vb, &vb->planes[plane]);
 }
 
@@ -421,11 +434,12 @@ static void init_buffer_cache_hints(struct vb2_queue *q, struct vb2_buffer *vb)
  */
 static void vb2_queue_add_buffer(struct vb2_queue *q, struct vb2_buffer *vb, unsigned int index)
 {
-	WARN_ON(index >= q->max_num_buffers || q->bufs[index] || vb->vb2_queue);
+	WARN_ON(index >= q->max_num_buffers || test_bit(index, q->bufs_bitmap) || vb->vb2_queue);
 
 	q->bufs[index] = vb;
 	vb->index = index;
 	vb->vb2_queue = q;
+	set_bit(index, q->bufs_bitmap);
 }
 
 /**
@@ -434,6 +448,7 @@ static void vb2_queue_add_buffer(struct vb2_queue *q, struct vb2_buffer *vb, uns
  */
 static void vb2_queue_remove_buffer(struct vb2_buffer *vb)
 {
+	clear_bit(vb->index, vb->vb2_queue->bufs_bitmap);
 	vb->vb2_queue->bufs[vb->index] = NULL;
 	vb->vb2_queue = NULL;
 }
@@ -442,16 +457,19 @@ static void vb2_queue_remove_buffer(struct vb2_buffer *vb)
  * __vb2_queue_alloc() - allocate vb2 buffer structures and (for MMAP type)
  * video buffer memory for all buffers/planes on the queue and initializes the
  * queue
+ * @first_index: index of the first created buffer, all newly allocated buffers
+ *		 have indices in the range [first_index..first_index+count-1]
  *
  * Returns the number of buffers successfully allocated.
  */
 static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 			     unsigned int num_buffers, unsigned int num_planes,
-			     const unsigned plane_sizes[VB2_MAX_PLANES])
+			     const unsigned int plane_sizes[VB2_MAX_PLANES],
+			     unsigned int *first_index)
 {
-	unsigned int q_num_buffers = vb2_get_num_buffers(q);
 	unsigned int buffer, plane;
 	struct vb2_buffer *vb;
+	unsigned long index = q->max_num_buffers;
 	int ret;
 
 	/*
@@ -459,7 +477,25 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 	 * in the queue is below q->max_num_buffers
 	 */
 	num_buffers = min_t(unsigned int, num_buffers,
-			    q->max_num_buffers - q_num_buffers);
+			    q->max_num_buffers - vb2_get_num_buffers(q));
+
+	while (num_buffers) {
+		index = bitmap_find_next_zero_area(q->bufs_bitmap, q->max_num_buffers,
+						   0, num_buffers, 0);
+
+		if (index < q->max_num_buffers)
+			break;
+		/* Try to find free space for less buffers */
+		num_buffers--;
+	}
+
+	/* If there is no space left to allocate buffers return 0 to indicate the error */
+	if (!num_buffers) {
+		*first_index = 0;
+		return 0;
+	}
+
+	*first_index = index;
 
 	for (buffer = 0; buffer < num_buffers; ++buffer) {
 		/* Allocate vb2 buffer structures */
@@ -479,7 +515,7 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 			vb->planes[plane].min_length = plane_sizes[plane];
 		}
 
-		vb2_queue_add_buffer(q, vb, q_num_buffers + buffer);
+		vb2_queue_add_buffer(q, vb, index++);
 		call_void_bufop(q, init_buffer, vb);
 
 		/* Allocate video buffer memory for the MMAP type */
@@ -517,17 +553,16 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 }
 
 /*
- * __vb2_free_mem() - release all video buffer memory for a given queue
+ * __vb2_free_mem() - release video buffer memory for a given range of
+ * buffers in a given queue
  */
-static void __vb2_free_mem(struct vb2_queue *q, unsigned int buffers)
+static void __vb2_free_mem(struct vb2_queue *q, unsigned int start, unsigned int count)
 {
-	unsigned int buffer;
+	unsigned int i;
 	struct vb2_buffer *vb;
-	unsigned int q_num_buffers = vb2_get_num_buffers(q);
 
-	for (buffer = q_num_buffers - buffers; buffer < q_num_buffers;
-	     ++buffer) {
-		vb = vb2_get_buffer(q, buffer);
+	for (i = start; i < start + count; i++) {
+		vb = vb2_get_buffer(q, i);
 		if (!vb)
 			continue;
 
@@ -542,35 +577,33 @@ static void __vb2_free_mem(struct vb2_queue *q, unsigned int buffers)
 }
 
 /*
- * __vb2_queue_free() - free buffers at the end of the queue - video memory and
+ * __vb2_queue_free() - free @count buffers from @start index of the queue - video memory and
  * related information, if no buffers are left return the queue to an
  * uninitialized state. Might be called even if the queue has already been freed.
  */
-static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
+static void __vb2_queue_free(struct vb2_queue *q, unsigned int start, unsigned int count)
 {
-	unsigned int buffer;
-	unsigned int q_num_buffers = vb2_get_num_buffers(q);
+	unsigned int i;
 
 	lockdep_assert_held(&q->mmap_lock);
 
 	/* Call driver-provided cleanup function for each buffer, if provided */
-	for (buffer = q_num_buffers - buffers; buffer < q_num_buffers;
-	     ++buffer) {
-		struct vb2_buffer *vb = vb2_get_buffer(q, buffer);
+	for (i = start; i < start + count; i++) {
+		struct vb2_buffer *vb = vb2_get_buffer(q, i);
 
 		if (vb && vb->planes[0].mem_priv)
 			call_void_vb_qop(vb, buf_cleanup, vb);
 	}
 
 	/* Release video buffer memory */
-	__vb2_free_mem(q, buffers);
+	__vb2_free_mem(q, start, count);
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	/*
 	 * Check that all the calls were balanced during the life-time of this
 	 * queue. If not then dump the counters to the kernel log.
 	 */
-	if (q_num_buffers) {
+	if (vb2_get_num_buffers(q)) {
 		bool unbalanced = q->cnt_start_streaming != q->cnt_stop_streaming ||
 				  q->cnt_prepare_streaming != q->cnt_unprepare_streaming ||
 				  q->cnt_wait_prepare != q->cnt_wait_finish;
@@ -596,8 +629,8 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 		q->cnt_stop_streaming = 0;
 		q->cnt_unprepare_streaming = 0;
 	}
-	for (buffer = 0; buffer < vb2_get_num_buffers(q); buffer++) {
-		struct vb2_buffer *vb = vb2_get_buffer(q, buffer);
+	for (i = start; i < start + count; i++) {
+		struct vb2_buffer *vb = vb2_get_buffer(q, i);
 		bool unbalanced;
 
 		if (!vb)
@@ -614,7 +647,7 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 
 		if (unbalanced) {
 			pr_info("unbalanced counters for queue %p, buffer %d:\n",
-				q, buffer);
+				q, i);
 			if (vb->cnt_buf_init != vb->cnt_buf_cleanup)
 				pr_info("     buf_init: %u buf_cleanup: %u\n",
 					vb->cnt_buf_init, vb->cnt_buf_cleanup);
@@ -648,9 +681,8 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 #endif
 
 	/* Free vb2 buffers */
-	for (buffer = q_num_buffers - buffers; buffer < q_num_buffers;
-	     ++buffer) {
-		struct vb2_buffer *vb = vb2_get_buffer(q, buffer);
+	for (i = start; i < start + count; i++) {
+		struct vb2_buffer *vb = vb2_get_buffer(q, i);
 
 		if (!vb)
 			continue;
@@ -659,7 +691,6 @@ static void __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 		kfree(vb);
 	}
 
-	q->num_buffers -= buffers;
 	if (!vb2_get_num_buffers(q)) {
 		q->memory = VB2_MEMORY_UNKNOWN;
 		INIT_LIST_HEAD(&q->queued_list);
@@ -691,7 +722,7 @@ EXPORT_SYMBOL(vb2_buffer_in_use);
 static bool __buffers_in_use(struct vb2_queue *q)
 {
 	unsigned int buffer;
-	for (buffer = 0; buffer < vb2_get_num_buffers(q); ++buffer) {
+	for (buffer = 0; buffer < q->max_num_buffers; ++buffer) {
 		struct vb2_buffer *vb = vb2_get_buffer(q, buffer);
 
 		if (!vb)
@@ -813,6 +844,32 @@ static bool verify_coherency_flags(struct vb2_queue *q, bool non_coherent_mem)
 	return true;
 }
 
+static int vb2_core_allocated_buffers_storage(struct vb2_queue *q)
+{
+	if (!q->bufs)
+		q->bufs = kcalloc(q->max_num_buffers, sizeof(*q->bufs), GFP_KERNEL);
+	if (!q->bufs)
+		return -ENOMEM;
+
+	if (!q->bufs_bitmap)
+		q->bufs_bitmap = bitmap_zalloc(q->max_num_buffers, GFP_KERNEL);
+	if (!q->bufs_bitmap) {
+		kfree(q->bufs);
+		q->bufs = NULL;
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void vb2_core_free_buffers_storage(struct vb2_queue *q)
+{
+	kfree(q->bufs);
+	q->bufs = NULL;
+	bitmap_free(q->bufs_bitmap);
+	q->bufs_bitmap = NULL;
+}
+
 int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 		     unsigned int flags, unsigned int *count)
 {
@@ -820,7 +877,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	unsigned int q_num_bufs = vb2_get_num_buffers(q);
 	unsigned plane_sizes[VB2_MAX_PLANES] = { };
 	bool non_coherent_mem = flags & V4L2_MEMORY_FLAG_NON_COHERENT;
-	unsigned int i;
+	unsigned int i, first_index;
 	int ret = 0;
 
 	if (q->streaming) {
@@ -851,9 +908,10 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 		 * queued without ever calling STREAMON.
 		 */
 		__vb2_queue_cancel(q);
-		__vb2_queue_free(q, q_num_bufs);
+		__vb2_queue_free(q, 0, q->max_num_buffers);
 		mutex_unlock(&q->mmap_lock);
 
+		q->is_busy = 0;
 		/*
 		 * In case of REQBUFS(0) return immediately without calling
 		 * driver's queue_setup() callback and allocating resources.
@@ -865,7 +923,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	/*
 	 * Make sure the requested values and current defaults are sane.
 	 */
-	num_buffers = max_t(unsigned int, *count, q->min_queued_buffers);
+	num_buffers = max_t(unsigned int, *count, q->min_reqbufs_allocation);
 	num_buffers = min_t(unsigned int, num_buffers, q->max_num_buffers);
 	memset(q->alloc_devs, 0, sizeof(q->alloc_devs));
 	/*
@@ -873,10 +931,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 * in the queue_setup op.
 	 */
 	mutex_lock(&q->mmap_lock);
-	if (!q->bufs)
-		q->bufs = kcalloc(q->max_num_buffers, sizeof(*q->bufs), GFP_KERNEL);
-	if (!q->bufs)
-		ret = -ENOMEM;
+	ret = vb2_core_allocated_buffers_storage(q);
 	q->memory = memory;
 	mutex_unlock(&q->mmap_lock);
 	if (ret)
@@ -906,8 +961,10 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers =
-		__vb2_queue_alloc(q, memory, num_buffers, num_planes, plane_sizes);
+		__vb2_queue_alloc(q, memory, num_buffers, num_planes, plane_sizes, &first_index);
 	if (allocated_buffers == 0) {
+		/* There shouldn't be any buffers allocated, so first_index == 0 */
+		WARN_ON(first_index);
 		dprintk(q, 1, "memory allocation failed\n");
 		ret = -ENOMEM;
 		goto error;
@@ -917,7 +974,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 * There is no point in continuing if we can't allocate the minimum
 	 * number of buffers needed by this vb2_queue.
 	 */
-	if (allocated_buffers < q->min_queued_buffers)
+	if (allocated_buffers < q->min_reqbufs_allocation)
 		ret = -ENOMEM;
 
 	/*
@@ -946,7 +1003,6 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	}
 
 	mutex_lock(&q->mmap_lock);
-	q->num_buffers = allocated_buffers;
 
 	if (ret < 0) {
 		/*
@@ -954,7 +1010,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 		 * from already queued buffers and it will reset q->memory to
 		 * VB2_MEMORY_UNKNOWN.
 		 */
-		__vb2_queue_free(q, allocated_buffers);
+		__vb2_queue_free(q, first_index, allocated_buffers);
 		mutex_unlock(&q->mmap_lock);
 		return ret;
 	}
@@ -966,6 +1022,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 */
 	*count = allocated_buffers;
 	q->waiting_for_buffers = !q->is_output;
+	q->is_busy = 1;
 
 	return 0;
 
@@ -973,6 +1030,7 @@ error:
 	mutex_lock(&q->mmap_lock);
 	q->memory = VB2_MEMORY_UNKNOWN;
 	mutex_unlock(&q->mmap_lock);
+	vb2_core_free_buffers_storage(q);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(vb2_core_reqbufs);
@@ -980,7 +1038,8 @@ EXPORT_SYMBOL_GPL(vb2_core_reqbufs);
 int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 			 unsigned int flags, unsigned int *count,
 			 unsigned int requested_planes,
-			 const unsigned int requested_sizes[])
+			 const unsigned int requested_sizes[],
+			 unsigned int *first_index)
 {
 	unsigned int num_planes = 0, num_buffers, allocated_buffers;
 	unsigned plane_sizes[VB2_MAX_PLANES] = { };
@@ -1005,11 +1064,8 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 		 * value in the queue_setup op.
 		 */
 		mutex_lock(&q->mmap_lock);
+		ret = vb2_core_allocated_buffers_storage(q);
 		q->memory = memory;
-		if (!q->bufs)
-			q->bufs = kcalloc(q->max_num_buffers, sizeof(*q->bufs), GFP_KERNEL);
-		if (!q->bufs)
-			ret = -ENOMEM;
 		mutex_unlock(&q->mmap_lock);
 		if (ret)
 			return ret;
@@ -1042,7 +1098,7 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers = __vb2_queue_alloc(q, memory, num_buffers,
-				num_planes, plane_sizes);
+				num_planes, plane_sizes, first_index);
 	if (allocated_buffers == 0) {
 		dprintk(q, 1, "memory allocation failed\n");
 		ret = -ENOMEM;
@@ -1072,7 +1128,6 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 	}
 
 	mutex_lock(&q->mmap_lock);
-	q->num_buffers += allocated_buffers;
 
 	if (ret < 0) {
 		/*
@@ -1080,7 +1135,7 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 		 * from already queued buffers and it will reset q->memory to
 		 * VB2_MEMORY_UNKNOWN.
 		 */
-		__vb2_queue_free(q, allocated_buffers);
+		__vb2_queue_free(q, *first_index, allocated_buffers);
 		mutex_unlock(&q->mmap_lock);
 		return -ENOMEM;
 	}
@@ -1091,6 +1146,7 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 	 * to the userspace.
 	 */
 	*count = allocated_buffers;
+	q->is_busy = 1;
 
 	return 0;
 
@@ -1327,7 +1383,7 @@ static int __prepare_dmabuf(struct vb2_buffer *vb)
 	struct vb2_plane planes[VB2_MAX_PLANES];
 	struct vb2_queue *q = vb->vb2_queue;
 	void *mem_priv;
-	unsigned int plane;
+	unsigned int plane, i;
 	int ret = 0;
 	bool reacquired = vb->planes[0].mem_priv == NULL;
 
@@ -1341,11 +1397,13 @@ static int __prepare_dmabuf(struct vb2_buffer *vb)
 	for (plane = 0; plane < vb->num_planes; ++plane) {
 		struct dma_buf *dbuf = dma_buf_get(planes[plane].m.fd);
 
+		planes[plane].dbuf = dbuf;
+
 		if (IS_ERR_OR_NULL(dbuf)) {
 			dprintk(q, 1, "invalid dmabuf fd for plane %d\n",
 				plane);
 			ret = -EINVAL;
-			goto err;
+			goto err_put_planes;
 		}
 
 		/* use DMABUF size if length is not provided */
@@ -1356,66 +1414,77 @@ static int __prepare_dmabuf(struct vb2_buffer *vb)
 			dprintk(q, 1, "invalid dmabuf length %u for plane %d, minimum length %u\n",
 				planes[plane].length, plane,
 				vb->planes[plane].min_length);
-			dma_buf_put(dbuf);
 			ret = -EINVAL;
-			goto err;
+			goto err_put_planes;
 		}
 
 		/* Skip the plane if already verified */
 		if (dbuf == vb->planes[plane].dbuf &&
-			vb->planes[plane].length == planes[plane].length) {
-			dma_buf_put(dbuf);
+		    vb->planes[plane].length == planes[plane].length)
 			continue;
-		}
 
 		dprintk(q, 3, "buffer for plane %d changed\n", plane);
 
-		if (!reacquired) {
-			reacquired = true;
+		reacquired = true;
+	}
+
+	if (reacquired) {
+		if (vb->planes[0].mem_priv) {
 			vb->copied_timestamp = 0;
 			call_void_vb_qop(vb, buf_cleanup, vb);
+			__vb2_buf_dmabuf_put(vb);
 		}
 
-		/* Release previously acquired memory if present */
-		__vb2_plane_dmabuf_put(vb, &vb->planes[plane]);
-		vb->planes[plane].bytesused = 0;
-		vb->planes[plane].length = 0;
-		vb->planes[plane].m.fd = 0;
-		vb->planes[plane].data_offset = 0;
+		for (plane = 0; plane < vb->num_planes; ++plane) {
+			/*
+			 * This is an optimization to reduce dma_buf attachment/mapping.
+			 * When the same dma_buf is used for multiple planes, there is no need
+			 * to create duplicated attachments.
+			 */
+			for (i = 0; i < plane; ++i) {
+				if (planes[plane].dbuf == vb->planes[i].dbuf &&
+				    q->alloc_devs[plane] == q->alloc_devs[i]) {
+					vb->planes[plane].dbuf_duplicated = true;
+					vb->planes[plane].dbuf = vb->planes[i].dbuf;
+					vb->planes[plane].mem_priv = vb->planes[i].mem_priv;
+					break;
+				}
+			}
 
-		/* Acquire each plane's memory */
-		mem_priv = call_ptr_memop(attach_dmabuf,
-					  vb,
-					  q->alloc_devs[plane] ? : q->dev,
-					  dbuf,
-					  planes[plane].length);
-		if (IS_ERR(mem_priv)) {
-			dprintk(q, 1, "failed to attach dmabuf\n");
-			ret = PTR_ERR(mem_priv);
-			dma_buf_put(dbuf);
-			goto err;
-		}
+			if (vb->planes[plane].dbuf_duplicated)
+				continue;
 
-		vb->planes[plane].dbuf = dbuf;
-		vb->planes[plane].mem_priv = mem_priv;
-	}
+			/* Acquire each plane's memory */
+			mem_priv = call_ptr_memop(attach_dmabuf,
+						  vb,
+						  q->alloc_devs[plane] ? : q->dev,
+						  planes[plane].dbuf,
+						  planes[plane].length);
+			if (IS_ERR(mem_priv)) {
+				dprintk(q, 1, "failed to attach dmabuf\n");
+				ret = PTR_ERR(mem_priv);
+				goto err_put_vb2_buf;
+			}
 
-	/*
-	 * This pins the buffer(s) with dma_buf_map_attachment()). It's done
-	 * here instead just before the DMA, while queueing the buffer(s) so
-	 * userspace knows sooner rather than later if the dma-buf map fails.
-	 */
-	for (plane = 0; plane < vb->num_planes; ++plane) {
-		if (vb->planes[plane].dbuf_mapped)
-			continue;
+			vb->planes[plane].dbuf = planes[plane].dbuf;
+			vb->planes[plane].mem_priv = mem_priv;
 
-		ret = call_memop(vb, map_dmabuf, vb->planes[plane].mem_priv);
-		if (ret) {
-			dprintk(q, 1, "failed to map dmabuf for plane %d\n",
-				plane);
-			goto err;
+			/*
+			 * This pins the buffer(s) with dma_buf_map_attachment()). It's done
+			 * here instead just before the DMA, while queueing the buffer(s) so
+			 * userspace knows sooner rather than later if the dma-buf map fails.
+			 */
+			ret = call_memop(vb, map_dmabuf, vb->planes[plane].mem_priv);
+			if (ret) {
+				dprintk(q, 1, "failed to map dmabuf for plane %d\n",
+					plane);
+				goto err_put_vb2_buf;
+			}
+			vb->planes[plane].dbuf_mapped = 1;
 		}
-		vb->planes[plane].dbuf_mapped = 1;
+	} else {
+		for (plane = 0; plane < vb->num_planes; ++plane)
+			dma_buf_put(planes[plane].dbuf);
 	}
 
 	/*
@@ -1437,7 +1506,7 @@ static int __prepare_dmabuf(struct vb2_buffer *vb)
 		ret = call_vb_qop(vb, buf_init, vb);
 		if (ret) {
 			dprintk(q, 1, "buffer initialization failed\n");
-			goto err;
+			goto err_put_vb2_buf;
 		}
 	}
 
@@ -1445,11 +1514,17 @@ static int __prepare_dmabuf(struct vb2_buffer *vb)
 	if (ret) {
 		dprintk(q, 1, "buffer preparation failed\n");
 		call_void_vb_qop(vb, buf_cleanup, vb);
-		goto err;
+		goto err_put_vb2_buf;
 	}
 
 	return 0;
-err:
+
+err_put_planes:
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		if (!IS_ERR_OR_NULL(planes[plane].dbuf))
+			dma_buf_put(planes[plane].dbuf);
+	}
+err_put_vb2_buf:
 	/* In case of errors, release planes that were already acquired */
 	__vb2_buf_dmabuf_put(vb);
 
@@ -1648,6 +1723,44 @@ int vb2_core_prepare_buf(struct vb2_queue *q, struct vb2_buffer *vb, void *pb)
 }
 EXPORT_SYMBOL_GPL(vb2_core_prepare_buf);
 
+int vb2_core_remove_bufs(struct vb2_queue *q, unsigned int start, unsigned int count)
+{
+	unsigned int i, ret = 0;
+	unsigned int q_num_bufs = vb2_get_num_buffers(q);
+
+	if (count == 0)
+		return 0;
+
+	if (count > q_num_bufs)
+		return -EINVAL;
+
+	if (start > q->max_num_buffers - count)
+		return -EINVAL;
+
+	mutex_lock(&q->mmap_lock);
+
+	/* Check that all buffers in the range exist */
+	for (i = start; i < start + count; i++) {
+		struct vb2_buffer *vb = vb2_get_buffer(q, i);
+
+		if (!vb) {
+			ret = -EINVAL;
+			goto unlock;
+		}
+		if (vb->state != VB2_BUF_STATE_DEQUEUED) {
+			ret = -EBUSY;
+			goto unlock;
+		}
+	}
+	__vb2_queue_free(q, start, count);
+	dprintk(q, 2, "%u buffers removed\n", count);
+
+unlock:
+	mutex_unlock(&q->mmap_lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vb2_core_remove_bufs);
+
 /*
  * vb2_start_streaming() - Attempt to start streaming.
  * @q:		videobuf2 queue
@@ -1694,7 +1807,7 @@ static int vb2_start_streaming(struct vb2_queue *q)
 		 * Forcefully reclaim buffers if the driver did not
 		 * correctly return them to vb2.
 		 */
-		for (i = 0; i < vb2_get_num_buffers(q); ++i) {
+		for (i = 0; i < q->max_num_buffers; ++i) {
 			vb = vb2_get_buffer(q, i);
 
 			if (!vb)
@@ -1924,7 +2037,10 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 		 * become ready or for streamoff. Driver's lock is released to
 		 * allow streamoff or qbuf to be called while waiting.
 		 */
-		call_void_qop(q, wait_prepare, q);
+		if (q->ops->wait_prepare)
+			call_void_qop(q, wait_prepare, q);
+		else if (q->lock)
+			mutex_unlock(q->lock);
 
 		/*
 		 * All locks have been released, it is safe to sleep now.
@@ -1934,12 +2050,16 @@ static int __vb2_wait_for_done_vb(struct vb2_queue *q, int nonblocking)
 				!list_empty(&q->done_list) || !q->streaming ||
 				q->error);
 
+		if (q->ops->wait_finish)
+			call_void_qop(q, wait_finish, q);
+		else if (q->lock)
+			mutex_lock(q->lock);
+
+		q->waiting_in_dqbuf = 0;
 		/*
 		 * We need to reevaluate both conditions again after reacquiring
 		 * the locks or return an error if one occurred.
 		 */
-		call_void_qop(q, wait_finish, q);
-		q->waiting_in_dqbuf = 0;
 		if (ret) {
 			dprintk(q, 1, "sleep was interrupted\n");
 			return ret;
@@ -2100,7 +2220,7 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 	 * to vb2 in stop_streaming().
 	 */
 	if (WARN_ON(atomic_read(&q->owned_by_drv_count))) {
-		for (i = 0; i < vb2_get_num_buffers(q); i++) {
+		for (i = 0; i < q->max_num_buffers; i++) {
 			struct vb2_buffer *vb = vb2_get_buffer(q, i);
 
 			if (!vb)
@@ -2144,7 +2264,7 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 	 * call to __fill_user_buffer() after buf_finish(). That order can't
 	 * be changed, so we can't move the buf_finish() to __vb2_dqbuf().
 	 */
-	for (i = 0; i < vb2_get_num_buffers(q); i++) {
+	for (i = 0; i < q->max_num_buffers; i++) {
 		struct vb2_buffer *vb;
 		struct media_request *req;
 
@@ -2211,7 +2331,7 @@ int vb2_core_streamon(struct vb2_queue *q, unsigned int type)
 	}
 
 	if (q_num_bufs < q->min_queued_buffers) {
-		dprintk(q, 1, "need at least %u queued buffers\n",
+		dprintk(q, 1, "need at least %u allocated buffers\n",
 			q->min_queued_buffers);
 		return -EINVAL;
 	}
@@ -2503,7 +2623,7 @@ int vb2_core_queue_init(struct vb2_queue *q)
 	    WARN_ON(!q->ops->buf_queue))
 		return -EINVAL;
 
-	if (WARN_ON(q->max_num_buffers > MAX_BUFFER_INDEX) ||
+	if (WARN_ON(q->max_num_buffers < VB2_MAX_FRAME) ||
 	    WARN_ON(q->min_queued_buffers > q->max_num_buffers))
 		return -EINVAL;
 
@@ -2521,6 +2641,26 @@ int vb2_core_queue_init(struct vb2_queue *q)
 	if (WARN_ON(q->supports_requests && q->min_queued_buffers))
 		return -EINVAL;
 
+	/*
+	 * If the driver needs 'min_queued_buffers' in the queue before
+	 * calling start_streaming() then the minimum requirement is
+	 * 'min_queued_buffers + 1' to keep at least one buffer available
+	 * for userspace.
+	 */
+	if (q->min_reqbufs_allocation < q->min_queued_buffers + 1)
+		q->min_reqbufs_allocation = q->min_queued_buffers + 1;
+
+	if (WARN_ON(q->min_reqbufs_allocation > q->max_num_buffers))
+		return -EINVAL;
+
+	/* Either both or none are set */
+	if (WARN_ON(!q->ops->wait_prepare ^ !q->ops->wait_finish))
+		return -EINVAL;
+
+	/* Warn if q->lock is NULL and no custom wait_prepare is provided */
+	if (WARN_ON(!q->lock && !q->ops->wait_prepare))
+		return -EINVAL;
+
 	INIT_LIST_HEAD(&q->queued_list);
 	INIT_LIST_HEAD(&q->done_list);
 	spin_lock_init(&q->done_lock);
@@ -2552,9 +2692,9 @@ void vb2_core_queue_release(struct vb2_queue *q)
 	__vb2_cleanup_fileio(q);
 	__vb2_queue_cancel(q);
 	mutex_lock(&q->mmap_lock);
-	__vb2_queue_free(q, vb2_get_num_buffers(q));
-	kfree(q->bufs);
-	q->bufs = NULL;
+	__vb2_queue_free(q, 0, q->max_num_buffers);
+	vb2_core_free_buffers_storage(q);
+	q->is_busy = 0;
 	mutex_unlock(&q->mmap_lock);
 }
 EXPORT_SYMBOL_GPL(vb2_core_queue_release);
@@ -2713,7 +2853,6 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	struct vb2_fileio_data *fileio;
 	struct vb2_buffer *vb;
 	int i, ret;
-	unsigned int count = 0;
 
 	/*
 	 * Sanity check
@@ -2734,18 +2873,8 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	if (q->streaming || vb2_get_num_buffers(q) > 0)
 		return -EBUSY;
 
-	/*
-	 * Start with q->min_queued_buffers + 1, driver can increase it in
-	 * queue_setup()
-	 *
-	 * 'min_queued_buffers' buffers need to be queued up before you
-	 * can start streaming, plus 1 for userspace (or in this case,
-	 * kernelspace) processing.
-	 */
-	count = max(2, q->min_queued_buffers + 1);
-
 	dprintk(q, 3, "setting up file io: mode %s, count %d, read_once %d, write_immediately %d\n",
-		(read) ? "read" : "write", count, q->fileio_read_once,
+		(read) ? "read" : "write", q->min_reqbufs_allocation, q->fileio_read_once,
 		q->fileio_write_immediately);
 
 	fileio = kzalloc(sizeof(*fileio), GFP_KERNEL);
@@ -2759,13 +2888,19 @@ static int __vb2_init_fileio(struct vb2_queue *q, int read)
 	 * Request buffers and use MMAP type to force driver
 	 * to allocate buffers by itself.
 	 */
-	fileio->count = count;
+	fileio->count = q->min_reqbufs_allocation;
 	fileio->memory = VB2_MEMORY_MMAP;
 	fileio->type = q->type;
 	q->fileio = fileio;
 	ret = vb2_core_reqbufs(q, fileio->memory, 0, &fileio->count);
 	if (ret)
 		goto err_kfree;
+	/* vb2_fileio_data supports max VB2_MAX_FRAME buffers */
+	if (fileio->count > VB2_MAX_FRAME) {
+		dprintk(q, 1, "fileio: more than VB2_MAX_FRAME buffers requested\n");
+		ret = -ENOSPC;
+		goto err_reqbufs;
+	}
 
 	/*
 	 * Userspace can never add or delete buffers later, so there
@@ -3085,10 +3220,17 @@ static int vb2_thread(void *data)
 				continue;
 			prequeue--;
 		} else {
-			call_void_qop(q, wait_finish, q);
-			if (!threadio->stop)
+			if (!threadio->stop) {
+				if (q->ops->wait_finish)
+					call_void_qop(q, wait_finish, q);
+				else if (q->lock)
+					mutex_lock(q->lock);
 				ret = vb2_core_dqbuf(q, &index, NULL, 0);
-			call_void_qop(q, wait_prepare, q);
+				if (q->ops->wait_prepare)
+					call_void_qop(q, wait_prepare, q);
+				else if (q->lock)
+					mutex_unlock(q->lock);
+			}
 			dprintk(q, 5, "file io: vb2_dqbuf result: %d\n", ret);
 			if (!ret)
 				vb = vb2_get_buffer(q, index);
@@ -3100,12 +3242,19 @@ static int vb2_thread(void *data)
 		if (vb->state != VB2_BUF_STATE_ERROR)
 			if (threadio->fnc(vb, threadio->priv))
 				break;
-		call_void_qop(q, wait_finish, q);
 		if (copy_timestamp)
 			vb->timestamp = ktime_get_ns();
-		if (!threadio->stop)
+		if (!threadio->stop) {
+			if (q->ops->wait_finish)
+				call_void_qop(q, wait_finish, q);
+			else if (q->lock)
+				mutex_lock(q->lock);
 			ret = vb2_core_qbuf(q, vb, NULL, NULL);
-		call_void_qop(q, wait_prepare, q);
+			if (q->ops->wait_prepare)
+				call_void_qop(q, wait_prepare, q);
+			else if (q->lock)
+				mutex_unlock(q->lock);
+		}
 		if (ret || threadio->stop)
 			break;
 	}
@@ -3185,4 +3334,4 @@ EXPORT_SYMBOL_GPL(vb2_thread_stop);
 MODULE_DESCRIPTION("Media buffer core framework");
 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>, Marek Szyprowski");
 MODULE_LICENSE("GPL");
-MODULE_IMPORT_NS(DMA_BUF);
+MODULE_IMPORT_NS("DMA_BUF");
diff --git a/drivers/media/common/videobuf2/videobuf2-dma-contig.c b/drivers/media/common/videobuf2/videobuf2-dma-contig.c
index 3d4fd4ef5310..a13ec569c82f 100644
--- a/drivers/media/common/videobuf2/videobuf2-dma-contig.c
+++ b/drivers/media/common/videobuf2/videobuf2-dma-contig.c
@@ -854,8 +854,7 @@ int vb2_dma_contig_set_max_seg_size(struct device *dev, unsigned int size)
 		return -ENODEV;
 	}
 	if (dma_get_max_seg_size(dev) < size)
-		return dma_set_max_seg_size(dev, size);
-
+		dma_set_max_seg_size(dev, size);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(vb2_dma_contig_set_max_seg_size);
@@ -863,4 +862,4 @@ EXPORT_SYMBOL_GPL(vb2_dma_contig_set_max_seg_size);
 MODULE_DESCRIPTION("DMA-contig memory handling routines for videobuf2");
 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>");
 MODULE_LICENSE("GPL");
-MODULE_IMPORT_NS(DMA_BUF);
+MODULE_IMPORT_NS("DMA_BUF");
diff --git a/drivers/media/common/videobuf2/videobuf2-dma-sg.c b/drivers/media/common/videobuf2/videobuf2-dma-sg.c
index 6975a71d740f..b3bf2173c14e 100644
--- a/drivers/media/common/videobuf2/videobuf2-dma-sg.c
+++ b/drivers/media/common/videobuf2/videobuf2-dma-sg.c
@@ -469,7 +469,7 @@ vb2_dma_sg_dmabuf_ops_begin_cpu_access(struct dma_buf *dbuf,
 	struct vb2_dma_sg_buf *buf = dbuf->priv;
 	struct sg_table *sgt = buf->dma_sgt;
 
-	dma_sync_sg_for_cpu(buf->dev, sgt->sgl, sgt->nents, buf->dma_dir);
+	dma_sync_sgtable_for_cpu(buf->dev, sgt, buf->dma_dir);
 	return 0;
 }
 
@@ -480,7 +480,7 @@ vb2_dma_sg_dmabuf_ops_end_cpu_access(struct dma_buf *dbuf,
 	struct vb2_dma_sg_buf *buf = dbuf->priv;
 	struct sg_table *sgt = buf->dma_sgt;
 
-	dma_sync_sg_for_device(buf->dev, sgt->sgl, sgt->nents, buf->dma_dir);
+	dma_sync_sgtable_for_device(buf->dev, sgt, buf->dma_dir);
 	return 0;
 }
 
@@ -676,4 +676,4 @@ EXPORT_SYMBOL_GPL(vb2_dma_sg_memops);
 MODULE_DESCRIPTION("dma scatter/gather memory handling routines for videobuf2");
 MODULE_AUTHOR("Andrzej Pietrasiewicz");
 MODULE_LICENSE("GPL");
-MODULE_IMPORT_NS(DMA_BUF);
+MODULE_IMPORT_NS("DMA_BUF");
diff --git a/drivers/media/common/videobuf2/videobuf2-dvb.c b/drivers/media/common/videobuf2/videobuf2-dvb.c
index 8c15bcd07eef..3746bf6d1c15 100644
--- a/drivers/media/common/videobuf2/videobuf2-dvb.c
+++ b/drivers/media/common/videobuf2/videobuf2-dvb.c
@@ -19,6 +19,7 @@
 /* ------------------------------------------------------------------ */
 
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
+MODULE_DESCRIPTION("Videobuf2 helper library for simple DVB cards");
 MODULE_LICENSE("GPL");
 
 /* ------------------------------------------------------------------ */
diff --git a/drivers/media/common/videobuf2/videobuf2-v4l2.c b/drivers/media/common/videobuf2/videobuf2-v4l2.c
index c575198e8354..1cd26faee503 100644
--- a/drivers/media/common/videobuf2/videobuf2-v4l2.c
+++ b/drivers/media/common/videobuf2/videobuf2-v4l2.c
@@ -231,7 +231,7 @@ static int vb2_fill_vb2_v4l2_buffer(struct vb2_buffer *vb, struct v4l2_buffer *b
 			break;
 		}
 
-		/* Fill in driver-provided information for OUTPUT types */
+		/* Fill in user-provided information for OUTPUT types */
 		if (V4L2_TYPE_IS_OUTPUT(b->type)) {
 			/*
 			 * Will have to go up to b->length when API starts
@@ -685,7 +685,7 @@ static void vb2_set_flags_and_caps(struct vb2_queue *q, u32 memory,
 		*flags &= V4L2_MEMORY_FLAG_NON_COHERENT;
 	}
 
-	*caps = V4L2_BUF_CAP_SUPPORTS_ORPHANED_BUFS;
+	*caps |= V4L2_BUF_CAP_SUPPORTS_ORPHANED_BUFS;
 	if (q->io_modes & VB2_MMAP)
 		*caps |= V4L2_BUF_CAP_SUPPORTS_MMAP;
 	if (q->io_modes & VB2_USERPTR)
@@ -795,11 +795,15 @@ int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 	for (i = 0; i < requested_planes; i++)
 		if (requested_sizes[i] == 0)
 			return -EINVAL;
-	return ret ? ret : vb2_core_create_bufs(q, create->memory,
-						create->flags,
-						&create->count,
-						requested_planes,
-						requested_sizes);
+	if (ret)
+		return ret;
+
+	return vb2_core_create_bufs(q, create->memory,
+				    create->flags,
+				    &create->count,
+				    requested_planes,
+				    requested_sizes,
+				    &create->index);
 }
 EXPORT_SYMBOL_GPL(vb2_create_bufs);
 
@@ -899,6 +903,11 @@ EXPORT_SYMBOL_GPL(vb2_expbuf);
 
 int vb2_queue_init_name(struct vb2_queue *q, const char *name)
 {
+	/* vb2_memory should match with v4l2_memory */
+	BUILD_BUG_ON(VB2_MEMORY_MMAP != (int)V4L2_MEMORY_MMAP);
+	BUILD_BUG_ON(VB2_MEMORY_USERPTR != (int)V4L2_MEMORY_USERPTR);
+	BUILD_BUG_ON(VB2_MEMORY_DMABUF != (int)V4L2_MEMORY_DMABUF);
+
 	/*
 	 * Sanity check
 	 */
@@ -912,12 +921,6 @@ int vb2_queue_init_name(struct vb2_queue *q, const char *name)
 	WARN_ON((q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
 		V4L2_BUF_FLAG_TIMESTAMP_UNKNOWN);
 
-	/* Warn that vb2_memory should match with v4l2_memory */
-	if (WARN_ON(VB2_MEMORY_MMAP != (int)V4L2_MEMORY_MMAP)
-		|| WARN_ON(VB2_MEMORY_USERPTR != (int)V4L2_MEMORY_USERPTR)
-		|| WARN_ON(VB2_MEMORY_DMABUF != (int)V4L2_MEMORY_DMABUF))
-		return -EINVAL;
-
 	if (q->buf_struct_size == 0)
 		q->buf_struct_size = sizeof(struct vb2_v4l2_buffer);
 
@@ -997,6 +1000,24 @@ EXPORT_SYMBOL_GPL(vb2_poll);
 
 /* vb2 ioctl helpers */
 
+int vb2_ioctl_remove_bufs(struct file *file, void *priv,
+			  struct v4l2_remove_buffers *d)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->queue->type != d->type)
+		return -EINVAL;
+
+	if (d->count == 0)
+		return 0;
+
+	if (vb2_queue_is_busy(vdev->queue, file))
+		return -EBUSY;
+
+	return vb2_core_remove_bufs(vdev->queue, d->index, d->count);
+}
+EXPORT_SYMBOL_GPL(vb2_ioctl_remove_bufs);
+
 int vb2_ioctl_reqbufs(struct file *file, void *priv,
 			  struct v4l2_requestbuffers *p)
 {
diff --git a/drivers/media/common/videobuf2/videobuf2-vmalloc.c b/drivers/media/common/videobuf2/videobuf2-vmalloc.c
index 7d953706f3f8..3f777068cd34 100644
--- a/drivers/media/common/videobuf2/videobuf2-vmalloc.c
+++ b/drivers/media/common/videobuf2/videobuf2-vmalloc.c
@@ -442,4 +442,4 @@ EXPORT_SYMBOL_GPL(vb2_vmalloc_memops);
 MODULE_DESCRIPTION("vmalloc memory handling routines for videobuf2");
 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>");
 MODULE_LICENSE("GPL");
-MODULE_IMPORT_NS(DMA_BUF);
+MODULE_IMPORT_NS("DMA_BUF");
diff --git a/drivers/media/dvb-core/dmxdev.c b/drivers/media/dvb-core/dmxdev.c
index 9ce5f010de3f..151177e5a06d 100644
--- a/drivers/media/dvb-core/dmxdev.c
+++ b/drivers/media/dvb-core/dmxdev.c
@@ -352,7 +352,8 @@ static int dvb_dmxdev_set_buffer_size(struct dmxdev_filter *dmxdevfilter,
 
 static void dvb_dmxdev_filter_timeout(struct timer_list *t)
 {
-	struct dmxdev_filter *dmxdevfilter = from_timer(dmxdevfilter, t, timer);
+	struct dmxdev_filter *dmxdevfilter = timer_container_of(dmxdevfilter,
+								t, timer);
 
 	dmxdevfilter->buffer.error = -ETIMEDOUT;
 	spin_lock_irq(&dmxdevfilter->dev->lock);
@@ -365,7 +366,7 @@ static void dvb_dmxdev_filter_timer(struct dmxdev_filter *dmxdevfilter)
 {
 	struct dmx_sct_filter_params *para = &dmxdevfilter->params.sec;
 
-	del_timer(&dmxdevfilter->timer);
+	timer_delete(&dmxdevfilter->timer);
 	if (para->timeout) {
 		dmxdevfilter->timer.expires =
 		    jiffies + 1 + (HZ / 2 + HZ * para->timeout) / 1000;
@@ -391,7 +392,7 @@ static int dvb_dmxdev_section_callback(const u8 *buffer1, size_t buffer1_len,
 		spin_unlock(&dmxdevfilter->dev->lock);
 		return 0;
 	}
-	del_timer(&dmxdevfilter->timer);
+	timer_delete(&dmxdevfilter->timer);
 	dprintk("section callback %*ph\n", 6, buffer1);
 	if (dvb_vb2_is_streaming(&dmxdevfilter->vb2_ctx)) {
 		ret = dvb_vb2_fill_buffer(&dmxdevfilter->vb2_ctx,
@@ -482,7 +483,7 @@ static int dvb_dmxdev_feed_stop(struct dmxdev_filter *dmxdevfilter)
 
 	switch (dmxdevfilter->type) {
 	case DMXDEV_TYPE_SEC:
-		del_timer(&dmxdevfilter->timer);
+		timer_delete(&dmxdevfilter->timer);
 		dmxdevfilter->feed.sec->stop_filtering(dmxdevfilter->feed.sec);
 		break;
 	case DMXDEV_TYPE_PES:
@@ -731,7 +732,7 @@ static int dvb_dmxdev_filter_start(struct dmxdev_filter *filter)
 		ret = (*secfeed)->allocate_filter(*secfeed, secfilter);
 		if (ret < 0) {
 			dvb_dmxdev_feed_restart(filter);
-			filter->feed.sec->start_filtering(*secfeed);
+			*secfeed = NULL;
 			dprintk("could not get filter\n");
 			return ret;
 		}
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index 4f78f30b3646..a05aa271a1ba 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -443,8 +443,8 @@ static int dvb_frontend_swzigzag_autotune(struct dvb_frontend *fe, int check_wra
 
 		default:
 			fepriv->auto_step++;
-			fepriv->auto_sub_step = -1; /* it'll be incremented to 0 in a moment */
-			break;
+			fepriv->auto_sub_step = 0;
+			continue;
 		}
 
 		if (!ready) fepriv->auto_sub_step++;
diff --git a/drivers/media/dvb-core/dvb_vb2.c b/drivers/media/dvb-core/dvb_vb2.c
index 192a8230c4aa..29edaaff7a5c 100644
--- a/drivers/media/dvb-core/dvb_vb2.c
+++ b/drivers/media/dvb-core/dvb_vb2.c
@@ -366,9 +366,15 @@ int dvb_vb2_querybuf(struct dvb_vb2_ctx *ctx, struct dmx_buffer *b)
 int dvb_vb2_expbuf(struct dvb_vb2_ctx *ctx, struct dmx_exportbuffer *exp)
 {
 	struct vb2_queue *q = &ctx->vb_q;
+	struct vb2_buffer *vb2 = vb2_get_buffer(q, exp->index);
 	int ret;
 
-	ret = vb2_core_expbuf(&ctx->vb_q, &exp->fd, q->type, q->bufs[exp->index],
+	if (!vb2) {
+		dprintk(1, "[%s] invalid buffer index\n", ctx->name);
+		return -EINVAL;
+	}
+
+	ret = vb2_core_expbuf(&ctx->vb_q, &exp->fd, q->type, vb2,
 			      0, exp->flags);
 	if (ret) {
 		dprintk(1, "[%s] index=%d errno=%d\n", ctx->name,
diff --git a/drivers/media/dvb-core/dvbdev.c b/drivers/media/dvb-core/dvbdev.c
index 733d0bc4b4cc..9df7c213716a 100644
--- a/drivers/media/dvb-core/dvbdev.c
+++ b/drivers/media/dvb-core/dvbdev.c
@@ -86,10 +86,15 @@ static DECLARE_RWSEM(minor_rwsem);
 static int dvb_device_open(struct inode *inode, struct file *file)
 {
 	struct dvb_device *dvbdev;
+	unsigned int minor = iminor(inode);
+
+	if (minor >= MAX_DVB_MINORS)
+		return -ENODEV;
 
 	mutex_lock(&dvbdev_mutex);
 	down_read(&minor_rwsem);
-	dvbdev = dvb_minors[iminor(inode)];
+
+	dvbdev = dvb_minors[minor];
 
 	if (dvbdev && dvbdev->fops) {
 		int err = 0;
@@ -525,7 +530,10 @@ int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
 	for (minor = 0; minor < MAX_DVB_MINORS; minor++)
 		if (!dvb_minors[minor])
 			break;
-	if (minor == MAX_DVB_MINORS) {
+#else
+	minor = nums2minor(adap->num, type, id);
+#endif
+	if (minor >= MAX_DVB_MINORS) {
 		if (new_node) {
 			list_del(&new_node->list_head);
 			kfree(dvbdevfops);
@@ -538,9 +546,7 @@ int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
 		mutex_unlock(&dvbdev_register_lock);
 		return -EINVAL;
 	}
-#else
-	minor = nums2minor(adap->num, type, id);
-#endif
+
 	dvbdev->minor = minor;
 	dvb_minors[minor] = dvb_device_get(dvbdev);
 	up_write(&minor_rwsem);
@@ -956,7 +962,7 @@ int dvb_usercopy(struct file *file,
 		 int (*func)(struct file *file,
 			     unsigned int cmd, void *arg))
 {
-	char    sbuf[128];
+	char    sbuf[128] = {};
 	void    *mbuf = NULL;
 	void    *parg = NULL;
 	int     err  = -EINVAL;
diff --git a/drivers/media/dvb-frontends/a8293.c b/drivers/media/dvb-frontends/a8293.c
index f39887c04978..bf2773c5b97a 100644
--- a/drivers/media/dvb-frontends/a8293.c
+++ b/drivers/media/dvb-frontends/a8293.c
@@ -256,7 +256,7 @@ static void a8293_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id a8293_id_table[] = {
-	{"a8293", 0},
+	{ "a8293" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, a8293_id_table);
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index a829c89792a4..befd6a4eafd9 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -1480,7 +1480,7 @@ static int af9013_probe(struct i2c_client *client)
 		goto err_regmap_exit;
 	}
 	state->muxc->priv = state;
-	ret = i2c_mux_add_adapter(state->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(state->muxc, 0, 0);
 	if (ret)
 		goto err_regmap_exit;
 
@@ -1553,7 +1553,7 @@ static void af9013_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id af9013_id_table[] = {
-	{"af9013", 0},
+	{ "af9013" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, af9013_id_table);
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index 49b7b04a7899..eed2ea4da8fa 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -1173,7 +1173,7 @@ static void af9033_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id af9033_id_table[] = {
-	{"af9033", 0},
+	{ "af9033" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, af9033_id_table);
diff --git a/drivers/media/dvb-frontends/as102_fe_types.h b/drivers/media/dvb-frontends/as102_fe_types.h
index 297f9520ebf9..8a4e392c8896 100644
--- a/drivers/media/dvb-frontends/as102_fe_types.h
+++ b/drivers/media/dvb-frontends/as102_fe_types.h
@@ -174,6 +174,6 @@ struct as10x_register_addr {
 	uint32_t addr;
 	/* register mode access */
 	uint8_t mode;
-};
+} __packed;
 
 #endif
diff --git a/drivers/media/dvb-frontends/au8522_decoder.c b/drivers/media/dvb-frontends/au8522_decoder.c
index acc27376c246..58c4c489bf97 100644
--- a/drivers/media/dvb-frontends/au8522_decoder.c
+++ b/drivers/media/dvb-frontends/au8522_decoder.c
@@ -25,6 +25,7 @@
 #include "au8522_priv.h"
 
 MODULE_AUTHOR("Devin Heitmueller");
+MODULE_DESCRIPTION("Auvitek AU8522 QAM/8VSB demodulator driver and video decoder");
 MODULE_LICENSE("GPL");
 
 static int au8522_analog_debug;
@@ -766,7 +767,7 @@ static void au8522_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id au8522_id[] = {
-	{"au8522", 0},
+	{ "au8522" },
 	{}
 };
 
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 27f1de21f571..d935fb10e620 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -729,7 +729,7 @@ static int bcm3510_init_cold(struct bcm3510_state *st)
 	int ret;
 	bcm3510_register_value v;
 
-	/* read Acquisation Processor status register and check it is not in RUN mode */
+	/* read Acquisition Processor status register and check it is not in RUN mode */
 	if ((ret = bcm3510_readB(st,0xa2,&v)) < 0)
 		return ret;
 	if (v.APSTAT1_a2.RUN) {
diff --git a/drivers/media/dvb-frontends/cx24116.c b/drivers/media/dvb-frontends/cx24116.c
index 8b978a9f74a4..f5dd3a81725a 100644
--- a/drivers/media/dvb-frontends/cx24116.c
+++ b/drivers/media/dvb-frontends/cx24116.c
@@ -741,6 +741,7 @@ static int cx24116_read_snr_pct(struct dvb_frontend *fe, u16 *snr)
 {
 	struct cx24116_state *state = fe->demodulator_priv;
 	u8 snr_reading;
+	int ret;
 	static const u32 snr_tab[] = { /* 10 x Table (rounded up) */
 		0x00000, 0x0199A, 0x03333, 0x04ccD, 0x06667,
 		0x08000, 0x0999A, 0x0b333, 0x0cccD, 0x0e667,
@@ -749,7 +750,11 @@ static int cx24116_read_snr_pct(struct dvb_frontend *fe, u16 *snr)
 
 	dprintk("%s()\n", __func__);
 
-	snr_reading = cx24116_readreg(state, CX24116_REG_QUALITY0);
+	ret = cx24116_readreg(state, CX24116_REG_QUALITY0);
+	if (ret  < 0)
+		return ret;
+
+	snr_reading = ret;
 
 	if (snr_reading >= 0xa0 /* 100% */)
 		*snr = 0xffff;
diff --git a/drivers/media/dvb-frontends/cxd2099.c b/drivers/media/dvb-frontends/cxd2099.c
index 3f3b85743666..5e6e18819a0d 100644
--- a/drivers/media/dvb-frontends/cxd2099.c
+++ b/drivers/media/dvb-frontends/cxd2099.c
@@ -672,7 +672,7 @@ static void cxd2099_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id cxd2099_id[] = {
-	{"cxd2099", 0},
+	{ "cxd2099" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, cxd2099_id);
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 7feb08dccfa1..c3d8ced6c3ba 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -723,7 +723,7 @@ static void cxd2820r_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id cxd2820r_id_table[] = {
-	{"cxd2820r", 0},
+	{ "cxd2820r" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, cxd2820r_id_table);
diff --git a/drivers/media/dvb-frontends/cxd2841er.c b/drivers/media/dvb-frontends/cxd2841er.c
index d925ca24183b..415f1f91cc30 100644
--- a/drivers/media/dvb-frontends/cxd2841er.c
+++ b/drivers/media/dvb-frontends/cxd2841er.c
@@ -311,12 +311,8 @@ static int cxd2841er_set_reg_bits(struct cxd2841er_priv *priv,
 
 static u32 cxd2841er_calc_iffreq_xtal(enum cxd2841er_xtal xtal, u32 ifhz)
 {
-	u64 tmp;
-
-	tmp = (u64) ifhz * 16777216;
-	do_div(tmp, ((xtal == SONY_XTAL_24000) ? 48000000 : 41000000));
-
-	return (u32) tmp;
+	return div_u64(ifhz * 16777216ull,
+		       (xtal == SONY_XTAL_24000) ? 48000000 : 41000000);
 }
 
 static u32 cxd2841er_calc_iffreq(u32 ifhz)
diff --git a/drivers/media/dvb-frontends/cxd2880/Kconfig b/drivers/media/dvb-frontends/cxd2880/Kconfig
index 9d989676e800..94a8e0b936b9 100644
--- a/drivers/media/dvb-frontends/cxd2880/Kconfig
+++ b/drivers/media/dvb-frontends/cxd2880/Kconfig
@@ -5,4 +5,4 @@ config DVB_CXD2880
 	depends on DVB_CORE && SPI
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
-	  Say Y when you want to support this frontend.
\ No newline at end of file
+	  Say Y when you want to support this frontend.
diff --git a/drivers/media/dvb-frontends/dib0090.c b/drivers/media/dvb-frontends/dib0090.c
index c958bcff026e..6cbbb351d545 100644
--- a/drivers/media/dvb-frontends/dib0090.c
+++ b/drivers/media/dvb-frontends/dib0090.c
@@ -78,7 +78,7 @@ MODULE_PARM_DESC(debug, "turn on debugging (default: 0)");
 #define SOC_8090_P1G_11R1 0x86
 #define SOC_8090_P1G_21R1 0x8e
 
-/* else use thos ones to check */
+/* else use those ones to check */
 #define P1A_B      0x0
 #define P1C	   0x1
 #define P1D_E_F    0x3
@@ -1574,7 +1574,7 @@ static int dib0090_reset(struct dvb_frontend *fe)
 	if (((state->identity.version & 0x1f) >= P1D_E_F) || (state->identity.in_soc))
 		dib0090_set_EFUSE(state);
 
-	/* Congigure in function of the crystal */
+	/* Configure in function of the crystal */
 	if (state->config->force_crystal_mode != 0)
 		dib0090_write_reg(state, 0x14,
 				state->config->force_crystal_mode & 3);
diff --git a/drivers/media/dvb-frontends/dib3000mb.c b/drivers/media/dvb-frontends/dib3000mb.c
index c598b2a63325..63bc7b74bc8b 100644
--- a/drivers/media/dvb-frontends/dib3000mb.c
+++ b/drivers/media/dvb-frontends/dib3000mb.c
@@ -51,7 +51,7 @@ MODULE_PARM_DESC(debug, "set debugging level (1=info,2=xfer,4=setfe,8=getfe (|-a
 static int dib3000_read_reg(struct dib3000_state *state, u16 reg)
 {
 	u8 wb[] = { ((reg >> 8) | 0x80) & 0xff, reg & 0xff };
-	u8 rb[2];
+	u8 rb[2] = {};
 	struct i2c_msg msg[] = {
 		{ .addr = state->config.demod_address, .flags = 0,        .buf = wb, .len = 2 },
 		{ .addr = state->config.demod_address, .flags = I2C_M_RD, .buf = rb, .len = 2 },
@@ -640,7 +640,7 @@ static int dib3000mb_read_ber(struct dvb_frontend* fe, u32 *ber)
 	return 0;
 }
 
-/* see dib3000-watch dvb-apps for exact calcuations of signal_strength and snr */
+/* see dib3000-watch dvb-apps for exact calculations of signal_strength and snr */
 static int dib3000mb_read_signal_strength(struct dvb_frontend* fe, u16 *strength)
 {
 	struct dib3000_state* state = fe->demodulator_priv;
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index 444fe1c4bf2d..b40daf242046 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -32,11 +32,6 @@ MODULE_PARM_DESC(buggy_sfn_workaround, "Enable work-around for buggy SFNs (defau
 		       __func__, ##arg);				\
 } while (0)
 
-struct i2c_device {
-	struct i2c_adapter *i2c_adap;
-	u8 i2c_addr;
-};
-
 struct dib7000p_state {
 	struct dvb_frontend demod;
 	struct dib7000p_config cfg;
@@ -2635,7 +2630,7 @@ static int dib7090_set_output_mode(struct dvb_frontend *fe, int mode)
 			dib7090_configMpegMux(state, 3, 1, 1);
 			dib7090_setHostBusMux(state, MPEG_ON_HOSTBUS);
 		} else {/* Use Smooth block */
-			dprintk("setting output mode TS_SERIAL using Smooth bloc\n");
+			dprintk("setting output mode TS_SERIAL using Smooth block\n");
 			dib7090_setHostBusMux(state, DEMOUT_ON_HOSTBUS);
 			outreg |= (2<<6) | (0 << 1);
 		}
@@ -2659,7 +2654,7 @@ static int dib7090_set_output_mode(struct dvb_frontend *fe, int mode)
 		outreg |= (1<<6);
 		break;
 
-	case OUTMODE_MPEG2_FIFO:	/* Using Smooth block because not supported by new Mpeg Mux bloc */
+	case OUTMODE_MPEG2_FIFO:	/* Using Smooth block because not supported by new Mpeg Mux block */
 		dprintk("setting output mode TS_FIFO using Smooth block\n");
 		dib7090_setHostBusMux(state, DEMOUT_ON_HOSTBUS);
 		outreg |= (5<<6);
diff --git a/drivers/media/dvb-frontends/dib8000.c b/drivers/media/dvb-frontends/dib8000.c
index 2f5165918163..d90f1b0b2051 100644
--- a/drivers/media/dvb-frontends/dib8000.c
+++ b/drivers/media/dvb-frontends/dib8000.c
@@ -1584,7 +1584,7 @@ static int dib8096p_set_output_mode(struct dvb_frontend *fe, int mode)
 				dib8096p_configMpegMux(state, 3, 1, 1);
 				dib8096p_setHostBusMux(state, MPEG_ON_HOSTBUS);
 			} else {/* Use Smooth block */
-				dprintk("dib8096P setting output mode TS_SERIAL using Smooth bloc\n");
+				dprintk("dib8096P setting output mode TS_SERIAL using Smooth block\n");
 				dib8096p_setHostBusMux(state,
 						DEMOUT_ON_HOSTBUS);
 				outreg |= (2 << 6) | (0 << 1);
@@ -1612,7 +1612,8 @@ static int dib8096p_set_output_mode(struct dvb_frontend *fe, int mode)
 
 	case OUTMODE_MPEG2_FIFO:
 			/* Using Smooth block because not supported
-			   by new Mpeg Mux bloc */
+			 * by new Mpeg Mux block
+			 */
 			dprintk("dib8096P setting output mode TS_FIFO using Smooth block\n");
 			dib8096p_setHostBusMux(state, DEMOUT_ON_HOSTBUS);
 			outreg |= (5 << 6);
@@ -2701,8 +2702,11 @@ static void dib8000_set_dds(struct dib8000_state *state, s32 offset_khz)
 	u8 ratio;
 
 	if (state->revision == 0x8090) {
+		u32 internal = dib8000_read32(state, 23) / 1000;
+
 		ratio = 4;
-		unit_khz_dds_val = (1<<26) / (dib8000_read32(state, 23) / 1000);
+
+		unit_khz_dds_val = (1<<26) / (internal ?: 1);
 		if (offset_khz < 0)
 			dds = (1 << 26) - (abs_offset_khz * unit_khz_dds_val);
 		else
diff --git a/drivers/media/dvb-frontends/dibx000_common.c b/drivers/media/dvb-frontends/dibx000_common.c
index 63a4c6a4afb5..bd5c5d7223aa 100644
--- a/drivers/media/dvb-frontends/dibx000_common.c
+++ b/drivers/media/dvb-frontends/dibx000_common.c
@@ -250,12 +250,12 @@ static int dibx000_i2c_master_xfer_gpio34(struct i2c_adapter *i2c_adap, struct i
 	return num;
 }
 
-static struct i2c_algorithm dibx000_i2c_master_gpio12_xfer_algo = {
+static const struct i2c_algorithm dibx000_i2c_master_gpio12_xfer_algo = {
 	.master_xfer = dibx000_i2c_master_xfer_gpio12,
 	.functionality = dibx000_i2c_func,
 };
 
-static struct i2c_algorithm dibx000_i2c_master_gpio34_xfer_algo = {
+static const struct i2c_algorithm dibx000_i2c_master_gpio34_xfer_algo = {
 	.master_xfer = dibx000_i2c_master_xfer_gpio34,
 	.functionality = dibx000_i2c_func,
 };
@@ -324,7 +324,7 @@ static int dibx000_i2c_gated_gpio67_xfer(struct i2c_adapter *i2c_adap,
 	return ret;
 }
 
-static struct i2c_algorithm dibx000_i2c_gated_gpio67_algo = {
+static const struct i2c_algorithm dibx000_i2c_gated_gpio67_algo = {
 	.master_xfer = dibx000_i2c_gated_gpio67_xfer,
 	.functionality = dibx000_i2c_func,
 };
@@ -369,7 +369,7 @@ static int dibx000_i2c_gated_tuner_xfer(struct i2c_adapter *i2c_adap,
 	return ret;
 }
 
-static struct i2c_algorithm dibx000_i2c_gated_tuner_algo = {
+static const struct i2c_algorithm dibx000_i2c_gated_tuner_algo = {
 	.master_xfer = dibx000_i2c_gated_tuner_xfer,
 	.functionality = dibx000_i2c_func,
 };
@@ -422,7 +422,7 @@ void dibx000_reset_i2c_master(struct dibx000_i2c_master *mst)
 EXPORT_SYMBOL(dibx000_reset_i2c_master);
 
 static int i2c_adapter_init(struct i2c_adapter *i2c_adap,
-				struct i2c_algorithm *algo, const char *name,
+				const struct i2c_algorithm *algo, const char *name,
 				struct dibx000_i2c_master *mst)
 {
 	strscpy(i2c_adap->name, name, sizeof(i2c_adap->name));
diff --git a/drivers/media/dvb-frontends/drx39xyj/drx_driver.h b/drivers/media/dvb-frontends/drx39xyj/drx_driver.h
index 15f7e58c5a30..2c2fd4bf79cc 100644
--- a/drivers/media/dvb-frontends/drx39xyj/drx_driver.h
+++ b/drivers/media/dvb-frontends/drx39xyj/drx_driver.h
@@ -33,7 +33,6 @@
 
 #include <linux/kernel.h>
 #include <linux/errno.h>
-#include <linux/firmware.h>
 #include <linux/i2c.h>
 
 /*
@@ -1910,7 +1909,6 @@ struct drx_demod_instance {
 	/* generic demodulator data */
 
 	struct i2c_adapter	*i2c;
-	const struct firmware	*firmware;
 };
 
 /*-------------------------------------------------------------------------
diff --git a/drivers/media/dvb-frontends/drx39xyj/drxj.c b/drivers/media/dvb-frontends/drx39xyj/drxj.c
index 19d8de400a68..779cce93e94a 100644
--- a/drivers/media/dvb-frontends/drx39xyj/drxj.c
+++ b/drivers/media/dvb-frontends/drx39xyj/drxj.c
@@ -56,6 +56,7 @@ INCLUDE FILES
 #define pr_fmt(fmt) KBUILD_MODNAME ":%s: " fmt, __func__
 
 #include <linux/module.h>
+#include <linux/firmware.h>
 #include <linux/init.h>
 #include <linux/string.h>
 #include <linux/slab.h>
@@ -975,13 +976,6 @@ static struct drx_aud_data drxj_default_aud_data_g = {
 /*-----------------------------------------------------------------------------
 STRUCTURES
 ----------------------------------------------------------------------------*/
-struct drxjeq_stat {
-	u16 eq_mse;
-	u8 eq_mode;
-	u8 eq_ctrl;
-	u8 eq_stat;
-};
-
 /* HI command */
 struct drxj_hi_cmd {
 	u16 cmd;
@@ -1444,8 +1438,7 @@ static int drxdap_fasi_read_block(struct i2c_device_addr *dev_addr,
 
 	/* Read block from I2C **************************************************** */
 	do {
-		u16 todo = (datasize < DRXDAP_MAX_RCHUNKSIZE ?
-			      datasize : DRXDAP_MAX_RCHUNKSIZE);
+		u16 todo = min(datasize, DRXDAP_MAX_RCHUNKSIZE);
 
 		bufx = 0;
 
@@ -1659,7 +1652,7 @@ static int drxdap_fasi_write_block(struct i2c_device_addr *dev_addr,
 		   Address must be rewritten because HI is reset after data transport and
 		   expects an address.
 		 */
-		todo = (block_size < datasize ? block_size : datasize);
+		todo = min(block_size, datasize);
 		if (todo == 0) {
 			u16 overhead_size_i2c_addr = 0;
 			u16 data_block_size = 0;
@@ -1681,9 +1674,7 @@ static int drxdap_fasi_write_block(struct i2c_device_addr *dev_addr,
 				first_err = st;
 			}
 			bufx = 0;
-			todo =
-			    (data_block_size <
-			     datasize ? data_block_size : datasize);
+			todo = min(data_block_size, datasize);
 		}
 		memcpy(&buf[bufx], data, todo);
 		/* write (address if can do and) data */
@@ -11750,6 +11741,7 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 	u8 *mc_data = NULL;
 	unsigned size;
 	char *mc_file;
+	const struct firmware *fw;
 
 	/* Check arguments */
 	if (!mc_info || !mc_info->mc_file)
@@ -11757,28 +11749,22 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 
 	mc_file = mc_info->mc_file;
 
-	if (!demod->firmware) {
-		const struct firmware *fw = NULL;
-
-		rc = request_firmware(&fw, mc_file, demod->i2c->dev.parent);
-		if (rc < 0) {
-			pr_err("Couldn't read firmware %s\n", mc_file);
-			return rc;
-		}
-		demod->firmware = fw;
-
-		if (demod->firmware->size < 2 * sizeof(u16)) {
-			rc = -EINVAL;
-			pr_err("Firmware is too short!\n");
-			goto release;
-		}
+	rc = request_firmware(&fw, mc_file, demod->i2c->dev.parent);
+	if (rc < 0) {
+		pr_err("Couldn't read firmware %s\n", mc_file);
+		return rc;
+	}
 
-		pr_info("Firmware %s, size %zu\n",
-			mc_file, demod->firmware->size);
+	if (fw->size < 2 * sizeof(u16)) {
+		rc = -EINVAL;
+		pr_err("Firmware is too short!\n");
+		goto release;
 	}
 
-	mc_data_init = demod->firmware->data;
-	size = demod->firmware->size;
+	pr_info("Firmware %s, size %zu\n", mc_file, fw->size);
+
+	mc_data_init = fw->data;
+	size = fw->size;
 
 	mc_data = (void *)mc_data_init;
 	/* Check data */
@@ -11874,7 +11860,8 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 						    0x0000)) {
 					pr_err("error reading firmware at pos %zd\n",
 					       mc_data - mc_data_init);
-					return -EIO;
+					rc = -EIO;
+					goto release;
 				}
 
 				result = memcmp(curr_ptr, mc_data_buffer,
@@ -11883,7 +11870,8 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 				if (result) {
 					pr_err("error verifying firmware at pos %zd\n",
 					       mc_data - mc_data_init);
-					return -EIO;
+					rc = -EIO;
+					goto release;
 				}
 
 				curr_addr += ((dr_xaddr_t)(bytes_to_comp / 2));
@@ -11893,17 +11881,17 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 			break;
 		}
 		default:
-			return -EINVAL;
+			rc = -EINVAL;
+			goto release;
 
 		}
 		mc_data += mc_block_nr_bytes;
 	}
 
-	return 0;
+	rc = 0;
 
 release:
-	release_firmware(demod->firmware);
-	demod->firmware = NULL;
+	release_firmware(fw);
 
 	return rc;
 }
@@ -12271,7 +12259,6 @@ static void drx39xxj_release(struct dvb_frontend *fe)
 	kfree(demod->my_ext_attr);
 	kfree(demod->my_common_attr);
 	kfree(demod->my_i2c_dev_addr);
-	release_firmware(demod->firmware);
 	kfree(demod);
 	kfree(state);
 }
diff --git a/drivers/media/dvb-frontends/lgdt3306a.c b/drivers/media/dvb-frontends/lgdt3306a.c
index 263887592415..6ab9d4de65ce 100644
--- a/drivers/media/dvb-frontends/lgdt3306a.c
+++ b/drivers/media/dvb-frontends/lgdt3306a.c
@@ -2176,6 +2176,11 @@ static int lgdt3306a_probe(struct i2c_client *client)
 	struct dvb_frontend *fe;
 	int ret;
 
+	if (!client->dev.platform_data) {
+		dev_err(&client->dev, "platform data is mandatory\n");
+		return -EINVAL;
+	}
+
 	config = kmemdup(client->dev.platform_data,
 			 sizeof(struct lgdt3306a_config), GFP_KERNEL);
 	if (config == NULL) {
@@ -2203,7 +2208,7 @@ static int lgdt3306a_probe(struct i2c_client *client)
 		goto err_kfree;
 	}
 	state->muxc->priv = client;
-	ret = i2c_mux_add_adapter(state->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(state->muxc, 0, 0);
 	if (ret)
 		goto err_kfree;
 
@@ -2239,7 +2244,7 @@ static void lgdt3306a_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lgdt3306a_id_table[] = {
-	{"lgdt3306a", 0},
+	{ "lgdt3306a" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, lgdt3306a_id_table);
diff --git a/drivers/media/dvb-frontends/lgdt330x.c b/drivers/media/dvb-frontends/lgdt330x.c
index 081d6ad3ce72..cab442a350a5 100644
--- a/drivers/media/dvb-frontends/lgdt330x.c
+++ b/drivers/media/dvb-frontends/lgdt330x.c
@@ -983,7 +983,7 @@ static void lgdt330x_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lgdt330x_id_table[] = {
-	{"lgdt330x", 0},
+	{ "lgdt330x" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, lgdt330x_id_table);
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
index e0272054fca5..5a03485686d9 100644
--- a/drivers/media/dvb-frontends/m88ds3103.c
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -1000,6 +1000,13 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	if (ret)
 		goto err;
 
+	if (dev->chiptype == M88DS3103_CHIPTYPE_3103B) {
+		/* to light up the LOCK led */
+		ret = m88ds3103_update_bits(dev, 0x11, 0x80, 0x00);
+		if (ret)
+			goto err;
+	}
+
 	dev->delivery_system = c->delivery_system;
 
 	return 0;
@@ -1866,7 +1873,7 @@ static int m88ds3103_probe(struct i2c_client *client)
 		goto err_kfree;
 	}
 	dev->muxc->priv = dev;
-	ret = i2c_mux_add_adapter(dev->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(dev->muxc, 0, 0);
 	if (ret)
 		goto err_kfree;
 
diff --git a/drivers/media/dvb-frontends/mb86a16.c b/drivers/media/dvb-frontends/mb86a16.c
index 0fc45896e7b8..9033e39d75f4 100644
--- a/drivers/media/dvb-frontends/mb86a16.c
+++ b/drivers/media/dvb-frontends/mb86a16.c
@@ -1854,5 +1854,6 @@ error:
 	return NULL;
 }
 EXPORT_SYMBOL_GPL(mb86a16_attach);
+MODULE_DESCRIPTION("Fujitsu MB86A16 DVB-S/DSS DC Receiver driver");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Manu Abraham");
diff --git a/drivers/media/dvb-frontends/mn88472.c b/drivers/media/dvb-frontends/mn88472.c
index 73d1e52de569..729751671c3d 100644
--- a/drivers/media/dvb-frontends/mn88472.c
+++ b/drivers/media/dvb-frontends/mn88472.c
@@ -708,7 +708,7 @@ static void mn88472_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mn88472_id_table[] = {
-	{"mn88472", 0},
+	{ "mn88472" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, mn88472_id_table);
diff --git a/drivers/media/dvb-frontends/mn88473.c b/drivers/media/dvb-frontends/mn88473.c
index eb50591c0e7a..fefc640d8afb 100644
--- a/drivers/media/dvb-frontends/mn88473.c
+++ b/drivers/media/dvb-frontends/mn88473.c
@@ -743,7 +743,7 @@ static void mn88473_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mn88473_id_table[] = {
-	{"mn88473", 0},
+	{ "mn88473" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, mn88473_id_table);
diff --git a/drivers/media/dvb-frontends/mxl5xx.c b/drivers/media/dvb-frontends/mxl5xx.c
index 4ebbcf05cc09..930da176e5bf 100644
--- a/drivers/media/dvb-frontends/mxl5xx.c
+++ b/drivers/media/dvb-frontends/mxl5xx.c
@@ -21,7 +21,7 @@
 #include <linux/mutex.h>
 #include <linux/vmalloc.h>
 #include <asm/div64.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/dvb_frontend.h>
 #include "mxl5xx.h"
@@ -1381,57 +1381,57 @@ static int config_ts(struct mxl *state, enum MXL_HYDRA_DEMOD_ID_E demod_id,
 	u32 nco_count_min = 0;
 	u32 clk_type = 0;
 
-	struct MXL_REG_FIELD_T xpt_sync_polarity[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_sync_polarity[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700010, 8, 1}, {0x90700010, 9, 1},
 		{0x90700010, 10, 1}, {0x90700010, 11, 1},
 		{0x90700010, 12, 1}, {0x90700010, 13, 1},
 		{0x90700010, 14, 1}, {0x90700010, 15, 1} };
-	struct MXL_REG_FIELD_T xpt_clock_polarity[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_clock_polarity[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700010, 16, 1}, {0x90700010, 17, 1},
 		{0x90700010, 18, 1}, {0x90700010, 19, 1},
 		{0x90700010, 20, 1}, {0x90700010, 21, 1},
 		{0x90700010, 22, 1}, {0x90700010, 23, 1} };
-	struct MXL_REG_FIELD_T xpt_valid_polarity[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_valid_polarity[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700014, 0, 1}, {0x90700014, 1, 1},
 		{0x90700014, 2, 1}, {0x90700014, 3, 1},
 		{0x90700014, 4, 1}, {0x90700014, 5, 1},
 		{0x90700014, 6, 1}, {0x90700014, 7, 1} };
-	struct MXL_REG_FIELD_T xpt_ts_clock_phase[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_ts_clock_phase[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700018, 0, 3}, {0x90700018, 4, 3},
 		{0x90700018, 8, 3}, {0x90700018, 12, 3},
 		{0x90700018, 16, 3}, {0x90700018, 20, 3},
 		{0x90700018, 24, 3}, {0x90700018, 28, 3} };
-	struct MXL_REG_FIELD_T xpt_lsb_first[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_lsb_first[MXL_HYDRA_DEMOD_MAX] = {
 		{0x9070000C, 16, 1}, {0x9070000C, 17, 1},
 		{0x9070000C, 18, 1}, {0x9070000C, 19, 1},
 		{0x9070000C, 20, 1}, {0x9070000C, 21, 1},
 		{0x9070000C, 22, 1}, {0x9070000C, 23, 1} };
-	struct MXL_REG_FIELD_T xpt_sync_byte[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_sync_byte[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700010, 0, 1}, {0x90700010, 1, 1},
 		{0x90700010, 2, 1}, {0x90700010, 3, 1},
 		{0x90700010, 4, 1}, {0x90700010, 5, 1},
 		{0x90700010, 6, 1}, {0x90700010, 7, 1} };
-	struct MXL_REG_FIELD_T xpt_enable_output[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_enable_output[MXL_HYDRA_DEMOD_MAX] = {
 		{0x9070000C, 0, 1}, {0x9070000C, 1, 1},
 		{0x9070000C, 2, 1}, {0x9070000C, 3, 1},
 		{0x9070000C, 4, 1}, {0x9070000C, 5, 1},
 		{0x9070000C, 6, 1}, {0x9070000C, 7, 1} };
-	struct MXL_REG_FIELD_T xpt_err_replace_sync[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_err_replace_sync[MXL_HYDRA_DEMOD_MAX] = {
 		{0x9070000C, 24, 1}, {0x9070000C, 25, 1},
 		{0x9070000C, 26, 1}, {0x9070000C, 27, 1},
 		{0x9070000C, 28, 1}, {0x9070000C, 29, 1},
 		{0x9070000C, 30, 1}, {0x9070000C, 31, 1} };
-	struct MXL_REG_FIELD_T xpt_err_replace_valid[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_err_replace_valid[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700014, 8, 1}, {0x90700014, 9, 1},
 		{0x90700014, 10, 1}, {0x90700014, 11, 1},
 		{0x90700014, 12, 1}, {0x90700014, 13, 1},
 		{0x90700014, 14, 1}, {0x90700014, 15, 1} };
-	struct MXL_REG_FIELD_T xpt_continuous_clock[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_continuous_clock[MXL_HYDRA_DEMOD_MAX] = {
 		{0x907001D4, 0, 1}, {0x907001D4, 1, 1},
 		{0x907001D4, 2, 1}, {0x907001D4, 3, 1},
 		{0x907001D4, 4, 1}, {0x907001D4, 5, 1},
 		{0x907001D4, 6, 1}, {0x907001D4, 7, 1} };
-	struct MXL_REG_FIELD_T xpt_nco_clock_rate[MXL_HYDRA_DEMOD_MAX] = {
+	static const struct MXL_REG_FIELD_T xpt_nco_clock_rate[MXL_HYDRA_DEMOD_MAX] = {
 		{0x90700044, 16, 80}, {0x90700044, 16, 81},
 		{0x90700044, 16, 82}, {0x90700044, 16, 83},
 		{0x90700044, 16, 84}, {0x90700044, 16, 85},
diff --git a/drivers/media/dvb-frontends/mxl5xx_defs.h b/drivers/media/dvb-frontends/mxl5xx_defs.h
index 097271f73740..bf08d82ba2cc 100644
--- a/drivers/media/dvb-frontends/mxl5xx_defs.h
+++ b/drivers/media/dvb-frontends/mxl5xx_defs.h
@@ -168,7 +168,7 @@ struct MBIN_FILE_HEADER_T {
 
 struct MBIN_FILE_T {
 	struct MBIN_FILE_HEADER_T header;
-	u8 data[1];
+	u8 data[];
 };
 
 struct MBIN_SEGMENT_HEADER_T {
@@ -179,7 +179,7 @@ struct MBIN_SEGMENT_HEADER_T {
 
 struct MBIN_SEGMENT_T {
 	struct MBIN_SEGMENT_HEADER_T header;
-	u8 data[1];
+	u8 data[];
 };
 
 enum MXL_CMD_TYPE_E { MXL_CMD_WRITE = 0, MXL_CMD_READ };
diff --git a/drivers/media/dvb-frontends/mxl692.c b/drivers/media/dvb-frontends/mxl692.c
index 2a31bde2630f..bbc2bc778225 100644
--- a/drivers/media/dvb-frontends/mxl692.c
+++ b/drivers/media/dvb-frontends/mxl692.c
@@ -1346,7 +1346,7 @@ static void mxl692_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mxl692_id_table[] = {
-	{"mxl692", 0},
+	{ "mxl692" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, mxl692_id_table);
diff --git a/drivers/media/dvb-frontends/rtl2830.c b/drivers/media/dvb-frontends/rtl2830.c
index 35c969fd2cb5..aa4ef9aedf17 100644
--- a/drivers/media/dvb-frontends/rtl2830.c
+++ b/drivers/media/dvb-frontends/rtl2830.c
@@ -609,7 +609,7 @@ static int rtl2830_pid_filter(struct dvb_frontend *fe, u8 index, u16 pid, int on
 		index, pid, onoff);
 
 	/* skip invalid PIDs (0x2000) */
-	if (pid > 0x1fff || index > 32)
+	if (pid > 0x1fff || index >= 32)
 		return 0;
 
 	if (onoff)
@@ -838,7 +838,7 @@ static int rtl2830_probe(struct i2c_client *client)
 		goto err_regmap_exit;
 	}
 	dev->muxc->priv = client;
-	ret = i2c_mux_add_adapter(dev->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(dev->muxc, 0, 0);
 	if (ret)
 		goto err_regmap_exit;
 
@@ -876,7 +876,7 @@ static void rtl2830_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id rtl2830_id_table[] = {
-	{"rtl2830", 0},
+	{ "rtl2830" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, rtl2830_id_table);
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index 601cf45c3935..3b4e46dac1bf 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -983,7 +983,7 @@ static int rtl2832_pid_filter(struct dvb_frontend *fe, u8 index, u16 pid,
 		index, pid, onoff, dev->slave_ts);
 
 	/* skip invalid PIDs (0x2000) */
-	if (pid > 0x1fff || index > 32)
+	if (pid > 0x1fff || index >= 32)
 		return 0;
 
 	if (onoff)
@@ -1082,7 +1082,7 @@ static int rtl2832_probe(struct i2c_client *client)
 		goto err_regmap_exit;
 	}
 	dev->muxc->priv = dev;
-	ret = i2c_mux_add_adapter(dev->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(dev->muxc, 0, 0);
 	if (ret)
 		goto err_regmap_exit;
 
@@ -1125,7 +1125,7 @@ static void rtl2832_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id rtl2832_id_table[] = {
-	{"rtl2832", 0},
+	{ "rtl2832" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, rtl2832_id_table);
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.c b/drivers/media/dvb-frontends/rtl2832_sdr.c
index 023db6e793f8..0357624968f1 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.c
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.c
@@ -947,8 +947,6 @@ static const struct vb2_ops rtl2832_sdr_vb2_ops = {
 	.buf_queue              = rtl2832_sdr_buf_queue,
 	.start_streaming        = rtl2832_sdr_start_streaming,
 	.stop_streaming         = rtl2832_sdr_stop_streaming,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 static int rtl2832_sdr_g_tuner(struct file *file, void *priv,
@@ -1365,6 +1363,7 @@ static int rtl2832_sdr_probe(struct platform_device *pdev)
 	dev->vb_queue.ops = &rtl2832_sdr_vb2_ops;
 	dev->vb_queue.mem_ops = &vb2_vmalloc_memops;
 	dev->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	dev->vb_queue.lock = &dev->vb_queue_lock;
 	ret = vb2_queue_init(&dev->vb_queue);
 	if (ret) {
 		dev_err(&pdev->dev, "Could not initialize vb2 queue\n");
@@ -1423,7 +1422,6 @@ static int rtl2832_sdr_probe(struct platform_device *pdev)
 	/* Init video_device structure */
 	dev->vdev = rtl2832_sdr_template;
 	dev->vdev.queue = &dev->vb_queue;
-	dev->vdev.queue->lock = &dev->vb_queue_lock;
 	video_set_drvdata(&dev->vdev, dev);
 
 	/* Register the v4l2_device structure */
@@ -1487,7 +1485,7 @@ static struct platform_driver rtl2832_sdr_driver = {
 		.name   = "rtl2832_sdr",
 	},
 	.probe          = rtl2832_sdr_probe,
-	.remove_new     = rtl2832_sdr_remove,
+	.remove         = rtl2832_sdr_remove,
 };
 module_platform_driver(rtl2832_sdr_driver);
 
diff --git a/drivers/media/dvb-frontends/si2165.c b/drivers/media/dvb-frontends/si2165.c
index 434d003bf397..f87c9357cee3 100644
--- a/drivers/media/dvb-frontends/si2165.c
+++ b/drivers/media/dvb-frontends/si2165.c
@@ -513,10 +513,8 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	ret = 0;
 	state->firmware_loaded = true;
 error:
-	if (fw) {
-		release_firmware(fw);
-		fw = NULL;
-	}
+	release_firmware(fw);
+	fw = NULL;
 
 	return ret;
 }
@@ -1283,7 +1281,7 @@ static void si2165_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id si2165_id_table[] = {
-	{"si2165", 0},
+	{ "si2165" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, si2165_id_table);
diff --git a/drivers/media/dvb-frontends/si2168.c b/drivers/media/dvb-frontends/si2168.c
index dae1f2153e8b..d6b6b8bc7d4e 100644
--- a/drivers/media/dvb-frontends/si2168.c
+++ b/drivers/media/dvb-frontends/si2168.c
@@ -744,7 +744,7 @@ static int si2168_probe(struct i2c_client *client)
 		goto err_kfree;
 	}
 	dev->muxc->priv = client;
-	ret = i2c_mux_add_adapter(dev->muxc, 0, 0, 0);
+	ret = i2c_mux_add_adapter(dev->muxc, 0, 0);
 	if (ret)
 		goto err_kfree;
 
@@ -788,7 +788,7 @@ static void si2168_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id si2168_id_table[] = {
-	{"si2168", 0},
+	{ "si2168" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, si2168_id_table);
diff --git a/drivers/media/dvb-frontends/sp2.c b/drivers/media/dvb-frontends/sp2.c
index 4d7d0b8b51b4..75adf2a4589f 100644
--- a/drivers/media/dvb-frontends/sp2.c
+++ b/drivers/media/dvb-frontends/sp2.c
@@ -407,7 +407,7 @@ static void sp2_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id sp2_id[] = {
-	{"sp2", 0},
+	{ "sp2" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, sp2_id);
diff --git a/drivers/media/dvb-frontends/stb0899_algo.c b/drivers/media/dvb-frontends/stb0899_algo.c
index df89c33dac23..40537c4ccb0d 100644
--- a/drivers/media/dvb-frontends/stb0899_algo.c
+++ b/drivers/media/dvb-frontends/stb0899_algo.c
@@ -269,7 +269,7 @@ static enum stb0899_status stb0899_search_carrier(struct stb0899_state *state)
 
 	short int derot_freq = 0, last_derot_freq = 0, derot_limit, next_loop = 3;
 	int index = 0;
-	u8 cfr[2];
+	u8 cfr[2] = {0};
 	u8 reg;
 
 	internal->status = NOCARRIER;
diff --git a/drivers/media/dvb-frontends/stb0899_drv.c b/drivers/media/dvb-frontends/stb0899_drv.c
index 2f4d8fb400cd..35634f9a8ab5 100644
--- a/drivers/media/dvb-frontends/stb0899_drv.c
+++ b/drivers/media/dvb-frontends/stb0899_drv.c
@@ -1277,7 +1277,7 @@ static int stb0899_get_dev_id(struct stb0899_state *state)
 	dprintk(state->verbose, FE_ERROR, 1, "Demodulator Core ID=[%s], Version=[%d]", (char *) &demod_str, demod_ver);
 	CONVERT32(STB0899_READ_S2REG(STB0899_S2FEC, FEC_CORE_ID_REG), (char *)&fec_str);
 	fec_ver = STB0899_READ_S2REG(STB0899_S2FEC, FEC_VER_ID_REG);
-	if (! (chip_id > 0)) {
+	if (!chip_id) {
 		dprintk(state->verbose, FE_ERROR, 1, "couldn't find a STB 0899");
 
 		return -ENODEV;
diff --git a/drivers/media/dvb-frontends/stv0299.c b/drivers/media/dvb-frontends/stv0299.c
index da7ff2c2e8e5..ba4bb3685095 100644
--- a/drivers/media/dvb-frontends/stv0299.c
+++ b/drivers/media/dvb-frontends/stv0299.c
@@ -250,7 +250,7 @@ static int stv0299_get_symbolrate (struct stv0299_state* state)
 	offset /= 128;
 
 	dprintk ("%s : srate = %i\n", __func__, srate);
-	dprintk ("%s : ofset = %i\n", __func__, offset);
+	dprintk ("%s : offset = %i\n", __func__, offset);
 
 	srate += offset;
 
diff --git a/drivers/media/dvb-frontends/stv0367_priv.h b/drivers/media/dvb-frontends/stv0367_priv.h
index 617f605947b2..7f056d1cce82 100644
--- a/drivers/media/dvb-frontends/stv0367_priv.h
+++ b/drivers/media/dvb-frontends/stv0367_priv.h
@@ -25,8 +25,11 @@
 #endif
 
 /* MACRO definitions */
+#ifndef MIN
 #define MAX(X, Y) ((X) >= (Y) ? (X) : (Y))
 #define MIN(X, Y) ((X) <= (Y) ? (X) : (Y))
+#endif
+
 #define INRANGE(X, Y, Z) \
 	((((X) <= (Y)) && ((Y) <= (Z))) || \
 	(((Z) <= (Y)) && ((Y) <= (X))) ? 1 : 0)
diff --git a/drivers/media/dvb-frontends/stv090x.c b/drivers/media/dvb-frontends/stv090x.c
index cc45139057ba..f273efa330cf 100644
--- a/drivers/media/dvb-frontends/stv090x.c
+++ b/drivers/media/dvb-frontends/stv090x.c
@@ -748,6 +748,22 @@ static int stv090x_write_reg(struct stv090x_state *state, unsigned int reg, u8 d
 	return stv090x_write_regs(state, reg, &tmp, 1);
 }
 
+static inline void stv090x_tuner_i2c_lock(struct stv090x_state *state)
+{
+	if (state->config->tuner_i2c_lock)
+		state->config->tuner_i2c_lock(&state->frontend, 1);
+	else
+		mutex_lock(&state->internal->tuner_lock);
+}
+
+static inline void stv090x_tuner_i2c_unlock(struct stv090x_state *state)
+{
+	if (state->config->tuner_i2c_lock)
+		state->config->tuner_i2c_lock(&state->frontend, 0);
+	else
+		mutex_unlock(&state->internal->tuner_lock);
+}
+
 static int stv090x_i2c_gate_ctrl(struct stv090x_state *state, int enable)
 {
 	u32 reg;
@@ -761,12 +777,8 @@ static int stv090x_i2c_gate_ctrl(struct stv090x_state *state, int enable)
 	 * In case of any error, the lock is unlocked and exit within the
 	 * relevant operations themselves.
 	 */
-	if (enable) {
-		if (state->config->tuner_i2c_lock)
-			state->config->tuner_i2c_lock(&state->frontend, 1);
-		else
-			mutex_lock(&state->internal->tuner_lock);
-	}
+	if (enable)
+		stv090x_tuner_i2c_lock(state);
 
 	reg = STV090x_READ_DEMOD(state, I2CRPT);
 	if (enable) {
@@ -782,20 +794,13 @@ static int stv090x_i2c_gate_ctrl(struct stv090x_state *state, int enable)
 			goto err;
 	}
 
-	if (!enable) {
-		if (state->config->tuner_i2c_lock)
-			state->config->tuner_i2c_lock(&state->frontend, 0);
-		else
-			mutex_unlock(&state->internal->tuner_lock);
-	}
+	if (!enable)
+		stv090x_tuner_i2c_unlock(state);
 
 	return 0;
 err:
 	dprintk(FE_ERROR, 1, "I/O error");
-	if (state->config->tuner_i2c_lock)
-		state->config->tuner_i2c_lock(&state->frontend, 0);
-	else
-		mutex_unlock(&state->internal->tuner_lock);
+	stv090x_tuner_i2c_unlock(state);
 	return -1;
 }
 
@@ -5074,7 +5079,7 @@ error:
 EXPORT_SYMBOL_GPL(stv090x_attach);
 
 static const struct i2c_device_id stv090x_id_table[] = {
-	{"stv090x", 0},
+	{ "stv090x" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, stv090x_id_table);
diff --git a/drivers/media/dvb-frontends/stv0910.c b/drivers/media/dvb-frontends/stv0910.c
index e517ff757744..069dec75129c 100644
--- a/drivers/media/dvb-frontends/stv0910.c
+++ b/drivers/media/dvb-frontends/stv0910.c
@@ -119,11 +119,6 @@ struct stv {
 	u8    vth[6];
 };
 
-struct sinit_table {
-	u16  address;
-	u8   data;
-};
-
 struct slookup {
 	s16  value;
 	u32  reg_value;
diff --git a/drivers/media/dvb-frontends/stv6110x.c b/drivers/media/dvb-frontends/stv6110x.c
index c678f47d2449..33c8105da1c3 100644
--- a/drivers/media/dvb-frontends/stv6110x.c
+++ b/drivers/media/dvb-frontends/stv6110x.c
@@ -470,7 +470,7 @@ const struct stv6110x_devctl *stv6110x_attach(struct dvb_frontend *fe,
 EXPORT_SYMBOL_GPL(stv6110x_attach);
 
 static const struct i2c_device_id stv6110x_id_table[] = {
-	{"stv6110x", 0},
+	{ "stv6110x" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, stv6110x_id_table);
diff --git a/drivers/media/dvb-frontends/stv6111.c b/drivers/media/dvb-frontends/stv6111.c
index 2d0adb6fcb08..0ac15273922d 100644
--- a/drivers/media/dvb-frontends/stv6111.c
+++ b/drivers/media/dvb-frontends/stv6111.c
@@ -161,7 +161,7 @@ static const struct slookup gain_rfagc_lookup[] = {
 };
 
 /*
- * This table is 6 dB too low comapred to the others (probably created with
+ * This table is 6 dB too low compared to the others (probably created with
  * a different BB_MAG setting)
  */
 static const struct slookup gain_channel_agc_nf_lookup[] = {
diff --git a/drivers/media/dvb-frontends/tda10048.c b/drivers/media/dvb-frontends/tda10048.c
index 5d5e4e9e4422..1f87eb0dcf2a 100644
--- a/drivers/media/dvb-frontends/tda10048.c
+++ b/drivers/media/dvb-frontends/tda10048.c
@@ -328,7 +328,8 @@ static int tda10048_set_wref(struct dvb_frontend *fe, u32 sample_freq_hz,
 			     u32 bw)
 {
 	struct tda10048_state *state = fe->demodulator_priv;
-	u64 t, z;
+	u64 t;
+	u32 z;
 
 	dprintk(1, "%s()\n", __func__);
 
@@ -341,6 +342,11 @@ static int tda10048_set_wref(struct dvb_frontend *fe, u32 sample_freq_hz,
 	/* t *= 2147483648 on 32bit platforms */
 	t *= (2048 * 1024);
 	t *= 1024;
+
+	/*
+	 * Sample frequency is typically 55 MHz, with a theoretical maximum of
+	 * 69 MHz. With a 32 bit z we have enough accuracy for up to 613 MHz.
+	 */
 	z = 7 * sample_freq_hz;
 	do_div(t, z);
 	t += 5;
@@ -410,6 +416,7 @@ static int tda10048_set_if(struct dvb_frontend *fe, u32 bw)
 	struct tda10048_config *config = &state->config;
 	int i;
 	u32 if_freq_khz;
+	u64 sample_freq;
 
 	dprintk(1, "%s(bw = %d)\n", __func__, bw);
 
@@ -451,9 +458,11 @@ static int tda10048_set_if(struct dvb_frontend *fe, u32 bw)
 	dprintk(1, "- pll_pfactor = %d\n", state->pll_pfactor);
 
 	/* Calculate the sample frequency */
-	state->sample_freq = state->xtal_hz * (state->pll_mfactor + 45);
-	state->sample_freq /= (state->pll_nfactor + 1);
-	state->sample_freq /= (state->pll_pfactor + 4);
+	sample_freq = state->xtal_hz;
+	sample_freq *= state->pll_mfactor + 45;
+	do_div(sample_freq, state->pll_nfactor + 1);
+	do_div(sample_freq, state->pll_pfactor + 4);
+	state->sample_freq = sample_freq;
 	dprintk(1, "- sample_freq = %d\n", state->sample_freq);
 
 	/* Update the I/F */
diff --git a/drivers/media/dvb-frontends/tda10071.c b/drivers/media/dvb-frontends/tda10071.c
index 6640851d8bbc..e23794b821cd 100644
--- a/drivers/media/dvb-frontends/tda10071.c
+++ b/drivers/media/dvb-frontends/tda10071.c
@@ -1230,7 +1230,7 @@ static void tda10071_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tda10071_id_table[] = {
-	{"tda10071_cx24118", 0},
+	{ "tda10071_cx24118" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, tda10071_id_table);
diff --git a/drivers/media/dvb-frontends/tda18271c2dd.c b/drivers/media/dvb-frontends/tda18271c2dd.c
index a34834487943..c11563853c07 100644
--- a/drivers/media/dvb-frontends/tda18271c2dd.c
+++ b/drivers/media/dvb-frontends/tda18271c2dd.c
@@ -328,7 +328,7 @@ static int CalcMainPLL(struct tda_state *state, u32 freq)
 
 	OscFreq = (u64) freq * (u64) Div;
 	OscFreq *= (u64) 16384;
-	do_div(OscFreq, (u64)16000000);
+	do_div(OscFreq, 16000000);
 	MainDiv = OscFreq;
 
 	state->m_Regs[MPD] = PostDiv & 0x77;
@@ -352,7 +352,7 @@ static int CalcCalPLL(struct tda_state *state, u32 freq)
 	OscFreq = (u64)freq * (u64)Div;
 	/* CalDiv = u32( OscFreq * 16384 / 16000000 ); */
 	OscFreq *= (u64)16384;
-	do_div(OscFreq, (u64)16000000);
+	do_div(OscFreq, 16000000);
 	CalDiv = OscFreq;
 
 	state->m_Regs[CPD] = PostDiv;
@@ -954,7 +954,7 @@ static int RFTrackingFiltersCorrection(struct tda_state *state,
 			Capprox = 255;
 
 
-		/* TODO Temperature compensation. There is defenitely a scale factor */
+		/* TODO Temperature compensation. There is definitely a scale factor */
 		/*      missing in the datasheet, so leave it out for now.           */
 		state->m_Regs[EB14] = Capprox;
 
diff --git a/drivers/media/dvb-frontends/ts2020.c b/drivers/media/dvb-frontends/ts2020.c
index a5ebce57f35e..e25add6cc38e 100644
--- a/drivers/media/dvb-frontends/ts2020.c
+++ b/drivers/media/dvb-frontends/ts2020.c
@@ -553,13 +553,19 @@ static void ts2020_regmap_unlock(void *__dev)
 static int ts2020_probe(struct i2c_client *client)
 {
 	struct ts2020_config *pdata = client->dev.platform_data;
-	struct dvb_frontend *fe = pdata->fe;
+	struct dvb_frontend *fe;
 	struct ts2020_priv *dev;
 	int ret;
 	u8 u8tmp;
 	unsigned int utmp;
 	char *chip_str;
 
+	if (!pdata) {
+		dev_err(&client->dev, "platform data is mandatory\n");
+		return -EINVAL;
+	}
+
+	fe = pdata->fe;
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev) {
 		ret = -ENOMEM;
@@ -710,8 +716,8 @@ static void ts2020_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ts2020_id_table[] = {
-	{"ts2020", 0},
-	{"ts2022", 0},
+	{ "ts2020" },
+	{ "ts2022" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, ts2020_id_table);
diff --git a/drivers/media/dvb-frontends/zd1301_demod.c b/drivers/media/dvb-frontends/zd1301_demod.c
index 17f6e373c13d..e8b9e67a8717 100644
--- a/drivers/media/dvb-frontends/zd1301_demod.c
+++ b/drivers/media/dvb-frontends/zd1301_demod.c
@@ -531,7 +531,7 @@ static struct platform_driver zd1301_demod_driver = {
 		.suppress_bind_attrs = true,
 	},
 	.probe          = zd1301_demod_probe,
-	.remove_new     = zd1301_demod_remove,
+	.remove         = zd1301_demod_remove,
 };
 module_platform_driver(zd1301_demod_driver);
 
diff --git a/drivers/media/dvb-frontends/zl10036.c b/drivers/media/dvb-frontends/zl10036.c
index 3df055be66d6..5ad987c6861b 100644
--- a/drivers/media/dvb-frontends/zl10036.c
+++ b/drivers/media/dvb-frontends/zl10036.c
@@ -89,7 +89,7 @@ static int zl10036_write(struct zl10036_state *state, u8 buf[], u8 count)
 	int ret;
 
 	if (zl10036_debug & 0x02) {
-		/* every 8bit-value satisifes this!
+		/* every 8bit-value satisfies this!
 		 * so only check for debug log */
 		if ((buf[0] & 0x80) == 0x00)
 			reg = 2;
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 56f276b920ab..e68202954a8f 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -70,6 +70,26 @@ config VIDEO_GC0308
 	  To compile this driver as a module, choose M here: the
 	  module will be called gc0308.
 
+config VIDEO_GC05A2
+	tristate "GalaxyCore gc05a2 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the GalaxyCore gc05a2
+	  camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gc05a2.
+
+config VIDEO_GC08A3
+	tristate "GalaxyCore gc08a3 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the GalaxyCore gc08a3
+	  camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called gc08a3.
+
 config VIDEO_GC2145
 	select V4L2_CCI_I2C
 	tristate "GalaxyCore GC2145 sensor support"
@@ -120,6 +140,7 @@ config VIDEO_IMX214
 	tristate "Sony IMX214 sensor support"
 	depends on GPIOLIB
 	select REGMAP_I2C
+	select V4L2_CCI_I2C
 	help
 	  This is a Video4Linux2 sensor driver for the Sony
 	  IMX214 camera.
@@ -139,6 +160,7 @@ config VIDEO_IMX219
 
 config VIDEO_IMX258
 	tristate "Sony IMX258 sensor support"
+	select V4L2_CCI_I2C
 	help
 	  This is a Video4Linux2 sensor driver for the Sony
 	  IMX258 camera.
@@ -153,6 +175,16 @@ config VIDEO_IMX274
 	  This is a V4L2 sensor driver for the Sony IMX274
 	  CMOS image sensor.
 
+config VIDEO_IMX283
+	tristate "Sony IMX283 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a V4L2 sensor driver for the Sony IMX283
+	  CMOS image sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called imx283.
+
 config VIDEO_IMX290
 	tristate "Sony IMX290 sensor support"
 	select REGMAP_I2C
@@ -185,6 +217,7 @@ config VIDEO_IMX319
 config VIDEO_IMX334
 	tristate "Sony IMX334 sensor support"
 	depends on OF_GPIO
+	select V4L2_CCI_I2C
 	help
 	  This is a Video4Linux2 sensor driver for the Sony
 	  IMX334 camera.
@@ -195,6 +228,7 @@ config VIDEO_IMX334
 config VIDEO_IMX335
 	tristate "Sony IMX335 sensor support"
 	depends on OF_GPIO
+	select V4L2_CCI_I2C
 	help
 	  This is a Video4Linux2 sensor driver for the Sony
 	  IMX335 camera.
@@ -323,6 +357,26 @@ config VIDEO_OV02A10
 	  To compile this driver as a module, choose M here: the
 	  module will be called ov02a10.
 
+config VIDEO_OV02E10
+	tristate "OmniVision OV02E10 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV02E10 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov02e10.
+
+config VIDEO_OV02C10
+	tristate "OmniVision OV02C10 sensor support"
+	select V4L2_CCI_I2C
+	help
+	  This is a Video4Linux2 sensor driver for the OmniVision
+	  OV02C10 camera.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ov02c10.
+
 config VIDEO_OV08D10
         tristate "OmniVision OV08D10 sensor support"
         help
@@ -405,6 +459,7 @@ config VIDEO_OV2740
 config VIDEO_OV4689
 	tristate "OmniVision OV4689 sensor support"
 	depends on GPIOLIB
+	select V4L2_CCI_I2C
 	help
 	  This is a Video4Linux2 sensor-level driver for the OmniVision
 	  OV4689 camera.
@@ -657,7 +712,29 @@ config VIDEO_S5K6A3
 	  This is a V4L2 sensor driver for Samsung S5K6A3 raw
 	  camera sensor.
 
-config VIDEO_ST_VGXY61
+config VIDEO_VD55G1
+	tristate "ST VD55G1 sensor support"
+	select V4L2_CCI_I2C
+	depends on GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the ST VD55G1
+	  camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vd55g1.
+
+config VIDEO_VD56G3
+	tristate "ST VD56G3 sensor support"
+	select V4L2_CCI_I2C
+	depends on GPIOLIB
+	help
+	  This is a Video4Linux2 sensor driver for the ST VD56G3
+	  camera sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vd56g3.
+
+config VIDEO_VGXY61
 	tristate "ST VGXY61 sensor support"
 	select V4L2_CCI_I2C
 	depends on OF && GPIOLIB
@@ -677,6 +754,7 @@ config VIDEO_THP7312
 	tristate "THine THP7312 support"
 	depends on I2C
 	select FW_LOADER
+	select FW_UPLOAD
 	select MEDIA_CONTROLLER
 	select V4L2_CCI_I2C
 	select V4L2_FWNODE
@@ -1112,6 +1190,20 @@ config VIDEO_ISL7998X
 	  Support for Intersil ISL7998x analog to MIPI-CSI2 or
 	  BT.656 decoder.
 
+config VIDEO_LT6911UXE
+	tristate "Lontium LT6911UXE decoder"
+	depends on ACPI && VIDEO_DEV && I2C
+	select V4L2_FWNODE
+	select V4L2_CCI_I2C
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  This is a Video4Linux2 sensor-level driver for the Lontium
+	  LT6911UXE HDMI to MIPI CSI-2 bridge.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called lt6911uxe.
+
 config VIDEO_KS0127
 	tristate "KS0127 video decoder"
 	depends on VIDEO_DEV && I2C
@@ -1573,6 +1665,40 @@ config VIDEO_DS90UB960
 	  Device driver for the Texas Instruments DS90UB960
 	  FPD-Link III Deserializer and DS90UB9702 FPD-Link IV Deserializer.
 
+config VIDEO_MAX96714
+	tristate "Maxim MAX96714 GMSL2 deserializer"
+	depends on OF && I2C && VIDEO_DEV
+	select I2C_MUX
+	select MEDIA_CONTROLLER
+	select GPIOLIB
+	select V4L2_CCI_I2C
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Device driver for the Maxim MAX96714 GMSL2 Deserializer.
+	  MAX96714 deserializers convert a GMSL2 input to MIPI CSI-2
+	  output.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max96714.
+
+config VIDEO_MAX96717
+	tristate "Maxim MAX96717 GMSL2 Serializer support"
+	depends on OF && I2C && VIDEO_DEV && COMMON_CLK
+	select I2C_MUX
+	select MEDIA_CONTROLLER
+	select GPIOLIB
+	select V4L2_CCI_I2C
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Device driver for the Maxim MAX96717 GMSL2 Serializer.
+	  MAX96717 serializers convert video on a MIPI CSI-2
+	  input to a GMSL2 output.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max96717.
+
 endmenu
 
 endif # VIDEO_DEV
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index dfbe6448b549..5873d29433ee 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -38,6 +38,8 @@ obj-$(CONFIG_VIDEO_DW9768) += dw9768.o
 obj-$(CONFIG_VIDEO_DW9807_VCM) += dw9807-vcm.o
 obj-$(CONFIG_VIDEO_ET8EK8) += et8ek8/
 obj-$(CONFIG_VIDEO_GC0308) += gc0308.o
+obj-$(CONFIG_VIDEO_GC05A2) += gc05a2.o
+obj-$(CONFIG_VIDEO_GC08A3) += gc08a3.o
 obj-$(CONFIG_VIDEO_GC2145) += gc2145.o
 obj-$(CONFIG_VIDEO_HI556) += hi556.o
 obj-$(CONFIG_VIDEO_HI846) += hi846.o
@@ -48,6 +50,7 @@ obj-$(CONFIG_VIDEO_IMX214) += imx214.o
 obj-$(CONFIG_VIDEO_IMX219) += imx219.o
 obj-$(CONFIG_VIDEO_IMX258) += imx258.o
 obj-$(CONFIG_VIDEO_IMX274) += imx274.o
+obj-$(CONFIG_VIDEO_IMX283) += imx283.o
 obj-$(CONFIG_VIDEO_IMX290) += imx290.o
 obj-$(CONFIG_VIDEO_IMX296) += imx296.o
 obj-$(CONFIG_VIDEO_IMX319) += imx319.o
@@ -61,9 +64,12 @@ obj-$(CONFIG_VIDEO_ISL7998X) += isl7998x.o
 obj-$(CONFIG_VIDEO_KS0127) += ks0127.o
 obj-$(CONFIG_VIDEO_LM3560) += lm3560.o
 obj-$(CONFIG_VIDEO_LM3646) += lm3646.o
+obj-$(CONFIG_VIDEO_LT6911UXE) += lt6911uxe.o
 obj-$(CONFIG_VIDEO_M52790) += m52790.o
 obj-$(CONFIG_VIDEO_MAX9271_LIB) += max9271.o
 obj-$(CONFIG_VIDEO_MAX9286) += max9286.o
+obj-$(CONFIG_VIDEO_MAX96714) += max96714.o
+obj-$(CONFIG_VIDEO_MAX96717) += max96717.o
 obj-$(CONFIG_VIDEO_ML86V7667) += ml86v7667.o
 obj-$(CONFIG_VIDEO_MSP3400) += msp3400.o
 obj-$(CONFIG_VIDEO_MT9M001) += mt9m001.o
@@ -77,6 +83,8 @@ obj-$(CONFIG_VIDEO_MT9V111) += mt9v111.o
 obj-$(CONFIG_VIDEO_OG01A1B) += og01a1b.o
 obj-$(CONFIG_VIDEO_OV01A10) += ov01a10.o
 obj-$(CONFIG_VIDEO_OV02A10) += ov02a10.o
+obj-$(CONFIG_VIDEO_OV02C10) += ov02c10.o
+obj-$(CONFIG_VIDEO_OV02E10) += ov02e10.o
 obj-$(CONFIG_VIDEO_OV08D10) += ov08d10.o
 obj-$(CONFIG_VIDEO_OV08X40) += ov08x40.o
 obj-$(CONFIG_VIDEO_OV13858) += ov13858.o
@@ -124,7 +132,6 @@ obj-$(CONFIG_VIDEO_SAA717X) += saa717x.o
 obj-$(CONFIG_VIDEO_SAA7185) += saa7185.o
 obj-$(CONFIG_VIDEO_SONY_BTF_MPX) += sony-btf-mpx.o
 obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
-obj-$(CONFIG_VIDEO_ST_VGXY61) += st-vgxy61.o
 obj-$(CONFIG_VIDEO_TC358743) += tc358743.o
 obj-$(CONFIG_VIDEO_TC358746) += tc358746.o
 obj-$(CONFIG_VIDEO_TDA1997X) += tda1997x.o
@@ -148,6 +155,9 @@ obj-$(CONFIG_VIDEO_TW9910) += tw9910.o
 obj-$(CONFIG_VIDEO_UDA1342) += uda1342.o
 obj-$(CONFIG_VIDEO_UPD64031A) += upd64031a.o
 obj-$(CONFIG_VIDEO_UPD64083) += upd64083.o
+obj-$(CONFIG_VIDEO_VD55G1) += vd55g1.o
+obj-$(CONFIG_VIDEO_VD56G3) += vd56g3.o
+obj-$(CONFIG_VIDEO_VGXY61) += vgxy61.o
 obj-$(CONFIG_VIDEO_VP27SMPX) += vp27smpx.o
 obj-$(CONFIG_VIDEO_VPX3220) += vpx3220.o
 obj-$(CONFIG_VIDEO_WM8739) += wm8739.o
diff --git a/drivers/media/i2c/ad5820.c b/drivers/media/i2c/ad5820.c
index 1543d24f522c..f60271082fb5 100644
--- a/drivers/media/i2c/ad5820.c
+++ b/drivers/media/i2c/ad5820.c
@@ -347,8 +347,8 @@ static void ad5820_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ad5820_id_table[] = {
-	{ "ad5820", 0 },
-	{ "ad5821", 0 },
+	{ "ad5820" },
+	{ "ad5821" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ad5820_id_table);
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
index 5ace7b5804d4..391bc75bfcd0 100644
--- a/drivers/media/i2c/adp1653.c
+++ b/drivers/media/i2c/adp1653.c
@@ -522,7 +522,7 @@ static void adp1653_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id adp1653_id_table[] = {
-	{ ADP1653_NAME, 0 },
+	{ ADP1653_NAME },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adp1653_id_table);
diff --git a/drivers/media/i2c/adv7170.c b/drivers/media/i2c/adv7170.c
index 4a2b9fd9e2da..ef8682b980b4 100644
--- a/drivers/media/i2c/adv7170.c
+++ b/drivers/media/i2c/adv7170.c
@@ -377,8 +377,8 @@ static void adv7170_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id adv7170_id[] = {
-	{ "adv7170", 0 },
-	{ "adv7171", 0 },
+	{ "adv7170" },
+	{ "adv7171" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adv7170_id);
diff --git a/drivers/media/i2c/adv7175.c b/drivers/media/i2c/adv7175.c
index e454cba4b026..384da1ec5bf9 100644
--- a/drivers/media/i2c/adv7175.c
+++ b/drivers/media/i2c/adv7175.c
@@ -432,8 +432,8 @@ static void adv7175_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id adv7175_id[] = {
-	{ "adv7175", 0 },
-	{ "adv7176", 0 },
+	{ "adv7175" },
+	{ "adv7176" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adv7175_id);
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index 4829cbe32419..6e50b14f888f 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -195,6 +195,7 @@ struct adv7180_state;
 #define ADV7180_FLAG_V2			BIT(1)
 #define ADV7180_FLAG_MIPI_CSI2		BIT(2)
 #define ADV7180_FLAG_I2P		BIT(3)
+#define ADV7180_FLAG_TEST_PATTERN	BIT(4)
 
 struct adv7180_chip_info {
 	unsigned int flags;
@@ -682,11 +683,15 @@ static int adv7180_init_controls(struct adv7180_state *state)
 			  ADV7180_HUE_MAX, 1, ADV7180_HUE_DEF);
 	v4l2_ctrl_new_custom(&state->ctrl_hdl, &adv7180_ctrl_fast_switch, NULL);
 
-	v4l2_ctrl_new_std_menu_items(&state->ctrl_hdl, &adv7180_ctrl_ops,
-				      V4L2_CID_TEST_PATTERN,
-				      ARRAY_SIZE(test_pattern_menu) - 1,
-				      0, ARRAY_SIZE(test_pattern_menu) - 1,
-				      test_pattern_menu);
+	if (state->chip_info->flags & ADV7180_FLAG_TEST_PATTERN) {
+		v4l2_ctrl_new_std_menu_items(&state->ctrl_hdl,
+					     &adv7180_ctrl_ops,
+					     V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(test_pattern_menu) - 1,
+					     0,
+					     ARRAY_SIZE(test_pattern_menu) - 1,
+					     test_pattern_menu);
+	}
 
 	state->sd.ctrl_handler = &state->ctrl_hdl;
 	if (state->ctrl_hdl.error) {
@@ -1221,7 +1226,7 @@ static const struct adv7180_chip_info adv7182_info = {
 };
 
 static const struct adv7180_chip_info adv7280_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P | ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN3) |
@@ -1235,7 +1240,8 @@ static const struct adv7180_chip_info adv7280_info = {
 };
 
 static const struct adv7180_chip_info adv7280_m_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P |
+		ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN3) |
@@ -1256,7 +1262,8 @@ static const struct adv7180_chip_info adv7280_m_info = {
 };
 
 static const struct adv7180_chip_info adv7281_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 |
+		ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN7) |
@@ -1271,7 +1278,8 @@ static const struct adv7180_chip_info adv7281_info = {
 };
 
 static const struct adv7180_chip_info adv7281_m_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 |
+		ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN3) |
@@ -1291,7 +1299,8 @@ static const struct adv7180_chip_info adv7281_m_info = {
 };
 
 static const struct adv7180_chip_info adv7281_ma_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 |
+		ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN3) |
@@ -1316,7 +1325,7 @@ static const struct adv7180_chip_info adv7281_ma_info = {
 };
 
 static const struct adv7180_chip_info adv7282_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_I2P | ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN7) |
@@ -1331,7 +1340,8 @@ static const struct adv7180_chip_info adv7282_info = {
 };
 
 static const struct adv7180_chip_info adv7282_m_info = {
-	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P,
+	.flags = ADV7180_FLAG_V2 | ADV7180_FLAG_MIPI_CSI2 | ADV7180_FLAG_I2P |
+		ADV7180_FLAG_TEST_PATTERN,
 	.valid_input_mask = BIT(ADV7182_INPUT_CVBS_AIN1) |
 		BIT(ADV7182_INPUT_CVBS_AIN2) |
 		BIT(ADV7182_INPUT_CVBS_AIN3) |
@@ -1440,7 +1450,8 @@ static int adv7180_probe(struct i2c_client *client)
 		return ret;
 	}
 
-	if (of_property_read_bool(np, "adv,force-bt656-4"))
+	if (of_property_read_bool(np, "adv,force-bt656-4") ||
+	    of_property_read_bool(np, "adi,force-bt656-4"))
 		state->force_bt656_4 = true;
 
 	if (state->chip_info->flags & ADV7180_FLAG_MIPI_CSI2) {
@@ -1486,7 +1497,7 @@ static int adv7180_probe(struct i2c_client *client)
 	if (ret)
 		goto err_media_entity_cleanup;
 
-	if (state->irq) {
+	if (state->irq > 0) {
 		ret = request_threaded_irq(client->irq, NULL, adv7180_irq,
 					   IRQF_ONESHOT | IRQF_TRIGGER_FALLING,
 					   KBUILD_MODNAME, state);
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index 2a2cace4a153..25a31a6dd456 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -619,8 +619,8 @@ static void adv7183_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id adv7183_id[] = {
-	{"adv7183", 0},
-	{},
+	{ "adv7183" },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, adv7183_id);
diff --git a/drivers/media/i2c/adv7343.c b/drivers/media/i2c/adv7343.c
index 4fbe4e18570e..b96443404a26 100644
--- a/drivers/media/i2c/adv7343.c
+++ b/drivers/media/i2c/adv7343.c
@@ -502,8 +502,8 @@ static void adv7343_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id adv7343_id[] = {
-	{"adv7343", 0},
-	{},
+	{ "adv7343" },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, adv7343_id);
diff --git a/drivers/media/i2c/adv7393.c b/drivers/media/i2c/adv7393.c
index 7638af455cef..c7994bd0bbd4 100644
--- a/drivers/media/i2c/adv7393.c
+++ b/drivers/media/i2c/adv7393.c
@@ -446,8 +446,8 @@ static void adv7393_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id adv7393_id[] = {
-	{"adv7393", 0},
-	{},
+	{ "adv7393" },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, adv7393_id);
 
diff --git a/drivers/media/i2c/adv748x/adv748x-afe.c b/drivers/media/i2c/adv748x/adv748x-afe.c
index 50d9fbadbe38..5edb3295dc58 100644
--- a/drivers/media/i2c/adv748x/adv748x-afe.c
+++ b/drivers/media/i2c/adv748x/adv748x-afe.c
@@ -114,7 +114,7 @@ static void adv748x_afe_fill_format(struct adv748x_afe *afe,
 {
 	memset(fmt, 0, sizeof(*fmt));
 
-	fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
 	fmt->colorspace = V4L2_COLORSPACE_SMPTE170M;
 	fmt->field = V4L2_FIELD_ALTERNATE;
 
@@ -337,7 +337,7 @@ static int adv748x_afe_enum_mbus_code(struct v4l2_subdev *sd,
 	if (code->index != 0)
 		return -EINVAL;
 
-	code->code = MEDIA_BUS_FMT_UYVY8_2X8;
+	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
 
 	return 0;
 }
diff --git a/drivers/media/i2c/adv748x/adv748x-csi2.c b/drivers/media/i2c/adv748x/adv748x-csi2.c
index 5b265b722394..ebe7da8ebed7 100644
--- a/drivers/media/i2c/adv748x/adv748x-csi2.c
+++ b/drivers/media/i2c/adv748x/adv748x-csi2.c
@@ -6,7 +6,6 @@
  */
 
 #include <linux/module.h>
-#include <linux/mutex.h>
 
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -14,6 +13,15 @@
 
 #include "adv748x.h"
 
+static const unsigned int adv748x_csi2_txa_fmts[] = {
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_RGB888_1X24,
+};
+
+static const unsigned int adv748x_csi2_txb_fmts[] = {
+	MEDIA_BUS_FMT_UYVY8_1X16,
+};
+
 int adv748x_csi2_set_virtual_channel(struct adv748x_csi2 *tx, unsigned int vc)
 {
 	return tx_write(tx, ADV748X_CSI_VC_REF, vc << ADV748X_CSI_VC_REF_SHIFT);
@@ -59,7 +67,33 @@ static int adv748x_csi2_register_link(struct adv748x_csi2 *tx,
 
 /* -----------------------------------------------------------------------------
  * v4l2_subdev_internal_ops
- *
+ */
+
+static int adv748x_csi2_init_state(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state)
+{
+	static const struct v4l2_mbus_framefmt adv748x_csi2_default_fmt = {
+		.width		= 1280,
+		.height		= 720,
+		.code		= MEDIA_BUS_FMT_UYVY8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.field		= V4L2_FIELD_NONE,
+		.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+		.quantization	= V4L2_QUANTIZATION_DEFAULT,
+		.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+	};
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = v4l2_subdev_state_get_format(state, ADV748X_CSI2_SINK);
+	*fmt = adv748x_csi2_default_fmt;
+
+	fmt = v4l2_subdev_state_get_format(state, ADV748X_CSI2_SOURCE);
+	*fmt = adv748x_csi2_default_fmt;
+
+	return 0;
+}
+
+/*
  * We use the internal registered operation to be able to ensure that our
  * incremental subdevices (not connected in the forward path) can be registered
  * against the resulting video path and media device.
@@ -109,6 +143,7 @@ static int adv748x_csi2_registered(struct v4l2_subdev *sd)
 }
 
 static const struct v4l2_subdev_internal_ops adv748x_csi2_internal_ops = {
+	.init_state = adv748x_csi2_init_state,
 	.registered = adv748x_csi2_registered,
 };
 
@@ -139,39 +174,55 @@ static const struct v4l2_subdev_video_ops adv748x_csi2_video_ops = {
  * But we must support setting the pad formats for format propagation.
  */
 
-static struct v4l2_mbus_framefmt *
-adv748x_csi2_get_pad_format(struct v4l2_subdev *sd,
-			    struct v4l2_subdev_state *sd_state,
-			    unsigned int pad, u32 which)
+static int adv748x_csi2_enum_mbus_code(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *sd_state,
+				       struct v4l2_subdev_mbus_code_enum *code)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
+	const unsigned int *codes = is_txa(tx) ?
+				    adv748x_csi2_txa_fmts :
+				    adv748x_csi2_txb_fmts;
+	size_t num_fmts = is_txa(tx) ? ARRAY_SIZE(adv748x_csi2_txa_fmts)
+				     : ARRAY_SIZE(adv748x_csi2_txb_fmts);
 
-	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return v4l2_subdev_state_get_format(sd_state, pad);
+	/*
+	 * The format available on the source pad is the one applied on the sink
+	 * pad.
+	 */
+	if (code->pad == ADV748X_CSI2_SOURCE) {
+		struct v4l2_mbus_framefmt *fmt;
 
-	return &tx->format;
-}
+		if (code->index)
+			return -EINVAL;
 
-static int adv748x_csi2_get_format(struct v4l2_subdev *sd,
-				   struct v4l2_subdev_state *sd_state,
-				   struct v4l2_subdev_format *sdformat)
-{
-	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
-	struct adv748x_state *state = tx->state;
-	struct v4l2_mbus_framefmt *mbusformat;
+		fmt = v4l2_subdev_state_get_format(sd_state, ADV748X_CSI2_SINK);
+		code->code = fmt->code;
 
-	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
-						 sdformat->which);
-	if (!mbusformat)
+		return 0;
+	}
+
+	if (code->index >= num_fmts)
 		return -EINVAL;
 
-	mutex_lock(&state->mutex);
+	code->code = codes[code->index];
 
-	sdformat->format = *mbusformat;
+	return 0;
+}
 
-	mutex_unlock(&state->mutex);
+static bool adv748x_csi2_is_fmt_supported(struct adv748x_csi2 *tx, u32 code)
+{
+	const unsigned int *codes = is_txa(tx) ?
+				    adv748x_csi2_txa_fmts :
+				    adv748x_csi2_txb_fmts;
+	size_t num_fmts = is_txa(tx) ? ARRAY_SIZE(adv748x_csi2_txa_fmts)
+				     : ARRAY_SIZE(adv748x_csi2_txb_fmts);
+
+	for (unsigned int i = 0; i < num_fmts; i++) {
+		if (codes[i] == code)
+			return true;
+	}
 
-	return 0;
+	return false;
 }
 
 static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
@@ -179,38 +230,26 @@ static int adv748x_csi2_set_format(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_format *sdformat)
 {
 	struct adv748x_csi2 *tx = adv748x_sd_to_csi2(sd);
-	struct adv748x_state *state = tx->state;
 	struct v4l2_mbus_framefmt *mbusformat;
-	int ret = 0;
-
-	mbusformat = adv748x_csi2_get_pad_format(sd, sd_state, sdformat->pad,
-						 sdformat->which);
-	if (!mbusformat)
-		return -EINVAL;
-
-	mutex_lock(&state->mutex);
 
-	if (sdformat->pad == ADV748X_CSI2_SOURCE) {
-		const struct v4l2_mbus_framefmt *sink_fmt;
+	if (sdformat->pad == ADV748X_CSI2_SOURCE)
+		return v4l2_subdev_get_fmt(sd, sd_state, sdformat);
 
-		sink_fmt = adv748x_csi2_get_pad_format(sd, sd_state,
-						       ADV748X_CSI2_SINK,
-						       sdformat->which);
-
-		if (!sink_fmt) {
-			ret = -EINVAL;
-			goto unlock;
-		}
-
-		sdformat->format = *sink_fmt;
-	}
+	/*
+	 * Make sure the format is supported, if not default it to
+	 * UYVY8 as it's supported by both TXes.
+	 */
+	if (!adv748x_csi2_is_fmt_supported(tx, sdformat->format.code))
+		sdformat->format.code = MEDIA_BUS_FMT_UYVY8_1X16;
 
+	mbusformat = v4l2_subdev_state_get_format(sd_state, sdformat->pad);
 	*mbusformat = sdformat->format;
 
-unlock:
-	mutex_unlock(&state->mutex);
+	/* Propagate format to the source pad. */
+	mbusformat = v4l2_subdev_state_get_format(sd_state, ADV748X_CSI2_SOURCE);
+	*mbusformat = sdformat->format;
 
-	return ret;
+	return 0;
 }
 
 static int adv748x_csi2_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
@@ -228,7 +267,8 @@ static int adv748x_csi2_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad
 }
 
 static const struct v4l2_subdev_pad_ops adv748x_csi2_pad_ops = {
-	.get_fmt = adv748x_csi2_get_format,
+	.enum_mbus_code = adv748x_csi2_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = adv748x_csi2_set_format,
 	.get_mbus_config = adv748x_csi2_get_mbus_config,
 };
@@ -320,6 +360,11 @@ int adv748x_csi2_init(struct adv748x_state *state, struct adv748x_csi2 *tx)
 	if (ret)
 		goto err_cleanup_subdev;
 
+	tx->sd.state_lock = &state->mutex;
+	ret = v4l2_subdev_init_finalize(&tx->sd);
+	if (ret)
+		goto err_free_ctrl;
+
 	ret = v4l2_async_register_subdev(&tx->sd);
 	if (ret)
 		goto err_free_ctrl;
diff --git a/drivers/media/i2c/adv748x/adv748x-hdmi.c b/drivers/media/i2c/adv748x/adv748x-hdmi.c
index ec151dc69c23..a4db9bae5f79 100644
--- a/drivers/media/i2c/adv748x/adv748x-hdmi.c
+++ b/drivers/media/i2c/adv748x/adv748x-hdmi.c
@@ -214,7 +214,7 @@ static int adv748x_hdmi_set_video_timings(struct adv748x_state *state,
  * v4l2_subdev_video_ops
  */
 
-static int adv748x_hdmi_s_dv_timings(struct v4l2_subdev *sd,
+static int adv748x_hdmi_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				     struct v4l2_dv_timings *timings)
 {
 	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
@@ -254,7 +254,7 @@ error:
 	return ret;
 }
 
-static int adv748x_hdmi_g_dv_timings(struct v4l2_subdev *sd,
+static int adv748x_hdmi_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				     struct v4l2_dv_timings *timings)
 {
 	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
@@ -269,7 +269,7 @@ static int adv748x_hdmi_g_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv748x_hdmi_query_dv_timings(struct v4l2_subdev *sd,
+static int adv748x_hdmi_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 					 struct v4l2_dv_timings *timings)
 {
 	struct adv748x_hdmi *hdmi = adv748x_sd_to_hdmi(sd);
@@ -392,9 +392,6 @@ static int adv748x_hdmi_g_pixelaspect(struct v4l2_subdev *sd,
 }
 
 static const struct v4l2_subdev_video_ops adv748x_video_ops_hdmi = {
-	.s_dv_timings = adv748x_hdmi_s_dv_timings,
-	.g_dv_timings = adv748x_hdmi_g_dv_timings,
-	.query_dv_timings = adv748x_hdmi_query_dv_timings,
 	.g_input_status = adv748x_hdmi_g_input_status,
 	.s_stream = adv748x_hdmi_s_stream,
 	.g_pixelaspect = adv748x_hdmi_g_pixelaspect,
@@ -413,7 +410,7 @@ static int adv748x_hdmi_propagate_pixelrate(struct adv748x_hdmi *hdmi)
 	if (!tx)
 		return -ENOLINK;
 
-	adv748x_hdmi_query_dv_timings(&hdmi->sd, &timings);
+	adv748x_hdmi_query_dv_timings(&hdmi->sd, 0, &timings);
 
 	return adv748x_csi2_set_pixelrate(tx, timings.bt.pixelclock);
 }
@@ -610,6 +607,9 @@ static const struct v4l2_subdev_pad_ops adv748x_pad_ops_hdmi = {
 	.get_fmt = adv748x_hdmi_get_format,
 	.get_edid = adv748x_hdmi_get_edid,
 	.set_edid = adv748x_hdmi_set_edid,
+	.s_dv_timings = adv748x_hdmi_s_dv_timings,
+	.g_dv_timings = adv748x_hdmi_g_dv_timings,
+	.query_dv_timings = adv748x_hdmi_query_dv_timings,
 	.dv_timings_cap = adv748x_hdmi_dv_timings_cap,
 	.enum_dv_timings = adv748x_hdmi_enum_dv_timings,
 };
@@ -734,7 +734,7 @@ int adv748x_hdmi_init(struct adv748x_hdmi *hdmi)
 	struct v4l2_dv_timings cea1280x720 = V4L2_DV_BT_CEA_1280X720P30;
 	int ret;
 
-	adv748x_hdmi_s_dv_timings(&hdmi->sd, &cea1280x720);
+	adv748x_hdmi_s_dv_timings(&hdmi->sd, 0, &cea1280x720);
 
 	/* Initialise a default 16:9 aspect ratio */
 	hdmi->aspect_ratio.numerator = 16;
diff --git a/drivers/media/i2c/adv748x/adv748x.h b/drivers/media/i2c/adv748x/adv748x.h
index d2b5e722e997..2c1db5968af8 100644
--- a/drivers/media/i2c/adv748x/adv748x.h
+++ b/drivers/media/i2c/adv748x/adv748x.h
@@ -75,7 +75,6 @@ enum adv748x_csi2_pads {
 
 struct adv748x_csi2 {
 	struct adv748x_state *state;
-	struct v4l2_mbus_framefmt format;
 	unsigned int page;
 	unsigned int port;
 	unsigned int num_lanes;
@@ -321,7 +320,7 @@ struct adv748x_state {
 
 /* Free run pattern select */
 #define ADV748X_SDP_FRP			0x14
-#define ADV748X_SDP_FRP_MASK		GENMASK(3, 1)
+#define ADV748X_SDP_FRP_MASK		GENMASK(2, 0)
 
 /* Saturation */
 #define ADV748X_SDP_SD_SAT_U		0xe3	/* user_map_rw_reg_e3 */
diff --git a/drivers/media/i2c/adv7511-v4l2.c b/drivers/media/i2c/adv7511-v4l2.c
index 0f780eb6ef63..853c7806de92 100644
--- a/drivers/media/i2c/adv7511-v4l2.c
+++ b/drivers/media/i2c/adv7511-v4l2.c
@@ -62,11 +62,6 @@ MODULE_LICENSE("GPL v2");
 **********************************************************************
 */
 
-struct i2c_reg_value {
-	unsigned char reg;
-	unsigned char value;
-};
-
 struct adv7511_state_edid {
 	/* total number of blocks */
 	u32 blocks;
@@ -121,6 +116,9 @@ struct adv7511_state {
 	unsigned edid_detect_counter;
 	struct workqueue_struct *work_queue;
 	struct delayed_work edid_handler; /* work entry */
+
+	struct dentry *debugfs_dir;
+	struct v4l2_debugfs_if *infoframes;
 };
 
 static void adv7511_check_monitor_present_status(struct v4l2_subdev *sd);
@@ -488,27 +486,25 @@ static u8 hdmi_infoframe_checksum(u8 *ptr, size_t size)
 	return 256 - csum;
 }
 
-static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_infoframe *cri)
+static int read_infoframe(struct v4l2_subdev *sd,
+			  const struct adv7511_cfg_read_infoframe *cri,
+			  u8 *buffer)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct device *dev = &client->dev;
-	union hdmi_infoframe frame;
-	u8 buffer[32];
 	u8 len;
 	int i;
 
 	if (!(adv7511_rd(sd, cri->present_reg) & cri->present_mask)) {
 		v4l2_info(sd, "%s infoframe not transmitted\n", cri->desc);
-		return;
+		return 0;
 	}
 
 	memcpy(buffer, cri->header, sizeof(cri->header));
 
 	len = buffer[2];
 
-	if (len + 4 > sizeof(buffer)) {
+	if (len + 4 > V4L2_DEBUGFS_IF_MAX_LEN) {
 		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
-		return;
+		return 0;
 	}
 
 	if (cri->payload_addr >= 0x100) {
@@ -521,21 +517,38 @@ static void log_infoframe(struct v4l2_subdev *sd, const struct adv7511_cfg_read_
 	buffer[3] = 0;
 	buffer[3] = hdmi_infoframe_checksum(buffer, len + 4);
 
-	if (hdmi_infoframe_unpack(&frame, buffer, len + 4) < 0) {
-		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
+	return len + 4;
+}
+
+static void log_infoframe(struct v4l2_subdev *sd,
+			  const struct adv7511_cfg_read_infoframe *cri)
+{
+	union hdmi_infoframe frame;
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	u8 buffer[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	int len = read_infoframe(sd, cri, buffer);
+
+	if (len <= 0)
+		return;
+
+	if (hdmi_infoframe_unpack(&frame, buffer, len) < 0) {
+		v4l2_err(sd, "%s: unpack of %s infoframe failed\n",
+			 __func__, cri->desc);
 		return;
 	}
 
 	hdmi_infoframe_log(KERN_INFO, dev, &frame);
 }
 
+static const struct adv7511_cfg_read_infoframe cri[] = {
+	{ "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
+	{ "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
+	{ "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
+};
+
 static void adv7511_log_infoframes(struct v4l2_subdev *sd)
 {
-	static const struct adv7511_cfg_read_infoframe cri[] = {
-		{ "AVI", 0x44, 0x10, { 0x82, 2, 13 }, 0x55 },
-		{ "Audio", 0x44, 0x08, { 0x84, 1, 10 }, 0x73 },
-		{ "SDP", 0x40, 0x40, { 0x83, 1, 25 }, 0x103 },
-	};
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(cri); i++)
@@ -995,8 +1008,8 @@ static int adv7511_s_stream(struct v4l2_subdev *sd, int enable)
 	return 0;
 }
 
-static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
-			       struct v4l2_dv_timings *timings)
+static int adv7511_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *timings)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 	struct v4l2_bt_timings *bt = &timings->bt;
@@ -1004,6 +1017,9 @@ static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	/* quick sanity check */
 	if (!v4l2_valid_dv_timings(timings, &adv7511_timings_cap, NULL, NULL))
 		return -EINVAL;
@@ -1042,13 +1058,16 @@ static int adv7511_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7511_g_dv_timings(struct v4l2_subdev *sd,
+static int adv7511_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_dv_timings *timings)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (!timings)
 		return -EINVAL;
 
@@ -1078,8 +1097,6 @@ static int adv7511_dv_timings_cap(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_video_ops adv7511_video_ops = {
 	.s_stream = adv7511_s_stream,
-	.s_dv_timings = adv7511_s_dv_timings,
-	.g_dv_timings = adv7511_g_dv_timings,
 };
 
 /* ------------------------------ AUDIO OPS ------------------------------ */
@@ -1353,9 +1370,9 @@ static int adv7511_set_fmt(struct v4l2_subdev *sd,
 	case V4L2_COLORSPACE_BT2020:
 		c = HDMI_COLORIMETRY_EXTENDED;
 		if (y && format->format.ycbcr_enc == V4L2_YCBCR_ENC_BT2020_CONST_LUM)
-			ec = 5; /* Not yet available in hdmi.h */
+			ec = HDMI_EXTENDED_COLORIMETRY_BT2020_CONST_LUM;
 		else
-			ec = 6; /* Not yet available in hdmi.h */
+			ec = HDMI_EXTENDED_COLORIMETRY_BT2020;
 		break;
 	default:
 		break;
@@ -1403,6 +1420,8 @@ static const struct v4l2_subdev_pad_ops adv7511_pad_ops = {
 	.enum_mbus_code = adv7511_enum_mbus_code,
 	.get_fmt = adv7511_get_fmt,
 	.set_fmt = adv7511_set_fmt,
+	.s_dv_timings = adv7511_s_dv_timings,
+	.g_dv_timings = adv7511_g_dv_timings,
 	.enum_dv_timings = adv7511_enum_dv_timings,
 	.dv_timings_cap = adv7511_dv_timings_cap,
 };
@@ -1645,7 +1664,9 @@ static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
 		if (!err) {
 			adv7511_dbg_dump_edid(2, debug, sd, segment, &state->edid.data[segment * 256]);
 			if (segment == 0) {
-				state->edid.blocks = state->edid.data[0x7e] + 1;
+				state->edid.blocks =
+					v4l2_num_edid_blocks(state->edid.data,
+							     EDID_MAX_SEGM * 2);
 				v4l2_dbg(1, debug, sd, "%s: %d blocks in total\n",
 					 __func__, state->edid.blocks);
 			}
@@ -1663,7 +1684,7 @@ static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
 		/* one more segment read ok */
 		state->edid.segments = segment + 1;
 		v4l2_ctrl_s_ctrl(state->have_edid0_ctrl, 0x1);
-		if (((state->edid.data[0x7e] >> 1) + 1) > state->edid.segments) {
+		if (state->edid.blocks > state->edid.segments * 2) {
 			/* Request next EDID segment */
 			v4l2_dbg(1, debug, sd, "%s: request segment %d\n", __func__, state->edid.segments);
 			adv7511_wr(sd, 0xc9, 0xf);
@@ -1692,6 +1713,34 @@ static bool adv7511_check_edid_status(struct v4l2_subdev *sd)
 	return false;
 }
 
+static ssize_t
+adv7511_debugfs_if_read(u32 type, void *priv,
+			struct file *filp, char __user *ubuf, size_t count, loff_t *ppos)
+{
+	u8 buf[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	struct v4l2_subdev *sd = priv;
+	int index;
+	int len;
+
+	switch (type) {
+	case V4L2_DEBUGFS_IF_AVI:
+		index = 0;
+		break;
+	case V4L2_DEBUGFS_IF_AUDIO:
+		index = 1;
+		break;
+	case V4L2_DEBUGFS_IF_SPD:
+		index = 2;
+		break;
+	default:
+		return 0;
+	}
+	len = read_infoframe(sd, &cri[index], buf);
+	if (len > 0)
+		len = simple_read_from_buffer(ubuf, count, ppos, buf, len);
+	return len < 0 ? 0 : len;
+}
+
 static int adv7511_registered(struct v4l2_subdev *sd)
 {
 	struct adv7511_state *state = get_adv7511_state(sd);
@@ -1699,9 +1748,16 @@ static int adv7511_registered(struct v4l2_subdev *sd)
 	int err;
 
 	err = cec_register_adapter(state->cec_adap, &client->dev);
-	if (err)
+	if (err) {
 		cec_delete_adapter(state->cec_adap);
-	return err;
+		return err;
+	}
+
+	state->debugfs_dir = debugfs_create_dir(sd->name, v4l2_debugfs_root());
+	state->infoframes = v4l2_debugfs_if_alloc(state->debugfs_dir,
+		V4L2_DEBUGFS_IF_AVI | V4L2_DEBUGFS_IF_AUDIO |
+		V4L2_DEBUGFS_IF_SPD, sd, adv7511_debugfs_if_read);
+	return 0;
 }
 
 static void adv7511_unregistered(struct v4l2_subdev *sd)
@@ -1709,6 +1765,10 @@ static void adv7511_unregistered(struct v4l2_subdev *sd)
 	struct adv7511_state *state = get_adv7511_state(sd);
 
 	cec_unregister_adapter(state->cec_adap);
+	v4l2_debugfs_if_free(state->infoframes);
+	state->infoframes = NULL;
+	debugfs_remove_recursive(state->debugfs_dir);
+	state->debugfs_dir = NULL;
 }
 
 static const struct v4l2_subdev_internal_ops adv7511_int_ops = {
@@ -1948,7 +2008,7 @@ static void adv7511_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id adv7511_id[] = {
-	{ "adv7511-v4l2", 0 },
+	{ "adv7511-v4l2" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adv7511_id);
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index 319db3e847c4..e271782b7b70 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -42,7 +42,7 @@ module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "debug level (0-2)");
 
 MODULE_DESCRIPTION("Analog Devices ADV7604/10/11/12 video decoder driver");
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
 MODULE_AUTHOR("Mats Randgaard <mats.randgaard@cisco.com>");
 MODULE_LICENSE("GPL");
 
@@ -193,6 +193,9 @@ struct adv76xx_state {
 	struct delayed_work delayed_work_enable_hotplug;
 	bool restart_stdi_once;
 
+	struct dentry *debugfs_dir;
+	struct v4l2_debugfs_if *infoframes;
+
 	/* CEC */
 	struct cec_adapter *cec_adap;
 	u8   cec_addr[ADV76XX_MAX_ADDRS];
@@ -1405,12 +1408,13 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
 	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs, 0,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			false, timings))
+			false, adv76xx_get_dv_timings_cap(sd, -1), timings))
 		return 0;
 	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
 			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
 			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			false, state->aspect_ratio, timings))
+			false, state->aspect_ratio,
+			adv76xx_get_dv_timings_cap(sd, -1), timings))
 		return 0;
 
 	v4l2_dbg(2, debug, sd,
@@ -1557,8 +1561,8 @@ static unsigned int adv76xx_read_hdmi_pixelclock(struct v4l2_subdev *sd)
 	return freq;
 }
 
-static int adv76xx_query_dv_timings(struct v4l2_subdev *sd,
-			struct v4l2_dv_timings *timings)
+static int adv76xx_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				    struct v4l2_dv_timings *timings)
 {
 	struct adv76xx_state *state = to_state(sd);
 	const struct adv76xx_chip_info *info = state->info;
@@ -1687,8 +1691,8 @@ found:
 	return 0;
 }
 
-static int adv76xx_s_dv_timings(struct v4l2_subdev *sd,
-		struct v4l2_dv_timings *timings)
+static int adv76xx_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *timings)
 {
 	struct adv76xx_state *state = to_state(sd);
 	struct v4l2_bt_timings *bt;
@@ -1730,8 +1734,8 @@ static int adv76xx_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv76xx_g_dv_timings(struct v4l2_subdev *sd,
-		struct v4l2_dv_timings *timings)
+static int adv76xx_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *timings)
 {
 	struct adv76xx_state *state = to_state(sd);
 
@@ -2458,10 +2462,9 @@ static const struct adv76xx_cfg_read_infoframe adv76xx_cri[] = {
 	{ "Vendor", 0x10, 0xec, 0x54 }
 };
 
-static int adv76xx_read_infoframe(struct v4l2_subdev *sd, int index,
-				  union hdmi_infoframe *frame)
+static int adv76xx_read_infoframe_buf(struct v4l2_subdev *sd, int index,
+				      u8 buf[V4L2_DEBUGFS_IF_MAX_LEN])
 {
-	uint8_t buffer[32];
 	u8 len;
 	int i;
 
@@ -2472,27 +2475,20 @@ static int adv76xx_read_infoframe(struct v4l2_subdev *sd, int index,
 	}
 
 	for (i = 0; i < 3; i++)
-		buffer[i] = infoframe_read(sd,
-					   adv76xx_cri[index].head_addr + i);
+		buf[i] = infoframe_read(sd, adv76xx_cri[index].head_addr + i);
 
-	len = buffer[2] + 1;
+	len = buf[2] + 1;
 
-	if (len + 3 > sizeof(buffer)) {
+	if (len + 3 > V4L2_DEBUGFS_IF_MAX_LEN) {
 		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__,
 			 adv76xx_cri[index].desc, len);
 		return -ENOENT;
 	}
 
 	for (i = 0; i < len; i++)
-		buffer[i + 3] = infoframe_read(sd,
-				       adv76xx_cri[index].payload_addr + i);
-
-	if (hdmi_infoframe_unpack(frame, buffer, len + 3) < 0) {
-		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__,
-			 adv76xx_cri[index].desc);
-		return -ENOENT;
-	}
-	return 0;
+		buf[i + 3] = infoframe_read(sd,
+					    adv76xx_cri[index].payload_addr + i);
+	return len + 3;
 }
 
 static void adv76xx_log_infoframes(struct v4l2_subdev *sd)
@@ -2505,10 +2501,19 @@ static void adv76xx_log_infoframes(struct v4l2_subdev *sd)
 	}
 
 	for (i = 0; i < ARRAY_SIZE(adv76xx_cri); i++) {
-		union hdmi_infoframe frame;
 		struct i2c_client *client = v4l2_get_subdevdata(sd);
+		u8 buffer[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+		union hdmi_infoframe frame;
+		int len;
+
+		len = adv76xx_read_infoframe_buf(sd, i, buffer);
+		if (len < 0)
+			continue;
 
-		if (!adv76xx_read_infoframe(sd, i, &frame))
+		if (hdmi_infoframe_unpack(&frame, buffer, len) < 0)
+			v4l2_err(sd, "%s: unpack of %s infoframe failed\n",
+				 __func__, adv76xx_cri[i].desc);
+		else
 			hdmi_infoframe_log(KERN_INFO, &client->dev, &frame);
 	}
 }
@@ -2519,10 +2524,10 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 	const struct adv76xx_chip_info *info = state->info;
 	struct v4l2_dv_timings timings;
 	struct stdi_readback stdi;
-	u8 reg_io_0x02 = io_read(sd, 0x02);
+	int ret;
+	u8 reg_io_0x02;
 	u8 edid_enabled;
 	u8 cable_det;
-
 	static const char * const csc_coeff_sel_rb[16] = {
 		"bypassed", "YPbPr601 -> RGB", "reserved", "YPbPr709 -> RGB",
 		"reserved", "RGB -> YPbPr601", "reserved", "RGB -> YPbPr709",
@@ -2607,7 +2612,7 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 				stdi.lcf, stdi.bl, stdi.lcvs,
 				stdi.interlaced ? "interlaced" : "progressive",
 				stdi.hs_pol, stdi.vs_pol);
-	if (adv76xx_query_dv_timings(sd, &timings))
+	if (adv76xx_query_dv_timings(sd, 0, &timings))
 		v4l2_info(sd, "No video detected\n");
 	else
 		v4l2_print_dv_timings(sd->name, "Detected format: ",
@@ -2621,13 +2626,21 @@ static int adv76xx_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "-----Color space-----\n");
 	v4l2_info(sd, "RGB quantization range ctrl: %s\n",
 			rgb_quantization_range_txt[state->rgb_quantization_range]);
-	v4l2_info(sd, "Input color space: %s\n",
-			input_color_space_txt[reg_io_0x02 >> 4]);
-	v4l2_info(sd, "Output color space: %s %s, alt-gamma %s\n",
-			(reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
-			(((reg_io_0x02 >> 2) & 0x01) ^ (reg_io_0x02 & 0x01)) ?
-				"(16-235)" : "(0-255)",
-			(reg_io_0x02 & 0x08) ? "enabled" : "disabled");
+
+	ret = io_read(sd, 0x02);
+	if (ret < 0) {
+		v4l2_info(sd, "Can't read Input/Output color space\n");
+	} else {
+		reg_io_0x02 = ret;
+
+		v4l2_info(sd, "Input color space: %s\n",
+				input_color_space_txt[reg_io_0x02 >> 4]);
+		v4l2_info(sd, "Output color space: %s %s, alt-gamma %s\n",
+				(reg_io_0x02 & 0x02) ? "RGB" : "YCbCr",
+				(((reg_io_0x02 >> 2) & 0x01) ^ (reg_io_0x02 & 0x01)) ?
+					"(16-235)" : "(0-255)",
+				(reg_io_0x02 & 0x08) ? "enabled" : "disabled");
+	}
 	v4l2_info(sd, "Color space conversion: %s\n",
 			csc_coeff_sel_rb[cp_read(sd, info->cp_csc) >> 4]);
 
@@ -2686,6 +2699,41 @@ static int adv76xx_subscribe_event(struct v4l2_subdev *sd,
 	}
 }
 
+static ssize_t
+adv76xx_debugfs_if_read(u32 type, void *priv, struct file *filp,
+			char __user *ubuf, size_t count, loff_t *ppos)
+{
+	u8 buf[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	struct v4l2_subdev *sd = priv;
+	int index;
+	int len;
+
+	if (!is_hdmi(sd))
+		return 0;
+
+	switch (type) {
+	case V4L2_DEBUGFS_IF_AVI:
+		index = 0;
+		break;
+	case V4L2_DEBUGFS_IF_AUDIO:
+		index = 1;
+		break;
+	case V4L2_DEBUGFS_IF_SPD:
+		index = 2;
+		break;
+	case V4L2_DEBUGFS_IF_HDMI:
+		index = 3;
+		break;
+	default:
+		return 0;
+	}
+
+	len = adv76xx_read_infoframe_buf(sd, index, buf);
+	if (len > 0)
+		len = simple_read_from_buffer(ubuf, count, ppos, buf, len);
+	return len < 0 ? 0 : len;
+}
+
 static int adv76xx_registered(struct v4l2_subdev *sd)
 {
 	struct adv76xx_state *state = to_state(sd);
@@ -2693,9 +2741,16 @@ static int adv76xx_registered(struct v4l2_subdev *sd)
 	int err;
 
 	err = cec_register_adapter(state->cec_adap, &client->dev);
-	if (err)
+	if (err) {
 		cec_delete_adapter(state->cec_adap);
-	return err;
+		return err;
+	}
+	state->debugfs_dir = debugfs_create_dir(sd->name, v4l2_debugfs_root());
+	state->infoframes = v4l2_debugfs_if_alloc(state->debugfs_dir,
+		V4L2_DEBUGFS_IF_AVI | V4L2_DEBUGFS_IF_AUDIO |
+		V4L2_DEBUGFS_IF_SPD | V4L2_DEBUGFS_IF_HDMI, sd,
+		adv76xx_debugfs_if_read);
+	return 0;
 }
 
 static void adv76xx_unregistered(struct v4l2_subdev *sd)
@@ -2703,6 +2758,10 @@ static void adv76xx_unregistered(struct v4l2_subdev *sd)
 	struct adv76xx_state *state = to_state(sd);
 
 	cec_unregister_adapter(state->cec_adap);
+	v4l2_debugfs_if_free(state->infoframes);
+	state->infoframes = NULL;
+	debugfs_remove_recursive(state->debugfs_dir);
+	state->debugfs_dir = NULL;
 }
 
 /* ----------------------------------------------------------------------- */
@@ -2726,9 +2785,6 @@ static const struct v4l2_subdev_core_ops adv76xx_core_ops = {
 static const struct v4l2_subdev_video_ops adv76xx_video_ops = {
 	.s_routing = adv76xx_s_routing,
 	.g_input_status = adv76xx_g_input_status,
-	.s_dv_timings = adv76xx_s_dv_timings,
-	.g_dv_timings = adv76xx_g_dv_timings,
-	.query_dv_timings = adv76xx_query_dv_timings,
 };
 
 static const struct v4l2_subdev_pad_ops adv76xx_pad_ops = {
@@ -2738,6 +2794,9 @@ static const struct v4l2_subdev_pad_ops adv76xx_pad_ops = {
 	.set_fmt = adv76xx_set_format,
 	.get_edid = adv76xx_get_edid,
 	.set_edid = adv76xx_set_edid,
+	.s_dv_timings = adv76xx_s_dv_timings,
+	.g_dv_timings = adv76xx_g_dv_timings,
+	.query_dv_timings = adv76xx_query_dv_timings,
 	.dv_timings_cap = adv76xx_dv_timings_cap,
 	.enum_dv_timings = adv76xx_enum_dv_timings,
 };
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index 2ad0f9f5503d..5545cd23e113 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -38,7 +38,7 @@ module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "debug level (0-2)");
 
 MODULE_DESCRIPTION("Analog Devices ADV7842 video decoder driver");
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
 MODULE_AUTHOR("Martin Bugge <marbugge@cisco.com>");
 MODULE_LICENSE("GPL");
 
@@ -114,6 +114,9 @@ struct adv7842_state {
 	bool restart_stdi_once;
 	bool hdmi_port_a;
 
+	struct dentry *debugfs_dir;
+	struct v4l2_debugfs_if *infoframes;
+
 	/* i2c clients */
 	struct i2c_client *i2c_sdp_io;
 	struct i2c_client *i2c_sdp;
@@ -1431,14 +1434,15 @@ static int stdi2dv_timings(struct v4l2_subdev *sd,
 	}
 
 	if (v4l2_detect_cvt(stdi->lcf + 1, hfreq, stdi->lcvs, 0,
-			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
-			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			false, timings))
+			    (stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			    (stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			    false, adv7842_get_dv_timings_cap(sd), timings))
 		return 0;
 	if (v4l2_detect_gtf(stdi->lcf + 1, hfreq, stdi->lcvs,
-			(stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
-			(stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
-			false, state->aspect_ratio, timings))
+			    (stdi->hs_pol == '+' ? V4L2_DV_HSYNC_POS_POL : 0) |
+			    (stdi->vs_pol == '+' ? V4L2_DV_VSYNC_POS_POL : 0),
+			    false, state->aspect_ratio,
+			    adv7842_get_dv_timings_cap(sd), timings))
 		return 0;
 
 	v4l2_dbg(2, debug, sd,
@@ -1518,7 +1522,7 @@ static void adv7842_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
 	timings->bt.flags |= V4L2_DV_FL_CAN_DETECT_REDUCED_FPS;
 }
 
-static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
+static int adv7842_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				    struct v4l2_dv_timings *timings)
 {
 	struct adv7842_state *state = to_state(sd);
@@ -1527,6 +1531,9 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	memset(timings, 0, sizeof(struct v4l2_dv_timings));
 
 	/* SDP block */
@@ -1643,7 +1650,7 @@ found:
 	return 0;
 }
 
-static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
+static int adv7842_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_dv_timings *timings)
 {
 	struct adv7842_state *state = to_state(sd);
@@ -1652,6 +1659,9 @@ static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
 
@@ -1689,11 +1699,14 @@ static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int adv7842_g_dv_timings(struct v4l2_subdev *sd,
+static int adv7842_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_dv_timings *timings)
 {
 	struct adv7842_state *state = to_state(sd);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
 	*timings = state->timings;
@@ -2556,58 +2569,65 @@ struct adv7842_cfg_read_infoframe {
 	u8 payload_addr;
 };
 
-static void log_infoframe(struct v4l2_subdev *sd, const struct adv7842_cfg_read_infoframe *cri)
+static const struct adv7842_cfg_read_infoframe adv7842_cri[] = {
+	{ "AVI", 0x01, 0xe0, 0x00 },
+	{ "Audio", 0x02, 0xe3, 0x1c },
+	{ "SDP", 0x04, 0xe6, 0x2a },
+	{ "Vendor", 0x10, 0xec, 0x54 }
+};
+
+static int adv7842_read_infoframe_buf(struct v4l2_subdev *sd, int index,
+				      u8 buf[V4L2_DEBUGFS_IF_MAX_LEN])
 {
-	int i;
-	u8 buffer[32];
-	union hdmi_infoframe frame;
-	u8 len;
-	struct i2c_client *client = v4l2_get_subdevdata(sd);
-	struct device *dev = &client->dev;
+	const struct adv7842_cfg_read_infoframe *cri = &adv7842_cri[index];
+	int len, i;
 
 	if (!(io_read(sd, 0x60) & cri->present_mask)) {
-		v4l2_info(sd, "%s infoframe not received\n", cri->desc);
-		return;
+		v4l2_dbg(1, debug, sd,
+			 "%s infoframe not received\n", cri->desc);
+		return -ENOENT;
 	}
 
 	for (i = 0; i < 3; i++)
-		buffer[i] = infoframe_read(sd, cri->head_addr + i);
+		buf[i] = infoframe_read(sd, cri->head_addr + i);
 
-	len = buffer[2] + 1;
+	len = buf[2] + 1;
 
-	if (len + 3 > sizeof(buffer)) {
-		v4l2_err(sd, "%s: invalid %s infoframe length %d\n", __func__, cri->desc, len);
-		return;
+	if (len + 3 > V4L2_DEBUGFS_IF_MAX_LEN) {
+		v4l2_err(sd, "%s: invalid %s infoframe length %d\n",
+			 __func__, cri->desc, len);
+		return -ENOENT;
 	}
 
 	for (i = 0; i < len; i++)
-		buffer[i + 3] = infoframe_read(sd, cri->payload_addr + i);
-
-	if (hdmi_infoframe_unpack(&frame, buffer, len + 3) < 0) {
-		v4l2_err(sd, "%s: unpack of %s infoframe failed\n", __func__, cri->desc);
-		return;
-	}
-
-	hdmi_infoframe_log(KERN_INFO, dev, &frame);
+		buf[i + 3] = infoframe_read(sd, cri->payload_addr + i);
+	return len + 3;
 }
 
 static void adv7842_log_infoframes(struct v4l2_subdev *sd)
 {
-	int i;
-	static const struct adv7842_cfg_read_infoframe cri[] = {
-		{ "AVI", 0x01, 0xe0, 0x00 },
-		{ "Audio", 0x02, 0xe3, 0x1c },
-		{ "SDP", 0x04, 0xe6, 0x2a },
-		{ "Vendor", 0x10, 0xec, 0x54 }
-	};
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct device *dev = &client->dev;
+	union hdmi_infoframe frame;
+	u8 buffer[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	int len, i;
 
 	if (!(hdmi_read(sd, 0x05) & 0x80)) {
 		v4l2_info(sd, "receive DVI-D signal, no infoframes\n");
 		return;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(cri); i++)
-		log_infoframe(sd, &cri[i]);
+	for (i = 0; i < ARRAY_SIZE(adv7842_cri); i++) {
+		len = adv7842_read_infoframe_buf(sd, i, buffer);
+		if (len < 0)
+			continue;
+
+		if (hdmi_infoframe_unpack(&frame, buffer, len) < 0)
+			v4l2_err(sd, "%s: unpack of %s infoframe failed\n",
+				 __func__, adv7842_cri[i].desc);
+		else
+			hdmi_infoframe_log(KERN_INFO, dev, &frame);
+	}
 }
 
 #if 0
@@ -2780,7 +2800,7 @@ static int adv7842_cp_log_status(struct v4l2_subdev *sd)
 				"interlaced" : "progressive",
 			hs_pol, vs_pol);
 	}
-	if (adv7842_query_dv_timings(sd, &timings))
+	if (adv7842_query_dv_timings(sd, 0, &timings))
 		v4l2_info(sd, "No video detected\n");
 	else
 		v4l2_print_dv_timings(sd->name, "Detected format: ",
@@ -3226,7 +3246,7 @@ static int adv7842_command_ram_test(struct v4l2_subdev *sd)
 
 	memset(&state->timings, 0, sizeof(struct v4l2_dv_timings));
 
-	adv7842_s_dv_timings(sd, &timings);
+	adv7842_s_dv_timings(sd, 0, &timings);
 
 	return ret;
 }
@@ -3254,6 +3274,41 @@ static int adv7842_subscribe_event(struct v4l2_subdev *sd,
 	}
 }
 
+static ssize_t
+adv7842_debugfs_if_read(u32 type, void *priv, struct file *filp,
+			char __user *ubuf, size_t count, loff_t *ppos)
+{
+	u8 buf[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	struct v4l2_subdev *sd = priv;
+	int index;
+	int len;
+
+	if (!is_hdmi(sd))
+		return 0;
+
+	switch (type) {
+	case V4L2_DEBUGFS_IF_AVI:
+		index = 0;
+		break;
+	case V4L2_DEBUGFS_IF_AUDIO:
+		index = 1;
+		break;
+	case V4L2_DEBUGFS_IF_SPD:
+		index = 2;
+		break;
+	case V4L2_DEBUGFS_IF_HDMI:
+		index = 3;
+		break;
+	default:
+		return 0;
+	}
+
+	len = adv7842_read_infoframe_buf(sd, index, buf);
+	if (len > 0)
+		len = simple_read_from_buffer(ubuf, count, ppos, buf, len);
+	return len < 0 ? 0 : len;
+}
+
 static int adv7842_registered(struct v4l2_subdev *sd)
 {
 	struct adv7842_state *state = to_state(sd);
@@ -3261,8 +3316,15 @@ static int adv7842_registered(struct v4l2_subdev *sd)
 	int err;
 
 	err = cec_register_adapter(state->cec_adap, &client->dev);
-	if (err)
+	if (err) {
 		cec_delete_adapter(state->cec_adap);
+	} else {
+		state->debugfs_dir = debugfs_create_dir(sd->name, v4l2_debugfs_root());
+		state->infoframes = v4l2_debugfs_if_alloc(state->debugfs_dir,
+			V4L2_DEBUGFS_IF_AVI | V4L2_DEBUGFS_IF_AUDIO |
+			V4L2_DEBUGFS_IF_SPD | V4L2_DEBUGFS_IF_HDMI, sd,
+			adv7842_debugfs_if_read);
+	}
 	return err;
 }
 
@@ -3271,6 +3333,10 @@ static void adv7842_unregistered(struct v4l2_subdev *sd)
 	struct adv7842_state *state = to_state(sd);
 
 	cec_unregister_adapter(state->cec_adap);
+	v4l2_debugfs_if_free(state->infoframes);
+	state->infoframes = NULL;
+	debugfs_remove_recursive(state->debugfs_dir);
+	state->debugfs_dir = NULL;
 }
 
 /* ----------------------------------------------------------------------- */
@@ -3298,9 +3364,6 @@ static const struct v4l2_subdev_video_ops adv7842_video_ops = {
 	.s_routing = adv7842_s_routing,
 	.querystd = adv7842_querystd,
 	.g_input_status = adv7842_g_input_status,
-	.s_dv_timings = adv7842_s_dv_timings,
-	.g_dv_timings = adv7842_g_dv_timings,
-	.query_dv_timings = adv7842_query_dv_timings,
 };
 
 static const struct v4l2_subdev_pad_ops adv7842_pad_ops = {
@@ -3309,6 +3372,9 @@ static const struct v4l2_subdev_pad_ops adv7842_pad_ops = {
 	.set_fmt = adv7842_set_format,
 	.get_edid = adv7842_get_edid,
 	.set_edid = adv7842_set_edid,
+	.s_dv_timings = adv7842_s_dv_timings,
+	.g_dv_timings = adv7842_g_dv_timings,
+	.query_dv_timings = adv7842_query_dv_timings,
 	.enum_dv_timings = adv7842_enum_dv_timings,
 	.dv_timings_cap = adv7842_dv_timings_cap,
 };
@@ -3608,7 +3674,7 @@ static void adv7842_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id adv7842_id[] = {
-	{ "adv7842", 0 },
+	{ "adv7842" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, adv7842_id);
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index ce840adc2aa7..ee575d01a676 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -304,8 +304,8 @@ static void ak881x_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ak881x_id[] = {
-	{ "ak8813", 0 },
-	{ "ak8814", 0 },
+	{ "ak8813" },
+	{ "ak8814" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ak881x_id);
diff --git a/drivers/media/i2c/alvium-csi2.c b/drivers/media/i2c/alvium-csi2.c
index e65702e3f73e..05b708bd0a64 100644
--- a/drivers/media/i2c/alvium-csi2.c
+++ b/drivers/media/i2c/alvium-csi2.c
@@ -16,7 +16,6 @@
 #include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
@@ -403,21 +402,22 @@ static int alvium_get_bcrm_vers(struct alvium_dev *alvium)
 static int alvium_get_fw_version(struct alvium_dev *alvium)
 {
 	struct device *dev = &alvium->i2c_client->dev;
-	u64 spec, maj, min, pat;
-	int ret = 0;
+	u64 val;
+	int ret;
 
-	ret = alvium_read(alvium, REG_BCRM_DEVICE_FW_SPEC_VERSION_R,
-			  &spec, &ret);
-	ret = alvium_read(alvium, REG_BCRM_DEVICE_FW_MAJOR_VERSION_R,
-			  &maj, &ret);
-	ret = alvium_read(alvium, REG_BCRM_DEVICE_FW_MINOR_VERSION_R,
-			  &min, &ret);
-	ret = alvium_read(alvium, REG_BCRM_DEVICE_FW_PATCH_VERSION_R,
-			  &pat, &ret);
+	ret = alvium_read(alvium, REG_BCRM_DEVICE_FW, &val, NULL);
 	if (ret)
 		return ret;
 
-	dev_info(dev, "fw version: %llu.%llu.%llu.%llu\n", spec, maj, min, pat);
+	dev_info(dev, "fw version: %02u.%02u.%04u.%08x\n",
+		 (u8)((val & BCRM_DEVICE_FW_SPEC_MASK) >>
+		       BCRM_DEVICE_FW_SPEC_SHIFT),
+		 (u8)((val & BCRM_DEVICE_FW_MAJOR_MASK) >>
+		       BCRM_DEVICE_FW_MAJOR_SHIFT),
+		 (u16)((val & BCRM_DEVICE_FW_MINOR_MASK) >>
+			BCRM_DEVICE_FW_MINOR_SHIFT),
+		 (u32)((val & BCRM_DEVICE_FW_PATCH_MASK) >>
+			BCRM_DEVICE_FW_PATCH_SHIFT));
 
 	return 0;
 }
@@ -1188,6 +1188,20 @@ static int alvium_set_frame_rate(struct alvium_dev *alvium, u64 fr)
 	struct device *dev = &alvium->i2c_client->dev;
 	int ret;
 
+	ret = alvium_write_hshake(alvium, REG_BCRM_ACQUISITION_FRAME_RATE_EN_RW,
+				  1);
+	if (ret) {
+		dev_err(dev, "Fail to set acquisition frame rate enable reg\n");
+		return ret;
+	}
+
+	ret = alvium_write_hshake(alvium, REG_BCRM_FRAME_START_TRIGGER_MODE_RW,
+				  0);
+	if (ret) {
+		dev_err(dev, "Fail to set frame start trigger mode reg\n");
+		return ret;
+	}
+
 	ret = alvium_write_hshake(alvium, REG_BCRM_ACQUISITION_FRAME_RATE_RW,
 				  fr);
 	if (ret) {
@@ -1707,6 +1721,27 @@ alvium_code_to_pixfmt(struct alvium_dev *alvium, u32 code)
 	return &alvium->alvium_csi2_fmt[0];
 }
 
+static int alvium_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	struct alvium_dev *alvium = sd_to_alvium(sd);
+	const struct alvium_pixfmt *alvium_csi2_fmt;
+
+	if (fse->index)
+		return -EINVAL;
+
+	alvium_csi2_fmt = alvium_code_to_pixfmt(alvium, fse->code);
+	if (fse->code != alvium_csi2_fmt->code)
+		return -EINVAL;
+
+	fse->min_width = alvium->img_min_width;
+	fse->max_width = alvium->img_max_width;
+	fse->min_height = alvium->img_min_height;
+	fse->max_height = alvium->img_max_height;
+	return 0;
+}
+
 static int alvium_set_mode(struct alvium_dev *alvium,
 			   struct v4l2_subdev_state *state)
 {
@@ -1962,7 +1997,7 @@ static int alvium_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 	int val;
 
 	switch (ctrl->id) {
-	case V4L2_CID_GAIN:
+	case V4L2_CID_ANALOGUE_GAIN:
 		val = alvium_get_gain(alvium);
 		if (val < 0)
 			return val;
@@ -1994,7 +2029,7 @@ static int alvium_s_ctrl(struct v4l2_ctrl *ctrl)
 		return 0;
 
 	switch (ctrl->id) {
-	case V4L2_CID_GAIN:
+	case V4L2_CID_ANALOGUE_GAIN:
 		ret = alvium_set_ctrl_gain(alvium, ctrl->val);
 		break;
 	case V4L2_CID_AUTOGAIN:
@@ -2123,7 +2158,7 @@ static int alvium_ctrl_init(struct alvium_dev *alvium)
 
 	if (alvium->avail_ft.gain) {
 		ctrls->gain = v4l2_ctrl_new_std(hdl, ops,
-						V4L2_CID_GAIN,
+						V4L2_CID_ANALOGUE_GAIN,
 						alvium->min_gain,
 						alvium->max_gain,
 						alvium->inc_gain,
@@ -2204,8 +2239,6 @@ free_ctrls:
 
 static const struct v4l2_subdev_core_ops alvium_core_ops = {
 	.log_status = v4l2_ctrl_subdev_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_video_ops alvium_video_ops = {
@@ -2214,6 +2247,7 @@ static const struct v4l2_subdev_video_ops alvium_video_ops = {
 
 static const struct v4l2_subdev_pad_ops alvium_pad_ops = {
 	.enum_mbus_code = alvium_enum_mbus_code,
+	.enum_frame_size = alvium_enum_frame_size,
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = alvium_set_fmt,
 	.get_selection = alvium_get_selection,
@@ -2252,7 +2286,7 @@ static int alvium_subdev_init(struct alvium_dev *alvium)
 	v4l2_i2c_subdev_init(sd, client, &alvium_subdev_ops);
 
 	sd->internal_ops = &alvium_internal_ops;
-	sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	alvium->pad.flags = MEDIA_PAD_FL_SOURCE;
 	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
diff --git a/drivers/media/i2c/alvium-csi2.h b/drivers/media/i2c/alvium-csi2.h
index 9463f8604fbc..978af44f76c7 100644
--- a/drivers/media/i2c/alvium-csi2.h
+++ b/drivers/media/i2c/alvium-csi2.h
@@ -31,10 +31,7 @@
 #define REG_BCRM_REG_ADDR_R				CCI_REG16(0x0014)
 
 #define REG_BCRM_FEATURE_INQUIRY_R			REG_BCRM_V4L2_64BIT(0x0008)
-#define REG_BCRM_DEVICE_FW_SPEC_VERSION_R		REG_BCRM_V4L2_8BIT(0x0010)
-#define REG_BCRM_DEVICE_FW_MAJOR_VERSION_R		REG_BCRM_V4L2_8BIT(0x0011)
-#define REG_BCRM_DEVICE_FW_MINOR_VERSION_R		REG_BCRM_V4L2_16BIT(0x0012)
-#define REG_BCRM_DEVICE_FW_PATCH_VERSION_R		REG_BCRM_V4L2_32BIT(0x0014)
+#define REG_BCRM_DEVICE_FW				REG_BCRM_V4L2_64BIT(0x0010)
 #define REG_BCRM_WRITE_HANDSHAKE_RW			REG_BCRM_V4L2_8BIT(0x0018)
 
 /* Streaming Control Registers */
@@ -66,7 +63,7 @@
 #define REG_BCRM_ACQUISITION_FRAME_RATE_MIN_R		REG_BCRM_V4L2_64BIT(0x0098)
 #define REG_BCRM_ACQUISITION_FRAME_RATE_MAX_R		REG_BCRM_V4L2_64BIT(0x00a0)
 #define REG_BCRM_ACQUISITION_FRAME_RATE_INC_R		REG_BCRM_V4L2_64BIT(0x00a8)
-#define REG_BCRM_ACQUISITION_FRAME_RATE_ENABLE_RW	REG_BCRM_V4L2_8BIT(0x00b0)
+#define REG_BCRM_ACQUISITION_FRAME_RATE_EN_RW		REG_BCRM_V4L2_8BIT(0x00b0)
 
 #define REG_BCRM_FRAME_START_TRIGGER_MODE_RW		REG_BCRM_V4L2_8BIT(0x00b4)
 #define REG_BCRM_FRAME_START_TRIGGER_SOURCE_RW		REG_BCRM_V4L2_8BIT(0x00b8)
@@ -205,6 +202,15 @@
 
 #define ALVIUM_LP2HS_DELAY_MS				100
 
+#define BCRM_DEVICE_FW_MAJOR_MASK			GENMASK_ULL(15, 8)
+#define BCRM_DEVICE_FW_MAJOR_SHIFT			8
+#define BCRM_DEVICE_FW_MINOR_MASK			GENMASK_ULL(31, 16)
+#define BCRM_DEVICE_FW_MINOR_SHIFT			16
+#define BCRM_DEVICE_FW_PATCH_MASK			GENMASK_ULL(63, 32)
+#define BCRM_DEVICE_FW_PATCH_SHIFT			32
+#define BCRM_DEVICE_FW_SPEC_MASK			GENMASK_ULL(7, 0)
+#define BCRM_DEVICE_FW_SPEC_SHIFT			0
+
 enum alvium_bcrm_mode {
 	ALVIUM_BCM_MODE,
 	ALVIUM_GENCP_MODE,
diff --git a/drivers/media/i2c/ar0521.c b/drivers/media/i2c/ar0521.c
index 09331cf95c62..24873149096c 100644
--- a/drivers/media/i2c/ar0521.c
+++ b/drivers/media/i2c/ar0521.c
@@ -255,10 +255,10 @@ static u32 calc_pll(struct ar0521_dev *sensor, u32 freq, u16 *pre_ptr, u16 *mult
 			continue; /* Minimum value */
 		if (new_mult > 254)
 			break; /* Maximum, larger pre won't work either */
-		if (sensor->extclk_freq * (u64)new_mult < AR0521_PLL_MIN *
+		if (sensor->extclk_freq * (u64)new_mult < (u64)AR0521_PLL_MIN *
 		    new_pre)
 			continue;
-		if (sensor->extclk_freq * (u64)new_mult > AR0521_PLL_MAX *
+		if (sensor->extclk_freq * (u64)new_mult > (u64)AR0521_PLL_MAX *
 		    new_pre)
 			break; /* Larger pre won't work either */
 		new_pll = div64_round_up(sensor->extclk_freq * (u64)new_mult,
@@ -835,21 +835,30 @@ static const struct initial_reg {
 	     be(0x0707)), /* 3F44: couple k factor 2 */
 };
 
-static int ar0521_power_off(struct device *dev)
+static void __ar0521_power_off(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct ar0521_dev *sensor = to_ar0521_dev(sd);
 	int i;
 
-	clk_disable_unprepare(sensor->extclk);
-
 	if (sensor->reset_gpio)
-		gpiod_set_value(sensor->reset_gpio, 1); /* assert RESET signal */
+		/* assert RESET signal */
+		gpiod_set_value_cansleep(sensor->reset_gpio, 1);
 
 	for (i = ARRAY_SIZE(ar0521_supply_names) - 1; i >= 0; i--) {
 		if (sensor->supplies[i])
 			regulator_disable(sensor->supplies[i]);
 	}
+}
+
+static int ar0521_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ar0521_dev *sensor = to_ar0521_dev(sd);
+
+	clk_disable_unprepare(sensor->extclk);
+	__ar0521_power_off(dev);
+
 	return 0;
 }
 
@@ -878,7 +887,7 @@ static int ar0521_power_on(struct device *dev)
 
 	if (sensor->reset_gpio)
 		/* deassert RESET signal */
-		gpiod_set_value(sensor->reset_gpio, 0);
+		gpiod_set_value_cansleep(sensor->reset_gpio, 0);
 	usleep_range(4500, 5000); /* min 45000 clocks */
 
 	for (cnt = 0; cnt < ARRAY_SIZE(initial_regs); cnt++) {
@@ -908,7 +917,8 @@ static int ar0521_power_on(struct device *dev)
 
 	return 0;
 off:
-	ar0521_power_off(dev);
+	clk_disable_unprepare(sensor->extclk);
+	__ar0521_power_off(dev);
 	return ret;
 }
 
diff --git a/drivers/media/i2c/bt819.c b/drivers/media/i2c/bt819.c
index b4a25cc996dc..f97245f91f88 100644
--- a/drivers/media/i2c/bt819.c
+++ b/drivers/media/i2c/bt819.c
@@ -457,9 +457,9 @@ static void bt819_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id bt819_id[] = {
-	{ "bt819a", 0 },
-	{ "bt817a", 0 },
-	{ "bt815a", 0 },
+	{ "bt819a" },
+	{ "bt817a" },
+	{ "bt815a" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, bt819_id);
diff --git a/drivers/media/i2c/bt856.c b/drivers/media/i2c/bt856.c
index 814acbd6a5a8..6852aa47cafb 100644
--- a/drivers/media/i2c/bt856.c
+++ b/drivers/media/i2c/bt856.c
@@ -230,7 +230,7 @@ static void bt856_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id bt856_id[] = {
-	{ "bt856", 0 },
+	{ "bt856" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, bt856_id);
diff --git a/drivers/media/i2c/bt866.c b/drivers/media/i2c/bt866.c
index dada059cbce4..a2cc34d35ed2 100644
--- a/drivers/media/i2c/bt866.c
+++ b/drivers/media/i2c/bt866.c
@@ -197,7 +197,7 @@ static void bt866_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id bt866_id[] = {
-	{ "bt866", 0 },
+	{ "bt866" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, bt866_id);
diff --git a/drivers/media/i2c/ccs-pll.c b/drivers/media/i2c/ccs-pll.c
index cf8858cb13d4..4eb83636e102 100644
--- a/drivers/media/i2c/ccs-pll.c
+++ b/drivers/media/i2c/ccs-pll.c
@@ -75,11 +75,11 @@ static const char *pll_string(unsigned int which)
 
 #define PLL_FL(f) CCS_PLL_FLAG_##f
 
-static void print_pll(struct device *dev, struct ccs_pll *pll)
+static void print_pll(struct device *dev, const struct ccs_pll *pll)
 {
 	const struct {
-		struct ccs_pll_branch_fr *fr;
-		struct ccs_pll_branch_bk *bk;
+		const struct ccs_pll_branch_fr *fr;
+		const struct ccs_pll_branch_bk *bk;
 		unsigned int which;
 	} branches[] = {
 		{ &pll->vt_fr, &pll->vt_bk, PLL_VT },
@@ -123,10 +123,15 @@ static void print_pll(struct device *dev, struct ccs_pll *pll)
 		pll->pixel_rate_pixel_array);
 	dev_dbg(dev, "pixel rate on CSI-2 bus:\t%u\n",
 		pll->pixel_rate_csi);
+}
 
-	dev_dbg(dev, "flags%s%s%s%s%s%s%s%s%s\n",
+static void print_pll_flags(struct device *dev, struct ccs_pll *pll)
+{
+	dev_dbg(dev, "PLL flags%s%s%s%s%s%s%s%s%s%s%s\n",
+		pll->flags & PLL_FL(OP_PIX_CLOCK_PER_LANE) ? " op-pix-clock-per-lane" : "",
+		pll->flags & PLL_FL(EVEN_PLL_MULTIPLIER) ? " even-pll-multiplier" : "",
+		pll->flags & PLL_FL(NO_OP_CLOCKS) ? " no-op-clocks" : "",
 		pll->flags & PLL_FL(LANE_SPEED_MODEL) ? " lane-speed" : "",
-		pll->flags & PLL_FL(LINK_DECOUPLED) ? " link-decoupled" : "",
 		pll->flags & PLL_FL(EXT_IP_PLL_DIVIDER) ?
 		" ext-ip-pll-divider" : "",
 		pll->flags & PLL_FL(FLEXIBLE_OP_PIX_CLK_DIV) ?
@@ -150,10 +155,10 @@ static u32 op_pix_ddr(u32 flags)
 
 static int check_fr_bounds(struct device *dev,
 			   const struct ccs_pll_limits *lim,
-			   struct ccs_pll *pll, unsigned int which)
+			   const struct ccs_pll *pll, unsigned int which)
 {
 	const struct ccs_pll_branch_limits_fr *lim_fr;
-	struct ccs_pll_branch_fr *pll_fr;
+	const struct ccs_pll_branch_fr *pll_fr;
 	const char *s = pll_string(which);
 	int rval;
 
@@ -190,10 +195,10 @@ static int check_fr_bounds(struct device *dev,
 
 static int check_bk_bounds(struct device *dev,
 			   const struct ccs_pll_limits *lim,
-			   struct ccs_pll *pll, unsigned int which)
+			   const struct ccs_pll *pll, unsigned int which)
 {
 	const struct ccs_pll_branch_limits_bk *lim_bk;
-	struct ccs_pll_branch_bk *pll_bk;
+	const struct ccs_pll_branch_bk *pll_bk;
 	const char *s = pll_string(which);
 	int rval;
 
@@ -230,7 +235,7 @@ static int check_bk_bounds(struct device *dev,
 	return rval;
 }
 
-static int check_ext_bounds(struct device *dev, struct ccs_pll *pll)
+static int check_ext_bounds(struct device *dev, const struct ccs_pll *pll)
 {
 	if (!(pll->flags & CCS_PLL_FLAG_FIFO_DERATING) &&
 	    pll->pixel_rate_pixel_array > pll->pixel_rate_csi) {
@@ -311,14 +316,24 @@ __ccs_pll_calculate_vt_tree(struct device *dev,
 	more_mul *= DIV_ROUND_UP(lim_fr->min_pll_multiplier, mul * more_mul);
 	dev_dbg(dev, "more_mul2: %u\n", more_mul);
 
-	pll_fr->pll_multiplier = mul * more_mul;
+	if (pll->flags & CCS_PLL_FLAG_EVEN_PLL_MULTIPLIER &&
+	    (mul & 1) && (more_mul & 1))
+		more_mul <<= 1;
 
-	if (pll_fr->pll_multiplier * pll_fr->pll_ip_clk_freq_hz >
-	    lim_fr->max_pll_op_clk_freq_hz)
+	pll_fr->pll_multiplier = mul * more_mul;
+	if (pll_fr->pll_multiplier > lim_fr->max_pll_multiplier) {
+		dev_dbg(dev, "pll multiplier %u too high\n",
+			pll_fr->pll_multiplier);
 		return -EINVAL;
+	}
 
 	pll_fr->pll_op_clk_freq_hz =
 		pll_fr->pll_ip_clk_freq_hz * pll_fr->pll_multiplier;
+	if (pll_fr->pll_op_clk_freq_hz > lim_fr->max_pll_op_clk_freq_hz) {
+		dev_dbg(dev, "too high OP clock %u\n",
+			pll_fr->pll_op_clk_freq_hz);
+		return -EINVAL;
+	}
 
 	vt_div = div * more_mul;
 
@@ -397,6 +412,8 @@ static int ccs_pll_calculate_vt_tree(struct device *dev,
 	min_pre_pll_clk_div = max_t(u16, min_pre_pll_clk_div,
 				    pll->ext_clk_freq_hz /
 				    lim_fr->max_pll_ip_clk_freq_hz);
+	if (!(pll->flags & CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER))
+		min_pre_pll_clk_div = clk_div_even(min_pre_pll_clk_div);
 
 	dev_dbg(dev, "vt min/max_pre_pll_clk_div: %u,%u\n",
 		min_pre_pll_clk_div, max_pre_pll_clk_div);
@@ -432,10 +449,11 @@ static int ccs_pll_calculate_vt_tree(struct device *dev,
 		return 0;
 	}
 
+	dev_dbg(dev, "unable to compute VT pre_pll divisor\n");
 	return -EINVAL;
 }
 
-static void
+static int
 ccs_pll_calculate_vt(struct device *dev, const struct ccs_pll_limits *lim,
 		     const struct ccs_pll_branch_limits_bk *op_lim_bk,
 		     struct ccs_pll *pll, struct ccs_pll_branch_fr *pll_fr,
@@ -558,6 +576,8 @@ ccs_pll_calculate_vt(struct device *dev, const struct ccs_pll_limits *lim,
 		if (best_pix_div < SHRT_MAX >> 1)
 			break;
 	}
+	if (best_pix_div == SHRT_MAX >> 1)
+		return -EINVAL;
 
 	pll->vt_bk.sys_clk_div = DIV_ROUND_UP(vt_div, best_pix_div);
 	pll->vt_bk.pix_clk_div = best_pix_div;
@@ -570,6 +590,8 @@ ccs_pll_calculate_vt(struct device *dev, const struct ccs_pll_limits *lim,
 out_calc_pixel_rate:
 	pll->pixel_rate_pixel_array =
 		pll->vt_bk.pix_clk_freq_hz * pll->vt_lanes;
+
+	return 0;
 }
 
 /*
@@ -659,6 +681,10 @@ ccs_pll_calculate_op(struct device *dev, const struct ccs_pll_limits *lim,
 	if (!is_one_or_even(i))
 		i <<= 1;
 
+	if (pll->flags & CCS_PLL_FLAG_EVEN_PLL_MULTIPLIER &&
+	    mul & 1 && i & 1)
+		i <<= 1;
+
 	dev_dbg(dev, "final more_mul: %u\n", i);
 	if (i > more_mul_max) {
 		dev_dbg(dev, "final more_mul is bad, max %u\n", more_mul_max);
@@ -716,6 +742,8 @@ int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
 	u32 i;
 	int rval = -EINVAL;
 
+	print_pll_flags(dev, pll);
+
 	if (!(pll->flags & CCS_PLL_FLAG_LANE_SPEED_MODEL)) {
 		pll->op_lanes = 1;
 		pll->vt_lanes = 1;
@@ -792,7 +820,7 @@ int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
 		op_lim_fr->min_pre_pll_clk_div, op_lim_fr->max_pre_pll_clk_div);
 	max_op_pre_pll_clk_div =
 		min_t(u16, op_lim_fr->max_pre_pll_clk_div,
-		      clk_div_even(pll->ext_clk_freq_hz /
+		      DIV_ROUND_UP(pll->ext_clk_freq_hz,
 				   op_lim_fr->min_pll_ip_clk_freq_hz));
 	min_op_pre_pll_clk_div =
 		max_t(u16, op_lim_fr->min_pre_pll_clk_div,
@@ -815,6 +843,8 @@ int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
 			      one_or_more(
 				      DIV_ROUND_UP(op_lim_fr->max_pll_op_clk_freq_hz,
 						   pll->ext_clk_freq_hz))));
+	if (!(pll->flags & CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER))
+		min_op_pre_pll_clk_div = clk_div_even(min_op_pre_pll_clk_div);
 	dev_dbg(dev, "pll_op check: min / max op_pre_pll_clk_div: %u / %u\n",
 		min_op_pre_pll_clk_div, max_op_pre_pll_clk_div);
 
@@ -843,8 +873,10 @@ int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
 		if (pll->flags & CCS_PLL_FLAG_DUAL_PLL)
 			break;
 
-		ccs_pll_calculate_vt(dev, lim, op_lim_bk, pll, op_pll_fr,
-				     op_pll_bk, cphy, phy_const);
+		rval = ccs_pll_calculate_vt(dev, lim, op_lim_bk, pll, op_pll_fr,
+					    op_pll_bk, cphy, phy_const);
+		if (rval)
+			continue;
 
 		rval = check_bk_bounds(dev, lim, pll, PLL_VT);
 		if (rval)
@@ -857,8 +889,7 @@ int ccs_pll_calculate(struct device *dev, const struct ccs_pll_limits *lim,
 	}
 
 	if (rval) {
-		dev_dbg(dev, "unable to compute pre_pll divisor\n");
-
+		dev_dbg(dev, "unable to compute OP pre_pll divisor\n");
 		return rval;
 	}
 
diff --git a/drivers/media/i2c/ccs-pll.h b/drivers/media/i2c/ccs-pll.h
index 6eb1b1c68e1e..e22903931e72 100644
--- a/drivers/media/i2c/ccs-pll.h
+++ b/drivers/media/i2c/ccs-pll.h
@@ -18,19 +18,40 @@
 #define CCS_PLL_BUS_TYPE_CSI2_DPHY				0x00
 #define CCS_PLL_BUS_TYPE_CSI2_CPHY				0x01
 
-/* Old SMIA and implementation specific flags */
-/* op pix clock is for all lanes in total normally */
+/* Old SMIA and implementation specific flags. */
+/* OP PIX clock is for all lanes in total normally. */
 #define CCS_PLL_FLAG_OP_PIX_CLOCK_PER_LANE			BIT(0)
-#define CCS_PLL_FLAG_NO_OP_CLOCKS				BIT(1)
+/* If set, the PLL multipliers are required to be even. */
+#define CCS_PLL_FLAG_EVEN_PLL_MULTIPLIER			BIT(3)
+
 /* CCS PLL flags */
+
+/* The sensor doesn't have OP clocks at all. */
+#define CCS_PLL_FLAG_NO_OP_CLOCKS				BIT(1)
+/* System speed model if this flag is unset. */
 #define CCS_PLL_FLAG_LANE_SPEED_MODEL				BIT(2)
-#define CCS_PLL_FLAG_LINK_DECOUPLED				BIT(3)
+/* If set, the pre-PLL divider may have odd values, too. */
 #define CCS_PLL_FLAG_EXT_IP_PLL_DIVIDER				BIT(4)
+/*
+ * If set, the OP PIX clock doesn't have to exactly match with data rate, it may
+ * be higher. See "OP Domain Formulas" in MIPI CCS 1.1 spec.
+ */
 #define CCS_PLL_FLAG_FLEXIBLE_OP_PIX_CLK_DIV			BIT(5)
+/* If set, the VT domain may run faster than the OP domain. */
 #define CCS_PLL_FLAG_FIFO_DERATING				BIT(6)
+/* If set, the VT domain may run slower than the OP domain. */
 #define CCS_PLL_FLAG_FIFO_OVERRATING				BIT(7)
+/* If set, the PLL tree has two PLLs instead of one. */
 #define CCS_PLL_FLAG_DUAL_PLL					BIT(8)
+/*
+ * If set, the OP SYS clock is a dual data rate clock, transferring two bits per
+ * cycle instead of one.
+ */
 #define CCS_PLL_FLAG_OP_SYS_DDR					BIT(9)
+/*
+ * If set, the OP PIX clock is a dual data rate clock, transferring two pixels
+ * per cycle instead of one.
+ */
 #define CCS_PLL_FLAG_OP_PIX_DDR					BIT(10)
 
 /**
diff --git a/drivers/media/i2c/ccs/ccs-core.c b/drivers/media/i2c/ccs/ccs-core.c
index e1ae0f9fad43..487bcabb4a19 100644
--- a/drivers/media/i2c/ccs/ccs-core.c
+++ b/drivers/media/i2c/ccs/ccs-core.c
@@ -1354,8 +1354,10 @@ static int ccs_change_cci_addr(struct ccs_sensor *sensor)
 
 	client->addr = sensor->hwcfg.i2c_addr_dfl;
 
-	rval = ccs_write(sensor, CCI_ADDRESS_CTRL,
-			 sensor->hwcfg.i2c_addr_alt << 1);
+	rval = read_poll_timeout(ccs_write, rval, !rval, CCS_RESET_DELAY_US,
+				 CCS_RESET_TIMEOUT_US, false, sensor,
+				 CCI_ADDRESS_CTRL,
+				 sensor->hwcfg.i2c_addr_alt << 1);
 	if (rval)
 		return rval;
 
@@ -1575,44 +1577,38 @@ static int ccs_power_on(struct device *dev)
 		if (ccsdev->flags & CCS_DEVICE_FLAG_IS_SMIA)
 			sleep = SMIAPP_RESET_DELAY(sensor->hwcfg.ext_clk);
 		else
-			sleep = 5000;
+			sleep = CCS_RESET_DELAY_US;
 
 		usleep_range(sleep, sleep);
 	}
 
 	/*
-	 * Failures to respond to the address change command have been noticed.
-	 * Those failures seem to be caused by the sensor requiring a longer
-	 * boot time than advertised. An additional 10ms delay seems to work
-	 * around the issue, but the SMIA++ I2C write retry hack makes the delay
-	 * unnecessary. The failures need to be investigated to find a proper
-	 * fix, and a delay will likely need to be added here if the I2C write
-	 * retry hack is reverted before the root cause of the boot time issue
-	 * is found.
+	 * Some devices take longer than the spec-defined time to respond
+	 * after reset. Try until some time has passed before flagging it
+	 * an error.
 	 */
-
 	if (!sensor->reset && !sensor->xshutdown) {
-		u8 retry = 100;
 		u32 reset;
 
-		rval = ccs_write(sensor, SOFTWARE_RESET, CCS_SOFTWARE_RESET_ON);
+		rval = read_poll_timeout(ccs_write, rval, !rval,
+					 CCS_RESET_DELAY_US,
+					 CCS_RESET_TIMEOUT_US,
+					 false, sensor, SOFTWARE_RESET,
+					 CCS_SOFTWARE_RESET_ON);
 		if (rval < 0) {
 			dev_err(dev, "software reset failed\n");
 			goto out_cci_addr_fail;
 		}
 
-		do {
-			rval = ccs_read(sensor, SOFTWARE_RESET, &reset);
-			reset = !rval && reset == CCS_SOFTWARE_RESET_OFF;
-			if (reset)
-				break;
-
-			usleep_range(1000, 2000);
-		} while (--retry);
-
-		if (!reset) {
-			dev_err(dev, "software reset failed\n");
-			rval = -EIO;
+		rval = read_poll_timeout(ccs_read, rval,
+					 !rval &&
+						reset == CCS_SOFTWARE_RESET_OFF,
+					 CCS_RESET_DELAY_US,
+					 CCS_RESET_TIMEOUT_US, false, sensor,
+					 SOFTWARE_RESET, &reset);
+		if (rval < 0) {
+			dev_err_probe(dev, rval,
+				      "failed to respond after reset\n");
 			goto out_cci_addr_fail;
 		}
 	}
@@ -2857,10 +2853,6 @@ static int ccs_identify_module(struct ccs_sensor *sensor)
 		break;
 	}
 
-	if (i >= ARRAY_SIZE(ccs_module_idents))
-		dev_warn(&client->dev,
-			 "no quirks for this module; let's hope it's fully compliant\n");
-
 	dev_dbg(&client->dev, "the sensor is called %s\n", minfo->name);
 
 	return 0;
@@ -3131,8 +3123,6 @@ static int ccs_get_hwconfig(struct ccs_sensor *sensor, struct device *dev)
 
 	rval = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
 					&hwcfg->ext_clk);
-	if (rval)
-		dev_info(dev, "can't get clock-frequency\n");
 
 	dev_dbg(dev, "clk %u, mode %u\n", hwcfg->ext_clk,
 		hwcfg->csi_signalling_mode);
@@ -3335,9 +3325,11 @@ static int ccs_probe(struct i2c_client *client)
 
 	rval = request_firmware(&fw, filename, &client->dev);
 	if (!rval) {
-		ccs_data_parse(&sensor->sdata, fw->data, fw->size, &client->dev,
-			       true);
+		rval = ccs_data_parse(&sensor->sdata, fw->data, fw->size,
+				      &client->dev, true);
 		release_firmware(fw);
+		if (rval)
+			goto out_power_off;
 	}
 
 	if (!(ccsdev->flags & CCS_DEVICE_FLAG_IS_SMIA) ||
@@ -3351,9 +3343,11 @@ static int ccs_probe(struct i2c_client *client)
 
 		rval = request_firmware(&fw, filename, &client->dev);
 		if (!rval) {
-			ccs_data_parse(&sensor->mdata, fw->data, fw->size,
-				       &client->dev, true);
+			rval = ccs_data_parse(&sensor->mdata, fw->data,
+					      fw->size, &client->dev, true);
 			release_firmware(fw);
+			if (rval)
+				goto out_release_sdata;
 		}
 	}
 
@@ -3447,7 +3441,6 @@ static int ccs_probe(struct i2c_client *client)
 				CCS_LIM(sensor, NUM_OF_VT_LANES) + 1;
 			sensor->pll.op_lanes =
 				CCS_LIM(sensor, NUM_OF_OP_LANES) + 1;
-			sensor->pll.flags |= CCS_PLL_FLAG_LINK_DECOUPLED;
 		} else {
 			sensor->pll.vt_lanes = sensor->pll.csi2.lanes;
 			sensor->pll.op_lanes = sensor->pll.csi2.lanes;
@@ -3562,19 +3555,20 @@ static int ccs_probe(struct i2c_client *client)
 out_disable_runtime_pm:
 	pm_runtime_put_noidle(&client->dev);
 	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
 
 out_cleanup:
 	ccs_cleanup(sensor);
 
+out_free_ccs_limits:
+	kfree(sensor->ccs_limits);
+
 out_release_mdata:
 	kvfree(sensor->mdata.backing);
 
 out_release_sdata:
 	kvfree(sensor->sdata.backing);
 
-out_free_ccs_limits:
-	kfree(sensor->ccs_limits);
-
 out_power_off:
 	ccs_power_off(&client->dev);
 	mutex_destroy(&sensor->mutex);
@@ -3591,9 +3585,10 @@ static void ccs_remove(struct i2c_client *client)
 	v4l2_async_unregister_subdev(subdev);
 
 	pm_runtime_disable(&client->dev);
-	if (!pm_runtime_status_suspended(&client->dev))
+	if (!pm_runtime_status_suspended(&client->dev)) {
 		ccs_power_off(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
 
 	for (i = 0; i < sensor->ssds_used; i++)
 		v4l2_device_unregister_subdev(&sensor->ssds[i].sd);
diff --git a/drivers/media/i2c/ccs/ccs-data.c b/drivers/media/i2c/ccs/ccs-data.c
index 08400edf77ce..f469afcea680 100644
--- a/drivers/media/i2c/ccs/ccs-data.c
+++ b/drivers/media/i2c/ccs/ccs-data.c
@@ -10,6 +10,7 @@
 #include <linux/limits.h>
 #include <linux/mm.h>
 #include <linux/slab.h>
+#include <linux/string.h>
 
 #include "ccs-data-defs.h"
 
@@ -97,7 +98,7 @@ ccs_data_parse_length_specifier(const struct __ccs_data_length_specifier *__len,
 		plen = ((size_t)
 			(__len3->length[0] &
 			 ((1 << CCS_DATA_LENGTH_SPECIFIER_SIZE_SHIFT) - 1))
-			<< 16) + (__len3->length[0] << 8) + __len3->length[1];
+			<< 16) + (__len3->length[1] << 8) + __len3->length[2];
 		break;
 	}
 	default:
@@ -948,15 +949,15 @@ int ccs_data_parse(struct ccs_data_container *ccsdata, const void *data,
 
 	rval = __ccs_data_parse(&bin, ccsdata, data, len, dev, verbose);
 	if (rval)
-		return rval;
+		goto out_cleanup;
 
 	rval = bin_backing_alloc(&bin);
 	if (rval)
-		return rval;
+		goto out_cleanup;
 
 	rval = __ccs_data_parse(&bin, ccsdata, data, len, dev, false);
 	if (rval)
-		goto out_free;
+		goto out_cleanup;
 
 	if (verbose && ccsdata->version)
 		print_ccs_data_version(dev, ccsdata->version);
@@ -965,15 +966,17 @@ int ccs_data_parse(struct ccs_data_container *ccsdata, const void *data,
 		rval = -EPROTO;
 		dev_dbg(dev, "parsing mismatch; base %p; now %p; end %p\n",
 			bin.base, bin.now, bin.end);
-		goto out_free;
+		goto out_cleanup;
 	}
 
 	ccsdata->backing = bin.base;
 
 	return 0;
 
-out_free:
+out_cleanup:
 	kvfree(bin.base);
+	memset(ccsdata, 0, sizeof(*ccsdata));
+	dev_warn(dev, "failed to parse CCS static data: %d\n", rval);
 
 	return rval;
 }
diff --git a/drivers/media/i2c/ccs/ccs-quirk.c b/drivers/media/i2c/ccs/ccs-quirk.c
index e3d4c7a275bc..e48a4fa1f5dd 100644
--- a/drivers/media/i2c/ccs/ccs-quirk.c
+++ b/drivers/media/i2c/ccs/ccs-quirk.c
@@ -190,8 +190,7 @@ static int jt8ev1_post_streamoff(struct ccs_sensor *sensor)
 
 static int jt8ev1_init(struct ccs_sensor *sensor)
 {
-	sensor->pll.flags |= CCS_PLL_FLAG_LANE_SPEED_MODEL |
-		CCS_PLL_FLAG_LINK_DECOUPLED;
+	sensor->pll.flags |= CCS_PLL_FLAG_LANE_SPEED_MODEL;
 	sensor->pll.vt_lanes = 1;
 	sensor->pll.op_lanes = sensor->pll.csi2.lanes;
 
diff --git a/drivers/media/i2c/ccs/ccs-reg-access.c b/drivers/media/i2c/ccs/ccs-reg-access.c
index ed79075505e6..fd36889ccc1d 100644
--- a/drivers/media/i2c/ccs/ccs-reg-access.c
+++ b/drivers/media/i2c/ccs/ccs-reg-access.c
@@ -9,7 +9,7 @@
  * Contact: Sakari Ailus <sakari.ailus@linux.intel.com>
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/delay.h>
 #include <linux/i2c.h>
@@ -210,7 +210,6 @@ int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val)
  */
 int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val)
 {
-	unsigned int retries = 10;
 	int rval;
 
 	rval = ccs_call_quirk(sensor, reg_access, true, &reg, &val);
@@ -219,13 +218,7 @@ int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val)
 	if (rval < 0)
 		return rval;
 
-	rval = 0;
-	do {
-		if (cci_write(sensor->regmap, reg, val, &rval))
-			fsleep(1000);
-	} while (rval && --retries);
-
-	return rval;
+	return cci_write(sensor->regmap, reg, val, NULL);
 }
 
 #define MAX_WRITE_LEN	32U
diff --git a/drivers/media/i2c/ccs/ccs-reg-access.h b/drivers/media/i2c/ccs/ccs-reg-access.h
index 78c43f92d99a..4b56b21a26b5 100644
--- a/drivers/media/i2c/ccs/ccs-reg-access.h
+++ b/drivers/media/i2c/ccs/ccs-reg-access.h
@@ -21,16 +21,13 @@
 
 struct ccs_sensor;
 
-int ccs_read_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 *val);
 int ccs_read_addr(struct ccs_sensor *sensor, u32 reg, u32 *val);
 int ccs_read_addr_8only(struct ccs_sensor *sensor, u32 reg, u32 *val);
 int ccs_read_addr_noconv(struct ccs_sensor *sensor, u32 reg, u32 *val);
-int ccs_write_addr_no_quirk(struct ccs_sensor *sensor, u32 reg, u32 val);
 int ccs_write_addr(struct ccs_sensor *sensor, u32 reg, u32 val);
 int ccs_write_data_regs(struct ccs_sensor *sensor, struct ccs_reg *regs,
 			size_t num_regs);
 
-unsigned int ccs_reg_width(u32 reg);
 u32 ccs_reg_conv(struct ccs_sensor *sensor, u32 reg, u32 val);
 
 #define ccs_read(sensor, reg_name, val) \
diff --git a/drivers/media/i2c/ccs/ccs.h b/drivers/media/i2c/ccs/ccs.h
index 096573845a10..0726c4687f0f 100644
--- a/drivers/media/i2c/ccs/ccs.h
+++ b/drivers/media/i2c/ccs/ccs.h
@@ -43,6 +43,8 @@
 #define SMIAPP_RESET_DELAY(clk)				\
 	(1000 +	(SMIAPP_RESET_DELAY_CLOCKS * 1000	\
 		 + (clk) / 1000 - 1) / ((clk) / 1000))
+#define CCS_RESET_DELAY_US		5000
+#define CCS_RESET_TIMEOUT_US		1000000
 
 #define CCS_COLOUR_COMPONENTS		4
 
diff --git a/drivers/media/i2c/cs3308.c b/drivers/media/i2c/cs3308.c
index 61afa3d799d2..078e0066ce4b 100644
--- a/drivers/media/i2c/cs3308.c
+++ b/drivers/media/i2c/cs3308.c
@@ -109,7 +109,7 @@ static void cs3308_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id cs3308_id[] = {
-	{ "cs3308", 0 },
+	{ "cs3308" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, cs3308_id);
diff --git a/drivers/media/i2c/cs5345.c b/drivers/media/i2c/cs5345.c
index 3019a132e079..3a9797a50e82 100644
--- a/drivers/media/i2c/cs5345.c
+++ b/drivers/media/i2c/cs5345.c
@@ -189,7 +189,7 @@ static void cs5345_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id cs5345_id[] = {
-	{ "cs5345", 0 },
+	{ "cs5345" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, cs5345_id);
diff --git a/drivers/media/i2c/cs53l32a.c b/drivers/media/i2c/cs53l32a.c
index 82881b79e730..c4cad3293905 100644
--- a/drivers/media/i2c/cs53l32a.c
+++ b/drivers/media/i2c/cs53l32a.c
@@ -200,7 +200,7 @@ static void cs53l32a_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id cs53l32a_id[] = {
-	{ "cs53l32a", 0 },
+	{ "cs53l32a" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, cs53l32a_id);
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index 04461c893d90..a90a9e5705a0 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -3964,7 +3964,7 @@ static void cx25840_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id cx25840_id[] = {
-	{ "cx25840", 0 },
+	{ "cx25840" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, cx25840_id);
diff --git a/drivers/media/i2c/ds90ub913.c b/drivers/media/i2c/ds90ub913.c
index ca9bb29dab89..6d3f8617ef13 100644
--- a/drivers/media/i2c/ds90ub913.c
+++ b/drivers/media/i2c/ds90ub913.c
@@ -8,10 +8,10 @@
  * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
  */
 
+#include <linux/bitfield.h>
 #include <linux/clk-provider.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
-#include <linux/fwnode.h>
 #include <linux/gpio/driver.h>
 #include <linux/i2c-atr.h>
 #include <linux/i2c.h>
@@ -118,31 +118,66 @@ static const struct ub913_format_info *ub913_find_format(u32 incode)
 	return NULL;
 }
 
-static int ub913_read(const struct ub913_data *priv, u8 reg, u8 *val)
+static int ub913_read(const struct ub913_data *priv, u8 reg, u8 *val,
+		      int *err)
 {
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	ret = regmap_read(priv->regmap, reg, &v);
-	if (ret < 0) {
+	if (ret) {
 		dev_err(&priv->client->dev,
 			"Cannot read register 0x%02x: %d!\n", reg, ret);
-		return ret;
+		goto out;
 	}
 
 	*val = v;
-	return 0;
+
+out:
+	if (ret && err)
+		*err = ret;
+
+	return ret;
 }
 
-static int ub913_write(const struct ub913_data *priv, u8 reg, u8 val)
+static int ub913_write(const struct ub913_data *priv, u8 reg, u8 val,
+		       int *err)
 {
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	ret = regmap_write(priv->regmap, reg, val);
 	if (ret < 0)
 		dev_err(&priv->client->dev,
 			"Cannot write register 0x%02x: %d!\n", reg, ret);
 
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int ub913_update_bits(const struct ub913_data *priv, u8 reg, u8 mask,
+			     u8 val, int *err)
+{
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	ret = regmap_update_bits(priv->regmap, reg, mask, val);
+	if (ret < 0)
+		dev_err(&priv->client->dev,
+			"Cannot update register 0x%02x %d!\n", reg, ret);
+
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -190,7 +225,7 @@ static int ub913_gpiochip_probe(struct ub913_data *priv)
 	int ret;
 
 	/* Initialize GPIOs 0 and 1 to local control, tri-state */
-	ub913_write(priv, UB913_REG_GPIO_CFG(0), 0);
+	ub913_write(priv, UB913_REG_GPIO_CFG(0), 0, NULL);
 
 	gc->label = dev_name(dev);
 	gc->parent = dev;
@@ -436,10 +471,10 @@ static int ub913_set_fmt(struct v4l2_subdev *sd,
 	if (!fmt)
 		return -EINVAL;
 
-	format->format.code = finfo->outcode;
-
 	*fmt = format->format;
 
+	fmt->code = finfo->outcode;
+
 	return 0;
 }
 
@@ -468,25 +503,41 @@ static int ub913_log_status(struct v4l2_subdev *sd)
 {
 	struct ub913_data *priv = sd_to_ub913(sd);
 	struct device *dev = &priv->client->dev;
-	u8 v = 0, v1 = 0, v2 = 0;
+	u8 v, v1, v2;
+	int ret;
+
+	ret = ub913_read(priv, UB913_REG_MODE_SEL, &v, NULL);
+	if (ret)
+		return ret;
 
-	ub913_read(priv, UB913_REG_MODE_SEL, &v);
 	dev_info(dev, "MODE_SEL %#02x\n", v);
 
-	ub913_read(priv, UB913_REG_CRC_ERRORS_LSB, &v1);
-	ub913_read(priv, UB913_REG_CRC_ERRORS_MSB, &v2);
+	ub913_read(priv, UB913_REG_CRC_ERRORS_LSB, &v1, &ret);
+	ub913_read(priv, UB913_REG_CRC_ERRORS_MSB, &v2, &ret);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CRC errors %u\n", v1 | (v2 << 8));
 
 	/* clear CRC errors */
-	ub913_read(priv, UB913_REG_GENERAL_CFG, &v);
+	ub913_read(priv, UB913_REG_GENERAL_CFG, &v, &ret);
 	ub913_write(priv, UB913_REG_GENERAL_CFG,
-		    v | UB913_REG_GENERAL_CFG_CRC_ERR_RESET);
-	ub913_write(priv, UB913_REG_GENERAL_CFG, v);
+		    v | UB913_REG_GENERAL_CFG_CRC_ERR_RESET, &ret);
+	ub913_write(priv, UB913_REG_GENERAL_CFG, v, &ret);
+
+	if (ret)
+		return ret;
+
+	ret = ub913_read(priv, UB913_REG_GENERAL_STATUS, &v, NULL);
+	if (ret)
+		return ret;
 
-	ub913_read(priv, UB913_REG_GENERAL_STATUS, &v);
 	dev_info(dev, "GENERAL_STATUS %#02x\n", v);
 
-	ub913_read(priv, UB913_REG_PLL_OVR, &v);
+	ret = ub913_read(priv, UB913_REG_PLL_OVR, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "PLL_OVR %#02x\n", v);
 
 	return 0;
@@ -642,11 +693,11 @@ static int ub913_i2c_master_init(struct ub913_data *priv)
 	scl_high = div64_u64((u64)scl_high * ref, 1000000000);
 	scl_low = div64_u64((u64)scl_low * ref, 1000000000);
 
-	ret = ub913_write(priv, UB913_REG_SCL_HIGH_TIME, scl_high);
+	ret = ub913_write(priv, UB913_REG_SCL_HIGH_TIME, scl_high, NULL);
 	if (ret)
 		return ret;
 
-	ret = ub913_write(priv, UB913_REG_SCL_LOW_TIME, scl_low);
+	ret = ub913_write(priv, UB913_REG_SCL_LOW_TIME, scl_low, NULL);
 	if (ret)
 		return ret;
 
@@ -656,6 +707,7 @@ static int ub913_i2c_master_init(struct ub913_data *priv)
 static int ub913_add_i2c_adapter(struct ub913_data *priv)
 {
 	struct device *dev = &priv->client->dev;
+	struct i2c_atr_adap_desc desc = { };
 	struct fwnode_handle *i2c_handle;
 	int ret;
 
@@ -663,8 +715,12 @@ static int ub913_add_i2c_adapter(struct ub913_data *priv)
 	if (!i2c_handle)
 		return 0;
 
-	ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
-				  dev, i2c_handle);
+	desc.chan_id = priv->plat_data->port;
+	desc.parent = dev;
+	desc.bus_handle = i2c_handle;
+	desc.num_aliases = 0;
+
+	ret = i2c_atr_add_adapter(priv->plat_data->atr, &desc);
 
 	fwnode_handle_put(i2c_handle);
 
@@ -715,7 +771,7 @@ static int ub913_hw_init(struct ub913_data *priv)
 	int ret;
 	u8 v;
 
-	ret = ub913_read(priv, UB913_REG_MODE_SEL, &v);
+	ret = ub913_read(priv, UB913_REG_MODE_SEL, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -733,10 +789,13 @@ static int ub913_hw_init(struct ub913_data *priv)
 	if (ret)
 		return dev_err_probe(dev, ret, "i2c master init failed\n");
 
-	ub913_read(priv, UB913_REG_GENERAL_CFG, &v);
-	v &= ~UB913_REG_GENERAL_CFG_PCLK_RISING;
-	v |= priv->pclk_polarity_rising ? UB913_REG_GENERAL_CFG_PCLK_RISING : 0;
-	ub913_write(priv, UB913_REG_GENERAL_CFG, v);
+	ret = ub913_update_bits(priv, UB913_REG_GENERAL_CFG,
+				UB913_REG_GENERAL_CFG_PCLK_RISING,
+				FIELD_PREP(UB913_REG_GENERAL_CFG_PCLK_RISING,
+					   priv->pclk_polarity_rising), NULL);
+
+	if (ret)
+		return ret;
 
 	return 0;
 }
@@ -793,7 +852,6 @@ static void ub913_subdev_uninit(struct ub913_data *priv)
 	v4l2_async_unregister_subdev(&priv->sd);
 	ub913_v4l2_nf_unregister(priv);
 	v4l2_subdev_cleanup(&priv->sd);
-	fwnode_handle_put(priv->sd.fwnode);
 	media_entity_cleanup(&priv->sd.entity);
 }
 
@@ -877,7 +935,10 @@ static void ub913_remove(struct i2c_client *client)
 	ub913_gpiochip_remove(priv);
 }
 
-static const struct i2c_device_id ub913_id[] = { { "ds90ub913a-q1", 0 }, {} };
+static const struct i2c_device_id ub913_id[] = {
+	{ "ds90ub913a-q1" },
+	{}
+};
 MODULE_DEVICE_TABLE(i2c, ub913_id);
 
 static const struct of_device_id ub913_dt_ids[] = {
@@ -901,4 +962,4 @@ MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Texas Instruments DS90UB913 FPD-Link III Serializer Driver");
 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
-MODULE_IMPORT_NS(I2C_ATR);
+MODULE_IMPORT_NS("I2C_ATR");
diff --git a/drivers/media/i2c/ds90ub953.c b/drivers/media/i2c/ds90ub953.c
index 16f88db14981..59bd92388845 100644
--- a/drivers/media/i2c/ds90ub953.c
+++ b/drivers/media/i2c/ds90ub953.c
@@ -11,7 +11,6 @@
 #include <linux/clk-provider.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
-#include <linux/fwnode.h>
 #include <linux/gpio/driver.h>
 #include <linux/i2c-atr.h>
 #include <linux/i2c.h>
@@ -24,11 +23,12 @@
 
 #include <media/i2c/ds90ub9xx.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-subdev.h>
 
+#include "ds90ub953.h"
+
 #define UB953_PAD_SINK			0
 #define UB953_PAD_SOURCE		1
 
@@ -36,86 +36,6 @@
 
 #define UB953_DEFAULT_CLKOUT_RATE	25000000UL
 
-#define UB953_REG_RESET_CTL			0x01
-#define UB953_REG_RESET_CTL_DIGITAL_RESET_1	BIT(1)
-#define UB953_REG_RESET_CTL_DIGITAL_RESET_0	BIT(0)
-
-#define UB953_REG_GENERAL_CFG			0x02
-#define UB953_REG_GENERAL_CFG_CONT_CLK		BIT(6)
-#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT	4
-#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_MASK	GENMASK(5, 4)
-#define UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE	BIT(1)
-#define UB953_REG_GENERAL_CFG_I2C_STRAP_MODE	BIT(0)
-
-#define UB953_REG_MODE_SEL			0x03
-#define UB953_REG_MODE_SEL_MODE_DONE		BIT(3)
-#define UB953_REG_MODE_SEL_MODE_OVERRIDE	BIT(4)
-#define UB953_REG_MODE_SEL_MODE_MASK		GENMASK(2, 0)
-
-#define UB953_REG_CLKOUT_CTRL0			0x06
-#define UB953_REG_CLKOUT_CTRL1			0x07
-
-#define UB953_REG_SCL_HIGH_TIME			0x0b
-#define UB953_REG_SCL_LOW_TIME			0x0c
-
-#define UB953_REG_LOCAL_GPIO_DATA		0x0d
-#define UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(n)		BIT(4 + (n))
-#define UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(n)	BIT(0 + (n))
-
-#define UB953_REG_GPIO_INPUT_CTRL		0x0e
-#define UB953_REG_GPIO_INPUT_CTRL_OUT_EN(n)	BIT(4 + (n))
-#define UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(n)	BIT(0 + (n))
-
-#define UB953_REG_REV_MASK_ID			0x50
-#define UB953_REG_GENERAL_STATUS		0x52
-
-#define UB953_REG_GPIO_PIN_STS			0x53
-#define UB953_REG_GPIO_PIN_STS_GPIO_STS(n)	BIT(0 + (n))
-
-#define UB953_REG_BIST_ERR_CNT			0x54
-#define UB953_REG_CRC_ERR_CNT1			0x55
-#define UB953_REG_CRC_ERR_CNT2			0x56
-
-#define UB953_REG_CSI_ERR_CNT			0x5c
-#define UB953_REG_CSI_ERR_STATUS		0x5d
-#define UB953_REG_CSI_ERR_DLANE01		0x5e
-#define UB953_REG_CSI_ERR_DLANE23		0x5f
-#define UB953_REG_CSI_ERR_CLK_LANE		0x60
-#define UB953_REG_CSI_PKT_HDR_VC_ID		0x61
-#define UB953_REG_PKT_HDR_WC_LSB		0x62
-#define UB953_REG_PKT_HDR_WC_MSB		0x63
-#define UB953_REG_CSI_ECC			0x64
-
-#define UB953_REG_IND_ACC_CTL			0xb0
-#define UB953_REG_IND_ACC_ADDR			0xb1
-#define UB953_REG_IND_ACC_DATA			0xb2
-
-#define UB953_REG_FPD3_RX_ID(n)			(0xf0 + (n))
-#define UB953_REG_FPD3_RX_ID_LEN		6
-
-/* Indirect register blocks */
-#define UB953_IND_TARGET_PAT_GEN		0x00
-#define UB953_IND_TARGET_FPD3_TX		0x01
-#define UB953_IND_TARGET_DIE_ID			0x02
-
-#define UB953_IND_PGEN_CTL			0x01
-#define UB953_IND_PGEN_CTL_PGEN_ENABLE		BIT(0)
-#define UB953_IND_PGEN_CFG			0x02
-#define UB953_IND_PGEN_CSI_DI			0x03
-#define UB953_IND_PGEN_LINE_SIZE1		0x04
-#define UB953_IND_PGEN_LINE_SIZE0		0x05
-#define UB953_IND_PGEN_BAR_SIZE1		0x06
-#define UB953_IND_PGEN_BAR_SIZE0		0x07
-#define UB953_IND_PGEN_ACT_LPF1			0x08
-#define UB953_IND_PGEN_ACT_LPF0			0x09
-#define UB953_IND_PGEN_TOT_LPF1			0x0a
-#define UB953_IND_PGEN_TOT_LPF0			0x0b
-#define UB953_IND_PGEN_LINE_PD1			0x0c
-#define UB953_IND_PGEN_LINE_PD0			0x0d
-#define UB953_IND_PGEN_VBP			0x0e
-#define UB953_IND_PGEN_VFP			0x0f
-#define UB953_IND_PGEN_COLOR(n)			(0x10 + (n)) /* n <= 15 */
-
 /* Note: Only sync mode supported for now */
 enum ub953_mode {
 	/* FPD-Link III CSI-2 synchronous mode */
@@ -183,11 +103,14 @@ static inline struct ub953_data *sd_to_ub953(struct v4l2_subdev *sd)
  * HW Access
  */
 
-static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val)
+static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val, int *err)
 {
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_read(priv->regmap, reg, &v);
@@ -202,13 +125,19 @@ static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub953_write(struct ub953_data *priv, u8 reg, u8 val)
+static int ub953_write(struct ub953_data *priv, u8 reg, u8 val, int *err)
 {
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_write(priv->regmap, reg, val);
@@ -218,6 +147,9 @@ static int ub953_write(struct ub953_data *priv, u8 reg, u8 val)
 
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -242,11 +174,15 @@ static int ub953_select_ind_reg_block(struct ub953_data *priv, u8 block)
 }
 
 __maybe_unused
-static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
+static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val,
+			  int *err)
 {
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub953_select_ind_reg_block(priv, block);
@@ -256,7 +192,7 @@ static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
 	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
 	if (ret) {
 		dev_err(&priv->client->dev,
-			"Write to IND_ACC_ADDR failed when reading %u:%x02x: %d\n",
+			"Write to IND_ACC_ADDR failed when reading %u:0x%02x: %d\n",
 			block, reg, ret);
 		goto out_unlock;
 	}
@@ -264,7 +200,7 @@ static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
 	ret = regmap_read(priv->regmap, UB953_REG_IND_ACC_DATA, &v);
 	if (ret) {
 		dev_err(&priv->client->dev,
-			"Write to IND_ACC_DATA failed when reading %u:%x02x: %d\n",
+			"Write to IND_ACC_DATA failed when reading %u:0x%02x: %d\n",
 			block, reg, ret);
 		goto out_unlock;
 	}
@@ -274,14 +210,21 @@ static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
 __maybe_unused
-static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val)
+static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val,
+			   int *err)
 {
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub953_select_ind_reg_block(priv, block);
@@ -291,7 +234,7 @@ static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val)
 	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
 	if (ret) {
 		dev_err(&priv->client->dev,
-			"Write to IND_ACC_ADDR failed when writing %u:%x02x: %d\n",
+			"Write to IND_ACC_ADDR failed when writing %u:0x%02x: %d\n",
 			block, reg, ret);
 		goto out_unlock;
 	}
@@ -299,13 +242,16 @@ static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val)
 	ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_DATA, val);
 	if (ret) {
 		dev_err(&priv->client->dev,
-			"Write to IND_ACC_DATA failed when writing %u:%x02x\n: %d\n",
+			"Write to IND_ACC_DATA failed when writing %u:0x%02x: %d\n",
 			block, reg, ret);
 	}
 
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -318,7 +264,7 @@ static int ub953_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
 	int ret;
 	u8 v;
 
-	ret = ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &v);
+	ret = ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -364,7 +310,7 @@ static int ub953_gpio_get(struct gpio_chip *gc, unsigned int offset)
 	int ret;
 	u8 v;
 
-	ret = ub953_read(priv, UB953_REG_GPIO_PIN_STS, &v);
+	ret = ub953_read(priv, UB953_REG_GPIO_PIN_STS, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -398,8 +344,13 @@ static int ub953_gpiochip_probe(struct ub953_data *priv)
 	int ret;
 
 	/* Set all GPIOs to local input mode */
-	ub953_write(priv, UB953_REG_LOCAL_GPIO_DATA, 0);
-	ub953_write(priv, UB953_REG_GPIO_INPUT_CTRL, 0xf);
+	ret = ub953_write(priv, UB953_REG_LOCAL_GPIO_DATA, 0, NULL);
+	if (ret)
+		return ret;
+
+	ret = ub953_write(priv, UB953_REG_GPIO_INPUT_CTRL, 0xf, NULL);
+	if (ret)
+		return ret;
 
 	gc->label = dev_name(dev);
 	gc->parent = dev;
@@ -600,53 +551,95 @@ static int ub953_log_status(struct v4l2_subdev *sd)
 {
 	struct ub953_data *priv = sd_to_ub953(sd);
 	struct device *dev = &priv->client->dev;
-	u8 v = 0, v1 = 0, v2 = 0;
-	unsigned int i;
 	char id[UB953_REG_FPD3_RX_ID_LEN];
-	u8 gpio_local_data = 0;
-	u8 gpio_input_ctrl = 0;
-	u8 gpio_pin_sts = 0;
+	u8 gpio_local_data;
+	u8 gpio_input_ctrl;
+	u8 gpio_pin_sts;
+	unsigned int i;
+	u8 v, v1, v2;
+	int ret;
 
-	for (i = 0; i < sizeof(id); i++)
-		ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i]);
+	for (i = 0; i < sizeof(id); i++) {
+		ret = ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i], NULL);
+		if (ret)
+			return ret;
+	}
 
 	dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
 
-	ub953_read(priv, UB953_REG_GENERAL_STATUS, &v);
+	ret = ub953_read(priv, UB953_REG_GENERAL_STATUS, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "GENERAL_STATUS %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1);
-	ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2);
+	ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1, &ret);
+	ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2, &ret);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CRC error count %u\n", v1 | (v2 << 8));
 
-	ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v);
+	/* Clear CRC error counter */
+	if (v1 || v2)
+		regmap_update_bits(priv->regmap, UB953_REG_BC_CTRL,
+				   UB953_REG_BC_CTRL_CRC_ERR_CLR,
+				   UB953_REG_BC_CTRL_CRC_ERR_CLR);
+
+	ret = ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI error count %u\n", v);
 
-	ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI_ERR_STATUS %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI_ERR_DLANE01 %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI_ERR_DLANE23 %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI_ERR_CLK_LANE %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI packet header VC %u ID %u\n", v >> 6, v & 0x3f);
 
-	ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1);
-	ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2);
+	ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1, &ret);
+	ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2, &ret);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI packet header WC %u\n", (v2 << 8) | v1);
 
-	ub953_read(priv, UB953_REG_CSI_ECC, &v);
+	ret = ub953_read(priv, UB953_REG_CSI_ECC, &v, NULL);
+	if (ret)
+		return ret;
+
 	dev_info(dev, "CSI ECC %#02x\n", v);
 
-	ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data);
-	ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl);
-	ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts);
+	ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data, &ret);
+	ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl, &ret);
+	ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts, &ret);
+	if (ret)
+		return ret;
 
 	for (i = 0; i < UB953_NUM_GPIOS; i++) {
 		dev_info(dev,
@@ -717,8 +710,6 @@ static const struct v4l2_subdev_pad_ops ub953_pad_ops = {
 
 static const struct v4l2_subdev_core_ops ub953_subdev_core_ops = {
 	.log_status = ub953_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_ops ub953_subdev_ops = {
@@ -832,11 +823,11 @@ static int ub953_i2c_master_init(struct ub953_data *priv)
 	scl_high = div64_u64((u64)scl_high * ref, 1000000000) - 5;
 	scl_low = div64_u64((u64)scl_low * ref, 1000000000) - 5;
 
-	ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high);
+	ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high, NULL);
 	if (ret)
 		return ret;
 
-	ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low);
+	ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low, NULL);
 	if (ret)
 		return ret;
 
@@ -961,10 +952,11 @@ static void ub953_calc_clkout_params(struct ub953_data *priv,
 	clkout_data->rate = clkout_rate;
 }
 
-static void ub953_write_clkout_regs(struct ub953_data *priv,
-				    const struct ub953_clkout_data *clkout_data)
+static int ub953_write_clkout_regs(struct ub953_data *priv,
+				   const struct ub953_clkout_data *clkout_data)
 {
 	u8 clkout_ctrl0, clkout_ctrl1;
+	int ret;
 
 	if (priv->hw_data->is_ub971)
 		clkout_ctrl0 = clkout_data->m;
@@ -974,8 +966,15 @@ static void ub953_write_clkout_regs(struct ub953_data *priv,
 
 	clkout_ctrl1 = clkout_data->n;
 
-	ub953_write(priv, UB953_REG_CLKOUT_CTRL0, clkout_ctrl0);
-	ub953_write(priv, UB953_REG_CLKOUT_CTRL1, clkout_ctrl1);
+	ret = ub953_write(priv, UB953_REG_CLKOUT_CTRL0, clkout_ctrl0, NULL);
+	if (ret)
+		return ret;
+
+	ret = ub953_write(priv, UB953_REG_CLKOUT_CTRL1, clkout_ctrl1, NULL);
+	if (ret)
+		return ret;
+
+	return 0;
 }
 
 static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
@@ -990,13 +989,13 @@ static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
 	u64 rate;
 	int ret;
 
-	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0);
+	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0, NULL);
 	if (ret) {
 		dev_err(dev, "Failed to read CLKOUT_CTRL0: %d\n", ret);
 		return 0;
 	}
 
-	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1);
+	ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1, NULL);
 	if (ret) {
 		dev_err(dev, "Failed to read CLKOUT_CTRL1: %d\n", ret);
 		return 0;
@@ -1055,9 +1054,7 @@ static int ub953_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
 	dev_dbg(&priv->client->dev, "%s %lu (requested %lu)\n", __func__,
 		clkout_data.rate, rate);
 
-	ub953_write_clkout_regs(priv, &clkout_data);
-
-	return 0;
+	return ub953_write_clkout_regs(priv, &clkout_data);
 }
 
 static const struct clk_ops ub953_clkout_ops = {
@@ -1082,7 +1079,9 @@ static int ub953_register_clkout(struct ub953_data *priv)
 
 	/* Initialize clkout to 25MHz by default */
 	ub953_calc_clkout_params(priv, UB953_DEFAULT_CLKOUT_RATE, &clkout_data);
-	ub953_write_clkout_regs(priv, &clkout_data);
+	ret = ub953_write_clkout_regs(priv, &clkout_data);
+	if (ret)
+		return ret;
 
 	priv->clkout_clk_hw.init = &init;
 
@@ -1103,6 +1102,7 @@ static int ub953_register_clkout(struct ub953_data *priv)
 static int ub953_add_i2c_adapter(struct ub953_data *priv)
 {
 	struct device *dev = &priv->client->dev;
+	struct i2c_atr_adap_desc desc = { };
 	struct fwnode_handle *i2c_handle;
 	int ret;
 
@@ -1110,8 +1110,12 @@ static int ub953_add_i2c_adapter(struct ub953_data *priv)
 	if (!i2c_handle)
 		return 0;
 
-	ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
-				  dev, i2c_handle);
+	desc.chan_id = priv->plat_data->port;
+	desc.parent = dev;
+	desc.bus_handle = i2c_handle;
+	desc.num_aliases = 0;
+
+	ret = i2c_atr_add_adapter(priv->plat_data->atr, &desc);
 
 	fwnode_handle_put(i2c_handle);
 
@@ -1172,7 +1176,7 @@ static int ub953_hw_init(struct ub953_data *priv)
 	int ret;
 	u8 v;
 
-	ret = ub953_read(priv, UB953_REG_MODE_SEL, &v);
+	ret = ub953_read(priv, UB953_REG_MODE_SEL, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -1212,13 +1216,13 @@ static int ub953_hw_init(struct ub953_data *priv)
 		return dev_err_probe(dev, -EINVAL,
 				     "clkin required for non-sync ext mode\n");
 
-	ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v);
+	ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v, NULL);
 	if (ret)
 		return dev_err_probe(dev, ret, "Failed to read revision");
 
 	dev_info(dev, "Found %s rev/mask %#04x\n", priv->hw_data->model, v);
 
-	ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v);
+	ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -1229,12 +1233,22 @@ static int ub953_hw_init(struct ub953_data *priv)
 	if (ret)
 		return dev_err_probe(dev, ret, "i2c init failed\n");
 
-	ub953_write(priv, UB953_REG_GENERAL_CFG,
-		    (priv->non_continous_clk ? 0 : UB953_REG_GENERAL_CFG_CONT_CLK) |
-		    ((priv->num_data_lanes - 1) << UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT) |
-		    UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE);
+	v = 0;
+	v |= priv->non_continous_clk ? 0 : UB953_REG_GENERAL_CFG_CONT_CLK;
+	v |= (priv->num_data_lanes - 1) <<
+		UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT;
+	v |= UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE;
 
-	return 0;
+	ret = ub953_write(priv, UB953_REG_GENERAL_CFG, v, NULL);
+	if (ret)
+		return ret;
+
+	v = 1U << UB953_REG_I2C_CONTROL2_SDA_OUTPUT_SETUP_SHIFT;
+	v |= UB953_REG_I2C_CONTROL2_BUS_SPEEDUP;
+
+	ret = ub953_write(priv, UB953_REG_I2C_CONTROL2, v, NULL);
+
+	return ret;
 }
 
 static int ub953_subdev_init(struct ub953_data *priv)
@@ -1246,7 +1260,7 @@ static int ub953_subdev_init(struct ub953_data *priv)
 	priv->sd.internal_ops = &ub953_internal_ops;
 
 	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			  V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
+			  V4L2_SUBDEV_FL_STREAMS;
 	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 	priv->sd.entity.ops = &ub953_entity_ops;
 
@@ -1291,7 +1305,6 @@ static void ub953_subdev_uninit(struct ub953_data *priv)
 	v4l2_async_unregister_subdev(&priv->sd);
 	ub953_v4l2_notifier_unregister(priv);
 	v4l2_subdev_cleanup(&priv->sd);
-	fwnode_handle_put(priv->sd.fwnode);
 	media_entity_cleanup(&priv->sd.entity);
 }
 
@@ -1428,4 +1441,4 @@ MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV CSI-2 Serializers Driver");
 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
-MODULE_IMPORT_NS(I2C_ATR);
+MODULE_IMPORT_NS("I2C_ATR");
diff --git a/drivers/media/i2c/ds90ub953.h b/drivers/media/i2c/ds90ub953.h
new file mode 100644
index 000000000000..97a6b3af326e
--- /dev/null
+++ b/drivers/media/i2c/ds90ub953.h
@@ -0,0 +1,104 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+
+#ifndef __MEDIA_I2C_DS90UB953_H__
+#define __MEDIA_I2C_DS90UB953_H__
+
+#include <linux/types.h>
+
+#define UB953_REG_RESET_CTL			0x01
+#define UB953_REG_RESET_CTL_DIGITAL_RESET_1	BIT(1)
+#define UB953_REG_RESET_CTL_DIGITAL_RESET_0	BIT(0)
+
+#define UB953_REG_GENERAL_CFG			0x02
+#define UB953_REG_GENERAL_CFG_CONT_CLK		BIT(6)
+#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT	4
+#define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_MASK	GENMASK(5, 4)
+#define UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE	BIT(1)
+#define UB953_REG_GENERAL_CFG_I2C_STRAP_MODE	BIT(0)
+
+#define UB953_REG_MODE_SEL			0x03
+#define UB953_REG_MODE_SEL_MODE_DONE		BIT(3)
+#define UB953_REG_MODE_SEL_MODE_OVERRIDE	BIT(4)
+#define UB953_REG_MODE_SEL_MODE_MASK		GENMASK(2, 0)
+
+#define UB953_REG_CLKOUT_CTRL0			0x06
+#define UB953_REG_CLKOUT_CTRL1			0x07
+
+#define UB953_REG_I2C_CONTROL2			0x0a
+#define UB953_REG_I2C_CONTROL2_SDA_OUTPUT_SETUP_SHIFT	4
+#define UB953_REG_I2C_CONTROL2_BUS_SPEEDUP	BIT(1)
+
+#define UB953_REG_SCL_HIGH_TIME			0x0b
+#define UB953_REG_SCL_LOW_TIME			0x0c
+
+#define UB953_REG_LOCAL_GPIO_DATA		0x0d
+#define UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(n)		BIT(4 + (n))
+#define UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(n)	BIT(0 + (n))
+
+#define UB953_REG_GPIO_INPUT_CTRL		0x0e
+#define UB953_REG_GPIO_INPUT_CTRL_OUT_EN(n)	BIT(4 + (n))
+#define UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(n)	BIT(0 + (n))
+
+#define UB953_REG_BC_CTRL			0x49
+#define UB953_REG_BC_CTRL_CRC_ERR_CLR		BIT(3)
+
+#define UB953_REG_REV_MASK_ID			0x50
+#define UB953_REG_GENERAL_STATUS		0x52
+
+#define UB953_REG_GPIO_PIN_STS			0x53
+#define UB953_REG_GPIO_PIN_STS_GPIO_STS(n)	BIT(0 + (n))
+
+#define UB953_REG_BIST_ERR_CNT			0x54
+#define UB953_REG_CRC_ERR_CNT1			0x55
+#define UB953_REG_CRC_ERR_CNT2			0x56
+
+#define UB953_REG_CSI_ERR_CNT			0x5c
+#define UB953_REG_CSI_ERR_STATUS		0x5d
+#define UB953_REG_CSI_ERR_DLANE01		0x5e
+#define UB953_REG_CSI_ERR_DLANE23		0x5f
+#define UB953_REG_CSI_ERR_CLK_LANE		0x60
+#define UB953_REG_CSI_PKT_HDR_VC_ID		0x61
+#define UB953_REG_PKT_HDR_WC_LSB		0x62
+#define UB953_REG_PKT_HDR_WC_MSB		0x63
+#define UB953_REG_CSI_ECC			0x64
+
+#define UB953_REG_IND_ACC_CTL			0xb0
+#define UB953_REG_IND_ACC_ADDR			0xb1
+#define UB953_REG_IND_ACC_DATA			0xb2
+
+#define UB953_REG_FPD3_RX_ID(n)			(0xf0 + (n))
+#define UB953_REG_FPD3_RX_ID_LEN		6
+
+/* Indirect register blocks */
+#define UB953_IND_TARGET_PAT_GEN		0x00
+#define UB953_IND_TARGET_ANALOG			0x01
+#define UB953_IND_TARGET_DIE_ID			0x02
+
+#define UB953_IND_PGEN_CTL			0x01
+#define UB953_IND_PGEN_CTL_PGEN_ENABLE		BIT(0)
+#define UB953_IND_PGEN_CFG			0x02
+#define UB953_IND_PGEN_CSI_DI			0x03
+#define UB953_IND_PGEN_LINE_SIZE1		0x04
+#define UB953_IND_PGEN_LINE_SIZE0		0x05
+#define UB953_IND_PGEN_BAR_SIZE1		0x06
+#define UB953_IND_PGEN_BAR_SIZE0		0x07
+#define UB953_IND_PGEN_ACT_LPF1			0x08
+#define UB953_IND_PGEN_ACT_LPF0			0x09
+#define UB953_IND_PGEN_TOT_LPF1			0x0a
+#define UB953_IND_PGEN_TOT_LPF0			0x0b
+#define UB953_IND_PGEN_LINE_PD1			0x0c
+#define UB953_IND_PGEN_LINE_PD0			0x0d
+#define UB953_IND_PGEN_VBP			0x0e
+#define UB953_IND_PGEN_VFP			0x0f
+#define UB953_IND_PGEN_COLOR(n)			(0x10 + (n)) /* n <= 15 */
+
+#define UB953_IND_ANA_TEMP_DYNAMIC_CFG		0x4b
+#define UB953_IND_ANA_TEMP_DYNAMIC_CFG_OV	BIT(5)
+#define UB953_IND_ANA_TEMP_STATIC_CFG		0x4c
+#define UB953_IND_ANA_TEMP_STATIC_CFG_MASK	GENMASK(6, 4)
+
+/* UB971 Registers */
+
+#define UB971_ENH_BC_CHK			0x4b
+
+#endif /* __MEDIA_I2C_DS90UB953_H__ */
diff --git a/drivers/media/i2c/ds90ub960.c b/drivers/media/i2c/ds90ub960.c
index ffe5f25f8647..082fc62b0f5b 100644
--- a/drivers/media/i2c/ds90ub960.c
+++ b/drivers/media/i2c/ds90ub960.c
@@ -27,6 +27,7 @@
  */
 
 #include <linux/bitops.h>
+#include <linux/cleanup.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/fwnode.h>
@@ -43,16 +44,27 @@
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
+#include <linux/units.h>
 #include <linux/workqueue.h>
 
 #include <media/i2c/ds90ub9xx.h>
 #include <media/mipi-csi2.h>
 #include <media/v4l2-ctrls.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
-#define MHZ(v) ((u32)((v) * 1000000U))
+#include "ds90ub953.h"
+
+#define MHZ(v) ((u32)((v) * HZ_PER_MHZ))
+
+/*
+ * If this is defined, the i2c addresses from UB960_DEBUG_I2C_RX_ID to
+ * UB960_DEBUG_I2C_RX_ID + 3 can be used to access the paged RX port registers
+ * directly.
+ *
+ * Only for debug purposes.
+ */
+/* #define UB960_DEBUG_I2C_RX_ID	0x40 */
 
 #define UB960_POLL_TIME_MS	500
 
@@ -234,13 +246,17 @@
 #define UB960_RR_BIST_ERR_COUNT			0x57
 
 #define UB960_RR_BCC_CONFIG			0x58
+#define UB960_RR_BCC_CONFIG_BC_ALWAYS_ON	BIT(4)
+#define UB960_RR_BCC_CONFIG_AUTO_ACK_ALL	BIT(5)
 #define UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH	BIT(6)
 #define UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK	GENMASK(2, 0)
 
 #define UB960_RR_DATAPATH_CTL1			0x59
 #define UB960_RR_DATAPATH_CTL2			0x5a
 #define UB960_RR_SER_ID				0x5b
+#define UB960_RR_SER_ID_FREEZE_DEVICE_ID	BIT(0)
 #define UB960_RR_SER_ALIAS_ID			0x5c
+#define UB960_RR_SER_ALIAS_ID_AUTO_ACK		BIT(0)
 
 /* For these two register sets: n < UB960_MAX_PORT_ALIASES */
 #define UB960_RR_SLAVE_ID(n)			(0x5d + (n))
@@ -298,8 +314,6 @@
 
 #define UB960_XR_REFCLK_FREQ			0xa5	/* UB960 */
 
-#define UB960_RR_VC_ID_MAP(x)			(0xa0 + (x)) /* UB9702 */
-
 #define UB960_SR_IND_ACC_CTL			0xb0
 #define UB960_SR_IND_ACC_CTL_IA_AUTO_INC	BIT(1)
 
@@ -312,9 +326,6 @@
 #define UB960_SR_FV_MIN_TIME			0xbc
 #define UB960_SR_GPIO_PD_CTL			0xbe
 
-#define UB960_SR_FPD_RATE_CFG			0xc2	/* UB9702 */
-#define UB960_SR_CSI_PLL_DIV			0xc9	/* UB9702 */
-
 #define UB960_RR_PORT_DEBUG			0xd0
 #define UB960_RR_AEQ_CTL2			0xd2
 #define UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR		BIT(2)
@@ -345,17 +356,15 @@
 #define UB960_RR_SEN_INT_RISE_STS		0xde
 #define UB960_RR_SEN_INT_FALL_STS		0xdf
 
-#define UB960_RR_CHANNEL_MODE			0xe4	/* UB9702 */
 
 #define UB960_SR_FPD3_RX_ID(n)			(0xf0 + (n))
 #define UB960_SR_FPD3_RX_ID_LEN			6
 
-#define UB960_SR_I2C_RX_ID(n)			(0xf8 + (n)) /* < UB960_FPD_RX_NPORTS */
+#define UB960_SR_I2C_RX_ID(n)			(0xf8 + (n))
 
 /* Indirect register blocks */
 #define UB960_IND_TARGET_PAT_GEN		0x00
 #define UB960_IND_TARGET_RX_ANA(n)		(0x01 + (n))
-#define UB960_IND_TARGET_CSI_CSIPLL_REG_1	0x92	/* UB9702 */
 #define UB960_IND_TARGET_CSI_ANA		0x07
 
 /* UB960_IR_PGEN_*: Indirect Registers for Test Pattern Generator */
@@ -387,6 +396,49 @@
 #define UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY	BIT(3)
 #define UB960_IR_RX_ANA_STROBE_SET_DATA_DELAY_MASK	GENMASK(2, 0)
 
+/* UB9702 Registers */
+
+#define UB9702_SR_CSI_EXCLUSIVE_FWD2		0x3c
+#define UB9702_SR_REFCLK_FREQ			0x3d
+#define UB9702_RR_RX_CTL_1			0x80
+#define UB9702_RR_RX_CTL_2			0x87
+#define UB9702_RR_VC_ID_MAP(x)			(0xa0 + (x))
+#define UB9702_SR_FPD_RATE_CFG			0xc2
+#define UB9702_SR_CSI_PLL_DIV			0xc9
+#define UB9702_RR_RX_SM_SEL_2			0xd4
+#define UB9702_RR_CHANNEL_MODE			0xe4
+
+#define UB9702_IND_TARGET_SAR_ADC		0x0a
+
+#define UB9702_IR_RX_ANA_FPD_BC_CTL0		0x04
+#define UB9702_IR_RX_ANA_FPD_BC_CTL1		0x0d
+#define UB9702_IR_RX_ANA_FPD_BC_CTL2		0x1b
+#define UB9702_IR_RX_ANA_SYSTEM_INIT_REG0	0x21
+#define UB9702_IR_RX_ANA_AEQ_ALP_SEL6		0x27
+#define UB9702_IR_RX_ANA_AEQ_ALP_SEL7		0x28
+#define UB9702_IR_RX_ANA_AEQ_ALP_SEL10		0x2b
+#define UB9702_IR_RX_ANA_AEQ_ALP_SEL11		0x2c
+#define UB9702_IR_RX_ANA_EQ_ADAPT_CTRL		0x2e
+#define UB9702_IR_RX_ANA_AEQ_CFG_1		0x34
+#define UB9702_IR_RX_ANA_AEQ_CFG_2		0x4d
+#define UB9702_IR_RX_ANA_GAIN_CTRL_0		0x71
+#define UB9702_IR_RX_ANA_GAIN_CTRL_0		0x71
+#define UB9702_IR_RX_ANA_VGA_CTRL_SEL_1		0x72
+#define UB9702_IR_RX_ANA_VGA_CTRL_SEL_2		0x73
+#define UB9702_IR_RX_ANA_VGA_CTRL_SEL_3		0x74
+#define UB9702_IR_RX_ANA_VGA_CTRL_SEL_6		0x77
+#define UB9702_IR_RX_ANA_AEQ_CFG_3		0x79
+#define UB9702_IR_RX_ANA_AEQ_CFG_4		0x85
+#define UB9702_IR_RX_ANA_EQ_CTRL_SEL_15		0x87
+#define UB9702_IR_RX_ANA_EQ_CTRL_SEL_24		0x90
+#define UB9702_IR_RX_ANA_EQ_CTRL_SEL_38		0x9e
+#define UB9702_IR_RX_ANA_FPD3_CDR_CTRL_SEL_5	0xa5
+#define UB9702_IR_RX_ANA_FPD3_AEQ_CTRL_SEL_1	0xa8
+#define UB9702_IR_RX_ANA_EQ_OVERRIDE_CTRL	0xf0
+#define UB9702_IR_RX_ANA_VGA_CTRL_SEL_8		0xf1
+
+#define UB9702_IR_CSI_ANA_CSIPLL_REG_1		0x92
+
 /* EQ related */
 
 #define UB960_MIN_AEQ_STROBE_POS -7
@@ -440,7 +492,9 @@ struct ub960_rxport {
 		struct fwnode_handle *fwnode;
 		struct i2c_client *client;
 		unsigned short alias; /* I2C alias (lower 7 bits) */
+		short addr; /* Local I2C address (lower 7 bits) */
 		struct ds90ub9xx_platform_data pdata;
+		struct regmap *regmap;
 	} ser;
 
 	enum ub960_rxport_mode  rx_mode;
@@ -468,7 +522,9 @@ struct ub960_rxport {
 		};
 	} eq;
 
-	const struct i2c_client *aliased_clients[UB960_MAX_PORT_ALIASES];
+	/* lock for aliased_addrs and associated registers */
+	struct mutex aliased_addrs_lock;
+	u16 aliased_addrs[UB960_MAX_PORT_ALIASES];
 };
 
 struct ub960_asd {
@@ -569,11 +625,23 @@ struct ub960_format_info {
 };
 
 static const struct ub960_format_info ub960_formats[] = {
+	{ .code = MEDIA_BUS_FMT_RGB888_1X24, .bpp = 24, .datatype = MIPI_CSI2_DT_RGB888, },
+
 	{ .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
 	{ .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
 	{ .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
 	{ .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, .datatype = MIPI_CSI2_DT_YUV422_8B, },
 
+	{ .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, .datatype = MIPI_CSI2_DT_RAW8, },
+	{ .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, .datatype = MIPI_CSI2_DT_RAW8, },
+	{ .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, .datatype = MIPI_CSI2_DT_RAW8, },
+	{ .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, .datatype = MIPI_CSI2_DT_RAW8, },
+
+	{ .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, .datatype = MIPI_CSI2_DT_RAW10, },
+	{ .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, .datatype = MIPI_CSI2_DT_RAW10, },
+	{ .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, .datatype = MIPI_CSI2_DT_RAW10, },
+	{ .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, .datatype = MIPI_CSI2_DT_RAW10, },
+
 	{ .code = MEDIA_BUS_FMT_SBGGR12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
 	{ .code = MEDIA_BUS_FMT_SGBRG12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
 	{ .code = MEDIA_BUS_FMT_SGRBG12_1X12, .bpp = 12, .datatype = MIPI_CSI2_DT_RAW12, },
@@ -592,16 +660,76 @@ static const struct ub960_format_info *ub960_find_format(u32 code)
 	return NULL;
 }
 
+struct ub960_rxport_iter {
+	unsigned int nport;
+	struct ub960_rxport *rxport;
+};
+
+enum ub960_iter_flags {
+	UB960_ITER_ACTIVE_ONLY = BIT(0),
+	UB960_ITER_FPD4_ONLY = BIT(1),
+};
+
+static struct ub960_rxport_iter ub960_iter_rxport(struct ub960_data *priv,
+						  struct ub960_rxport_iter it,
+						  enum ub960_iter_flags flags)
+{
+	for (; it.nport < priv->hw_data->num_rxports; it.nport++) {
+		it.rxport = priv->rxports[it.nport];
+
+		if ((flags & UB960_ITER_ACTIVE_ONLY) && !it.rxport)
+			continue;
+
+		if ((flags & UB960_ITER_FPD4_ONLY) &&
+		    it.rxport->cdr_mode != RXPORT_CDR_FPD4)
+			continue;
+
+		return it;
+	}
+
+	it.rxport = NULL;
+
+	return it;
+}
+
+#define for_each_rxport(priv, it)                                             \
+	for (struct ub960_rxport_iter it =                                    \
+		     ub960_iter_rxport(priv, (struct ub960_rxport_iter){ 0 }, \
+				       0);                                    \
+	     it.nport < (priv)->hw_data->num_rxports;                         \
+	     it.nport++, it = ub960_iter_rxport(priv, it, 0))
+
+#define for_each_active_rxport(priv, it)                                      \
+	for (struct ub960_rxport_iter it =                                    \
+		     ub960_iter_rxport(priv, (struct ub960_rxport_iter){ 0 }, \
+				       UB960_ITER_ACTIVE_ONLY);               \
+	     it.nport < (priv)->hw_data->num_rxports;                         \
+	     it.nport++, it = ub960_iter_rxport(priv, it,                     \
+						UB960_ITER_ACTIVE_ONLY))
+
+#define for_each_active_rxport_fpd4(priv, it)                                 \
+	for (struct ub960_rxport_iter it =                                    \
+		     ub960_iter_rxport(priv, (struct ub960_rxport_iter){ 0 }, \
+				       UB960_ITER_ACTIVE_ONLY |               \
+					       UB960_ITER_FPD4_ONLY);         \
+	     it.nport < (priv)->hw_data->num_rxports;                         \
+	     it.nport++, it = ub960_iter_rxport(priv, it,                     \
+						UB960_ITER_ACTIVE_ONLY |      \
+							UB960_ITER_FPD4_ONLY))
+
 /* -----------------------------------------------------------------------------
  * Basic device access
  */
 
-static int ub960_read(struct ub960_data *priv, u8 reg, u8 *val)
+static int ub960_read(struct ub960_data *priv, u8 reg, u8 *val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_read(priv->regmap, reg, &v);
@@ -616,14 +744,20 @@ static int ub960_read(struct ub960_data *priv, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_write(struct ub960_data *priv, u8 reg, u8 val)
+static int ub960_write(struct ub960_data *priv, u8 reg, u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_write(priv->regmap, reg, val);
@@ -633,14 +767,21 @@ static int ub960_write(struct ub960_data *priv, u8 reg, u8 val)
 
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_update_bits(struct ub960_data *priv, u8 reg, u8 mask, u8 val)
+static int ub960_update_bits(struct ub960_data *priv, u8 reg, u8 mask, u8 val,
+			     int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_update_bits(priv->regmap, reg, mask, val);
@@ -650,15 +791,21 @@ static int ub960_update_bits(struct ub960_data *priv, u8 reg, u8 mask, u8 val)
 
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_read16(struct ub960_data *priv, u8 reg, u16 *val)
+static int ub960_read16(struct ub960_data *priv, u8 reg, u16 *val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	__be16 __v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = regmap_bulk_read(priv->regmap, reg, &__v, sizeof(__v));
@@ -673,6 +820,9 @@ static int ub960_read16(struct ub960_data *priv, u8 reg, u16 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -699,12 +849,16 @@ static int ub960_rxport_select(struct ub960_data *priv, u8 nport)
 	return 0;
 }
 
-static int ub960_rxport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
+static int ub960_rxport_read(struct ub960_data *priv, u8 nport, u8 reg,
+			     u8 *val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_rxport_select(priv, nport);
@@ -723,14 +877,21 @@ static int ub960_rxport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_rxport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
+static int ub960_rxport_write(struct ub960_data *priv, u8 nport, u8 reg,
+			      u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_rxport_select(priv, nport);
@@ -745,15 +906,21 @@ static int ub960_rxport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
 static int ub960_rxport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
-				    u8 mask, u8 val)
+				    u8 mask, u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_rxport_select(priv, nport);
@@ -768,16 +935,22 @@ static int ub960_rxport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
 static int ub960_rxport_read16(struct ub960_data *priv, u8 nport, u8 reg,
-			       u16 *val)
+			       u16 *val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	__be16 __v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_rxport_select(priv, nport);
@@ -796,6 +969,9 @@ static int ub960_rxport_read16(struct ub960_data *priv, u8 nport, u8 reg,
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -822,12 +998,16 @@ static int ub960_txport_select(struct ub960_data *priv, u8 nport)
 	return 0;
 }
 
-static int ub960_txport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
+static int ub960_txport_read(struct ub960_data *priv, u8 nport, u8 reg,
+			     u8 *val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_txport_select(priv, nport);
@@ -846,14 +1026,21 @@ static int ub960_txport_read(struct ub960_data *priv, u8 nport, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_txport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
+static int ub960_txport_write(struct ub960_data *priv, u8 nport, u8 reg,
+			      u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_txport_select(priv, nport);
@@ -868,15 +1055,21 @@ static int ub960_txport_write(struct ub960_data *priv, u8 nport, u8 reg, u8 val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
 static int ub960_txport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
-				    u8 mask, u8 val)
+				    u8 mask, u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_txport_select(priv, nport);
@@ -891,6 +1084,9 @@ static int ub960_txport_update_bits(struct ub960_data *priv, u8 nport, u8 reg,
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
@@ -916,12 +1112,16 @@ static int ub960_select_ind_reg_block(struct ub960_data *priv, u8 block)
 	return 0;
 }
 
-static int ub960_read_ind(struct ub960_data *priv, u8 block, u8 reg, u8 *val)
+static int ub960_read_ind(struct ub960_data *priv, u8 block, u8 reg, u8 *val,
+			  int *err)
 {
 	struct device *dev = &priv->client->dev;
 	unsigned int v;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_select_ind_reg_block(priv, block);
@@ -949,14 +1149,21 @@ static int ub960_read_ind(struct ub960_data *priv, u8 block, u8 reg, u8 *val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
-static int ub960_write_ind(struct ub960_data *priv, u8 block, u8 reg, u8 val)
+static int ub960_write_ind(struct ub960_data *priv, u8 block, u8 reg, u8 val,
+			   int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_select_ind_reg_block(priv, block);
@@ -982,15 +1189,21 @@ static int ub960_write_ind(struct ub960_data *priv, u8 block, u8 reg, u8 val)
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
 	return ret;
 }
 
 static int ub960_ind_update_bits(struct ub960_data *priv, u8 block, u8 reg,
-				 u8 mask, u8 val)
+				 u8 mask, u8 val, int *err)
 {
 	struct device *dev = &priv->client->dev;
 	int ret;
 
+	if (err && *err)
+		return *err;
+
 	mutex_lock(&priv->reg_lock);
 
 	ret = ub960_select_ind_reg_block(priv, block);
@@ -1017,6 +1230,36 @@ static int ub960_ind_update_bits(struct ub960_data *priv, u8 block, u8 reg,
 out_unlock:
 	mutex_unlock(&priv->reg_lock);
 
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int ub960_reset(struct ub960_data *priv, bool reset_regs)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int v;
+	int ret;
+	u8 bit;
+
+	bit = reset_regs ? UB960_SR_RESET_DIGITAL_RESET1 :
+			   UB960_SR_RESET_DIGITAL_RESET0;
+
+	ret = ub960_write(priv, UB960_SR_RESET, bit, NULL);
+	if (ret)
+		return ret;
+
+	mutex_lock(&priv->reg_lock);
+
+	ret = regmap_read_poll_timeout(priv->regmap, UB960_SR_RESET, v,
+				       (v & bit) == 0, 2000, 100000);
+
+	mutex_unlock(&priv->reg_lock);
+
+	if (ret)
+		dev_err(dev, "reset failed: %d\n", ret);
+
 	return ret;
 }
 
@@ -1024,67 +1267,82 @@ out_unlock:
  * I2C-ATR (address translator)
  */
 
-static int ub960_atr_attach_client(struct i2c_atr *atr, u32 chan_id,
-				   const struct i2c_client *client, u16 alias)
+static int ub960_atr_attach_addr(struct i2c_atr *atr, u32 chan_id,
+				 u16 addr, u16 alias)
 {
 	struct ub960_data *priv = i2c_atr_get_driver_data(atr);
 	struct ub960_rxport *rxport = priv->rxports[chan_id];
 	struct device *dev = &priv->client->dev;
 	unsigned int reg_idx;
+	int ret = 0;
 
-	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_clients); reg_idx++) {
-		if (!rxport->aliased_clients[reg_idx])
+	guard(mutex)(&rxport->aliased_addrs_lock);
+
+	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_addrs); reg_idx++) {
+		if (!rxport->aliased_addrs[reg_idx])
 			break;
 	}
 
-	if (reg_idx == ARRAY_SIZE(rxport->aliased_clients)) {
+	if (reg_idx == ARRAY_SIZE(rxport->aliased_addrs)) {
 		dev_err(dev, "rx%u: alias pool exhausted\n", rxport->nport);
 		return -EADDRNOTAVAIL;
 	}
 
-	rxport->aliased_clients[reg_idx] = client;
+	rxport->aliased_addrs[reg_idx] = addr;
 
 	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ID(reg_idx),
-			   client->addr << 1);
+			   addr << 1, &ret);
 	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ALIAS(reg_idx),
-			   alias << 1);
+			   alias << 1, &ret);
+
+	if (ret)
+		return ret;
 
 	dev_dbg(dev, "rx%u: client 0x%02x assigned alias 0x%02x at slot %u\n",
-		rxport->nport, client->addr, alias, reg_idx);
+		rxport->nport, addr, alias, reg_idx);
 
 	return 0;
 }
 
-static void ub960_atr_detach_client(struct i2c_atr *atr, u32 chan_id,
-				    const struct i2c_client *client)
+static void ub960_atr_detach_addr(struct i2c_atr *atr, u32 chan_id,
+				  u16 addr)
 {
 	struct ub960_data *priv = i2c_atr_get_driver_data(atr);
 	struct ub960_rxport *rxport = priv->rxports[chan_id];
 	struct device *dev = &priv->client->dev;
 	unsigned int reg_idx;
+	int ret;
+
+	guard(mutex)(&rxport->aliased_addrs_lock);
 
-	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_clients); reg_idx++) {
-		if (rxport->aliased_clients[reg_idx] == client)
+	for (reg_idx = 0; reg_idx < ARRAY_SIZE(rxport->aliased_addrs); reg_idx++) {
+		if (rxport->aliased_addrs[reg_idx] == addr)
 			break;
 	}
 
-	if (reg_idx == ARRAY_SIZE(rxport->aliased_clients)) {
+	if (reg_idx == ARRAY_SIZE(rxport->aliased_addrs)) {
 		dev_err(dev, "rx%u: client 0x%02x is not mapped!\n",
-			rxport->nport, client->addr);
+			rxport->nport, addr);
 		return;
 	}
 
-	rxport->aliased_clients[reg_idx] = NULL;
+	rxport->aliased_addrs[reg_idx] = 0;
 
-	ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ALIAS(reg_idx), 0);
+	ret = ub960_rxport_write(priv, chan_id, UB960_RR_SLAVE_ALIAS(reg_idx),
+				 0, NULL);
+	if (ret) {
+		dev_err(dev, "rx%u: unable to fully unmap client 0x%02x: %d\n",
+			rxport->nport, addr, ret);
+		return;
+	}
 
 	dev_dbg(dev, "rx%u: client 0x%02x released at slot %u\n", rxport->nport,
-		client->addr, reg_idx);
+		addr, reg_idx);
 }
 
 static const struct i2c_atr_ops ub960_atr_ops = {
-	.attach_client = ub960_atr_attach_client,
-	.detach_client = ub960_atr_detach_client,
+	.attach_addr = ub960_atr_attach_addr,
+	.detach_addr = ub960_atr_detach_addr,
 };
 
 static int ub960_init_atr(struct ub960_data *priv)
@@ -1093,7 +1351,7 @@ static int ub960_init_atr(struct ub960_data *priv)
 	struct i2c_adapter *parent_adap = priv->client->adapter;
 
 	priv->atr = i2c_atr_new(parent_adap, dev, &ub960_atr_ops,
-				priv->hw_data->num_rxports);
+				priv->hw_data->num_rxports, 0);
 	if (IS_ERR(priv->atr))
 		return PTR_ERR(priv->atr);
 
@@ -1171,21 +1429,24 @@ err_free_txport:
 	return ret;
 }
 
-static void ub960_csi_handle_events(struct ub960_data *priv, u8 nport)
+static int  ub960_csi_handle_events(struct ub960_data *priv, u8 nport)
 {
 	struct device *dev = &priv->client->dev;
 	u8 csi_tx_isr;
 	int ret;
 
-	ret = ub960_txport_read(priv, nport, UB960_TR_CSI_TX_ISR, &csi_tx_isr);
+	ret = ub960_txport_read(priv, nport, UB960_TR_CSI_TX_ISR, &csi_tx_isr,
+				NULL);
 	if (ret)
-		return;
+		return ret;
 
 	if (csi_tx_isr & UB960_TR_CSI_TX_ISR_IS_CSI_SYNC_ERROR)
 		dev_warn(dev, "TX%u: CSI_SYNC_ERROR\n", nport);
 
 	if (csi_tx_isr & UB960_TR_CSI_TX_ISR_IS_CSI_PASS_ERROR)
 		dev_warn(dev, "TX%u: CSI_PASS_ERROR\n", nport);
+
+	return 0;
 }
 
 /* -----------------------------------------------------------------------------
@@ -1194,25 +1455,25 @@ static void ub960_csi_handle_events(struct ub960_data *priv, u8 nport)
 
 static int ub960_rxport_enable_vpocs(struct ub960_data *priv)
 {
-	unsigned int nport;
+	unsigned int failed_nport;
 	int ret;
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-
-		if (!rxport || !rxport->vpoc)
+	for_each_active_rxport(priv, it) {
+		if (!it.rxport->vpoc)
 			continue;
 
-		ret = regulator_enable(rxport->vpoc);
-		if (ret)
+		ret = regulator_enable(it.rxport->vpoc);
+		if (ret) {
+			failed_nport = it.nport;
 			goto err_disable_vpocs;
+		}
 	}
 
 	return 0;
 
 err_disable_vpocs:
-	while (nport--) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+	while (failed_nport--) {
+		struct ub960_rxport *rxport = priv->rxports[failed_nport];
 
 		if (!rxport || !rxport->vpoc)
 			continue;
@@ -1225,40 +1486,44 @@ err_disable_vpocs:
 
 static void ub960_rxport_disable_vpocs(struct ub960_data *priv)
 {
-	unsigned int nport;
-
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-
-		if (!rxport || !rxport->vpoc)
+	for_each_active_rxport(priv, it) {
+		if (!it.rxport->vpoc)
 			continue;
 
-		regulator_disable(rxport->vpoc);
+		regulator_disable(it.rxport->vpoc);
 	}
 }
 
-static void ub960_rxport_clear_errors(struct ub960_data *priv,
-				      unsigned int nport)
+static int ub960_rxport_clear_errors(struct ub960_data *priv,
+				     unsigned int nport)
 {
+	int ret = 0;
 	u8 v;
 
-	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v);
-	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v);
-	ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &v);
-	ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v, &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v, &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &v, &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &v, &ret);
 
-	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_HI, &v);
-	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_LO, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_HI, &v, &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PAR_ERR_LO, &v, &ret);
 
-	ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER, &v);
+	ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER, &v, &ret);
+
+	return ret;
 }
 
-static void ub960_clear_rx_errors(struct ub960_data *priv)
+static int ub960_clear_rx_errors(struct ub960_data *priv)
 {
-	unsigned int nport;
+	int ret;
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++)
-		ub960_rxport_clear_errors(priv, nport);
+	for_each_rxport(priv, it) {
+		ret = ub960_rxport_clear_errors(priv, it.nport);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
 }
 
 static int ub960_rxport_get_strobe_pos(struct ub960_data *priv,
@@ -1268,25 +1533,29 @@ static int ub960_rxport_get_strobe_pos(struct ub960_data *priv,
 	u8 clk_delay, data_delay;
 	int ret;
 
-	ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
-		       UB960_IR_RX_ANA_STROBE_SET_CLK, &v);
+	ret = ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			     UB960_IR_RX_ANA_STROBE_SET_CLK, &v, NULL);
+	if (ret)
+		return ret;
 
 	clk_delay = (v & UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY) ?
 			    0 : UB960_MANUAL_STROBE_EXTRA_DELAY;
 
-	ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
-		       UB960_IR_RX_ANA_STROBE_SET_DATA, &v);
+	ret = ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			     UB960_IR_RX_ANA_STROBE_SET_DATA, &v, NULL);
+	if (ret)
+		return ret;
 
 	data_delay = (v & UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY) ?
 			     0 : UB960_MANUAL_STROBE_EXTRA_DELAY;
 
-	ret = ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_0, &v);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_0, &v, NULL);
 	if (ret)
 		return ret;
 
 	clk_delay += v & UB960_IR_RX_ANA_STROBE_SET_CLK_DELAY_MASK;
 
-	ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_1, &v);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_SFILTER_STS_1, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -1297,10 +1566,11 @@ static int ub960_rxport_get_strobe_pos(struct ub960_data *priv,
 	return 0;
 }
 
-static void ub960_rxport_set_strobe_pos(struct ub960_data *priv,
-					unsigned int nport, s8 strobe_pos)
+static int ub960_rxport_set_strobe_pos(struct ub960_data *priv,
+				       unsigned int nport, s8 strobe_pos)
 {
 	u8 clk_delay, data_delay;
+	int ret = 0;
 
 	clk_delay = UB960_IR_RX_ANA_STROBE_SET_CLK_NO_EXTRA_DELAY;
 	data_delay = UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY;
@@ -1315,22 +1585,25 @@ static void ub960_rxport_set_strobe_pos(struct ub960_data *priv,
 		data_delay = strobe_pos | UB960_IR_RX_ANA_STROBE_SET_DATA_NO_EXTRA_DELAY;
 
 	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
-			UB960_IR_RX_ANA_STROBE_SET_CLK, clk_delay);
+			UB960_IR_RX_ANA_STROBE_SET_CLK, clk_delay, &ret);
 
 	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
-			UB960_IR_RX_ANA_STROBE_SET_DATA, data_delay);
+			UB960_IR_RX_ANA_STROBE_SET_DATA, data_delay, &ret);
+
+	return ret;
 }
 
-static void ub960_rxport_set_strobe_range(struct ub960_data *priv,
-					  s8 strobe_min, s8 strobe_max)
+static int ub960_rxport_set_strobe_range(struct ub960_data *priv, s8 strobe_min,
+					 s8 strobe_max)
 {
 	/* Convert the signed strobe pos to positive zero based value */
 	strobe_min -= UB960_MIN_AEQ_STROBE_POS;
 	strobe_max -= UB960_MIN_AEQ_STROBE_POS;
 
-	ub960_write(priv, UB960_XR_SFILTER_CFG,
-		    ((u8)strobe_min << UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) |
-		    ((u8)strobe_max << UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT));
+	return ub960_write(priv, UB960_XR_SFILTER_CFG,
+			   ((u8)strobe_min << UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) |
+			   ((u8)strobe_max << UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT),
+			   NULL);
 }
 
 static int ub960_rxport_get_eq_level(struct ub960_data *priv,
@@ -1339,7 +1612,7 @@ static int ub960_rxport_get_eq_level(struct ub960_data *priv,
 	int ret;
 	u8 v;
 
-	ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_STATUS, &v);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_STATUS, &v, NULL);
 	if (ret)
 		return ret;
 
@@ -1349,11 +1622,12 @@ static int ub960_rxport_get_eq_level(struct ub960_data *priv,
 	return 0;
 }
 
-static void ub960_rxport_set_eq_level(struct ub960_data *priv,
-				      unsigned int nport, u8 eq_level)
+static int ub960_rxport_set_eq_level(struct ub960_data *priv,
+				     unsigned int nport, u8 eq_level)
 {
 	u8 eq_stage_1_select_value, eq_stage_2_select_value;
 	const unsigned int eq_stage_max = 7;
+	int ret;
 	u8 v;
 
 	if (eq_level <= eq_stage_max) {
@@ -1364,7 +1638,9 @@ static void ub960_rxport_set_eq_level(struct ub960_data *priv,
 		eq_stage_2_select_value = eq_level - eq_stage_max;
 	}
 
-	ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v, NULL);
+	if (ret)
+		return ret;
 
 	v &= ~(UB960_RR_AEQ_BYPASS_EQ_STAGE1_VALUE_MASK |
 	       UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_MASK);
@@ -1372,67 +1648,102 @@ static void ub960_rxport_set_eq_level(struct ub960_data *priv,
 	v |= eq_stage_2_select_value << UB960_RR_AEQ_BYPASS_EQ_STAGE2_VALUE_SHIFT;
 	v |= UB960_RR_AEQ_BYPASS_ENABLE;
 
-	ub960_rxport_write(priv, nport, UB960_RR_AEQ_BYPASS, v);
+	ret = ub960_rxport_write(priv, nport, UB960_RR_AEQ_BYPASS, v, NULL);
+	if (ret)
+		return ret;
+
+	return 0;
 }
 
-static void ub960_rxport_set_eq_range(struct ub960_data *priv,
-				      unsigned int nport, u8 eq_min, u8 eq_max)
+static int ub960_rxport_set_eq_range(struct ub960_data *priv,
+				     unsigned int nport, u8 eq_min, u8 eq_max)
 {
+	int ret = 0;
+
 	ub960_rxport_write(priv, nport, UB960_RR_AEQ_MIN_MAX,
 			   (eq_min << UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT) |
-			   (eq_max << UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT));
+			   (eq_max << UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT),
+			   &ret);
 
 	/* Enable AEQ min setting */
 	ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_CTL2,
 				 UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR,
-				 UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR);
+				 UB960_RR_AEQ_CTL2_SET_AEQ_FLOOR, &ret);
+
+	return ret;
 }
 
-static void ub960_rxport_config_eq(struct ub960_data *priv, unsigned int nport)
+static int ub960_rxport_config_eq(struct ub960_data *priv, unsigned int nport)
 {
 	struct ub960_rxport *rxport = priv->rxports[nport];
+	int ret;
 
 	/* We also set common settings here. Should be moved elsewhere. */
 
 	if (priv->strobe.manual) {
 		/* Disable AEQ_SFILTER_EN */
-		ub960_update_bits(priv, UB960_XR_AEQ_CTL1,
-				  UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN, 0);
+		ret = ub960_update_bits(priv, UB960_XR_AEQ_CTL1,
+					UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN, 0,
+					NULL);
+		if (ret)
+			return ret;
 	} else {
 		/* Enable SFILTER and error control */
-		ub960_write(priv, UB960_XR_AEQ_CTL1,
-			    UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_MASK |
-				    UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN);
+		ret = ub960_write(priv, UB960_XR_AEQ_CTL1,
+				  UB960_XR_AEQ_CTL1_AEQ_ERR_CTL_MASK |
+					  UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN,
+				  NULL);
+
+		if (ret)
+			return ret;
 
 		/* Set AEQ strobe range */
-		ub960_rxport_set_strobe_range(priv, priv->strobe.min,
-					      priv->strobe.max);
+		ret = ub960_rxport_set_strobe_range(priv, priv->strobe.min,
+						    priv->strobe.max);
+		if (ret)
+			return ret;
 	}
 
 	/* The rest are port specific */
 
 	if (priv->strobe.manual)
-		ub960_rxport_set_strobe_pos(priv, nport, rxport->eq.strobe_pos);
+		ret = ub960_rxport_set_strobe_pos(priv, nport,
+						  rxport->eq.strobe_pos);
 	else
-		ub960_rxport_set_strobe_pos(priv, nport, 0);
+		ret = ub960_rxport_set_strobe_pos(priv, nport, 0);
+
+	if (ret)
+		return ret;
 
 	if (rxport->eq.manual_eq) {
-		ub960_rxport_set_eq_level(priv, nport,
-					  rxport->eq.manual.eq_level);
+		ret = ub960_rxport_set_eq_level(priv, nport,
+						rxport->eq.manual.eq_level);
+		if (ret)
+			return ret;
 
 		/* Enable AEQ Bypass */
-		ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
-					 UB960_RR_AEQ_BYPASS_ENABLE,
-					 UB960_RR_AEQ_BYPASS_ENABLE);
+		ret = ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
+					       UB960_RR_AEQ_BYPASS_ENABLE,
+					       UB960_RR_AEQ_BYPASS_ENABLE,
+					       NULL);
+		if (ret)
+			return ret;
 	} else {
-		ub960_rxport_set_eq_range(priv, nport,
-					  rxport->eq.aeq.eq_level_min,
-					  rxport->eq.aeq.eq_level_max);
+		ret = ub960_rxport_set_eq_range(priv, nport,
+						rxport->eq.aeq.eq_level_min,
+						rxport->eq.aeq.eq_level_max);
+		if (ret)
+			return ret;
 
 		/* Disable AEQ Bypass */
-		ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
-					 UB960_RR_AEQ_BYPASS_ENABLE, 0);
+		ret = ub960_rxport_update_bits(priv, nport, UB960_RR_AEQ_BYPASS,
+					       UB960_RR_AEQ_BYPASS_ENABLE, 0,
+					       NULL);
+		if (ret)
+			return ret;
 	}
+
+	return 0;
 }
 
 static int ub960_rxport_link_ok(struct ub960_data *priv, unsigned int nport,
@@ -1447,7 +1758,7 @@ static int ub960_rxport_link_ok(struct ub960_data *priv, unsigned int nport,
 	bool errors;
 
 	ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1,
-				&rx_port_sts1);
+				&rx_port_sts1, NULL);
 	if (ret)
 		return ret;
 
@@ -1457,25 +1768,27 @@ static int ub960_rxport_link_ok(struct ub960_data *priv, unsigned int nport,
 	}
 
 	ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2,
-				&rx_port_sts2);
+				&rx_port_sts2, NULL);
 	if (ret)
 		return ret;
 
-	ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &csi_rx_sts);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &csi_rx_sts,
+				NULL);
 	if (ret)
 		return ret;
 
 	ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER,
-				&csi_err_cnt);
+				&csi_err_cnt, NULL);
 	if (ret)
 		return ret;
 
-	ret = ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &bcc_sts);
+	ret = ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &bcc_sts,
+				NULL);
 	if (ret)
 		return ret;
 
 	ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI,
-				  &parity_errors);
+				  &parity_errors, NULL);
 	if (ret)
 		return ret;
 
@@ -1490,6 +1803,23 @@ static int ub960_rxport_link_ok(struct ub960_data *priv, unsigned int nport,
 	return 0;
 }
 
+static int ub960_rxport_lockup_wa_ub9702(struct ub960_data *priv)
+{
+	int ret;
+
+	/* Toggle PI_MODE to avoid possible FPD RX lockup */
+
+	ret = ub960_update_bits(priv, UB9702_RR_CHANNEL_MODE, GENMASK(4, 3),
+				2 << 3, NULL);
+	if (ret)
+		return ret;
+
+	usleep_range(1000, 5000);
+
+	return ub960_update_bits(priv, UB9702_RR_CHANNEL_MODE, GENMASK(4, 3),
+				 0, NULL);
+}
+
 /*
  * Wait for the RX ports to lock, have no errors and have stable strobe position
  * and EQ level.
@@ -1520,6 +1850,7 @@ static int ub960_rxport_wait_locks(struct ub960_data *priv,
 	link_ok_mask = 0;
 
 	while (time_before(jiffies, timeout)) {
+		bool fpd4_wa = false;
 		missing = 0;
 
 		for_each_set_bit(nport, &port_mask,
@@ -1534,6 +1865,9 @@ static int ub960_rxport_wait_locks(struct ub960_data *priv,
 			if (ret)
 				return ret;
 
+			if (!ok && rxport->cdr_mode == RXPORT_CDR_FPD4)
+				fpd4_wa = true;
+
 			/*
 			 * We want the link to be ok for two consecutive loops,
 			 * as a link could get established just before our test
@@ -1553,7 +1887,18 @@ static int ub960_rxport_wait_locks(struct ub960_data *priv,
 		if (missing == 0)
 			break;
 
-		msleep(50);
+		if (fpd4_wa) {
+			ret = ub960_rxport_lockup_wa_ub9702(priv);
+			if (ret)
+				return ret;
+		}
+
+		/*
+		 * The sleep time of 10 ms was found by testing to give a lock
+		 * with a few iterations. It can be decreased if on some setups
+		 * the lock can be achieved much faster.
+		 */
+		fsleep(10 * USEC_PER_MSEC);
 	}
 
 	if (lock_mask)
@@ -1573,18 +1918,30 @@ static int ub960_rxport_wait_locks(struct ub960_data *priv,
 			continue;
 		}
 
-		ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH, &v);
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH,
+					  &v, NULL);
 
-		ret = ub960_rxport_get_strobe_pos(priv, nport, &strobe_pos);
 		if (ret)
 			return ret;
 
-		ret = ub960_rxport_get_eq_level(priv, nport, &eq_level);
-		if (ret)
-			return ret;
+		if (priv->hw_data->is_ub9702) {
+			dev_dbg(dev, "\trx%u: locked, freq %llu Hz\n",
+				nport, ((u64)v * HZ_PER_MHZ) >> 8);
+		} else {
+			ret = ub960_rxport_get_strobe_pos(priv, nport,
+							  &strobe_pos);
+			if (ret)
+				return ret;
+
+			ret = ub960_rxport_get_eq_level(priv, nport, &eq_level);
+			if (ret)
+				return ret;
 
-		dev_dbg(dev, "\trx%u: locked, SP: %d, EQ: %u, freq %llu Hz\n",
-			nport, strobe_pos, eq_level, (v * 1000000ULL) >> 8);
+			dev_dbg(dev,
+				"\trx%u: locked, SP: %d, EQ: %u, freq %llu Hz\n",
+				nport, strobe_pos, eq_level,
+				((u64)v * HZ_PER_MHZ) >> 8);
+		}
 	}
 
 	return 0;
@@ -1641,13 +1998,188 @@ static unsigned long ub960_calc_bc_clk_rate_ub9702(struct ub960_data *priv,
 	}
 }
 
+static int ub960_rxport_serializer_write(struct ub960_rxport *rxport, u8 reg,
+					 u8 val, int *err)
+{
+	struct ub960_data *priv = rxport->priv;
+	struct device *dev = &priv->client->dev;
+	union i2c_smbus_data data;
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	data.byte = val;
+
+	ret = i2c_smbus_xfer(priv->client->adapter, rxport->ser.alias, 0,
+			     I2C_SMBUS_WRITE, reg, I2C_SMBUS_BYTE_DATA, &data);
+	if (ret)
+		dev_err(dev,
+			"rx%u: cannot write serializer register 0x%02x (%d)!\n",
+			rxport->nport, reg, ret);
+
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int ub960_rxport_serializer_read(struct ub960_rxport *rxport, u8 reg,
+					u8 *val, int *err)
+{
+	struct ub960_data *priv = rxport->priv;
+	struct device *dev = &priv->client->dev;
+	union i2c_smbus_data data = { 0 };
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	ret = i2c_smbus_xfer(priv->client->adapter, rxport->ser.alias,
+			     priv->client->flags, I2C_SMBUS_READ, reg,
+			     I2C_SMBUS_BYTE_DATA, &data);
+	if (ret)
+		dev_err(dev,
+			"rx%u: cannot read serializer register 0x%02x (%d)!\n",
+			rxport->nport, reg, ret);
+	else
+		*val = data.byte;
+
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int ub960_serializer_temp_ramp(struct ub960_rxport *rxport)
+{
+	struct ub960_data *priv = rxport->priv;
+	short temp_dynamic_offset[] = {-1, -1, 0, 0, 1, 1, 1, 3};
+	u8 temp_dynamic_cfg;
+	u8 nport = rxport->nport;
+	u8 ser_temp_code;
+	int ret = 0;
+
+	/* Configure temp ramp only on UB953 */
+	if (!fwnode_device_is_compatible(rxport->ser.fwnode, "ti,ds90ub953-q1"))
+		return 0;
+
+	/* Read current serializer die temperature */
+	ub960_rxport_read(priv, nport, UB960_RR_SENSOR_STS_2, &ser_temp_code,
+			  &ret);
+
+	/* Enable I2C passthrough on back channel */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH, &ret);
+
+	if (ret)
+		return ret;
+
+	/* Select indirect page for analog regs on the serializer */
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_CTL,
+				      UB953_IND_TARGET_ANALOG << 2, &ret);
+
+	/* Set temperature ramp dynamic and static config */
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_ADDR,
+				      UB953_IND_ANA_TEMP_DYNAMIC_CFG, &ret);
+	ub960_rxport_serializer_read(rxport, UB953_REG_IND_ACC_DATA,
+				     &temp_dynamic_cfg, &ret);
+
+	if (ret)
+		return ret;
+
+	temp_dynamic_cfg |= UB953_IND_ANA_TEMP_DYNAMIC_CFG_OV;
+	temp_dynamic_cfg += temp_dynamic_offset[ser_temp_code];
+
+	/* Update temp static config */
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_ADDR,
+				      UB953_IND_ANA_TEMP_STATIC_CFG, &ret);
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_DATA,
+				      UB953_IND_ANA_TEMP_STATIC_CFG_MASK, &ret);
+
+	/* Update temperature ramp dynamic config */
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_ADDR,
+				      UB953_IND_ANA_TEMP_DYNAMIC_CFG, &ret);
+
+	/* Enable I2C auto ack on BC before we set dynamic cfg and reset */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL,
+				 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL, &ret);
+
+	ub960_rxport_serializer_write(rxport, UB953_REG_IND_ACC_DATA,
+				      temp_dynamic_cfg, &ret);
+
+	if (ret)
+		return ret;
+
+	/* Soft reset to apply PLL updates */
+	ub960_rxport_serializer_write(rxport, UB953_REG_RESET_CTL,
+				      UB953_REG_RESET_CTL_DIGITAL_RESET_0,
+				      &ret);
+	msleep(20);
+
+	/* Disable I2C passthrough and auto-ack on BC */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH |
+					 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL,
+				 0x0, &ret);
+
+	return ret;
+}
+
+static int ub960_rxport_bc_ser_config(struct ub960_rxport *rxport)
+{
+	struct ub960_data *priv = rxport->priv;
+	struct device *dev = &priv->client->dev;
+	u8 nport = rxport->nport;
+	int ret = 0;
+
+	/* Skip port if serializer's address is not known */
+	if (rxport->ser.addr < 0) {
+		dev_dbg(dev,
+			"rx%u: serializer address missing, skip configuration\n",
+			nport);
+		return 0;
+	}
+
+	/*
+	 * Note: the code here probably only works for CSI-2 serializers in
+	 * sync mode. To support other serializers the BC related configuration
+	 * should be done before calling this function.
+	 */
+
+	/* Enable I2C passthrough and auto-ack on BC */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH |
+					 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH |
+					 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL,
+				 &ret);
+
+	if (ret)
+		return ret;
+
+	/* Disable BC alternate mode auto detect */
+	ub960_rxport_serializer_write(rxport, UB971_ENH_BC_CHK, 0x02, &ret);
+	/* Decrease link detect timer */
+	ub960_rxport_serializer_write(rxport, UB953_REG_BC_CTRL, 0x06, &ret);
+
+	/* Disable I2C passthrough and auto-ack on BC */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH |
+					 UB960_RR_BCC_CONFIG_AUTO_ACK_ALL,
+				 0x0, &ret);
+
+	return ret;
+}
+
 static int ub960_rxport_add_serializer(struct ub960_data *priv, u8 nport)
 {
 	struct ub960_rxport *rxport = priv->rxports[nport];
 	struct device *dev = &priv->client->dev;
 	struct ds90ub9xx_platform_data *ser_pdata = &rxport->ser.pdata;
 	struct i2c_board_info ser_info = {
-		.of_node = to_of_node(rxport->ser.fwnode),
 		.fwnode = rxport->ser.fwnode,
 		.platform_data = ser_pdata,
 	};
@@ -1691,30 +2223,27 @@ static void ub960_rxport_remove_serializer(struct ub960_data *priv, u8 nport)
 /* Add serializer i2c devices for all initialized ports */
 static int ub960_rxport_add_serializers(struct ub960_data *priv)
 {
-	unsigned int nport;
+	unsigned int failed_nport;
 	int ret;
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-
-		if (!rxport)
-			continue;
-
-		ret = ub960_rxport_add_serializer(priv, nport);
-		if (ret)
+	for_each_active_rxport(priv, it) {
+		ret = ub960_rxport_add_serializer(priv, it.nport);
+		if (ret) {
+			failed_nport = it.nport;
 			goto err_remove_sers;
+		}
 	}
 
 	return 0;
 
 err_remove_sers:
-	while (nport--) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+	while (failed_nport--) {
+		struct ub960_rxport *rxport = priv->rxports[failed_nport];
 
 		if (!rxport)
 			continue;
 
-		ub960_rxport_remove_serializer(priv, nport);
+		ub960_rxport_remove_serializer(priv, failed_nport);
 	}
 
 	return ret;
@@ -1722,20 +2251,12 @@ err_remove_sers:
 
 static void ub960_rxport_remove_serializers(struct ub960_data *priv)
 {
-	unsigned int nport;
-
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-
-		if (!rxport)
-			continue;
-
-		ub960_rxport_remove_serializer(priv, nport);
-	}
+	for_each_active_rxport(priv, it)
+		ub960_rxport_remove_serializer(priv, it.nport);
 }
 
-static void ub960_init_tx_port(struct ub960_data *priv,
-			       struct ub960_txport *txport)
+static int ub960_init_tx_port(struct ub960_data *priv,
+			      struct ub960_txport *txport)
 {
 	unsigned int nport = txport->nport;
 	u8 csi_ctl = 0;
@@ -1752,76 +2273,114 @@ static void ub960_init_tx_port(struct ub960_data *priv,
 	if (!txport->non_continous_clk)
 		csi_ctl |= UB960_TR_CSI_CTL_CSI_CONTS_CLOCK;
 
-	ub960_txport_write(priv, nport, UB960_TR_CSI_CTL, csi_ctl);
+	return ub960_txport_write(priv, nport, UB960_TR_CSI_CTL, csi_ctl, NULL);
 }
 
-static int ub960_init_tx_ports(struct ub960_data *priv)
+static int ub960_init_tx_ports_ub960(struct ub960_data *priv)
 {
-	unsigned int nport;
 	u8 speed_select;
-	u8 pll_div;
-
-	/* TX ports */
 
 	switch (priv->tx_data_rate) {
+	case MHZ(400):
+		speed_select = 3;
+		break;
+	case MHZ(800):
+		speed_select = 2;
+		break;
+	case MHZ(1200):
+		speed_select = 1;
+		break;
 	case MHZ(1600):
 	default:
 		speed_select = 0;
+		break;
+	}
+
+	return ub960_write(priv, UB960_SR_CSI_PLL_CTL, speed_select, NULL);
+}
+
+static int ub960_init_tx_ports_ub9702(struct ub960_data *priv)
+{
+	u8 speed_select;
+	u8 ana_pll_div;
+	u8 pll_div;
+	int ret = 0;
+
+	switch (priv->tx_data_rate) {
+	case MHZ(400):
+		speed_select = 3;
 		pll_div = 0x10;
+		ana_pll_div = 0xa2;
+		break;
+	case MHZ(800):
+		speed_select = 2;
+		pll_div = 0x10;
+		ana_pll_div = 0x92;
 		break;
 	case MHZ(1200):
 		speed_select = 1;
 		pll_div = 0x18;
+		ana_pll_div = 0x90;
 		break;
-	case MHZ(800):
-		speed_select = 2;
-		pll_div = 0x10;
+	case MHZ(1500):
+		speed_select = 0;
+		pll_div = 0x0f;
+		ana_pll_div = 0x82;
 		break;
-	case MHZ(400):
-		speed_select = 3;
+	case MHZ(1600):
+	default:
+		speed_select = 0;
 		pll_div = 0x10;
+		ana_pll_div = 0x82;
+		break;
+	case MHZ(2500):
+		speed_select = 0x10;
+		pll_div = 0x19;
+		ana_pll_div = 0x80;
 		break;
 	}
 
-	ub960_write(priv, UB960_SR_CSI_PLL_CTL, speed_select);
+	ub960_write(priv, UB960_SR_CSI_PLL_CTL, speed_select, &ret);
+	ub960_write(priv, UB9702_SR_CSI_PLL_DIV, pll_div, &ret);
+	ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA,
+			UB9702_IR_CSI_ANA_CSIPLL_REG_1, ana_pll_div, &ret);
 
-	if (priv->hw_data->is_ub9702) {
-		ub960_write(priv, UB960_SR_CSI_PLL_DIV, pll_div);
+	return ret;
+}
 
-		switch (priv->tx_data_rate) {
-		case MHZ(1600):
-		default:
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0x80);
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4b, 0x2a);
-			break;
-		case MHZ(800):
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0x90);
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4f, 0x2a);
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x4b, 0x2a);
-			break;
-		case MHZ(400):
-			ub960_write_ind(priv, UB960_IND_TARGET_CSI_ANA, 0x92, 0xa0);
-			break;
-		}
-	}
+static int ub960_init_tx_ports(struct ub960_data *priv)
+{
+	int ret;
 
-	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+	if (priv->hw_data->is_ub9702)
+		ret = ub960_init_tx_ports_ub9702(priv);
+	else
+		ret = ub960_init_tx_ports_ub960(priv);
+
+	if (ret)
+		return ret;
+
+	for (unsigned int nport = 0; nport < priv->hw_data->num_txports;
+	     nport++) {
 		struct ub960_txport *txport = priv->txports[nport];
 
 		if (!txport)
 			continue;
 
-		ub960_init_tx_port(priv, txport);
+		ret = ub960_init_tx_port(priv, txport);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
 }
 
-static void ub960_init_rx_port_ub960(struct ub960_data *priv,
-				     struct ub960_rxport *rxport)
+static int ub960_init_rx_port_ub960(struct ub960_data *priv,
+				    struct ub960_rxport *rxport)
 {
 	unsigned int nport = rxport->nport;
 	u32 bc_freq_val;
+	int ret = 0;
 
 	/*
 	 * Back channel frequency select.
@@ -1850,306 +2409,870 @@ static void ub960_init_rx_port_ub960(struct ub960_data *priv,
 		break;
 
 	default:
-		return;
+		return -EINVAL;
 	}
 
 	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
 				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK,
-				 bc_freq_val);
+				 bc_freq_val, &ret);
 
 	switch (rxport->rx_mode) {
 	case RXPORT_MODE_RAW10:
 		/* FPD3_MODE = RAW10 Mode (DS90UB913A-Q1 / DS90UB933-Q1 compatible) */
 		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG,
 					 UB960_RR_PORT_CONFIG_FPD3_MODE_MASK,
-					 0x3);
+					 0x3, &ret);
 
 		/*
 		 * RAW10_8BIT_CTL = 0b10 : 8-bit processing using upper 8 bits
 		 */
 		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2,
 			UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_MASK,
-			0x2 << UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_SHIFT);
+			0x2 << UB960_RR_PORT_CONFIG2_RAW10_8BIT_CTL_SHIFT,
+			&ret);
 
 		break;
 
 	case RXPORT_MODE_RAW12_HF:
 	case RXPORT_MODE_RAW12_LF:
 		/* Not implemented */
-		return;
+		return -EINVAL;
 
 	case RXPORT_MODE_CSI2_SYNC:
 	case RXPORT_MODE_CSI2_NONSYNC:
 		/* CSI-2 Mode (DS90UB953-Q1 compatible) */
 		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG, 0x3,
-					 0x0);
+					 0x0, &ret);
 
 		break;
 	}
 
 	/* LV_POLARITY & FV_POLARITY */
 	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3,
-				 rxport->lv_fv_pol);
+				 rxport->lv_fv_pol, &ret);
 
 	/* Enable all interrupt sources from this port */
-	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_HI, 0x07);
-	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_LO, 0x7f);
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_HI, 0x07, &ret);
+	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_LO, 0x7f, &ret);
 
 	/* Enable I2C_PASS_THROUGH */
 	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
 				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
-				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH);
+				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH, &ret);
 
 	/* Enable I2C communication to the serializer via the alias addr */
 	ub960_rxport_write(priv, nport, UB960_RR_SER_ALIAS_ID,
-			   rxport->ser.alias << 1);
+			   rxport->ser.alias << 1, &ret);
 
 	/* Configure EQ related settings */
 	ub960_rxport_config_eq(priv, nport);
 
 	/* Enable RX port */
-	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport));
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport),
+			  &ret);
+
+	return ret;
 }
 
-static void ub960_init_rx_port_ub9702_fpd3(struct ub960_data *priv,
-					   struct ub960_rxport *rxport)
+static int ub960_init_rx_ports_ub960(struct ub960_data *priv)
 {
-	unsigned int nport = rxport->nport;
-	u8 bc_freq_val;
-	u8 fpd_func_mode;
+	struct device *dev = &priv->client->dev;
+	unsigned int port_lock_mask;
+	unsigned int port_mask;
+	int ret;
 
-	switch (rxport->rx_mode) {
-	case RXPORT_MODE_RAW10:
-		bc_freq_val = 0;
-		fpd_func_mode = 5;
-		break;
+	for_each_active_rxport(priv, it) {
+		ret = ub960_init_rx_port_ub960(priv, it.rxport);
+		if (ret)
+			return ret;
+	}
 
-	case RXPORT_MODE_RAW12_HF:
-		bc_freq_val = 0;
-		fpd_func_mode = 4;
-		break;
+	ret = ub960_reset(priv, false);
+	if (ret)
+		return ret;
 
-	case RXPORT_MODE_RAW12_LF:
-		bc_freq_val = 0;
-		fpd_func_mode = 6;
-		break;
+	port_mask = 0;
 
-	case RXPORT_MODE_CSI2_SYNC:
-		bc_freq_val = 6;
-		fpd_func_mode = 2;
-		break;
+	for_each_active_rxport(priv, it)
+		port_mask |= BIT(it.nport);
 
-	case RXPORT_MODE_CSI2_NONSYNC:
-		bc_freq_val = 2;
-		fpd_func_mode = 2;
-		break;
+	ret = ub960_rxport_wait_locks(priv, port_mask, &port_lock_mask);
+	if (ret)
+		return ret;
 
-	default:
-		return;
+	if (port_mask != port_lock_mask) {
+		ret = -EIO;
+		dev_err_probe(dev, ret, "Failed to lock all RX ports\n");
+		return ret;
 	}
 
-	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG, 0x7,
-				 bc_freq_val);
-	ub960_rxport_write(priv, nport, UB960_RR_CHANNEL_MODE, fpd_func_mode);
+	/* Set temperature ramp on serializer */
+	for_each_active_rxport(priv, it) {
+		ret = ub960_serializer_temp_ramp(it.rxport);
+		if (ret)
+			return ret;
+
+		ub960_rxport_update_bits(priv, it.nport, UB960_RR_BCC_CONFIG,
+					 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+					 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+					 &ret);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * Clear any errors caused by switching the RX port settings while
+	 * probing.
+	 */
+	ret = ub960_clear_rx_errors(priv);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+/*
+ * UB9702 specific initial RX port configuration
+ */
+
+static int ub960_turn_off_rxport_ub9702(struct ub960_data *priv,
+					unsigned int nport)
+{
+	int ret = 0;
+
+	/* Disable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), 0, &ret);
+
+	/* Disable FPD Rx and FPD BC CMR */
+	ub960_rxport_write(priv, nport, UB9702_RR_RX_CTL_2, 0x1b, &ret);
+
+	/* Disable FPD BC Tx */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG, BIT(4), 0,
+				 &ret);
 
-	/* set serdes_eq_mode = 1 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xa8, 0x80);
+	/* Disable internal RX blocks */
+	ub960_rxport_write(priv, nport, UB9702_RR_RX_CTL_1, 0x15, &ret);
 
-	/* enable serdes driver */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x0d, 0x7f);
+	/* Disable AEQ */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_CFG_2, 0x03, &ret);
 
-	/* set serdes_eq_offset=4 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x04);
+	/* PI disabled and oDAC disabled */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_CFG_4, 0x09, &ret);
 
-	/* init default serdes_eq_max in 0xa9 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xa9, 0x23);
+	/* AEQ configured for disabled link */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_CFG_1, 0x20, &ret);
 
-	/* init serdes_eq_min in 0xaa */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xaa, 0);
+	/* disable AEQ clock and DFE */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_CFG_3, 0x45, &ret);
 
-	/* serdes_driver_ctl2 control: DS90UB953-Q1/DS90UB933-Q1/DS90UB913A-Q1 */
-	ub960_ind_update_bits(priv, UB960_IND_TARGET_RX_ANA(nport), 0x1b,
-			      BIT(3), BIT(3));
+	/* Powerdown FPD3 CDR */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_FPD3_CDR_CTRL_SEL_5, 0x82, &ret);
 
-	/* RX port to half-rate */
-	ub960_update_bits(priv, UB960_SR_FPD_RATE_CFG, 0x3 << (nport * 2),
-			  BIT(nport * 2));
+	return ret;
 }
 
-static void ub960_init_rx_port_ub9702_fpd4_aeq(struct ub960_data *priv,
-					       struct ub960_rxport *rxport)
+static int ub960_set_bc_drv_config_ub9702(struct ub960_data *priv,
+					  unsigned int nport)
 {
-	unsigned int nport = rxport->nport;
-	bool first_time_power_up = true;
-
-	if (first_time_power_up) {
-		u8 v;
+	u8 fpd_bc_ctl0;
+	u8 fpd_bc_ctl1;
+	u8 fpd_bc_ctl2;
+	int ret = 0;
 
-		/* AEQ init */
-		ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2c, &v);
+	if (priv->rxports[nport]->cdr_mode == RXPORT_CDR_FPD4) {
+		/* Set FPD PBC drv into FPD IV mode */
 
-		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x27, v);
-		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x28, v + 1);
+		fpd_bc_ctl0 = 0;
+		fpd_bc_ctl1 = 0;
+		fpd_bc_ctl2 = 0;
+	} else {
+		/* Set FPD PBC drv into FPD III mode */
 
-		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x00);
+		fpd_bc_ctl0 = 2;
+		fpd_bc_ctl1 = 1;
+		fpd_bc_ctl2 = 5;
 	}
 
-	/* enable serdes_eq_ctl2 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x9e, 0x00);
+	ub960_ind_update_bits(priv, UB960_IND_TARGET_RX_ANA(nport),
+			      UB9702_IR_RX_ANA_FPD_BC_CTL0, GENMASK(7, 5),
+			      fpd_bc_ctl0 << 5, &ret);
+
+	ub960_ind_update_bits(priv, UB960_IND_TARGET_RX_ANA(nport),
+			      UB9702_IR_RX_ANA_FPD_BC_CTL1, BIT(6),
+			      fpd_bc_ctl1 << 6, &ret);
+
+	ub960_ind_update_bits(priv, UB960_IND_TARGET_RX_ANA(nport),
+			      UB9702_IR_RX_ANA_FPD_BC_CTL2, GENMASK(6, 3),
+			      fpd_bc_ctl2 << 3, &ret);
+
+	return ret;
+}
+
+static int ub960_set_fpd4_sync_mode_ub9702(struct ub960_data *priv,
+					   unsigned int nport)
+{
+	int ret = 0;
+
+	/* FPD4 Sync Mode */
+	ub960_rxport_write(priv, nport, UB9702_RR_CHANNEL_MODE, 0x0, &ret);
+
+	/* BC_FREQ_SELECT = (PLL_FREQ/3200) Mbps */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK, 6, &ret);
+
+	if (ret)
+		return ret;
+
+	ret = ub960_set_bc_drv_config_ub9702(priv, nport);
+	if (ret)
+		return ret;
+
+	/* Set AEQ timer to 400us/step */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_SYSTEM_INIT_REG0, 0x2f, &ret);
 
-	/* enable serdes_eq_ctl1 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x90, 0x40);
+	/* Disable FPD4 Auto Recovery */
+	ub960_update_bits(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, GENMASK(5, 4), 0,
+			  &ret);
 
-	/* enable serdes_eq_en */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2e, 0x40);
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport),
+			  &ret);
 
-	/* disable serdes_eq_override */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0xf0, 0x00);
+	/* Enable FPD4 Auto Recovery */
+	ub960_update_bits(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, GENMASK(5, 4),
+			  BIT(4), &ret);
 
-	/* disable serdes_gain_override */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x71, 0x00);
+	return ret;
 }
 
-static void ub960_init_rx_port_ub9702_fpd4(struct ub960_data *priv,
-					   struct ub960_rxport *rxport)
+static int ub960_set_fpd4_async_mode_ub9702(struct ub960_data *priv,
+					    unsigned int nport)
 {
-	unsigned int nport = rxport->nport;
-	u8 bc_freq_val;
+	int ret = 0;
 
-	switch (rxport->rx_mode) {
-	case RXPORT_MODE_RAW10:
-		bc_freq_val = 0;
-		break;
+	/* FPD4 ASync Mode */
+	ub960_rxport_write(priv, nport, UB9702_RR_CHANNEL_MODE, 0x1, &ret);
 
-	case RXPORT_MODE_RAW12_HF:
-		bc_freq_val = 0;
-		break;
+	/* 10Mbps w/ BC enabled */
+	/* BC_FREQ_SELECT=(PLL_FREQ/3200) Mbps */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK, 2, &ret);
 
-	case RXPORT_MODE_RAW12_LF:
-		bc_freq_val = 0;
-		break;
+	if (ret)
+		return ret;
 
-	case RXPORT_MODE_CSI2_SYNC:
-		bc_freq_val = 6;
-		break;
+	ret = ub960_set_bc_drv_config_ub9702(priv, nport);
+	if (ret)
+		return ret;
 
-	case RXPORT_MODE_CSI2_NONSYNC:
-		bc_freq_val = 2;
-		break;
+	/* Set AEQ timer to 400us/step */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_SYSTEM_INIT_REG0, 0x2f, &ret);
 
-	default:
-		return;
-	}
+	/* Disable FPD4 Auto Recover */
+	ub960_update_bits(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, GENMASK(5, 4), 0,
+			  &ret);
 
-	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG, 0x7,
-				 bc_freq_val);
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport),
+			  &ret);
 
-	/* FPD4 Sync Mode */
-	ub960_rxport_write(priv, nport, UB960_RR_CHANNEL_MODE, 0);
+	/* Enable FPD4 Auto Recovery */
+	ub960_update_bits(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, GENMASK(5, 4),
+			  BIT(4), &ret);
 
-	/* add serdes_eq_offset of 4 */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x2b, 0x04);
+	return ret;
+}
 
-	/* FPD4 serdes_start_eq in 0x27: assign default */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x27, 0x0);
-	/* FPD4 serdes_end_eq in 0x28: assign default */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x28, 0x23);
+static int ub960_set_fpd3_sync_mode_ub9702(struct ub960_data *priv,
+					   unsigned int nport)
+{
+	int ret = 0;
 
-	/* set serdes_driver_mode into FPD IV mode */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x04, 0x00);
-	/* set FPD PBC drv into FPD IV mode */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x1b, 0x00);
+	/* FPD3 Sync Mode */
+	ub960_rxport_write(priv, nport, UB9702_RR_CHANNEL_MODE, 0x2, &ret);
 
-	/* set serdes_system_init to 0x2f */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x21, 0x2f);
-	/* set serdes_system_rst in reset mode */
-	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x25, 0xc1);
+	/* BC_FREQ_SELECT=(PLL_FREQ/3200) Mbps */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK, 6, &ret);
 
-	/* RX port to 7.55G mode */
-	ub960_update_bits(priv, UB960_SR_FPD_RATE_CFG, 0x3 << (nport * 2),
-			  0 << (nport * 2));
+	/* Set AEQ_LOCK_MODE = 1 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_FPD3_AEQ_CTRL_SEL_1, BIT(7), &ret);
 
-	ub960_init_rx_port_ub9702_fpd4_aeq(priv, rxport);
+	if (ret)
+		return ret;
+
+	ret = ub960_set_bc_drv_config_ub9702(priv, nport);
+	if (ret)
+		return ret;
+
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport),
+			  &ret);
+
+	return ret;
 }
 
-static void ub960_init_rx_port_ub9702(struct ub960_data *priv,
-				      struct ub960_rxport *rxport)
+static int ub960_set_raw10_dvp_mode_ub9702(struct ub960_data *priv,
+					   unsigned int nport)
 {
-	unsigned int nport = rxport->nport;
+	int ret = 0;
 
-	if (rxport->cdr_mode == RXPORT_CDR_FPD3)
-		ub960_init_rx_port_ub9702_fpd3(priv, rxport);
-	else /* RXPORT_CDR_FPD4 */
-		ub960_init_rx_port_ub9702_fpd4(priv, rxport);
+	/* FPD3 RAW10 Mode */
+	ub960_rxport_write(priv, nport, UB9702_RR_CHANNEL_MODE, 0x5, &ret);
 
-	switch (rxport->rx_mode) {
-	case RXPORT_MODE_RAW10:
-		/*
-		 * RAW10_8BIT_CTL = 0b11 : 8-bit processing using lower 8 bits
-		 * 0b10 : 8-bit processing using upper 8 bits
-		 */
-		ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2,
-					 0x3 << 6, 0x2 << 6);
+	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
+				 UB960_RR_BCC_CONFIG_BC_FREQ_SEL_MASK, 0, &ret);
+
+	/* Set AEQ_LOCK_MODE = 1 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_FPD3_AEQ_CTRL_SEL_1, BIT(7), &ret);
+
+	/*
+	 * RAW10_8BIT_CTL = 0b11 : 8-bit processing using lower 8 bits
+	 * 0b10 : 8-bit processing using upper 8 bits
+	 */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3 << 6,
+				 0x2 << 6, &ret);
+
+	/* LV_POLARITY & FV_POLARITY */
+	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3,
+				 priv->rxports[nport]->lv_fv_pol, &ret);
+
+	if (ret)
+		return ret;
+
+	ret = ub960_set_bc_drv_config_ub9702(priv, nport);
+	if (ret)
+		return ret;
+
+	/* Enable RX port */
+	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport),
+			  &ret);
+
+	return ret;
+}
+
+static int ub960_configure_rx_port_ub9702(struct ub960_data *priv,
+					  unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+	struct ub960_rxport *rxport = priv->rxports[nport];
+	int ret;
+
+	if (!rxport) {
+		ret = ub960_turn_off_rxport_ub9702(priv, nport);
+		if (ret)
+			return ret;
 
+		dev_dbg(dev, "rx%u: disabled\n", nport);
+		return 0;
+	}
+
+	switch (rxport->cdr_mode) {
+	case RXPORT_CDR_FPD4:
+		switch (rxport->rx_mode) {
+		case RXPORT_MODE_CSI2_SYNC:
+			ret = ub960_set_fpd4_sync_mode_ub9702(priv, nport);
+			if (ret)
+				return ret;
+
+			dev_dbg(dev, "rx%u: FPD-Link IV SYNC mode\n", nport);
+			break;
+		case RXPORT_MODE_CSI2_NONSYNC:
+			ret = ub960_set_fpd4_async_mode_ub9702(priv, nport);
+			if (ret)
+				return ret;
+
+			dev_dbg(dev, "rx%u: FPD-Link IV ASYNC mode\n", nport);
+			break;
+		default:
+			dev_err(dev, "rx%u: unsupported FPD4 mode %u\n", nport,
+				rxport->rx_mode);
+			return -EINVAL;
+		}
 		break;
 
-	case RXPORT_MODE_RAW12_HF:
-	case RXPORT_MODE_RAW12_LF:
-		/* Not implemented */
-		return;
+	case RXPORT_CDR_FPD3:
+		switch (rxport->rx_mode) {
+		case RXPORT_MODE_CSI2_SYNC:
+			ret = ub960_set_fpd3_sync_mode_ub9702(priv, nport);
+			if (ret)
+				return ret;
 
-	case RXPORT_MODE_CSI2_SYNC:
-	case RXPORT_MODE_CSI2_NONSYNC:
+			dev_dbg(dev, "rx%u: FPD-Link III SYNC mode\n", nport);
+			break;
+		case RXPORT_MODE_RAW10:
+			ret = ub960_set_raw10_dvp_mode_ub9702(priv, nport);
+			if (ret)
+				return ret;
 
+			dev_dbg(dev, "rx%u: FPD-Link III RAW10 DVP mode\n",
+				nport);
+			break;
+		default:
+			dev_err(&priv->client->dev,
+				"rx%u: unsupported FPD3 mode %u\n", nport,
+				rxport->rx_mode);
+			return -EINVAL;
+		}
 		break;
+
+	default:
+		dev_err(&priv->client->dev, "rx%u: unsupported CDR mode %u\n",
+			nport, rxport->cdr_mode);
+		return -EINVAL;
 	}
 
-	/* LV_POLARITY & FV_POLARITY */
-	ub960_rxport_update_bits(priv, nport, UB960_RR_PORT_CONFIG2, 0x3,
-				 rxport->lv_fv_pol);
+	return 0;
+}
 
-	/* Enable all interrupt sources from this port */
-	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_HI, 0x07);
-	ub960_rxport_write(priv, nport, UB960_RR_PORT_ICR_LO, 0x7f);
+static int ub960_lock_recovery_ub9702(struct ub960_data *priv,
+				      unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+	/* Assumption that max AEQ should be under 16 */
+	const u8 rx_aeq_limit = 16;
+	u8 prev_aeq = 0xff;
+	bool rx_lock;
+
+	for (unsigned int retry = 0; retry < 3; ++retry) {
+		u8 port_sts1;
+		u8 rx_aeq;
+		int ret;
 
-	/* Enable I2C_PASS_THROUGH */
-	ub960_rxport_update_bits(priv, nport, UB960_RR_BCC_CONFIG,
-				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
-				 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH);
+		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1,
+					&port_sts1, NULL);
+		if (ret)
+			return ret;
 
-	/* Enable I2C communication to the serializer via the alias addr */
-	ub960_rxport_write(priv, nport, UB960_RR_SER_ALIAS_ID,
-			   rxport->ser.alias << 1);
+		rx_lock = port_sts1 & UB960_RR_RX_PORT_STS1_PORT_PASS;
 
-	/* Enable RX port */
-	ub960_update_bits(priv, UB960_SR_RX_PORT_CTL, BIT(nport), BIT(nport));
+		if (!rx_lock) {
+			ret = ub960_rxport_lockup_wa_ub9702(priv);
+			if (ret)
+				return ret;
+
+			/* Restart AEQ by changing max to 0 --> 0x23 */
+			ret = ub960_write_ind(priv,
+					      UB960_IND_TARGET_RX_ANA(nport),
+					      UB9702_IR_RX_ANA_AEQ_ALP_SEL7, 0,
+					      NULL);
+			if (ret)
+				return ret;
+
+			msleep(20);
+
+			/* AEQ Restart */
+			ret = ub960_write_ind(priv,
+					      UB960_IND_TARGET_RX_ANA(nport),
+					      UB9702_IR_RX_ANA_AEQ_ALP_SEL7,
+					      0x23, NULL);
+
+			if (ret)
+				return ret;
+
+			msleep(20);
+			dev_dbg(dev, "rx%u: no lock, retry = %u\n", nport,
+				retry);
+
+			continue;
+		}
+
+		ret = ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+				     UB9702_IR_RX_ANA_AEQ_ALP_SEL11, &rx_aeq,
+				     NULL);
+		if (ret)
+			return ret;
+
+		if (rx_aeq < rx_aeq_limit) {
+			dev_dbg(dev,
+				"rx%u: locked and AEQ normal before setting AEQ window\n",
+				nport);
+			return 0;
+		}
+
+		if (rx_aeq != prev_aeq) {
+			ret = ub960_rxport_lockup_wa_ub9702(priv);
+			if (ret)
+				return ret;
 
-	if (rxport->cdr_mode == RXPORT_CDR_FPD4) {
-		/* unreset 960 AEQ */
-		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport), 0x25, 0x41);
+			/* Restart AEQ by changing max to 0 --> 0x23 */
+			ret = ub960_write_ind(priv,
+					      UB960_IND_TARGET_RX_ANA(nport),
+					      UB9702_IR_RX_ANA_AEQ_ALP_SEL7,
+					      0, NULL);
+			if (ret)
+				return ret;
+
+			msleep(20);
+
+			/* AEQ Restart */
+			ret = ub960_write_ind(priv,
+					      UB960_IND_TARGET_RX_ANA(nport),
+					      UB9702_IR_RX_ANA_AEQ_ALP_SEL7,
+					      0x23, NULL);
+			if (ret)
+				return ret;
+
+			msleep(20);
+
+			dev_dbg(dev,
+				"rx%u: high AEQ at initial check recovery loop, retry=%u\n",
+				nport, retry);
+
+			prev_aeq = rx_aeq;
+		} else {
+			dev_dbg(dev,
+				"rx%u: lossy cable detected, RX_AEQ %#x, RX_AEQ_LIMIT %#x, retry %u\n",
+				nport, rx_aeq, rx_aeq_limit, retry);
+			dev_dbg(dev,
+				"rx%u: will continue with initiation sequence but high AEQ\n",
+				nport);
+			return 0;
+		}
 	}
+
+	dev_err(dev, "rx%u: max number of retries: %s\n", nport,
+		rx_lock ? "unstable AEQ" : "no lock");
+
+	return -EIO;
 }
 
-static int ub960_init_rx_ports(struct ub960_data *priv)
+static int ub960_enable_aeq_lms_ub9702(struct ub960_data *priv,
+				       unsigned int nport)
 {
-	unsigned int nport;
+	struct device *dev = &priv->client->dev;
+	u8 read_aeq_init;
+	int ret;
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+	ret = ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			     UB9702_IR_RX_ANA_AEQ_ALP_SEL11, &read_aeq_init,
+			     NULL);
+	if (ret)
+		return ret;
 
-		if (!rxport)
-			continue;
+	dev_dbg(dev, "rx%u: initial AEQ = %#x\n", nport, read_aeq_init);
 
-		if (priv->hw_data->is_ub9702)
-			ub960_init_rx_port_ub9702(priv, rxport);
-		else
-			ub960_init_rx_port_ub960(priv, rxport);
+	/* Set AEQ Min */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_ALP_SEL6, read_aeq_init, &ret);
+	/* Set AEQ Max */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_ALP_SEL7, read_aeq_init + 1, &ret);
+	/* Set AEQ offset to 0 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_AEQ_ALP_SEL10, 0x0, &ret);
+
+	/* Enable AEQ tap2 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_EQ_CTRL_SEL_38, 0x00, &ret);
+	/* Set VGA Gain 1 Gain 2 override to 0 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_VGA_CTRL_SEL_8, 0x00, &ret);
+	/* Set VGA Initial Sweep Gain to 0 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_VGA_CTRL_SEL_6, 0x80, &ret);
+	/* Set VGA_Adapt (VGA Gain) override to 0 (thermometer encoded) */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_VGA_CTRL_SEL_3, 0x00, &ret);
+	/* Enable VGA_SWEEP */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_EQ_ADAPT_CTRL, 0x40, &ret);
+	/* Disable VGA_SWEEP_GAIN_OV, disable VGA_TUNE_OV */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_EQ_OVERRIDE_CTRL, 0x00, &ret);
+
+	/* Set VGA HIGH Threshold to 43 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_VGA_CTRL_SEL_1, 0x2b, &ret);
+	/* Set VGA LOW Threshold to 18 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_VGA_CTRL_SEL_2, 0x12, &ret);
+	/* Set vga_sweep_th to 32 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_EQ_CTRL_SEL_15, 0x20, &ret);
+	/* Set AEQ timer to 400us/step and parity threshold to 7 */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_SYSTEM_INIT_REG0, 0xef, &ret);
+
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "rx%u: enable FPD-Link IV AEQ LMS\n", nport);
+
+	return 0;
+}
+
+static int ub960_enable_dfe_lms_ub9702(struct ub960_data *priv,
+				       unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+	int ret = 0;
+
+	/* Enable DFE LMS */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_EQ_CTRL_SEL_24, 0x40, &ret);
+	/* Disable VGA Gain1 override */
+	ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			UB9702_IR_RX_ANA_GAIN_CTRL_0, 0x20, &ret);
+
+	if (ret)
+		return ret;
+
+	usleep_range(1000, 5000);
+
+	/* Disable VGA Gain2 override */
+	ret = ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(nport),
+			      UB9702_IR_RX_ANA_GAIN_CTRL_0, 0x00, NULL);
+	if (ret)
+		return ret;
+
+	dev_dbg(dev, "rx%u: enabled FPD-Link IV DFE LMS", nport);
+
+	return 0;
+}
+
+static int ub960_init_rx_ports_ub9702(struct ub960_data *priv)
+{
+	struct device *dev = &priv->client->dev;
+	unsigned int port_lock_mask;
+	unsigned int port_mask = 0;
+	bool have_fpd4 = false;
+	int ret;
+
+	for_each_active_rxport(priv, it) {
+		ret = ub960_rxport_update_bits(priv, it.nport,
+					       UB960_RR_BCC_CONFIG,
+					       UB960_RR_BCC_CONFIG_BC_ALWAYS_ON,
+					       UB960_RR_BCC_CONFIG_BC_ALWAYS_ON,
+					       NULL);
+		if (ret)
+			return ret;
+	}
+
+	/* Disable FPD4 Auto Recovery */
+	ret = ub960_write(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, 0x0f, NULL);
+	if (ret)
+		return ret;
+
+	for_each_active_rxport(priv, it) {
+		if (it.rxport->ser.addr >= 0) {
+			/*
+			 * Set serializer's I2C address if set in the dts file,
+			 * and freeze it to prevent updates from the FC.
+			 */
+			ub960_rxport_write(priv, it.nport, UB960_RR_SER_ID,
+					   it.rxport->ser.addr << 1 |
+					   UB960_RR_SER_ID_FREEZE_DEVICE_ID,
+					   &ret);
+		}
+
+		/* Set serializer I2C alias with auto-ack */
+		ub960_rxport_write(priv, it.nport, UB960_RR_SER_ALIAS_ID,
+				   it.rxport->ser.alias << 1 |
+				   UB960_RR_SER_ALIAS_ID_AUTO_ACK, &ret);
+
+		if (ret)
+			return ret;
+	}
+
+	for_each_active_rxport(priv, it) {
+		if (fwnode_device_is_compatible(it.rxport->ser.fwnode,
+						"ti,ds90ub971-q1")) {
+			ret = ub960_rxport_bc_ser_config(it.rxport);
+			if (ret)
+				return ret;
+		}
+	}
+
+	for_each_active_rxport_fpd4(priv, it) {
+		/* Hold state machine in reset */
+		ub960_rxport_write(priv, it.nport, UB9702_RR_RX_SM_SEL_2, 0x10,
+				   &ret);
+
+		/* Set AEQ max to 0 */
+		ub960_write_ind(priv, UB960_IND_TARGET_RX_ANA(it.nport),
+				UB9702_IR_RX_ANA_AEQ_ALP_SEL7, 0, &ret);
+
+		if (ret)
+			return ret;
+
+		dev_dbg(dev,
+			"rx%u: holding state machine and adjusting AEQ max to 0",
+			it.nport);
+	}
+
+	for_each_active_rxport(priv, it) {
+		port_mask |= BIT(it.nport);
+
+		if (it.rxport->cdr_mode == RXPORT_CDR_FPD4)
+			have_fpd4 = true;
+	}
+
+	for_each_rxport(priv, it) {
+		ret = ub960_configure_rx_port_ub9702(priv, it.nport);
+		if (ret)
+			return ret;
+	}
+
+	ret = ub960_reset(priv, false);
+	if (ret)
+		return ret;
+
+	if (have_fpd4) {
+		for_each_active_rxport_fpd4(priv, it) {
+			/* Release state machine */
+			ret = ub960_rxport_write(priv, it.nport,
+						 UB9702_RR_RX_SM_SEL_2, 0x0,
+						 NULL);
+			if (ret)
+				return ret;
+
+			dev_dbg(dev, "rx%u: state machine released\n",
+				it.nport);
+		}
+
+		/* Wait for SM to resume */
+		fsleep(5000);
+
+		for_each_active_rxport_fpd4(priv, it) {
+			ret = ub960_write_ind(priv,
+					      UB960_IND_TARGET_RX_ANA(it.nport),
+					      UB9702_IR_RX_ANA_AEQ_ALP_SEL7,
+					      0x23, NULL);
+			if (ret)
+				return ret;
+
+			dev_dbg(dev, "rx%u: AEQ restart\n", it.nport);
+		}
+
+		/* Wait for lock */
+		fsleep(20000);
+
+		for_each_active_rxport_fpd4(priv, it) {
+			ret = ub960_lock_recovery_ub9702(priv, it.nport);
+			if (ret)
+				return ret;
+		}
+
+		for_each_active_rxport_fpd4(priv, it) {
+			ret = ub960_enable_aeq_lms_ub9702(priv, it.nport);
+			if (ret)
+				return ret;
+		}
+
+		for_each_active_rxport_fpd4(priv, it) {
+			/* Hold state machine in reset */
+			ret = ub960_rxport_write(priv, it.nport,
+						 UB9702_RR_RX_SM_SEL_2, 0x10,
+						 NULL);
+			if (ret)
+				return ret;
+		}
+
+		ret = ub960_reset(priv, false);
+		if (ret)
+			return ret;
+
+		for_each_active_rxport_fpd4(priv, it) {
+			/* Release state machine */
+			ret = ub960_rxport_write(priv, it.nport,
+						 UB9702_RR_RX_SM_SEL_2, 0,
+						 NULL);
+			if (ret)
+				return ret;
+		}
+	}
+
+	/* Wait time for stable lock */
+	fsleep(15000);
+
+	/* Set temperature ramp on serializer */
+	for_each_active_rxport(priv, it) {
+		ret = ub960_serializer_temp_ramp(it.rxport);
+		if (ret)
+			return ret;
+	}
+
+	for_each_active_rxport_fpd4(priv, it) {
+		ret = ub960_enable_dfe_lms_ub9702(priv, it.nport);
+		if (ret)
+			return ret;
+	}
+
+	/* Wait for DFE and LMS to adapt */
+	fsleep(5000);
+
+	ret = ub960_rxport_wait_locks(priv, port_mask, &port_lock_mask);
+	if (ret)
+		return ret;
+
+	if (port_mask != port_lock_mask) {
+		ret = -EIO;
+		dev_err_probe(dev, ret, "Failed to lock all RX ports\n");
+		return ret;
+	}
+
+	for_each_active_rxport(priv, it) {
+		/* Enable all interrupt sources from this port */
+		ub960_rxport_write(priv, it.nport, UB960_RR_PORT_ICR_HI, 0x07,
+				   &ret);
+		ub960_rxport_write(priv, it.nport, UB960_RR_PORT_ICR_LO, 0x7f,
+				   &ret);
+
+		/* Clear serializer I2C alias auto-ack */
+		ub960_rxport_update_bits(priv, it.nport, UB960_RR_SER_ALIAS_ID,
+					 UB960_RR_SER_ALIAS_ID_AUTO_ACK, 0,
+					 &ret);
+
+		/* Enable I2C_PASS_THROUGH */
+		ub960_rxport_update_bits(priv, it.nport, UB960_RR_BCC_CONFIG,
+					 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+					 UB960_RR_BCC_CONFIG_I2C_PASS_THROUGH,
+					 &ret);
+
+		if (ret)
+			return ret;
 	}
 
+	/* Enable FPD4 Auto Recovery, Recovery loop active */
+	ret = ub960_write(priv, UB9702_SR_CSI_EXCLUSIVE_FWD2, 0x18, NULL);
+	if (ret)
+		return ret;
+
+	for_each_active_rxport_fpd4(priv, it) {
+		u8 final_aeq;
+
+		ret = ub960_read_ind(priv, UB960_IND_TARGET_RX_ANA(it.nport),
+				     UB9702_IR_RX_ANA_AEQ_ALP_SEL11, &final_aeq,
+				     NULL);
+		if (ret)
+			return ret;
+
+		dev_dbg(dev, "rx%u: final AEQ = %#x\n", it.nport, final_aeq);
+	}
+
+	/*
+	 * Clear any errors caused by switching the RX port settings while
+	 * probing.
+	 */
+
+	ret = ub960_clear_rx_errors(priv);
+	if (ret)
+		return ret;
+
 	return 0;
 }
 
-static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
+static int ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
 {
 	struct device *dev = &priv->client->dev;
 	u8 rx_port_sts1;
@@ -2159,27 +3282,21 @@ static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
 	int ret = 0;
 
 	/* Read interrupts (also clears most of them) */
-	if (!ret)
-		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1,
-					&rx_port_sts1);
-	if (!ret)
-		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2,
-					&rx_port_sts2);
-	if (!ret)
-		ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS,
-					&csi_rx_sts);
-	if (!ret)
-		ret = ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS,
-					&bcc_sts);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &rx_port_sts1,
+			  &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &rx_port_sts2,
+			  &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_CSI_RX_STS, &csi_rx_sts, &ret);
+	ub960_rxport_read(priv, nport, UB960_RR_BCC_STATUS, &bcc_sts, &ret);
 
 	if (ret)
-		return;
+		return ret;
 
 	if (rx_port_sts1 & UB960_RR_RX_PORT_STS1_PARITY_ERROR) {
 		u16 v;
 
 		ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI,
-					  &v);
+					  &v, NULL);
 		if (!ret)
 			dev_err(dev, "rx%u parity errors: %u\n", nport, v);
 	}
@@ -2238,7 +3355,8 @@ static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
 	if (rx_port_sts2 & UB960_RR_RX_PORT_STS2_LINE_LEN_CHG) {
 		u16 v;
 
-		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1, &v);
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1,
+					  &v, NULL);
 		if (!ret)
 			dev_dbg(dev, "rx%u line len changed: %u\n", nport, v);
 	}
@@ -2247,7 +3365,7 @@ static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
 		u16 v;
 
 		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_COUNT_HI,
-					  &v);
+					  &v, NULL);
 		if (!ret)
 			dev_dbg(dev, "rx%u line count changed: %u\n", nport, v);
 	}
@@ -2267,6 +3385,8 @@ static void ub960_rxport_handle_events(struct ub960_data *priv, u8 nport)
 				"stable freq" :
 				"unstable freq");
 	}
+
+	return 0;
 }
 
 /* -----------------------------------------------------------------------------
@@ -2319,17 +3439,17 @@ static int ub960_enable_tx_port(struct ub960_data *priv, unsigned int nport)
 
 	return ub960_txport_update_bits(priv, nport, UB960_TR_CSI_CTL,
 					UB960_TR_CSI_CTL_CSI_ENABLE,
-					UB960_TR_CSI_CTL_CSI_ENABLE);
+					UB960_TR_CSI_CTL_CSI_ENABLE, NULL);
 }
 
-static void ub960_disable_tx_port(struct ub960_data *priv, unsigned int nport)
+static int ub960_disable_tx_port(struct ub960_data *priv, unsigned int nport)
 {
 	struct device *dev = &priv->client->dev;
 
 	dev_dbg(dev, "disable TX port %u\n", nport);
 
-	ub960_txport_update_bits(priv, nport, UB960_TR_CSI_CTL,
-				 UB960_TR_CSI_CTL_CSI_ENABLE, 0);
+	return ub960_txport_update_bits(priv, nport, UB960_TR_CSI_CTL,
+					UB960_TR_CSI_CTL_CSI_ENABLE, 0, NULL);
 }
 
 static int ub960_enable_rx_port(struct ub960_data *priv, unsigned int nport)
@@ -2340,19 +3460,19 @@ static int ub960_enable_rx_port(struct ub960_data *priv, unsigned int nport)
 
 	/* Enable forwarding */
 	return ub960_update_bits(priv, UB960_SR_FWD_CTL1,
-				 UB960_SR_FWD_CTL1_PORT_DIS(nport), 0);
+				 UB960_SR_FWD_CTL1_PORT_DIS(nport), 0, NULL);
 }
 
-static void ub960_disable_rx_port(struct ub960_data *priv, unsigned int nport)
+static int ub960_disable_rx_port(struct ub960_data *priv, unsigned int nport)
 {
 	struct device *dev = &priv->client->dev;
 
 	dev_dbg(dev, "disable RX port %u\n", nport);
 
 	/* Disable forwarding */
-	ub960_update_bits(priv, UB960_SR_FWD_CTL1,
-			  UB960_SR_FWD_CTL1_PORT_DIS(nport),
-			  UB960_SR_FWD_CTL1_PORT_DIS(nport));
+	return ub960_update_bits(priv, UB960_SR_FWD_CTL1,
+				 UB960_SR_FWD_CTL1_PORT_DIS(nport),
+				 UB960_SR_FWD_CTL1_PORT_DIS(nport), NULL);
 }
 
 /*
@@ -2361,20 +3481,14 @@ static void ub960_disable_rx_port(struct ub960_data *priv, unsigned int nport)
  */
 static int ub960_validate_stream_vcs(struct ub960_data *priv)
 {
-	unsigned int nport;
-	unsigned int i;
-
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+	for_each_active_rxport(priv, it) {
 		struct v4l2_mbus_frame_desc desc;
 		int ret;
 		u8 vc;
 
-		if (!rxport)
-			continue;
-
-		ret = v4l2_subdev_call(rxport->source.sd, pad, get_frame_desc,
-				       rxport->source.pad, &desc);
+		ret = v4l2_subdev_call(it.rxport->source.sd, pad,
+				       get_frame_desc, it.rxport->source.pad,
+				       &desc);
 		if (ret)
 			return ret;
 
@@ -2386,13 +3500,13 @@ static int ub960_validate_stream_vcs(struct ub960_data *priv)
 
 		vc = desc.entry[0].bus.csi2.vc;
 
-		for (i = 1; i < desc.num_entries; i++) {
+		for (unsigned int i = 1; i < desc.num_entries; i++) {
 			if (vc == desc.entry[i].bus.csi2.vc)
 				continue;
 
 			dev_err(&priv->client->dev,
 				"rx%u: source with multiple virtual-channels is not supported\n",
-				nport);
+				it.nport);
 			return -ENODEV;
 		}
 	}
@@ -2413,7 +3527,6 @@ static int ub960_configure_ports_for_streaming(struct ub960_data *priv,
 	} rx_data[UB960_MAX_RX_NPORTS] = {};
 	u8 vc_map[UB960_MAX_RX_NPORTS] = {};
 	struct v4l2_subdev_route *route;
-	unsigned int nport;
 	int ret;
 
 	ret = ub960_validate_stream_vcs(priv);
@@ -2483,22 +3596,24 @@ static int ub960_configure_ports_for_streaming(struct ub960_data *priv,
 	 */
 	fwd_ctl = GENMASK(7, 4);
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+	for_each_active_rxport(priv, it) {
+		unsigned long nport = it.nport;
+
 		u8 vc = vc_map[nport];
 
 		if (rx_data[nport].num_streams == 0)
 			continue;
 
-		switch (rxport->rx_mode) {
+		switch (it.rxport->rx_mode) {
 		case RXPORT_MODE_RAW10:
 			ub960_rxport_write(priv, nport, UB960_RR_RAW10_ID,
-				rx_data[nport].pixel_dt | (vc << UB960_RR_RAW10_ID_VC_SHIFT));
+				rx_data[nport].pixel_dt | (vc << UB960_RR_RAW10_ID_VC_SHIFT),
+				&ret);
 
-			ub960_rxport_write(priv, rxport->nport,
+			ub960_rxport_write(priv, nport,
 				UB960_RR_RAW_EMBED_DTYPE,
 				(rx_data[nport].meta_lines << UB960_RR_RAW_EMBED_DTYPE_LINES_SHIFT) |
-					rx_data[nport].meta_dt);
+					rx_data[nport].meta_dt, &ret);
 
 			break;
 
@@ -2515,15 +3630,17 @@ static int ub960_configure_ports_for_streaming(struct ub960_data *priv,
 						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(3)) |
 						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(2)) |
 						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(1)) |
-						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(0)));
+						   (vc << UB960_RR_CSI_VC_MAP_SHIFT(0)),
+						   &ret);
 			} else {
 				unsigned int i;
 
 				/* Map all VCs from this port to VC(nport) */
 				for (i = 0; i < 8; i++)
 					ub960_rxport_write(priv, nport,
-							   UB960_RR_VC_ID_MAP(i),
-							   nport);
+							   UB9702_RR_VC_ID_MAP(i),
+							   (nport << 4) | nport,
+							   &ret);
 			}
 
 			break;
@@ -2535,9 +3652,9 @@ static int ub960_configure_ports_for_streaming(struct ub960_data *priv,
 			fwd_ctl &= ~BIT(nport); /* forward to TX0 */
 	}
 
-	ub960_write(priv, UB960_SR_FWD_CTL1, fwd_ctl);
+	ub960_write(priv, UB960_SR_FWD_CTL1, fwd_ctl, &ret);
 
-	return 0;
+	return ret;
 }
 
 static void ub960_update_streaming_status(struct ub960_data *priv)
@@ -2561,7 +3678,6 @@ static int ub960_enable_streams(struct v4l2_subdev *sd,
 	u64 sink_streams[UB960_MAX_RX_NPORTS] = {};
 	struct v4l2_subdev_route *route;
 	unsigned int failed_port;
-	unsigned int nport;
 	int ret;
 
 	if (!priv->streaming) {
@@ -2583,6 +3699,8 @@ static int ub960_enable_streams(struct v4l2_subdev *sd,
 
 	/* Collect sink streams per pad which we need to enable */
 	for_each_active_route(&state->routing, route) {
+		unsigned int nport;
+
 		if (route->source_pad != source_pad)
 			continue;
 
@@ -2594,7 +3712,9 @@ static int ub960_enable_streams(struct v4l2_subdev *sd,
 		sink_streams[nport] |= BIT_ULL(route->sink_stream);
 	}
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+	for_each_rxport(priv, it) {
+		unsigned int nport = it.nport;
+
 		if (!sink_streams[nport])
 			continue;
 
@@ -2632,7 +3752,7 @@ static int ub960_enable_streams(struct v4l2_subdev *sd,
 	return 0;
 
 err:
-	for (nport = 0; nport < failed_port; nport++) {
+	for (unsigned int nport = 0; nport < failed_port; nport++) {
 		if (!sink_streams[nport])
 			continue;
 
@@ -2672,11 +3792,12 @@ static int ub960_disable_streams(struct v4l2_subdev *sd,
 	struct device *dev = &priv->client->dev;
 	u64 sink_streams[UB960_MAX_RX_NPORTS] = {};
 	struct v4l2_subdev_route *route;
-	unsigned int nport;
 	int ret;
 
 	/* Collect sink streams per pad which we need to disable */
 	for_each_active_route(&state->routing, route) {
+		unsigned int nport;
+
 		if (route->source_pad != source_pad)
 			continue;
 
@@ -2688,7 +3809,9 @@ static int ub960_disable_streams(struct v4l2_subdev *sd,
 		sink_streams[nport] |= BIT_ULL(route->sink_stream);
 	}
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+	for_each_rxport(priv, it) {
+		unsigned int nport = it.nport;
+
 		if (!sink_streams[nport])
 			continue;
 
@@ -2940,25 +4063,93 @@ static const struct v4l2_subdev_pad_ops ub960_pad_ops = {
 	.set_fmt = ub960_set_fmt,
 };
 
+static int ub960_log_status_ub960_sp_eq(struct ub960_data *priv,
+					unsigned int nport)
+{
+	struct device *dev = &priv->client->dev;
+	u8 eq_level;
+	s8 strobe_pos;
+	int ret;
+	u8 v;
+
+	/* Strobe */
+
+	ret = ub960_read(priv, UB960_XR_AEQ_CTL1, &v, NULL);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "\t%s strobe\n",
+		 (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) ? "Adaptive" :
+							  "Manual");
+
+	if (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) {
+		ret = ub960_read(priv, UB960_XR_SFILTER_CFG, &v, NULL);
+		if (ret)
+			return ret;
+
+		dev_info(dev, "\tStrobe range [%d, %d]\n",
+			 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) & 0xf) - 7,
+			 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT) & 0xf) - 7);
+	}
+
+	ret = ub960_rxport_get_strobe_pos(priv, nport, &strobe_pos);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "\tStrobe pos %d\n", strobe_pos);
+
+	/* EQ */
+
+	ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v, NULL);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "\t%s EQ\n",
+		 (v & UB960_RR_AEQ_BYPASS_ENABLE) ? "Manual" :
+						    "Adaptive");
+
+	if (!(v & UB960_RR_AEQ_BYPASS_ENABLE)) {
+		ret = ub960_rxport_read(priv, nport, UB960_RR_AEQ_MIN_MAX, &v,
+					NULL);
+		if (ret)
+			return ret;
+
+		dev_info(dev, "\tEQ range [%u, %u]\n",
+			 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT) & 0xf,
+			 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT) & 0xf);
+	}
+
+	ret = ub960_rxport_get_eq_level(priv, nport, &eq_level);
+	if (ret)
+		return ret;
+
+	dev_info(dev, "\tEQ level %u\n", eq_level);
+
+	return 0;
+}
+
 static int ub960_log_status(struct v4l2_subdev *sd)
 {
 	struct ub960_data *priv = sd_to_ub960(sd);
 	struct device *dev = &priv->client->dev;
 	struct v4l2_subdev_state *state;
-	unsigned int nport;
-	unsigned int i;
 	u16 v16 = 0;
 	u8 v = 0;
 	u8 id[UB960_SR_FPD3_RX_ID_LEN];
+	int ret = 0;
 
 	state = v4l2_subdev_lock_and_get_active_state(sd);
 
-	for (i = 0; i < sizeof(id); i++)
-		ub960_read(priv, UB960_SR_FPD3_RX_ID(i), &id[i]);
+	for (unsigned int i = 0; i < sizeof(id); i++) {
+		ret = ub960_read(priv, UB960_SR_FPD3_RX_ID(i), &id[i], NULL);
+		if (ret)
+			return ret;
+	}
 
 	dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
 
-	for (nport = 0; nport < priv->hw_data->num_txports; nport++) {
+	for (unsigned int nport = 0; nport < priv->hw_data->num_txports;
+	     nport++) {
 		struct ub960_txport *txport = priv->txports[nport];
 
 		dev_info(dev, "TX %u\n", nport);
@@ -2968,37 +4159,56 @@ static int ub960_log_status(struct v4l2_subdev *sd)
 			continue;
 		}
 
-		ub960_txport_read(priv, nport, UB960_TR_CSI_STS, &v);
+		ret = ub960_txport_read(priv, nport, UB960_TR_CSI_STS, &v, NULL);
+		if (ret)
+			return ret;
+
 		dev_info(dev, "\tsync %u, pass %u\n", v & (u8)BIT(1),
 			 v & (u8)BIT(0));
 
-		ub960_read16(priv, UB960_SR_CSI_FRAME_COUNT_HI(nport), &v16);
+		ret = ub960_read16(priv, UB960_SR_CSI_FRAME_COUNT_HI(nport),
+				   &v16, NULL);
+		if (ret)
+			return ret;
+
 		dev_info(dev, "\tframe counter %u\n", v16);
 
-		ub960_read16(priv, UB960_SR_CSI_FRAME_ERR_COUNT_HI(nport), &v16);
+		ret = ub960_read16(priv, UB960_SR_CSI_FRAME_ERR_COUNT_HI(nport),
+				   &v16, NULL);
+		if (ret)
+			return ret;
+
 		dev_info(dev, "\tframe error counter %u\n", v16);
 
-		ub960_read16(priv, UB960_SR_CSI_LINE_COUNT_HI(nport), &v16);
+		ret = ub960_read16(priv, UB960_SR_CSI_LINE_COUNT_HI(nport),
+				   &v16, NULL);
+		if (ret)
+			return ret;
+
 		dev_info(dev, "\tline counter %u\n", v16);
 
-		ub960_read16(priv, UB960_SR_CSI_LINE_ERR_COUNT_HI(nport), &v16);
+		ret = ub960_read16(priv, UB960_SR_CSI_LINE_ERR_COUNT_HI(nport),
+				   &v16, NULL);
+		if (ret)
+			return ret;
+
 		dev_info(dev, "\tline error counter %u\n", v16);
 	}
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-		u8 eq_level;
-		s8 strobe_pos;
-		unsigned int i;
+	for_each_rxport(priv, it) {
+		unsigned int nport = it.nport;
 
 		dev_info(dev, "RX %u\n", nport);
 
-		if (!rxport) {
+		if (!it.rxport) {
 			dev_info(dev, "\tNot initialized\n");
 			continue;
 		}
 
-		ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v);
+		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS1, &v,
+					NULL);
+		if (ret)
+			return ret;
 
 		if (v & UB960_RR_RX_PORT_STS1_LOCK_STS)
 			dev_info(dev, "\tLocked\n");
@@ -3006,72 +4216,65 @@ static int ub960_log_status(struct v4l2_subdev *sd)
 			dev_info(dev, "\tNot locked\n");
 
 		dev_info(dev, "\trx_port_sts1 %#02x\n", v);
-		ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v);
-		dev_info(dev, "\trx_port_sts2 %#02x\n", v);
-
-		ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH, &v16);
-		dev_info(dev, "\tlink freq %llu Hz\n", (v16 * 1000000ULL) >> 8);
-
-		ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI, &v16);
-		dev_info(dev, "\tparity errors %u\n", v16);
-
-		ub960_rxport_read16(priv, nport, UB960_RR_LINE_COUNT_HI, &v16);
-		dev_info(dev, "\tlines per frame %u\n", v16);
-
-		ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1, &v16);
-		dev_info(dev, "\tbytes per line %u\n", v16);
-
-		ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER, &v);
-		dev_info(dev, "\tcsi_err_counter %u\n", v);
+		ret = ub960_rxport_read(priv, nport, UB960_RR_RX_PORT_STS2, &v,
+					NULL);
+		if (ret)
+			return ret;
 
-		/* Strobe */
+		dev_info(dev, "\trx_port_sts2 %#02x\n", v);
 
-		ub960_read(priv, UB960_XR_AEQ_CTL1, &v);
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_FREQ_HIGH,
+					  &v16, NULL);
+		if (ret)
+			return ret;
 
-		dev_info(dev, "\t%s strobe\n",
-			 (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) ? "Adaptive" :
-								  "Manual");
+		dev_info(dev, "\tlink freq %llu Hz\n", ((u64)v16 * HZ_PER_MHZ) >> 8);
 
-		if (v & UB960_XR_AEQ_CTL1_AEQ_SFILTER_EN) {
-			ub960_read(priv, UB960_XR_SFILTER_CFG, &v);
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_RX_PAR_ERR_HI,
+					  &v16, NULL);
+		if (ret)
+			return ret;
 
-			dev_info(dev, "\tStrobe range [%d, %d]\n",
-				 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MIN_SHIFT) & 0xf) - 7,
-				 ((v >> UB960_XR_SFILTER_CFG_SFILTER_MAX_SHIFT) & 0xf) - 7);
-		}
+		dev_info(dev, "\tparity errors %u\n", v16);
 
-		ub960_rxport_get_strobe_pos(priv, nport, &strobe_pos);
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_COUNT_HI,
+					  &v16, NULL);
+		if (ret)
+			return ret;
 
-		dev_info(dev, "\tStrobe pos %d\n", strobe_pos);
+		dev_info(dev, "\tlines per frame %u\n", v16);
 
-		/* EQ */
+		ret = ub960_rxport_read16(priv, nport, UB960_RR_LINE_LEN_1,
+					  &v16, NULL);
+		if (ret)
+			return ret;
 
-		ub960_rxport_read(priv, nport, UB960_RR_AEQ_BYPASS, &v);
+		dev_info(dev, "\tbytes per line %u\n", v16);
 
-		dev_info(dev, "\t%s EQ\n",
-			 (v & UB960_RR_AEQ_BYPASS_ENABLE) ? "Manual" :
-							    "Adaptive");
+		ret = ub960_rxport_read(priv, nport, UB960_RR_CSI_ERR_COUNTER,
+					&v, NULL);
+		if (ret)
+			return ret;
 
-		if (!(v & UB960_RR_AEQ_BYPASS_ENABLE)) {
-			ub960_rxport_read(priv, nport, UB960_RR_AEQ_MIN_MAX, &v);
+		dev_info(dev, "\tcsi_err_counter %u\n", v);
 
-			dev_info(dev, "\tEQ range [%u, %u]\n",
-				 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_FLOOR_SHIFT) & 0xf,
-				 (v >> UB960_RR_AEQ_MIN_MAX_AEQ_MAX_SHIFT) & 0xf);
+		if (!priv->hw_data->is_ub9702) {
+			ret = ub960_log_status_ub960_sp_eq(priv, nport);
+			if (ret)
+				return ret;
 		}
 
-		if (ub960_rxport_get_eq_level(priv, nport, &eq_level) == 0)
-			dev_info(dev, "\tEQ level %u\n", eq_level);
-
 		/* GPIOs */
-		for (i = 0; i < UB960_NUM_BC_GPIOS; i++) {
+		for (unsigned int i = 0; i < UB960_NUM_BC_GPIOS; i++) {
 			u8 ctl_reg;
 			u8 ctl_shift;
 
 			ctl_reg = UB960_RR_BC_GPIO_CTL(i / 2);
 			ctl_shift = (i % 2) * 4;
 
-			ub960_rxport_read(priv, nport, ctl_reg, &v);
+			ret = ub960_rxport_read(priv, nport, ctl_reg, &v, NULL);
+			if (ret)
+				return ret;
 
 			dev_info(dev, "\tGPIO%u: mode %u\n", i,
 				 (v >> ctl_shift) & 0xf);
@@ -3085,8 +4288,6 @@ static int ub960_log_status(struct v4l2_subdev *sd)
 
 static const struct v4l2_subdev_core_ops ub960_subdev_core_ops = {
 	.log_status = ub960_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_internal_ops ub960_internal_ops = {
@@ -3111,34 +4312,36 @@ static const struct media_entity_operations ub960_entity_ops = {
 static irqreturn_t ub960_handle_events(int irq, void *arg)
 {
 	struct ub960_data *priv = arg;
-	unsigned int i;
 	u8 int_sts;
 	u8 fwd_sts;
 	int ret;
 
-	ret = ub960_read(priv, UB960_SR_INTERRUPT_STS, &int_sts);
+	ret = ub960_read(priv, UB960_SR_INTERRUPT_STS, &int_sts, NULL);
 	if (ret || !int_sts)
 		return IRQ_NONE;
 
 	dev_dbg(&priv->client->dev, "INTERRUPT_STS %x\n", int_sts);
 
-	ret = ub960_read(priv, UB960_SR_FWD_STS, &fwd_sts);
+	ret = ub960_read(priv, UB960_SR_FWD_STS, &fwd_sts, NULL);
 	if (ret)
 		return IRQ_NONE;
 
 	dev_dbg(&priv->client->dev, "FWD_STS %#02x\n", fwd_sts);
 
-	for (i = 0; i < priv->hw_data->num_txports; i++) {
-		if (int_sts & UB960_SR_INTERRUPT_STS_IS_CSI_TX(i))
-			ub960_csi_handle_events(priv, i);
+	for (unsigned int i = 0; i < priv->hw_data->num_txports; i++) {
+		if (int_sts & UB960_SR_INTERRUPT_STS_IS_CSI_TX(i)) {
+			ret = ub960_csi_handle_events(priv, i);
+			if (ret)
+				return IRQ_NONE;
+		}
 	}
 
-	for (i = 0; i < priv->hw_data->num_rxports; i++) {
-		if (!priv->rxports[i])
-			continue;
-
-		if (int_sts & UB960_SR_INTERRUPT_STS_IS_RX(i))
-			ub960_rxport_handle_events(priv, i);
+	for_each_active_rxport(priv, it) {
+		if (int_sts & UB960_SR_INTERRUPT_STS_IS_RX(it.nport)) {
+			ret = ub960_rxport_handle_events(priv, it.nport);
+			if (ret)
+				return IRQ_NONE;
+		}
 	}
 
 	return IRQ_HANDLED;
@@ -3173,19 +4376,14 @@ static void ub960_txport_free_ports(struct ub960_data *priv)
 
 static void ub960_rxport_free_ports(struct ub960_data *priv)
 {
-	unsigned int nport;
+	for_each_active_rxport(priv, it) {
+		fwnode_handle_put(it.rxport->source.ep_fwnode);
+		fwnode_handle_put(it.rxport->ser.fwnode);
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
+		mutex_destroy(&it.rxport->aliased_addrs_lock);
 
-		if (!rxport)
-			continue;
-
-		fwnode_handle_put(rxport->source.ep_fwnode);
-		fwnode_handle_put(rxport->ser.fwnode);
-
-		kfree(rxport);
-		priv->rxports[nport] = NULL;
+		kfree(it.rxport);
+		priv->rxports[it.nport] = NULL;
 	}
 }
 
@@ -3201,6 +4399,7 @@ ub960_parse_dt_rxport_link_properties(struct ub960_data *priv,
 	s32 strobe_pos;
 	u32 eq_level;
 	u32 ser_i2c_alias;
+	u32 ser_i2c_addr;
 	int ret;
 
 	cdr_mode = RXPORT_CDR_FPD3;
@@ -3312,6 +4511,13 @@ ub960_parse_dt_rxport_link_properties(struct ub960_data *priv,
 		return -EINVAL;
 	}
 
+	ret = fwnode_property_read_u32(rxport->ser.fwnode, "reg",
+				       &ser_i2c_addr);
+	if (ret)
+		rxport->ser.addr = -EINVAL;
+	else
+		rxport->ser.addr = ser_i2c_addr;
+
 	return 0;
 }
 
@@ -3404,6 +4610,8 @@ static int ub960_parse_dt_rxport(struct ub960_data *priv, unsigned int nport,
 	if (ret)
 		goto err_put_remote_fwnode;
 
+	mutex_init(&rxport->aliased_addrs_lock);
+
 	return 0;
 
 err_put_remote_fwnode:
@@ -3444,7 +4652,6 @@ static int ub960_parse_dt_rxports(struct ub960_data *priv)
 {
 	struct device *dev = &priv->client->dev;
 	struct fwnode_handle *links_fwnode;
-	unsigned int nport;
 	int ret;
 
 	links_fwnode = fwnode_get_named_child_node(dev_fwnode(dev), "links");
@@ -3459,9 +4666,10 @@ static int ub960_parse_dt_rxports(struct ub960_data *priv)
 
 	priv->strobe.manual = fwnode_property_read_bool(links_fwnode, "ti,manual-strobe");
 
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
+	for_each_rxport(priv, it) {
 		struct fwnode_handle *link_fwnode;
 		struct fwnode_handle *ep_fwnode;
+		unsigned int nport = it.nport;
 
 		link_fwnode = ub960_fwnode_get_link_by_regs(links_fwnode, nport);
 		if (!link_fwnode)
@@ -3550,7 +4758,6 @@ static int ub960_notify_bound(struct v4l2_async_notifier *notifier,
 	struct ub960_rxport *rxport = to_ub960_asd(asd)->rxport;
 	struct device *dev = &priv->client->dev;
 	u8 nport = rxport->nport;
-	unsigned int i;
 	int ret;
 
 	ret = media_entity_get_fwnode_pad(&subdev->entity,
@@ -3575,8 +4782,8 @@ static int ub960_notify_bound(struct v4l2_async_notifier *notifier,
 		return ret;
 	}
 
-	for (i = 0; i < priv->hw_data->num_rxports; i++) {
-		if (priv->rxports[i] && !priv->rxports[i]->source.sd) {
+	for_each_active_rxport(priv, it) {
+		if (!it.rxport->source.sd) {
 			dev_dbg(dev, "Waiting for more subdevs to be bound\n");
 			return 0;
 		}
@@ -3602,29 +4809,24 @@ static const struct v4l2_async_notifier_operations ub960_notify_ops = {
 static int ub960_v4l2_notifier_register(struct ub960_data *priv)
 {
 	struct device *dev = &priv->client->dev;
-	unsigned int i;
 	int ret;
 
 	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
 
-	for (i = 0; i < priv->hw_data->num_rxports; i++) {
-		struct ub960_rxport *rxport = priv->rxports[i];
+	for_each_active_rxport(priv, it) {
 		struct ub960_asd *asd;
 
-		if (!rxport)
-			continue;
-
 		asd = v4l2_async_nf_add_fwnode(&priv->notifier,
-					       rxport->source.ep_fwnode,
+					       it.rxport->source.ep_fwnode,
 					       struct ub960_asd);
 		if (IS_ERR(asd)) {
 			dev_err(dev, "Failed to add subdev for source %u: %pe",
-				i, asd);
+				it.nport, asd);
 			v4l2_async_nf_cleanup(&priv->notifier);
 			return PTR_ERR(asd);
 		}
 
-		asd->rxport = rxport;
+		asd->rxport = it.rxport;
 	}
 
 	priv->notifier.ops = &ub960_notify_ops;
@@ -3667,7 +4869,7 @@ static int ub960_create_subdev(struct ub960_data *priv)
 	}
 
 	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			  V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
+			  V4L2_SUBDEV_FL_STREAMS;
 	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 	priv->sd.entity.ops = &ub960_entity_ops;
 
@@ -3742,29 +4944,6 @@ static const struct regmap_config ub960_regmap_config = {
 	.disable_locking = true,
 };
 
-static void ub960_reset(struct ub960_data *priv, bool reset_regs)
-{
-	struct device *dev = &priv->client->dev;
-	unsigned int v;
-	int ret;
-	u8 bit;
-
-	bit = reset_regs ? UB960_SR_RESET_DIGITAL_RESET1 :
-			   UB960_SR_RESET_DIGITAL_RESET0;
-
-	ub960_write(priv, UB960_SR_RESET, bit);
-
-	mutex_lock(&priv->reg_lock);
-
-	ret = regmap_read_poll_timeout(priv->regmap, UB960_SR_RESET, v,
-				       (v & bit) == 0, 2000, 100000);
-
-	mutex_unlock(&priv->reg_lock);
-
-	if (ret)
-		dev_err(dev, "reset failed: %d\n", ret);
-}
-
 static int ub960_get_hw_resources(struct ub960_data *priv)
 {
 	struct device *dev = &priv->client->dev;
@@ -3821,10 +5000,12 @@ static int ub960_enable_core_hw(struct ub960_data *priv)
 		fsleep(2000);
 	}
 
-	ub960_reset(priv, true);
+	ret = ub960_reset(priv, true);
+	if (ret)
+		goto err_pd_gpio;
 
 	/* Runtime check register accessibility */
-	ret = ub960_read(priv, UB960_SR_REV_MASK, &rev_mask);
+	ret = ub960_read(priv, UB960_SR_REV_MASK, &rev_mask, NULL);
 	if (ret) {
 		dev_err_probe(dev, ret, "Cannot read first register, abort\n");
 		goto err_pd_gpio;
@@ -3833,20 +5014,25 @@ static int ub960_enable_core_hw(struct ub960_data *priv)
 	dev_dbg(dev, "Found %s (rev/mask %#04x)\n", priv->hw_data->model,
 		rev_mask);
 
-	ret = ub960_read(priv, UB960_SR_DEVICE_STS, &dev_sts);
+	ret = ub960_read(priv, UB960_SR_DEVICE_STS, &dev_sts, NULL);
 	if (ret)
 		goto err_pd_gpio;
 
-	ret = ub960_read(priv, UB960_XR_REFCLK_FREQ, &refclk_freq);
+	if (priv->hw_data->is_ub9702)
+		ret = ub960_read(priv, UB9702_SR_REFCLK_FREQ, &refclk_freq,
+				 NULL);
+	else
+		ret = ub960_read(priv, UB960_XR_REFCLK_FREQ, &refclk_freq,
+				 NULL);
 	if (ret)
 		goto err_pd_gpio;
 
 	dev_dbg(dev, "refclk valid %u freq %u MHz (clk fw freq %lu MHz)\n",
 		!!(dev_sts & BIT(4)), refclk_freq,
-		clk_get_rate(priv->refclk) / 1000000);
+		clk_get_rate(priv->refclk) / HZ_PER_MHZ);
 
 	/* Disable all RX ports by default */
-	ret = ub960_write(priv, UB960_SR_RX_PORT_CTL, 0);
+	ret = ub960_write(priv, UB960_SR_RX_PORT_CTL, 0, NULL);
 	if (ret)
 		goto err_pd_gpio;
 
@@ -3854,7 +5040,8 @@ static int ub960_enable_core_hw(struct ub960_data *priv)
 	if (priv->hw_data->is_ub9702) {
 		ret = ub960_update_bits(priv, UB960_SR_RESET,
 					UB960_SR_RESET_GPIO_LOCK_RELEASE,
-					UB960_SR_RESET_GPIO_LOCK_RELEASE);
+					UB960_SR_RESET_GPIO_LOCK_RELEASE,
+					NULL);
 		if (ret)
 			goto err_pd_gpio;
 	}
@@ -3881,9 +5068,6 @@ static int ub960_probe(struct i2c_client *client)
 {
 	struct device *dev = &client->dev;
 	struct ub960_data *priv;
-	unsigned int port_lock_mask;
-	unsigned int port_mask;
-	unsigned int nport;
 	int ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
@@ -3926,39 +5110,14 @@ static int ub960_probe(struct i2c_client *client)
 	if (ret)
 		goto err_free_ports;
 
-	ret = ub960_init_rx_ports(priv);
-	if (ret)
-		goto err_disable_vpocs;
-
-	ub960_reset(priv, false);
-
-	port_mask = 0;
-
-	for (nport = 0; nport < priv->hw_data->num_rxports; nport++) {
-		struct ub960_rxport *rxport = priv->rxports[nport];
-
-		if (!rxport)
-			continue;
-
-		port_mask |= BIT(nport);
-	}
+	if (priv->hw_data->is_ub9702)
+		ret = ub960_init_rx_ports_ub9702(priv);
+	else
+		ret = ub960_init_rx_ports_ub960(priv);
 
-	ret = ub960_rxport_wait_locks(priv, port_mask, &port_lock_mask);
 	if (ret)
 		goto err_disable_vpocs;
 
-	if (port_mask != port_lock_mask) {
-		ret = -EIO;
-		dev_err_probe(dev, ret, "Failed to lock all RX ports\n");
-		goto err_disable_vpocs;
-	}
-
-	/*
-	 * Clear any errors caused by switching the RX port settings while
-	 * probing.
-	 */
-	ub960_clear_rx_errors(priv);
-
 	ret = ub960_init_atr(priv);
 	if (ret)
 		goto err_disable_vpocs;
@@ -3977,6 +5136,12 @@ static int ub960_probe(struct i2c_client *client)
 	schedule_delayed_work(&priv->poll_work,
 			      msecs_to_jiffies(UB960_POLL_TIME_MS));
 
+#ifdef UB960_DEBUG_I2C_RX_ID
+	for_each_rxport(priv, it)
+		ub960_write(priv, UB960_SR_I2C_RX_ID(it.nport),
+			    (UB960_DEBUG_I2C_RX_ID + it.nport) << 1, NULL);
+#endif
+
 	return 0;
 
 err_free_sers:
@@ -4055,4 +5220,4 @@ MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV Deserializers Driver");
 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
-MODULE_IMPORT_NS(I2C_ATR);
+MODULE_IMPORT_NS("I2C_ATR");
diff --git a/drivers/media/i2c/dw9714.c b/drivers/media/i2c/dw9714.c
index 84d29bcf0ccd..2ddd7daa79e2 100644
--- a/drivers/media/i2c/dw9714.c
+++ b/drivers/media/i2c/dw9714.c
@@ -279,8 +279,8 @@ static int  __maybe_unused dw9714_vcm_resume(struct device *dev)
 }
 
 static const struct i2c_device_id dw9714_id_table[] = {
-	{ DW9714_NAME, 0 },
-	{ { 0 } }
+	{ DW9714_NAME },
+	{ }
 };
 MODULE_DEVICE_TABLE(i2c, dw9714_id_table);
 
@@ -310,8 +310,8 @@ module_i2c_driver(dw9714_i2c_driver);
 
 MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
 MODULE_AUTHOR("Jian Xu Zheng");
-MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
-MODULE_AUTHOR("Jouni Ukkonen <jouni.ukkonen@intel.com>");
-MODULE_AUTHOR("Tommi Franttila <tommi.franttila@intel.com>");
+MODULE_AUTHOR("Yuning Pu");
+MODULE_AUTHOR("Jouni Ukkonen");
+MODULE_AUTHOR("Tommi Franttila");
 MODULE_DESCRIPTION("DW9714 VCM driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/dw9719.c b/drivers/media/i2c/dw9719.c
index c626ed845928..032fbcb981f2 100644
--- a/drivers/media/i2c/dw9719.c
+++ b/drivers/media/i2c/dw9719.c
@@ -2,8 +2,10 @@
 // Copyright (c) 2012 Intel Corporation
 
 /*
- * Based on linux/modules/camera/drivers/media/i2c/imx/dw9719.c in this repo:
- * https://github.com/ZenfoneArea/android_kernel_asus_zenfone5
+ * Based on linux/modules/camera/drivers/media/i2c/imx/dw9719.c from:
+ * https://github.com/ZenfoneArea/android_kernel_asus_zenfone5 and
+ * latte-l-oss/drivers/external_drivers/camera/drivers/media/i2c/micam/dw9761.c
+ * from: https://github.com/MiCode/Xiaomi_Kernel_OpenSource/
  */
 
 #include <linux/delay.h>
@@ -23,26 +25,45 @@
 
 #define DW9719_INFO			CCI_REG8(0)
 #define DW9719_ID			0xF1
+#define DW9761_ID			0xF4
 
 #define DW9719_CONTROL			CCI_REG8(2)
+#define DW9719_STANDBY			0x00
+#define DW9719_SHUTDOWN			0x01
 #define DW9719_ENABLE_RINGING		0x02
 
 #define DW9719_VCM_CURRENT		CCI_REG16(3)
 
+#define DW9719_STATUS			CCI_REG16(5)
+#define DW9719_STATUS_BUSY		BIT(0)
+
 #define DW9719_MODE			CCI_REG8(6)
 #define DW9719_MODE_SAC_SHIFT		4
-#define DW9719_MODE_SAC3		4
+#define DW9719_DEFAULT_SAC		4
+#define DW9761_DEFAULT_SAC		6
 
 #define DW9719_VCM_FREQ			CCI_REG8(7)
 #define DW9719_DEFAULT_VCM_FREQ		0x60
+#define DW9761_DEFAULT_VCM_FREQ		0x3E
+
+#define DW9761_VCM_PRELOAD		CCI_REG8(8)
+#define DW9761_DEFAULT_VCM_PRELOAD	0x73
+
 
 #define to_dw9719_device(x) container_of(x, struct dw9719_device, sd)
 
+enum dw9719_model {
+	DW9719,
+	DW9761,
+};
+
 struct dw9719_device {
 	struct v4l2_subdev sd;
 	struct device *dev;
 	struct regmap *regmap;
 	struct regulator *regulator;
+	enum dw9719_model model;
+	u32 mode_low_bits;
 	u32 sac_mode;
 	u32 vcm_freq;
 
@@ -52,30 +73,14 @@ struct dw9719_device {
 	} ctrls;
 };
 
-static int dw9719_detect(struct dw9719_device *dw9719)
-{
-	int ret;
-	u64 val;
-
-	ret = cci_read(dw9719->regmap, DW9719_INFO, &val, NULL);
-	if (ret < 0)
-		return ret;
-
-	if (val != DW9719_ID) {
-		dev_err(dw9719->dev, "Failed to detect correct id\n");
-		return -ENXIO;
-	}
-
-	return 0;
-}
-
 static int dw9719_power_down(struct dw9719_device *dw9719)
 {
 	return regulator_disable(dw9719->regulator);
 }
 
-static int dw9719_power_up(struct dw9719_device *dw9719)
+static int dw9719_power_up(struct dw9719_device *dw9719, bool detect)
 {
+	u64 val;
 	int ret;
 
 	ret = regulator_enable(dw9719->regulator);
@@ -83,16 +88,54 @@ static int dw9719_power_up(struct dw9719_device *dw9719)
 		return ret;
 
 	/* Jiggle SCL pin to wake up device */
-	cci_write(dw9719->regmap, DW9719_CONTROL, 1, &ret);
-
+	cci_write(dw9719->regmap, DW9719_CONTROL, DW9719_SHUTDOWN, &ret);
+	fsleep(100);
+	cci_write(dw9719->regmap, DW9719_CONTROL, DW9719_STANDBY, &ret);
 	/* Need 100us to transit from SHUTDOWN to STANDBY */
 	fsleep(100);
 
+	if (detect) {
+		ret = cci_read(dw9719->regmap, DW9719_INFO, &val, NULL);
+		if (ret < 0)
+			return ret;
+
+		switch (val) {
+		case DW9719_ID:
+			dw9719->model = DW9719;
+			dw9719->mode_low_bits = 0x00;
+			dw9719->sac_mode = DW9719_DEFAULT_SAC;
+			dw9719->vcm_freq = DW9719_DEFAULT_VCM_FREQ;
+			break;
+		case DW9761_ID:
+			dw9719->model = DW9761;
+			dw9719->mode_low_bits = 0x01;
+			dw9719->sac_mode = DW9761_DEFAULT_SAC;
+			dw9719->vcm_freq = DW9761_DEFAULT_VCM_FREQ;
+			break;
+		default:
+			dev_err(dw9719->dev,
+				"Error unknown device id 0x%02llx\n", val);
+			return -ENXIO;
+		}
+
+		/* Optional indication of SAC mode select */
+		device_property_read_u32(dw9719->dev, "dongwoon,sac-mode",
+					 &dw9719->sac_mode);
+
+		/* Optional indication of VCM frequency */
+		device_property_read_u32(dw9719->dev, "dongwoon,vcm-freq",
+					 &dw9719->vcm_freq);
+	}
+
 	cci_write(dw9719->regmap, DW9719_CONTROL, DW9719_ENABLE_RINGING, &ret);
-	cci_write(dw9719->regmap, DW9719_MODE,
-		  dw9719->sac_mode << DW9719_MODE_SAC_SHIFT, &ret);
+	cci_write(dw9719->regmap, DW9719_MODE, dw9719->mode_low_bits |
+			  (dw9719->sac_mode << DW9719_MODE_SAC_SHIFT), &ret);
 	cci_write(dw9719->regmap, DW9719_VCM_FREQ, dw9719->vcm_freq, &ret);
 
+	if (dw9719->model == DW9761)
+		cci_write(dw9719->regmap, DW9761_VCM_PRELOAD,
+			  DW9761_DEFAULT_VCM_PRELOAD, &ret);
+
 	if (ret)
 		dw9719_power_down(dw9719);
 
@@ -159,7 +202,7 @@ static int dw9719_resume(struct device *dev)
 	int ret;
 	int val;
 
-	ret = dw9719_power_up(dw9719);
+	ret = dw9719_power_up(dw9719, false);
 	if (ret)
 		return ret;
 
@@ -237,16 +280,6 @@ static int dw9719_probe(struct i2c_client *client)
 		return PTR_ERR(dw9719->regmap);
 
 	dw9719->dev = &client->dev;
-	dw9719->sac_mode = DW9719_MODE_SAC3;
-	dw9719->vcm_freq = DW9719_DEFAULT_VCM_FREQ;
-
-	/* Optional indication of SAC mode select */
-	device_property_read_u32(&client->dev, "dongwoon,sac-mode",
-				 &dw9719->sac_mode);
-
-	/* Optional indication of VCM frequency */
-	device_property_read_u32(&client->dev, "dongwoon,vcm-freq",
-				 &dw9719->vcm_freq);
 
 	dw9719->regulator = devm_regulator_get(&client->dev, "vdd");
 	if (IS_ERR(dw9719->regulator))
@@ -274,14 +307,10 @@ static int dw9719_probe(struct i2c_client *client)
 	 * will work.
 	 */
 
-	ret = dw9719_power_up(dw9719);
+	ret = dw9719_power_up(dw9719, true);
 	if (ret)
 		goto err_cleanup_media;
 
-	ret = dw9719_detect(dw9719);
-	if (ret)
-		goto err_powerdown;
-
 	pm_runtime_set_active(&client->dev);
 	pm_runtime_get_noresume(&client->dev);
 	pm_runtime_enable(&client->dev);
@@ -299,7 +328,6 @@ static int dw9719_probe(struct i2c_client *client)
 err_pm_runtime:
 	pm_runtime_disable(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
-err_powerdown:
 	dw9719_power_down(dw9719);
 err_cleanup_media:
 	media_entity_cleanup(&dw9719->sd.entity);
@@ -327,6 +355,7 @@ static void dw9719_remove(struct i2c_client *client)
 
 static const struct i2c_device_id dw9719_id_table[] = {
 	{ "dw9719" },
+	{ "dw9761" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, dw9719_id_table);
diff --git a/drivers/media/i2c/dw9768.c b/drivers/media/i2c/dw9768.c
index daabbece8c7e..3a4d100b9199 100644
--- a/drivers/media/i2c/dw9768.c
+++ b/drivers/media/i2c/dw9768.c
@@ -115,11 +115,6 @@ static inline struct dw9768 *sd_to_dw9768(struct v4l2_subdev *subdev)
 	return container_of(subdev, struct dw9768, sd);
 }
 
-struct regval_list {
-	u8 reg_num;
-	u8 value;
-};
-
 struct dw9768_aac_mode_ot_multi {
 	u32 aac_mode_enum;
 	u32 ot_multi_base100;
@@ -379,7 +374,8 @@ static int dw9768_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 
 static int dw9768_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
-	pm_runtime_put(sd->dev);
+	pm_runtime_mark_last_busy(sd->dev);
+	pm_runtime_put_autosuspend(sd->dev);
 
 	return 0;
 }
@@ -476,10 +472,9 @@ static int dw9768_probe(struct i2c_client *client)
 	 * to be powered on in an ACPI system. Similarly for power off in
 	 * remove.
 	 */
-	pm_runtime_enable(dev);
 	full_power = (is_acpi_node(dev_fwnode(dev)) &&
 		      acpi_dev_state_d0(dev)) ||
-		     (is_of_node(dev_fwnode(dev)) && !pm_runtime_enabled(dev));
+		     (is_of_node(dev_fwnode(dev)) && !IS_ENABLED(CONFIG_PM));
 	if (full_power) {
 		ret = dw9768_runtime_resume(dev);
 		if (ret < 0) {
@@ -489,23 +484,26 @@ static int dw9768_probe(struct i2c_client *client)
 		pm_runtime_set_active(dev);
 	}
 
+	pm_runtime_enable(dev);
 	ret = v4l2_async_register_subdev(&dw9768->sd);
 	if (ret < 0) {
 		dev_err(dev, "failed to register V4L2 subdev: %d", ret);
 		goto err_power_off;
 	}
 
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
 	pm_runtime_idle(dev);
 
 	return 0;
 
 err_power_off:
+	pm_runtime_disable(dev);
 	if (full_power) {
 		dw9768_runtime_suspend(dev);
 		pm_runtime_set_suspended(dev);
 	}
 err_clean_entity:
-	pm_runtime_disable(dev);
 	media_entity_cleanup(&dw9768->sd.entity);
 err_free_handler:
 	v4l2_ctrl_handler_free(&dw9768->ctrls);
@@ -522,12 +520,12 @@ static void dw9768_remove(struct i2c_client *client)
 	v4l2_async_unregister_subdev(&dw9768->sd);
 	v4l2_ctrl_handler_free(&dw9768->ctrls);
 	media_entity_cleanup(&dw9768->sd.entity);
+	pm_runtime_disable(dev);
 	if ((is_acpi_node(dev_fwnode(dev)) && acpi_dev_state_d0(dev)) ||
-	    (is_of_node(dev_fwnode(dev)) && !pm_runtime_enabled(dev))) {
+	    (is_of_node(dev_fwnode(dev)) && !IS_ENABLED(CONFIG_PM))) {
 		dw9768_runtime_suspend(dev);
 		pm_runtime_set_suspended(dev);
 	}
-	pm_runtime_disable(dev);
 }
 
 static const struct of_device_id dw9768_of_table[] = {
diff --git a/drivers/media/i2c/et8ek8/et8ek8_driver.c b/drivers/media/i2c/et8ek8/et8ek8_driver.c
index f548b1bb75fb..7519863d77b1 100644
--- a/drivers/media/i2c/et8ek8/et8ek8_driver.c
+++ b/drivers/media/i2c/et8ek8/et8ek8_driver.c
@@ -1475,7 +1475,7 @@ err_mutex:
 	return ret;
 }
 
-static void __exit et8ek8_remove(struct i2c_client *client)
+static void et8ek8_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
 	struct et8ek8_sensor *sensor = to_et8ek8_sensor(subdev);
@@ -1501,7 +1501,7 @@ static const struct of_device_id et8ek8_of_table[] = {
 MODULE_DEVICE_TABLE(of, et8ek8_of_table);
 
 static const struct i2c_device_id et8ek8_id_table[] = {
-	{ ET8EK8_NAME, 0 },
+	{ ET8EK8_NAME },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, et8ek8_id_table);
@@ -1517,7 +1517,7 @@ static struct i2c_driver et8ek8_i2c_driver = {
 		.of_match_table	= et8ek8_of_table,
 	},
 	.probe		= et8ek8_probe,
-	.remove		= __exit_p(et8ek8_remove),
+	.remove		= et8ek8_remove,
 	.id_table	= et8ek8_id_table,
 };
 
diff --git a/drivers/media/i2c/gc0308.c b/drivers/media/i2c/gc0308.c
index fa754a8a39a6..069f42785b3c 100644
--- a/drivers/media/i2c/gc0308.c
+++ b/drivers/media/i2c/gc0308.c
@@ -18,7 +18,6 @@
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
@@ -987,8 +986,6 @@ static const struct v4l2_ctrl_ops gc0308_ctrl_ops = {
 
 static const struct v4l2_subdev_core_ops gc0308_core_ops = {
 	.log_status = v4l2_ctrl_subdev_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.g_register	= gc0308_g_register,
 	.s_register	= gc0308_s_register,
@@ -1338,7 +1335,6 @@ static int gc0308_probe(struct i2c_client *client)
 	v4l2_i2c_subdev_init(&gc0308->sd, client, &gc0308_subdev_ops);
 	gc0308->sd.internal_ops = &gc0308_internal_ops;
 	gc0308->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	gc0308->sd.flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
 
 	ret = gc0308_init_controls(gc0308);
 	if (ret)
diff --git a/drivers/media/i2c/gc05a2.c b/drivers/media/i2c/gc05a2.c
new file mode 100644
index 000000000000..3f7f3d5abeeb
--- /dev/null
+++ b/drivers/media/i2c/gc05a2.c
@@ -0,0 +1,1351 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for GalaxyCore gc05a2 image sensor
+ *
+ * Copyright 2024 MediaTek
+ *
+ * Zhi Mao <zhi.mao@mediatek.com>
+ */
+#include <linux/array_size.h>
+#include <linux/bits.h>
+#include <linux/clk.h>
+#include <linux/container_of.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/math64.h>
+#include <linux/mod_devicetable.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/types.h>
+#include <linux/units.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define GC05A2_REG_TEST_PATTERN_EN CCI_REG8(0x008c)
+#define GC05A2_REG_TEST_PATTERN_IDX CCI_REG8(0x008d)
+#define GC05A2_TEST_PATTERN_EN 0x01
+
+#define GC05A2_STREAMING_REG CCI_REG8(0x0100)
+
+#define GC05A2_FLIP_REG CCI_REG8(0x0101)
+#define GC05A2_FLIP_H_MASK BIT(0)
+#define GC05A2_FLIP_V_MASK BIT(1)
+
+#define GC05A2_EXP_REG CCI_REG16(0x0202)
+#define GC05A2_EXP_MARGIN 16
+#define GC05A2_EXP_MIN 4
+#define GC05A2_EXP_STEP 1
+
+#define GC05A2_AGAIN_REG CCI_REG16(0x0204)
+#define GC05A2_AGAIN_MIN 1024
+#define GC05A2_AGAIN_MAX (1024 * 16)
+#define GC05A2_AGAIN_STEP 1
+
+#define GC05A2_FRAME_LENGTH_REG CCI_REG16(0x0340)
+#define GC05A2_VTS_MAX 0xffff
+
+#define GC05A2_REG_CHIP_ID CCI_REG16(0x03f0)
+#define GC05A2_CHIP_ID 0x05a2
+
+#define GC05A2_NATIVE_WIDTH 2592
+#define GC05A2_NATIVE_HEIGHT 1944
+
+#define GC05A2_DEFAULT_CLK_FREQ (24 * HZ_PER_MHZ)
+#define GC05A2_MBUS_CODE MEDIA_BUS_FMT_SGRBG10_1X10
+#define GC05A2_DATA_LANES 2
+#define GC05A2_RGB_DEPTH 10
+#define GC05A2_SLEEP_US  (2 * USEC_PER_MSEC)
+
+static const char *const gc05a2_test_pattern_menu[] = {
+	"No Pattern",  "Fade_to_gray_Color Bar", "Color Bar",
+	"PN9",	       "Horizontal_gradient",    "Checkboard Pattern",
+	"Slant",       "Resolution",	         "Solid Black",
+	"Solid White",
+};
+
+static const s64 gc05a2_link_freq_menu_items[] = {
+	(448 * HZ_PER_MHZ),
+	(224 * HZ_PER_MHZ),
+};
+
+static const char *const gc05a2_supply_name[] = {
+	"avdd",
+	"dvdd",
+	"dovdd",
+};
+
+struct gc05a2 {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct clk *xclk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(gc05a2_supply_name)];
+	struct gpio_desc *reset_gpio;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+
+	struct regmap *regmap;
+	unsigned long link_freq_bitmap;
+
+	/* True if the device has been identified */
+	bool identified;
+	const struct gc05a2_mode *cur_mode;
+};
+
+struct gc05a2_reg_list {
+	u32 num_of_regs;
+	const struct cci_reg_sequence *regs;
+};
+
+static const struct cci_reg_sequence mode_2592x1944[] = {
+	/* system */
+	{ CCI_REG8(0x0135), 0x01 },
+	{ CCI_REG8(0x0084), 0x21 },
+	{ CCI_REG8(0x0d05), 0xcc },
+	{ CCI_REG8(0x0218), 0x00 },
+	{ CCI_REG8(0x005e), 0x48 },
+	{ CCI_REG8(0x0d06), 0x01 },
+	{ CCI_REG8(0x0007), 0x16 },
+	{ CCI_REG8(0x0101), 0x00 },
+
+	/* analog */
+	{ CCI_REG8(0x0342), 0x07 },
+	{ CCI_REG8(0x0343), 0x28 },
+	{ CCI_REG8(0x0220), 0x07 },
+	{ CCI_REG8(0x0221), 0xd0 },
+	{ CCI_REG8(0x0202), 0x07 },
+	{ CCI_REG8(0x0203), 0x32 },
+	{ CCI_REG8(0x0340), 0x07 },
+	{ CCI_REG8(0x0341), 0xf0 },
+	{ CCI_REG8(0x0219), 0x00 },
+	{ CCI_REG8(0x0346), 0x00 },
+	{ CCI_REG8(0x0347), 0x04 },
+	{ CCI_REG8(0x0d14), 0x00 },
+	{ CCI_REG8(0x0d13), 0x05 },
+	{ CCI_REG8(0x0d16), 0x05 },
+	{ CCI_REG8(0x0d15), 0x1d },
+	{ CCI_REG8(0x00c0), 0x0a },
+	{ CCI_REG8(0x00c1), 0x30 },
+	{ CCI_REG8(0x034a), 0x07 },
+	{ CCI_REG8(0x034b), 0xa8 },
+	{ CCI_REG8(0x0e0a), 0x00 },
+	{ CCI_REG8(0x0e0b), 0x00 },
+	{ CCI_REG8(0x0e0e), 0x03 },
+	{ CCI_REG8(0x0e0f), 0x00 },
+	{ CCI_REG8(0x0e06), 0x0a },
+	{ CCI_REG8(0x0e23), 0x15 },
+	{ CCI_REG8(0x0e24), 0x15 },
+	{ CCI_REG8(0x0e2a), 0x10 },
+	{ CCI_REG8(0x0e2b), 0x10 },
+	{ CCI_REG8(0x0e17), 0x49 },
+	{ CCI_REG8(0x0e1b), 0x1c },
+	{ CCI_REG8(0x0e3a), 0x36 },
+	{ CCI_REG8(0x0d11), 0x84 },
+	{ CCI_REG8(0x0e52), 0x14 },
+	{ CCI_REG8(0x000b), 0x10 },
+	{ CCI_REG8(0x0008), 0x08 },
+	{ CCI_REG8(0x0223), 0x17 },
+	{ CCI_REG8(0x0d27), 0x39 },
+	{ CCI_REG8(0x0d22), 0x00 },
+	{ CCI_REG8(0x03f6), 0x0d },
+	{ CCI_REG8(0x0d04), 0x07 },
+	{ CCI_REG8(0x03f3), 0x72 },
+	{ CCI_REG8(0x03f4), 0xb8 },
+	{ CCI_REG8(0x03f5), 0xbc },
+	{ CCI_REG8(0x0d02), 0x73 },
+
+	/* auto load start */
+	{ CCI_REG8(0x00cb), 0x00 },
+
+	/* OUT 2592*1944 */
+	{ CCI_REG8(0x0350), 0x01 },
+	{ CCI_REG8(0x0353), 0x00 },
+	{ CCI_REG8(0x0354), 0x08 },
+	{ CCI_REG16(0x034c), 2592 }, /* Width */
+	{ CCI_REG8(0x021f), 0x14 },
+
+	/* MIPI */
+	{ CCI_REG8(0x0107), 0x05 },
+	{ CCI_REG8(0x0117), 0x01 },
+	{ CCI_REG8(0x0d81), 0x00 },
+	{ CCI_REG8(0x0d84), 0x0c },
+	{ CCI_REG8(0x0d85), 0xa8 },
+	{ CCI_REG8(0x0d86), 0x06 },
+	{ CCI_REG8(0x0d87), 0x55 },
+	{ CCI_REG8(0x0db3), 0x06 },
+	{ CCI_REG8(0x0db4), 0x08 },
+	{ CCI_REG8(0x0db5), 0x1e },
+	{ CCI_REG8(0x0db6), 0x02 },
+	{ CCI_REG8(0x0db8), 0x12 },
+	{ CCI_REG8(0x0db9), 0x0a },
+	{ CCI_REG8(0x0d93), 0x06 },
+	{ CCI_REG8(0x0d94), 0x09 },
+	{ CCI_REG8(0x0d95), 0x0d },
+	{ CCI_REG8(0x0d99), 0x0b },
+	{ CCI_REG8(0x0084), 0x01 },
+	{ CCI_REG8(0x0110), 0x01 },
+};
+
+static const struct cci_reg_sequence mode_1280x720[] = {
+	/* system */
+	{ CCI_REG8(0x0135), 0x05 },
+	{ CCI_REG8(0x0084), 0x21 },
+	{ CCI_REG8(0x0d05), 0xcc },
+	{ CCI_REG8(0x0218), 0x80 },
+	{ CCI_REG8(0x005e), 0x49 },
+	{ CCI_REG8(0x0d06), 0x81 },
+	{ CCI_REG8(0x0007), 0x16 },
+	{ CCI_REG8(0x0101), 0x00 },
+
+	/* analog */
+	{ CCI_REG8(0x0342), 0x07 },
+	{ CCI_REG8(0x0343), 0x10 },
+	{ CCI_REG8(0x0220), 0x07 },
+	{ CCI_REG8(0x0221), 0xd0 },
+	{ CCI_REG8(0x0202), 0x03 },
+	{ CCI_REG8(0x0203), 0x32 },
+	{ CCI_REG8(0x0340), 0x04 },
+	{ CCI_REG8(0x0341), 0x08 },
+	{ CCI_REG8(0x0219), 0x00 },
+	{ CCI_REG8(0x0346), 0x01 },
+	{ CCI_REG8(0x0347), 0x00 },
+	{ CCI_REG8(0x0d14), 0x00 },
+	{ CCI_REG8(0x0d13), 0x05 },
+	{ CCI_REG8(0x0d16), 0x05 },
+	{ CCI_REG8(0x0d15), 0x1d },
+	{ CCI_REG8(0x00c0), 0x0a },
+	{ CCI_REG8(0x00c1), 0x30 },
+	{ CCI_REG8(0x034a), 0x05 },
+	{ CCI_REG8(0x034b), 0xb0 },
+	{ CCI_REG8(0x0e0a), 0x00 },
+	{ CCI_REG8(0x0e0b), 0x00 },
+	{ CCI_REG8(0x0e0e), 0x03 },
+	{ CCI_REG8(0x0e0f), 0x00 },
+	{ CCI_REG8(0x0e06), 0x0a },
+	{ CCI_REG8(0x0e23), 0x15 },
+	{ CCI_REG8(0x0e24), 0x15 },
+	{ CCI_REG8(0x0e2a), 0x10 },
+	{ CCI_REG8(0x0e2b), 0x10 },
+	{ CCI_REG8(0x0e17), 0x49 },
+	{ CCI_REG8(0x0e1b), 0x1c },
+	{ CCI_REG8(0x0e3a), 0x36 },
+	{ CCI_REG8(0x0d11), 0x84 },
+	{ CCI_REG8(0x0e52), 0x14 },
+	{ CCI_REG8(0x000b), 0x0e },
+	{ CCI_REG8(0x0008), 0x03 },
+	{ CCI_REG8(0x0223), 0x16 },
+	{ CCI_REG8(0x0d27), 0x39 },
+	{ CCI_REG8(0x0d22), 0x00 },
+	{ CCI_REG8(0x03f6), 0x0d },
+	{ CCI_REG8(0x0d04), 0x07 },
+	{ CCI_REG8(0x03f3), 0x72 },
+	{ CCI_REG8(0x03f4), 0xb8 },
+	{ CCI_REG8(0x03f5), 0xbc },
+	{ CCI_REG8(0x0d02), 0x73 },
+
+	/* auto load start */
+	{ CCI_REG8(0x00cb), 0xfc },
+
+	/* OUT 1280x720 */
+	{ CCI_REG8(0x0350), 0x01 },
+	{ CCI_REG8(0x0353), 0x00 },
+	{ CCI_REG8(0x0354), 0x0c },
+	{ CCI_REG16(0x034c), 1280 }, /* Width */
+	{ CCI_REG8(0x021f), 0x14 },
+
+	/* MIPI */
+	{ CCI_REG8(0x0107), 0x05 },
+	{ CCI_REG8(0x0117), 0x01 },
+	{ CCI_REG8(0x0d81), 0x00 },
+	{ CCI_REG8(0x0d84), 0x06 },
+	{ CCI_REG8(0x0d85), 0x40 },
+	{ CCI_REG8(0x0d86), 0x03 },
+	{ CCI_REG8(0x0d87), 0x21 },
+	{ CCI_REG8(0x0db3), 0x03 },
+	{ CCI_REG8(0x0db4), 0x04 },
+	{ CCI_REG8(0x0db5), 0x0d },
+	{ CCI_REG8(0x0db6), 0x01 },
+	{ CCI_REG8(0x0db8), 0x04 },
+	{ CCI_REG8(0x0db9), 0x06 },
+	{ CCI_REG8(0x0d93), 0x03 },
+	{ CCI_REG8(0x0d94), 0x04 },
+	{ CCI_REG8(0x0d95), 0x05 },
+	{ CCI_REG8(0x0d99), 0x06 },
+	{ CCI_REG8(0x0084), 0x01 },
+	{ CCI_REG8(0x0110), 0x01 },
+};
+
+static const struct cci_reg_sequence mode_table_common[] = {
+	{ GC05A2_STREAMING_REG, 0x00 },
+	/* system */
+	{ CCI_REG8(0x0315), 0xd4 },
+	{ CCI_REG8(0x0d06), 0x01 },
+	{ CCI_REG8(0x0a70), 0x80 },
+	{ CCI_REG8(0x031a), 0x00 },
+	{ CCI_REG8(0x0314), 0x00 },
+	{ CCI_REG8(0x0130), 0x08 },
+	{ CCI_REG8(0x0132), 0x01 },
+	{ CCI_REG8(0x0136), 0x38 },
+	{ CCI_REG8(0x0137), 0x03 },
+	{ CCI_REG8(0x0134), 0x5b },
+	{ CCI_REG8(0x031c), 0xe0 },
+	{ CCI_REG8(0x0d82), 0x14 },
+	{ CCI_REG8(0x0dd1), 0x56 },
+	{ CCI_REG8(0x0af4), 0x01 },
+	{ CCI_REG8(0x0002), 0x10 },
+	{ CCI_REG8(0x00c3), 0x34 },
+	{ CCI_REG8(0x00c4), 0x00 },
+	{ CCI_REG8(0x00c5), 0x01 },
+	{ CCI_REG8(0x0af6), 0x00 },
+	{ CCI_REG8(0x0ba0), 0x17 },
+	{ CCI_REG8(0x0ba1), 0x00 },
+	{ CCI_REG8(0x0ba2), 0x00 },
+	{ CCI_REG8(0x0ba3), 0x00 },
+	{ CCI_REG8(0x0ba4), 0x03 },
+	{ CCI_REG8(0x0ba5), 0x00 },
+	{ CCI_REG8(0x0ba6), 0x00 },
+	{ CCI_REG8(0x0ba7), 0x00 },
+	{ CCI_REG8(0x0ba8), 0x40 },
+	{ CCI_REG8(0x0ba9), 0x00 },
+	{ CCI_REG8(0x0baa), 0x00 },
+	{ CCI_REG8(0x0bab), 0x00 },
+	{ CCI_REG8(0x0bac), 0x40 },
+	{ CCI_REG8(0x0bad), 0x00 },
+	{ CCI_REG8(0x0bae), 0x00 },
+	{ CCI_REG8(0x0baf), 0x00 },
+	{ CCI_REG8(0x0bb0), 0x02 },
+	{ CCI_REG8(0x0bb1), 0x00 },
+	{ CCI_REG8(0x0bb2), 0x00 },
+	{ CCI_REG8(0x0bb3), 0x00 },
+	{ CCI_REG8(0x0bb8), 0x02 },
+	{ CCI_REG8(0x0bb9), 0x00 },
+	{ CCI_REG8(0x0bba), 0x00 },
+	{ CCI_REG8(0x0bbb), 0x00 },
+	{ CCI_REG8(0x0a70), 0x80 },
+	{ CCI_REG8(0x0a71), 0x00 },
+	{ CCI_REG8(0x0a72), 0x00 },
+	{ CCI_REG8(0x0a66), 0x00 },
+	{ CCI_REG8(0x0a67), 0x80 },
+	{ CCI_REG8(0x0a4d), 0x4e },
+	{ CCI_REG8(0x0a50), 0x00 },
+	{ CCI_REG8(0x0a4f), 0x0c },
+	{ CCI_REG8(0x0a66), 0x00 },
+	{ CCI_REG8(0x00ca), 0x00 },
+	{ CCI_REG8(0x00cc), 0x00 },
+	{ CCI_REG8(0x00cd), 0x00 },
+	{ CCI_REG8(0x0aa1), 0x00 },
+	{ CCI_REG8(0x0aa2), 0xe0 },
+	{ CCI_REG8(0x0aa3), 0x00 },
+	{ CCI_REG8(0x0aa4), 0x40 },
+	{ CCI_REG8(0x0a90), 0x03 },
+	{ CCI_REG8(0x0a91), 0x0e },
+	{ CCI_REG8(0x0a94), 0x80 },
+	{ CCI_REG8(0x0af6), 0x20 },
+	{ CCI_REG8(0x0b00), 0x91 },
+	{ CCI_REG8(0x0b01), 0x17 },
+	{ CCI_REG8(0x0b02), 0x01 },
+	{ CCI_REG8(0x0b03), 0x00 },
+	{ CCI_REG8(0x0b04), 0x01 },
+	{ CCI_REG8(0x0b05), 0x17 },
+	{ CCI_REG8(0x0b06), 0x01 },
+	{ CCI_REG8(0x0b07), 0x00 },
+	{ CCI_REG8(0x0ae9), 0x01 },
+	{ CCI_REG8(0x0aea), 0x02 },
+	{ CCI_REG8(0x0ae8), 0x53 },
+	{ CCI_REG8(0x0ae8), 0x43 },
+	{ CCI_REG8(0x0af6), 0x30 },
+	{ CCI_REG8(0x0b00), 0x08 },
+	{ CCI_REG8(0x0b01), 0x0f },
+	{ CCI_REG8(0x0b02), 0x00 },
+	{ CCI_REG8(0x0b04), 0x1c },
+	{ CCI_REG8(0x0b05), 0x24 },
+	{ CCI_REG8(0x0b06), 0x00 },
+	{ CCI_REG8(0x0b08), 0x30 },
+	{ CCI_REG8(0x0b09), 0x40 },
+	{ CCI_REG8(0x0b0a), 0x00 },
+	{ CCI_REG8(0x0b0c), 0x0e },
+	{ CCI_REG8(0x0b0d), 0x2a },
+	{ CCI_REG8(0x0b0e), 0x00 },
+	{ CCI_REG8(0x0b10), 0x0e },
+	{ CCI_REG8(0x0b11), 0x2b },
+	{ CCI_REG8(0x0b12), 0x00 },
+	{ CCI_REG8(0x0b14), 0x0e },
+	{ CCI_REG8(0x0b15), 0x23 },
+	{ CCI_REG8(0x0b16), 0x00 },
+	{ CCI_REG8(0x0b18), 0x0e },
+	{ CCI_REG8(0x0b19), 0x24 },
+	{ CCI_REG8(0x0b1a), 0x00 },
+	{ CCI_REG8(0x0b1c), 0x0c },
+	{ CCI_REG8(0x0b1d), 0x0c },
+	{ CCI_REG8(0x0b1e), 0x00 },
+	{ CCI_REG8(0x0b20), 0x03 },
+	{ CCI_REG8(0x0b21), 0x03 },
+	{ CCI_REG8(0x0b22), 0x00 },
+	{ CCI_REG8(0x0b24), 0x0e },
+	{ CCI_REG8(0x0b25), 0x0e },
+	{ CCI_REG8(0x0b26), 0x00 },
+	{ CCI_REG8(0x0b28), 0x03 },
+	{ CCI_REG8(0x0b29), 0x03 },
+	{ CCI_REG8(0x0b2a), 0x00 },
+	{ CCI_REG8(0x0b2c), 0x12 },
+	{ CCI_REG8(0x0b2d), 0x12 },
+	{ CCI_REG8(0x0b2e), 0x00 },
+	{ CCI_REG8(0x0b30), 0x08 },
+	{ CCI_REG8(0x0b31), 0x08 },
+	{ CCI_REG8(0x0b32), 0x00 },
+	{ CCI_REG8(0x0b34), 0x14 },
+	{ CCI_REG8(0x0b35), 0x14 },
+	{ CCI_REG8(0x0b36), 0x00 },
+	{ CCI_REG8(0x0b38), 0x10 },
+	{ CCI_REG8(0x0b39), 0x10 },
+	{ CCI_REG8(0x0b3a), 0x00 },
+	{ CCI_REG8(0x0b3c), 0x16 },
+	{ CCI_REG8(0x0b3d), 0x16 },
+	{ CCI_REG8(0x0b3e), 0x00 },
+	{ CCI_REG8(0x0b40), 0x10 },
+	{ CCI_REG8(0x0b41), 0x10 },
+	{ CCI_REG8(0x0b42), 0x00 },
+	{ CCI_REG8(0x0b44), 0x19 },
+	{ CCI_REG8(0x0b45), 0x19 },
+	{ CCI_REG8(0x0b46), 0x00 },
+	{ CCI_REG8(0x0b48), 0x16 },
+	{ CCI_REG8(0x0b49), 0x16 },
+	{ CCI_REG8(0x0b4a), 0x00 },
+	{ CCI_REG8(0x0b4c), 0x19 },
+	{ CCI_REG8(0x0b4d), 0x19 },
+	{ CCI_REG8(0x0b4e), 0x00 },
+	{ CCI_REG8(0x0b50), 0x16 },
+	{ CCI_REG8(0x0b51), 0x16 },
+	{ CCI_REG8(0x0b52), 0x00 },
+	{ CCI_REG8(0x0b80), 0x01 },
+	{ CCI_REG8(0x0b81), 0x00 },
+	{ CCI_REG8(0x0b82), 0x00 },
+	{ CCI_REG8(0x0b84), 0x00 },
+	{ CCI_REG8(0x0b85), 0x00 },
+	{ CCI_REG8(0x0b86), 0x00 },
+	{ CCI_REG8(0x0b88), 0x01 },
+	{ CCI_REG8(0x0b89), 0x6a },
+	{ CCI_REG8(0x0b8a), 0x00 },
+	{ CCI_REG8(0x0b8c), 0x00 },
+	{ CCI_REG8(0x0b8d), 0x01 },
+	{ CCI_REG8(0x0b8e), 0x00 },
+	{ CCI_REG8(0x0b90), 0x01 },
+	{ CCI_REG8(0x0b91), 0xf6 },
+	{ CCI_REG8(0x0b92), 0x00 },
+	{ CCI_REG8(0x0b94), 0x00 },
+	{ CCI_REG8(0x0b95), 0x02 },
+	{ CCI_REG8(0x0b96), 0x00 },
+	{ CCI_REG8(0x0b98), 0x02 },
+	{ CCI_REG8(0x0b99), 0xc4 },
+	{ CCI_REG8(0x0b9a), 0x00 },
+	{ CCI_REG8(0x0b9c), 0x00 },
+	{ CCI_REG8(0x0b9d), 0x03 },
+	{ CCI_REG8(0x0b9e), 0x00 },
+	{ CCI_REG8(0x0ba0), 0x03 },
+	{ CCI_REG8(0x0ba1), 0xd8 },
+	{ CCI_REG8(0x0ba2), 0x00 },
+	{ CCI_REG8(0x0ba4), 0x00 },
+	{ CCI_REG8(0x0ba5), 0x04 },
+	{ CCI_REG8(0x0ba6), 0x00 },
+	{ CCI_REG8(0x0ba8), 0x05 },
+	{ CCI_REG8(0x0ba9), 0x4d },
+	{ CCI_REG8(0x0baa), 0x00 },
+	{ CCI_REG8(0x0bac), 0x00 },
+	{ CCI_REG8(0x0bad), 0x05 },
+	{ CCI_REG8(0x0bae), 0x00 },
+	{ CCI_REG8(0x0bb0), 0x07 },
+	{ CCI_REG8(0x0bb1), 0x3e },
+	{ CCI_REG8(0x0bb2), 0x00 },
+	{ CCI_REG8(0x0bb4), 0x00 },
+	{ CCI_REG8(0x0bb5), 0x06 },
+	{ CCI_REG8(0x0bb6), 0x00 },
+	{ CCI_REG8(0x0bb8), 0x0a },
+	{ CCI_REG8(0x0bb9), 0x1a },
+	{ CCI_REG8(0x0bba), 0x00 },
+	{ CCI_REG8(0x0bbc), 0x09 },
+	{ CCI_REG8(0x0bbd), 0x36 },
+	{ CCI_REG8(0x0bbe), 0x00 },
+	{ CCI_REG8(0x0bc0), 0x0e },
+	{ CCI_REG8(0x0bc1), 0x66 },
+	{ CCI_REG8(0x0bc2), 0x00 },
+	{ CCI_REG8(0x0bc4), 0x10 },
+	{ CCI_REG8(0x0bc5), 0x06 },
+	{ CCI_REG8(0x0bc6), 0x00 },
+	{ CCI_REG8(0x02c1), 0xe0 },
+	{ CCI_REG8(0x0207), 0x04 },
+	{ CCI_REG8(0x02c2), 0x10 },
+	{ CCI_REG8(0x02c3), 0x74 },
+	{ CCI_REG8(0x02c5), 0x09 },
+	{ CCI_REG8(0x02c1), 0xe0 },
+	{ CCI_REG8(0x0207), 0x04 },
+	{ CCI_REG8(0x02c2), 0x10 },
+	{ CCI_REG8(0x02c5), 0x09 },
+	{ CCI_REG8(0x02c1), 0xe0 },
+	{ CCI_REG8(0x0207), 0x04 },
+	{ CCI_REG8(0x02c2), 0x10 },
+	{ CCI_REG8(0x02c5), 0x09 },
+	{ CCI_REG8(0x0aa1), 0x15 },
+	{ CCI_REG8(0x0aa2), 0x50 },
+	{ CCI_REG8(0x0aa3), 0x00 },
+	{ CCI_REG8(0x0aa4), 0x09 },
+	{ CCI_REG8(0x0a90), 0x25 },
+	{ CCI_REG8(0x0a91), 0x0e },
+	{ CCI_REG8(0x0a94), 0x80 },
+
+	/* ISP */
+	{ CCI_REG8(0x0050), 0x00 },
+	{ CCI_REG8(0x0089), 0x83 },
+	{ CCI_REG8(0x005a), 0x40 },
+	{ CCI_REG8(0x00c3), 0x35 },
+	{ CCI_REG8(0x00c4), 0x80 },
+	{ CCI_REG8(0x0080), 0x10 },
+	{ CCI_REG8(0x0040), 0x12 },
+	{ CCI_REG8(0x0053), 0x0a },
+	{ CCI_REG8(0x0054), 0x44 },
+	{ CCI_REG8(0x0055), 0x32 },
+	{ CCI_REG8(0x0058), 0x89 },
+	{ CCI_REG8(0x004a), 0x03 },
+	{ CCI_REG8(0x0048), 0xf0 },
+	{ CCI_REG8(0x0049), 0x0f },
+	{ CCI_REG8(0x0041), 0x20 },
+	{ CCI_REG8(0x0043), 0x0a },
+	{ CCI_REG8(0x009d), 0x08 },
+	{ CCI_REG8(0x0236), 0x40 },
+	{ CCI_REG8(0x0204), 0x04 },
+	{ CCI_REG8(0x0205), 0x00 },
+	{ CCI_REG8(0x02b3), 0x00 },
+	{ CCI_REG8(0x02b4), 0x00 },
+	{ CCI_REG8(0x009e), 0x01 },
+	{ CCI_REG8(0x009f), 0x94 },
+
+	/* auto load REG */
+	{ CCI_REG8(0x0aa1), 0x10 },
+	{ CCI_REG8(0x0aa2), 0xf8 },
+	{ CCI_REG8(0x0aa3), 0x00 },
+	{ CCI_REG8(0x0aa4), 0x1f },
+	{ CCI_REG8(0x0a90), 0x11 },
+	{ CCI_REG8(0x0a91), 0x0e },
+	{ CCI_REG8(0x0a94), 0x80 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x0a90), 0x00 },
+	{ CCI_REG8(0x0a70), 0x00 },
+	{ CCI_REG8(0x0a67), 0x00 },
+	{ CCI_REG8(0x0af4), 0x29 },
+
+	/* DPHY */
+	{ CCI_REG8(0x0d80), 0x07 },
+	{ CCI_REG8(0x0dd3), 0x18 },
+
+	/* CISCTL_Reset */
+	{ CCI_REG8(0x031c), 0x80 },
+	{ CCI_REG8(0x03fe), 0x30 },
+	{ CCI_REG8(0x0d17), 0x06 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x0d17), 0x00 },
+	{ CCI_REG8(0x031c), 0x93 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x031c), 0x80 },
+	{ CCI_REG8(0x03fe), 0x30 },
+	{ CCI_REG8(0x0d17), 0x06 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x0d17), 0x00 },
+	{ CCI_REG8(0x031c), 0x93 },
+};
+
+struct gc05a2_mode {
+	u32 width;
+	u32 height;
+	const struct gc05a2_reg_list reg_list;
+
+	u32 hts; /* Horizontal timining size */
+	u32 vts_def; /* Default vertical timining size */
+	u32 vts_min; /* Min vertical timining size */
+};
+
+/* Declare modes in order, from biggest to smallest height. */
+static const struct gc05a2_mode gc05a2_modes[] = {
+	{
+		/* 2592*1944@30fps */
+		.width = GC05A2_NATIVE_WIDTH,
+		.height = GC05A2_NATIVE_HEIGHT,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_2592x1944),
+			.regs = mode_2592x1944,
+		},
+		.hts = 3664,
+		.vts_def = 2032,
+		.vts_min = 2032,
+	},
+	{
+		/* 1280*720@60fps */
+		.width = 1280,
+		.height = 720,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720),
+			.regs = mode_1280x720,
+		},
+		.hts = 3616,
+		.vts_def = 1032,
+		.vts_min = 1032,
+	},
+};
+
+static inline struct gc05a2 *to_gc05a2(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct gc05a2, sd);
+}
+
+static int gc05a2_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct gc05a2 *gc05a2 = to_gc05a2(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(gc05a2_supply_name),
+				    gc05a2->supplies);
+	if (ret < 0) {
+		dev_err(gc05a2->dev, "failed to enable regulators: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(gc05a2->xclk);
+	if (ret < 0) {
+		regulator_bulk_disable(ARRAY_SIZE(gc05a2_supply_name),
+				       gc05a2->supplies);
+		dev_err(gc05a2->dev, "clk prepare enable failed\n");
+		return ret;
+	}
+
+	fsleep(GC05A2_SLEEP_US);
+
+	gpiod_set_value_cansleep(gc05a2->reset_gpio, 0);
+	fsleep(GC05A2_SLEEP_US);
+
+	return 0;
+}
+
+static int gc05a2_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct gc05a2 *gc05a2 = to_gc05a2(sd);
+
+	clk_disable_unprepare(gc05a2->xclk);
+	gpiod_set_value_cansleep(gc05a2->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(gc05a2_supply_name),
+			       gc05a2->supplies);
+
+	return 0;
+}
+
+static int gc05a2_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = GC05A2_MBUS_CODE;
+
+	return 0;
+}
+
+static int gc05a2_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->code != GC05A2_MBUS_CODE)
+		return -EINVAL;
+
+	if (fse->index >= ARRAY_SIZE(gc05a2_modes))
+		return -EINVAL;
+
+	fse->min_width = gc05a2_modes[fse->index].width;
+	fse->max_width = gc05a2_modes[fse->index].width;
+	fse->min_height = gc05a2_modes[fse->index].height;
+	fse->max_height = gc05a2_modes[fse->index].height;
+
+	return 0;
+}
+
+static int gc05a2_update_cur_mode_controls(struct gc05a2 *gc05a2,
+					   const struct gc05a2_mode *mode)
+{
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ret = __v4l2_ctrl_modify_range(gc05a2->vblank,
+				       mode->vts_min - mode->height,
+				       GC05A2_VTS_MAX - mode->height, 1,
+				       mode->vts_def - mode->height);
+	if (ret) {
+		dev_err(gc05a2->dev, "VB ctrl range update failed\n");
+		return ret;
+	}
+
+	h_blank = mode->hts - mode->width;
+	ret = __v4l2_ctrl_modify_range(gc05a2->hblank, h_blank, h_blank, 1,
+				       h_blank);
+	if (ret) {
+		dev_err(gc05a2->dev, "HB ctrl range update failed\n");
+		return ret;
+	}
+
+	exposure_max = mode->vts_def - GC05A2_EXP_MARGIN;
+	ret = __v4l2_ctrl_modify_range(gc05a2->exposure, GC05A2_EXP_MIN,
+				       exposure_max, GC05A2_EXP_STEP,
+				       exposure_max);
+	if (ret) {
+		dev_err(gc05a2->dev, "exposure ctrl range update failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void gc05a2_update_pad_format(struct gc05a2 *gc08a3,
+				     const struct gc05a2_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = GC05A2_MBUS_CODE;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int gc05a2_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct gc05a2 *gc05a2 = to_gc05a2(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt;
+	struct v4l2_rect *crop;
+	const struct gc05a2_mode *mode;
+
+	mode = v4l2_find_nearest_size(gc05a2_modes, ARRAY_SIZE(gc05a2_modes),
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	/* update crop info to subdev state */
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	crop->width = mode->width;
+	crop->height = mode->height;
+
+	/* update fmt info to subdev state */
+	gc05a2_update_pad_format(gc05a2, mode, &fmt->format);
+	mbus_fmt = v4l2_subdev_state_get_format(state, 0);
+	*mbus_fmt = fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+	gc05a2->cur_mode = mode;
+	gc05a2_update_cur_mode_controls(gc05a2, mode);
+
+	return 0;
+}
+
+static int gc05a2_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_state_get_crop(state, 0);
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = GC05A2_NATIVE_WIDTH;
+		sel->r.height = GC05A2_NATIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int gc05a2_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.pad = 0,
+		.format = {
+			.code = GC05A2_MBUS_CODE,
+			.width = gc05a2_modes[0].width,
+			.height = gc05a2_modes[0].height,
+		},
+	};
+
+	gc05a2_set_format(sd, state, &fmt);
+
+	return 0;
+}
+
+static int gc05a2_set_ctrl_hflip(struct gc05a2 *gc05a2, u32 ctrl_val)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(gc05a2->regmap, GC05A2_FLIP_REG, &val, NULL);
+	if (ret) {
+		dev_err(gc05a2->dev, "read hflip register failed: %d\n", ret);
+		return ret;
+	}
+
+	return cci_update_bits(gc05a2->regmap, GC05A2_FLIP_REG,
+			       GC05A2_FLIP_H_MASK,
+			       ctrl_val ? GC05A2_FLIP_H_MASK : 0, NULL);
+}
+
+static int gc05a2_set_ctrl_vflip(struct gc05a2 *gc05a2, u32 ctrl_val)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(gc05a2->regmap, GC05A2_FLIP_REG, &val, NULL);
+	if (ret) {
+		dev_err(gc05a2->dev, "read vflip register failed: %d\n", ret);
+		return ret;
+	}
+
+	return cci_update_bits(gc05a2->regmap, GC05A2_FLIP_REG,
+			       GC05A2_FLIP_V_MASK,
+			       ctrl_val ? GC05A2_FLIP_V_MASK : 0, NULL);
+}
+
+static int gc05a2_test_pattern(struct gc05a2 *gc05a2, u32 pattern_menu)
+{
+	u32 pattern;
+	int ret;
+
+	if (pattern_menu) {
+		switch (pattern_menu) {
+		case 1:
+		case 2:
+		case 3:
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			pattern = pattern_menu << 4;
+			break;
+
+		case 8:
+			pattern = 0;
+			break;
+
+		case 9:
+			pattern = 4;
+			break;
+
+		default:
+			/* Set pattern to 0, it's a safe default. */
+			pattern = 0;
+			break;
+		}
+
+		ret = cci_write(gc05a2->regmap, GC05A2_REG_TEST_PATTERN_IDX,
+				pattern, NULL);
+		if (ret)
+			return ret;
+
+		return cci_write(gc05a2->regmap, GC05A2_REG_TEST_PATTERN_EN,
+				 GC05A2_TEST_PATTERN_EN, NULL);
+	} else {
+		return cci_write(gc05a2->regmap, GC05A2_REG_TEST_PATTERN_EN,
+				 0x00, NULL);
+	}
+}
+
+static int gc05a2_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gc05a2 *gc05a2 =
+		container_of(ctrl->handler, struct gc05a2, ctrls);
+	int ret = 0;
+	s64 exposure_max;
+	struct v4l2_subdev_state *state;
+	const struct v4l2_mbus_framefmt *format;
+
+	state = v4l2_subdev_get_locked_active_state(&gc05a2->sd);
+	format = v4l2_subdev_state_get_format(state, 0);
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = format->height + ctrl->val - GC05A2_EXP_MARGIN;
+		__v4l2_ctrl_modify_range(gc05a2->exposure,
+					 gc05a2->exposure->minimum,
+					 exposure_max, gc05a2->exposure->step,
+					 exposure_max);
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is on for streaming.
+	 */
+	if (!pm_runtime_get_if_active(gc05a2->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = cci_write(gc05a2->regmap, GC05A2_EXP_REG,
+				ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = cci_write(gc05a2->regmap, GC05A2_AGAIN_REG,
+				ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = cci_write(gc05a2->regmap, GC05A2_FRAME_LENGTH_REG,
+				gc05a2->cur_mode->height + ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ret = gc05a2_set_ctrl_hflip(gc05a2, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		ret = gc05a2_set_ctrl_vflip(gc05a2, ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = gc05a2_test_pattern(gc05a2, ctrl->val);
+		break;
+
+	default:
+		break;
+	}
+
+	pm_runtime_put(gc05a2->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops gc05a2_ctrl_ops = {
+	.s_ctrl = gc05a2_set_ctrl,
+};
+
+static int gc05a2_identify_module(struct gc05a2 *gc05a2)
+{
+	u64 val;
+	int ret;
+
+	if (gc05a2->identified)
+		return 0;
+
+	ret = cci_read(gc05a2->regmap, GC05A2_REG_CHIP_ID, &val, NULL);
+	if (ret)
+		return ret;
+
+	if (val != GC05A2_CHIP_ID) {
+		dev_err(gc05a2->dev, "chip id mismatch: 0x%x!=0x%llx",
+			GC05A2_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	gc05a2->identified = true;
+
+	return 0;
+}
+
+static int gc05a2_start_streaming(struct gc05a2 *gc05a2)
+{
+	const struct gc05a2_mode *mode;
+	const struct gc05a2_reg_list *reg_list;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(gc05a2->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = gc05a2_identify_module(gc05a2);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = cci_multi_reg_write(gc05a2->regmap,
+				  mode_table_common,
+				  ARRAY_SIZE(mode_table_common), NULL);
+	if (ret)
+		goto err_rpm_put;
+
+	mode = gc05a2->cur_mode;
+	reg_list = &mode->reg_list;
+
+	ret = cci_multi_reg_write(gc05a2->regmap,
+				  reg_list->regs, reg_list->num_of_regs, NULL);
+	if (ret < 0)
+		goto err_rpm_put;
+
+	ret = __v4l2_ctrl_handler_setup(&gc05a2->ctrls);
+	if (ret < 0) {
+		dev_err(gc05a2->dev, "could not sync v4l2 controls\n");
+		goto err_rpm_put;
+	}
+
+	ret = cci_write(gc05a2->regmap, GC05A2_STREAMING_REG, 1, NULL);
+	if (ret < 0) {
+		dev_err(gc05a2->dev, "write STREAMING_REG failed: %d\n", ret);
+		goto err_rpm_put;
+	}
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(gc05a2->dev);
+	return ret;
+}
+
+static int gc05a2_stop_streaming(struct gc05a2 *gc05a2)
+{
+	int ret;
+
+	ret = cci_write(gc05a2->regmap, GC05A2_STREAMING_REG, 0, NULL);
+	if (ret < 0)
+		dev_err(gc05a2->dev, "could not sent stop streaming %d\n", ret);
+
+	pm_runtime_put(gc05a2->dev);
+	return ret;
+}
+
+static int gc05a2_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct gc05a2 *gc05a2 = to_gc05a2(subdev);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	state = v4l2_subdev_lock_and_get_active_state(subdev);
+
+	if (enable)
+		ret = gc05a2_start_streaming(gc05a2);
+	else
+		ret = gc05a2_stop_streaming(gc05a2);
+
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops gc05a2_video_ops = {
+	.s_stream = gc05a2_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops gc05a2_subdev_pad_ops = {
+	.enum_mbus_code = gc05a2_enum_mbus_code,
+	.enum_frame_size = gc05a2_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = gc05a2_set_format,
+	.get_selection = gc05a2_get_selection,
+};
+
+static const struct v4l2_subdev_ops gc05a2_subdev_ops = {
+	.video = &gc05a2_video_ops,
+	.pad = &gc05a2_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops gc05a2_internal_ops = {
+	.init_state = gc05a2_init_state,
+};
+
+static int gc05a2_get_regulators(struct device *dev, struct gc05a2 *gc05a2)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gc05a2_supply_name); i++)
+		gc05a2->supplies[i].supply = gc05a2_supply_name[i];
+
+	return devm_regulator_bulk_get(dev, ARRAY_SIZE(gc05a2_supply_name),
+				       gc05a2->supplies);
+}
+
+static int gc05a2_parse_fwnode(struct gc05a2 *gc05a2)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	int ret;
+	struct device *dev = gc05a2->dev;
+
+	endpoint =
+		fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
+						FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!endpoint)
+		return dev_err_probe(dev, -EINVAL, "Missing endpoint node\n");
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
+	if (ret) {
+		dev_err_probe(dev, ret, "parsing endpoint node failed\n");
+		goto done;
+	}
+
+	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       gc05a2_link_freq_menu_items,
+				       ARRAY_SIZE(gc05a2_link_freq_menu_items),
+				       &gc05a2->link_freq_bitmap);
+	if (ret)
+		goto done;
+
+done:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(endpoint);
+	return ret;
+}
+
+static u64 gc05a2_to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate =
+		gc05a2_link_freq_menu_items[f_index] * 2 * GC05A2_DATA_LANES;
+
+	return div_u64(pixel_rate, GC05A2_RGB_DEPTH);
+}
+
+static int gc05a2_init_controls(struct gc05a2 *gc05a2)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&gc05a2->sd);
+	const struct gc05a2_mode *mode = &gc05a2_modes[0];
+	const struct v4l2_ctrl_ops *ops = &gc05a2_ctrl_ops;
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &gc05a2->ctrls;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 9);
+	if (ret)
+		return ret;
+
+	gc05a2->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &gc05a2_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	gc05a2->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &gc05a2_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &gc05a2->hflip);
+
+	gc05a2->link_freq =
+	v4l2_ctrl_new_int_menu(ctrl_hdlr,
+			       &gc05a2_ctrl_ops,
+			       V4L2_CID_LINK_FREQ,
+			       ARRAY_SIZE(gc05a2_link_freq_menu_items) - 1,
+			       0,
+			       gc05a2_link_freq_menu_items);
+	if (gc05a2->link_freq)
+		gc05a2->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	gc05a2->pixel_rate =
+		v4l2_ctrl_new_std(ctrl_hdlr,
+				  &gc05a2_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 0,
+				  gc05a2_to_pixel_rate(0),
+				  1,
+				  gc05a2_to_pixel_rate(0));
+
+	gc05a2->vblank =
+		v4l2_ctrl_new_std(ctrl_hdlr,
+				  &gc05a2_ctrl_ops, V4L2_CID_VBLANK,
+				  mode->vts_min - mode->height,
+				  GC05A2_VTS_MAX - mode->height, 1,
+				  mode->vts_def - mode->height);
+
+	h_blank = mode->hts - mode->width;
+	gc05a2->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &gc05a2_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (gc05a2->hblank)
+		gc05a2->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &gc05a2_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, GC05A2_AGAIN_MIN,
+			  GC05A2_AGAIN_MAX, GC05A2_AGAIN_STEP,
+			  GC05A2_AGAIN_MIN);
+
+	exposure_max = mode->vts_def - GC05A2_EXP_MARGIN;
+	gc05a2->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &gc05a2_ctrl_ops,
+					     V4L2_CID_EXPOSURE, GC05A2_EXP_MIN,
+					     exposure_max, GC05A2_EXP_STEP,
+					     exposure_max);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &gc05a2_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(gc05a2_test_pattern_menu) - 1,
+				     0, 0, gc05a2_test_pattern_menu);
+
+	/* register properties to fwnode (e.g. rotation, orientation) */
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error_ctrls;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, ops, &props);
+	if (ret)
+		goto error_ctrls;
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		goto error_ctrls;
+	}
+
+	gc05a2->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error_ctrls:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static int gc05a2_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct gc05a2 *gc05a2;
+	int ret;
+
+	gc05a2 = devm_kzalloc(dev, sizeof(*gc05a2), GFP_KERNEL);
+	if (!gc05a2)
+		return -ENOMEM;
+
+	gc05a2->dev = dev;
+
+	ret = gc05a2_parse_fwnode(gc05a2);
+	if (ret)
+		return ret;
+
+	gc05a2->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(gc05a2->regmap))
+		return dev_err_probe(dev, PTR_ERR(gc05a2->regmap),
+				     "failed to init CCI\n");
+
+	gc05a2->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(gc05a2->xclk))
+		return dev_err_probe(dev, PTR_ERR(gc05a2->xclk),
+				     "failed to get xclk\n");
+
+	ret = clk_set_rate(gc05a2->xclk, GC05A2_DEFAULT_CLK_FREQ);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to set xclk frequency\n");
+
+	ret = gc05a2_get_regulators(dev, gc05a2);
+	if (ret < 0)
+		return dev_err_probe(dev, ret,
+				     "failed to get regulators\n");
+
+	gc05a2->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(gc05a2->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(gc05a2->reset_gpio),
+				     "failed to get gpio\n");
+
+	v4l2_i2c_subdev_init(&gc05a2->sd, client, &gc05a2_subdev_ops);
+	gc05a2->sd.internal_ops = &gc05a2_internal_ops;
+	gc05a2->cur_mode = &gc05a2_modes[0];
+
+	ret = gc05a2_init_controls(gc05a2);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to init controls\n");
+
+	gc05a2->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	gc05a2->pad.flags = MEDIA_PAD_FL_SOURCE;
+	gc05a2->sd.dev = &client->dev;
+	gc05a2->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&gc05a2->sd.entity, 1, &gc05a2->pad);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "could not register media entity\n");
+		goto err_v4l2_ctrl_handler_free;
+	}
+
+	gc05a2->sd.state_lock = gc05a2->ctrls.lock;
+	ret = v4l2_subdev_init_finalize(&gc05a2->sd);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "v4l2 subdev init error\n");
+		goto err_media_entity_cleanup;
+	}
+
+	pm_runtime_enable(gc05a2->dev);
+	pm_runtime_set_autosuspend_delay(gc05a2->dev, 1000);
+	pm_runtime_use_autosuspend(gc05a2->dev);
+	pm_runtime_idle(gc05a2->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&gc05a2->sd);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "could not register v4l2 device\n");
+		goto err_rpm;
+	}
+
+	return 0;
+
+err_rpm:
+	pm_runtime_disable(gc05a2->dev);
+	v4l2_subdev_cleanup(&gc05a2->sd);
+
+err_media_entity_cleanup:
+	media_entity_cleanup(&gc05a2->sd.entity);
+
+err_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(&gc05a2->ctrls);
+
+	return ret;
+}
+
+static void gc05a2_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc05a2 *gc05a2 = to_gc05a2(sd);
+
+	v4l2_async_unregister_subdev(&gc05a2->sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&gc05a2->sd.entity);
+	v4l2_ctrl_handler_free(&gc05a2->ctrls);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		gc05a2_power_off(gc05a2->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id gc05a2_of_match[] = {
+	{ .compatible = "galaxycore,gc05a2" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, gc05a2_of_match);
+
+static DEFINE_RUNTIME_DEV_PM_OPS(gc05a2_pm_ops,
+				 gc05a2_power_off,
+				 gc05a2_power_on,
+				 NULL);
+
+static struct i2c_driver gc05a2_i2c_driver = {
+	.driver = {
+		.of_match_table = gc05a2_of_match,
+		.pm = pm_ptr(&gc05a2_pm_ops),
+		.name  = "gc05a2",
+	},
+	.probe = gc05a2_probe,
+	.remove = gc05a2_remove,
+};
+module_i2c_driver(gc05a2_i2c_driver);
+
+MODULE_DESCRIPTION("GalaxyCore gc05a2 Camera driver");
+MODULE_AUTHOR("Zhi Mao <zhi.mao@mediatek.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/gc08a3.c b/drivers/media/i2c/gc08a3.c
new file mode 100644
index 000000000000..938709a677b6
--- /dev/null
+++ b/drivers/media/i2c/gc08a3.c
@@ -0,0 +1,1331 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for GalaxyCore gc08a3 image sensor
+ *
+ * Copyright 2024 MediaTek
+ *
+ * Zhi Mao <zhi.mao@mediatek.com>
+ */
+#include <linux/array_size.h>
+#include <linux/bits.h>
+#include <linux/clk.h>
+#include <linux/container_of.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/math64.h>
+#include <linux/mod_devicetable.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/types.h>
+#include <linux/units.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define GC08A3_REG_TEST_PATTERN_EN CCI_REG8(0x008c)
+#define GC08A3_REG_TEST_PATTERN_IDX CCI_REG8(0x008d)
+#define GC08A3_TEST_PATTERN_EN 0x01
+
+#define GC08A3_STREAMING_REG CCI_REG8(0x0100)
+
+#define GC08A3_FLIP_REG CCI_REG8(0x0101)
+#define GC08A3_FLIP_H_MASK BIT(0)
+#define GC08A3_FLIP_V_MASK BIT(1)
+
+#define GC08A3_EXP_REG CCI_REG16(0x0202)
+#define GC08A3_EXP_MARGIN 16
+#define GC08A3_EXP_MIN 4
+#define GC08A3_EXP_STEP 1
+
+#define GC08A3_AGAIN_REG CCI_REG16(0x0204)
+#define GC08A3_AGAIN_MIN 1024
+#define GC08A3_AGAIN_MAX (1024 * 16)
+#define GC08A3_AGAIN_STEP 1
+
+#define GC08A3_FRAME_LENGTH_REG CCI_REG16(0x0340)
+#define GC08A3_VTS_MAX 0xfff0
+
+#define GC08A3_REG_CHIP_ID CCI_REG16(0x03f0)
+#define GC08A3_CHIP_ID 0x08a3
+
+#define GC08A3_NATIVE_WIDTH 3264
+#define GC08A3_NATIVE_HEIGHT 2448
+
+#define GC08A3_DEFAULT_CLK_FREQ (24 * HZ_PER_MHZ)
+#define GC08A3_MBUS_CODE MEDIA_BUS_FMT_SRGGB10_1X10
+#define GC08A3_DATA_LANES 4
+
+#define GC08A3_RGB_DEPTH 10
+
+#define GC08A3_SLEEP_US  (2 * USEC_PER_MSEC)
+
+static const char *const gc08a3_test_pattern_menu[] = {
+	"No Pattern", "Solid Black", "Colour Bar", "Solid White",
+	"Solid Red", "Solid Green", "Solid Blue", "Solid Yellow",
+};
+
+static const s64 gc08a3_link_freq_menu_items[] = {
+	(336 * HZ_PER_MHZ),
+	(207 * HZ_PER_MHZ),
+};
+
+static const char *const gc08a3_supply_name[] = {
+	"avdd",
+	"dvdd",
+	"dovdd",
+};
+
+struct gc08a3 {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct clk *xclk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(gc08a3_supply_name)];
+	struct gpio_desc *reset_gpio;
+
+	struct v4l2_ctrl_handler ctrls;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
+
+	struct regmap *regmap;
+	unsigned long link_freq_bitmap;
+	const struct gc08a3_mode *cur_mode;
+};
+
+struct gc08a3_reg_list {
+	u32 num_of_regs;
+	const struct cci_reg_sequence *regs;
+};
+
+static const struct cci_reg_sequence mode_3264x2448[] = {
+	/* system */
+	{ CCI_REG8(0x0336), 0x70 },
+	{ CCI_REG8(0x0383), 0xbb },
+	{ CCI_REG8(0x0344), 0x00 },
+	{ CCI_REG8(0x0345), 0x06 },
+	{ CCI_REG8(0x0346), 0x00 },
+	{ CCI_REG8(0x0347), 0x04 },
+	{ CCI_REG8(0x0348), 0x0c },
+	{ CCI_REG8(0x0349), 0xd0 },
+	{ CCI_REG8(0x034a), 0x09 },
+	{ CCI_REG8(0x034b), 0x9c },
+	{ CCI_REG8(0x0202), 0x09 },
+	{ CCI_REG8(0x0203), 0x04 },
+	{ CCI_REG8(0x0340), 0x09 },
+	{ CCI_REG8(0x0341), 0xf4 },
+	{ CCI_REG8(0x0342), 0x07 },
+	{ CCI_REG8(0x0343), 0x1c },
+
+	{ CCI_REG8(0x0226), 0x00 },
+	{ CCI_REG8(0x0227), 0x28 },
+	{ CCI_REG8(0x0e38), 0x49 },
+	{ CCI_REG8(0x0210), 0x13 },
+	{ CCI_REG8(0x0218), 0x00 },
+	{ CCI_REG8(0x0241), 0x88 },
+	{ CCI_REG8(0x0392), 0x60 },
+
+	/* ISP */
+	{ CCI_REG8(0x00a2), 0x00 },
+	{ CCI_REG8(0x00a3), 0x00 },
+	{ CCI_REG8(0x00ab), 0x00 },
+	{ CCI_REG8(0x00ac), 0x00 },
+
+	/* GAIN */
+	{ CCI_REG8(0x0204), 0x04 },
+	{ CCI_REG8(0x0205), 0x00 },
+	{ CCI_REG8(0x0050), 0x5c },
+	{ CCI_REG8(0x0051), 0x44 },
+
+	/* out window */
+	{ CCI_REG8(0x009a), 0x66 },
+	{ CCI_REG8(0x0351), 0x00 },
+	{ CCI_REG8(0x0352), 0x06 },
+	{ CCI_REG8(0x0353), 0x00 },
+	{ CCI_REG8(0x0354), 0x08 },
+	{ CCI_REG8(0x034c), 0x0c },
+	{ CCI_REG8(0x034d), 0xc0 },
+	{ CCI_REG8(0x034e), 0x09 },
+	{ CCI_REG8(0x034f), 0x90 },
+
+	/* MIPI */
+	{ CCI_REG8(0x0114), 0x03 },
+	{ CCI_REG8(0x0180), 0x65 },
+	{ CCI_REG8(0x0181), 0xf0 },
+	{ CCI_REG8(0x0185), 0x01 },
+	{ CCI_REG8(0x0115), 0x30 },
+	{ CCI_REG8(0x011b), 0x12 },
+	{ CCI_REG8(0x011c), 0x12 },
+	{ CCI_REG8(0x0121), 0x06 },
+	{ CCI_REG8(0x0122), 0x06 },
+	{ CCI_REG8(0x0123), 0x15 },
+	{ CCI_REG8(0x0124), 0x01 },
+	{ CCI_REG8(0x0125), 0x0b },
+	{ CCI_REG8(0x0126), 0x08 },
+	{ CCI_REG8(0x0129), 0x06 },
+	{ CCI_REG8(0x012a), 0x08 },
+	{ CCI_REG8(0x012b), 0x08 },
+
+	{ CCI_REG8(0x0a73), 0x60 },
+	{ CCI_REG8(0x0a70), 0x11 },
+	{ CCI_REG8(0x0313), 0x80 },
+	{ CCI_REG8(0x0aff), 0x00 },
+	{ CCI_REG8(0x0a70), 0x00 },
+	{ CCI_REG8(0x00a4), 0x80 },
+	{ CCI_REG8(0x0316), 0x01 },
+	{ CCI_REG8(0x0a67), 0x00 },
+	{ CCI_REG8(0x0084), 0x10 },
+	{ CCI_REG8(0x0102), 0x09 },
+};
+
+static const struct cci_reg_sequence mode_1920x1080[] = {
+	/* system */
+	{ CCI_REG8(0x0336), 0x45 },
+	{ CCI_REG8(0x0383), 0x8b },
+	{ CCI_REG8(0x0344), 0x02 },
+	{ CCI_REG8(0x0345), 0xa6 },
+	{ CCI_REG8(0x0346), 0x02 },
+	{ CCI_REG8(0x0347), 0xb0 },
+	{ CCI_REG8(0x0348), 0x07 },
+	{ CCI_REG8(0x0349), 0x90 },
+	{ CCI_REG8(0x034a), 0x04 },
+	{ CCI_REG8(0x034b), 0x44 },
+	{ CCI_REG8(0x0202), 0x03 },
+	{ CCI_REG8(0x0203), 0x00 },
+	{ CCI_REG8(0x0340), 0x04 },
+	{ CCI_REG8(0x0341), 0xfc },
+	{ CCI_REG8(0x0342), 0x07 },
+	{ CCI_REG8(0x0343), 0x1c },
+	{ CCI_REG8(0x0226), 0x00 },
+	{ CCI_REG8(0x0227), 0x88 },
+	{ CCI_REG8(0x0e38), 0x49 },
+	{ CCI_REG8(0x0210), 0x13 },
+	{ CCI_REG8(0x0218), 0x00 },
+	{ CCI_REG8(0x0241), 0x88 },
+	{ CCI_REG8(0x0392), 0x60 },
+
+	/* ISP */
+	{ CCI_REG8(0x00a2), 0xac },
+	{ CCI_REG8(0x00a3), 0x02 },
+	{ CCI_REG8(0x00ab), 0xa0 },
+	{ CCI_REG8(0x00ac), 0x02 },
+
+	/* GAIN */
+	{ CCI_REG8(0x0204), 0x04 },
+	{ CCI_REG8(0x0205), 0x00 },
+	{ CCI_REG8(0x0050), 0x38 },
+	{ CCI_REG8(0x0051), 0x20 },
+
+	/* out window */
+	{ CCI_REG8(0x009a), 0x66 },
+	{ CCI_REG8(0x0351), 0x00 },
+	{ CCI_REG8(0x0352), 0x06 },
+	{ CCI_REG8(0x0353), 0x00 },
+	{ CCI_REG8(0x0354), 0x08 },
+	{ CCI_REG8(0x034c), 0x07 },
+	{ CCI_REG8(0x034d), 0x80 },
+	{ CCI_REG8(0x034e), 0x04 },
+	{ CCI_REG8(0x034f), 0x38 },
+
+	/* MIPI */
+	{ CCI_REG8(0x0114), 0x03 },
+	{ CCI_REG8(0x0180), 0x65 },
+	{ CCI_REG8(0x0181), 0xf0 },
+	{ CCI_REG8(0x0185), 0x01 },
+	{ CCI_REG8(0x0115), 0x30 },
+	{ CCI_REG8(0x011b), 0x12 },
+	{ CCI_REG8(0x011c), 0x12 },
+	{ CCI_REG8(0x0121), 0x02 },
+	{ CCI_REG8(0x0122), 0x03 },
+	{ CCI_REG8(0x0123), 0x0c },
+	{ CCI_REG8(0x0124), 0x00 },
+	{ CCI_REG8(0x0125), 0x09 },
+	{ CCI_REG8(0x0126), 0x06 },
+	{ CCI_REG8(0x0129), 0x04 },
+	{ CCI_REG8(0x012a), 0x03 },
+	{ CCI_REG8(0x012b), 0x06 },
+
+	{ CCI_REG8(0x0a73), 0x60 },
+	{ CCI_REG8(0x0a70), 0x11 },
+	{ CCI_REG8(0x0313), 0x80 },
+	{ CCI_REG8(0x0aff), 0x00 },
+	{ CCI_REG8(0x0a70), 0x00 },
+	{ CCI_REG8(0x00a4), 0x80 },
+	{ CCI_REG8(0x0316), 0x01 },
+	{ CCI_REG8(0x0a67), 0x00 },
+	{ CCI_REG8(0x0084), 0x10 },
+	{ CCI_REG8(0x0102), 0x09 },
+};
+
+static const struct cci_reg_sequence mode_table_common[] = {
+	{ GC08A3_STREAMING_REG, 0x00 },
+	/* system */
+	{ CCI_REG8(0x031c), 0x60 },
+	{ CCI_REG8(0x0337), 0x04 },
+	{ CCI_REG8(0x0335), 0x51 },
+	{ CCI_REG8(0x0336), 0x70 },
+	{ CCI_REG8(0x0383), 0xbb },
+	{ CCI_REG8(0x031a), 0x00 },
+	{ CCI_REG8(0x0321), 0x10 },
+	{ CCI_REG8(0x0327), 0x03 },
+	{ CCI_REG8(0x0325), 0x40 },
+	{ CCI_REG8(0x0326), 0x23 },
+	{ CCI_REG8(0x0314), 0x11 },
+	{ CCI_REG8(0x0315), 0xd6 },
+	{ CCI_REG8(0x0316), 0x01 },
+	{ CCI_REG8(0x0334), 0x40 },
+	{ CCI_REG8(0x0324), 0x42 },
+	{ CCI_REG8(0x031c), 0x00 },
+	{ CCI_REG8(0x031c), 0x9f },
+	{ CCI_REG8(0x039a), 0x13 },
+	{ CCI_REG8(0x0084), 0x30 },
+	{ CCI_REG8(0x02b3), 0x08 },
+	{ CCI_REG8(0x0057), 0x0c },
+	{ CCI_REG8(0x05c3), 0x50 },
+	{ CCI_REG8(0x0311), 0x90 },
+	{ CCI_REG8(0x05a0), 0x02 },
+	{ CCI_REG8(0x0074), 0x0a },
+	{ CCI_REG8(0x0059), 0x11 },
+	{ CCI_REG8(0x0070), 0x05 },
+	{ CCI_REG8(0x0101), 0x00 },
+
+	/* analog */
+	{ CCI_REG8(0x0344), 0x00 },
+	{ CCI_REG8(0x0345), 0x06 },
+	{ CCI_REG8(0x0346), 0x00 },
+	{ CCI_REG8(0x0347), 0x04 },
+	{ CCI_REG8(0x0348), 0x0c },
+	{ CCI_REG8(0x0349), 0xd0 },
+	{ CCI_REG8(0x034a), 0x09 },
+	{ CCI_REG8(0x034b), 0x9c },
+	{ CCI_REG8(0x0202), 0x09 },
+	{ CCI_REG8(0x0203), 0x04 },
+
+	{ CCI_REG8(0x0219), 0x05 },
+	{ CCI_REG8(0x0226), 0x00 },
+	{ CCI_REG8(0x0227), 0x28 },
+	{ CCI_REG8(0x0e0a), 0x00 },
+	{ CCI_REG8(0x0e0b), 0x00 },
+	{ CCI_REG8(0x0e24), 0x04 },
+	{ CCI_REG8(0x0e25), 0x04 },
+	{ CCI_REG8(0x0e26), 0x00 },
+	{ CCI_REG8(0x0e27), 0x10 },
+	{ CCI_REG8(0x0e01), 0x74 },
+	{ CCI_REG8(0x0e03), 0x47 },
+	{ CCI_REG8(0x0e04), 0x33 },
+	{ CCI_REG8(0x0e05), 0x44 },
+	{ CCI_REG8(0x0e06), 0x44 },
+	{ CCI_REG8(0x0e0c), 0x1e },
+	{ CCI_REG8(0x0e17), 0x3a },
+	{ CCI_REG8(0x0e18), 0x3c },
+	{ CCI_REG8(0x0e19), 0x40 },
+	{ CCI_REG8(0x0e1a), 0x42 },
+	{ CCI_REG8(0x0e28), 0x21 },
+	{ CCI_REG8(0x0e2b), 0x68 },
+	{ CCI_REG8(0x0e2c), 0x0d },
+	{ CCI_REG8(0x0e2d), 0x08 },
+	{ CCI_REG8(0x0e34), 0xf4 },
+	{ CCI_REG8(0x0e35), 0x44 },
+	{ CCI_REG8(0x0e36), 0x07 },
+	{ CCI_REG8(0x0e38), 0x49 },
+	{ CCI_REG8(0x0210), 0x13 },
+	{ CCI_REG8(0x0218), 0x00 },
+	{ CCI_REG8(0x0241), 0x88 },
+	{ CCI_REG8(0x0e32), 0x00 },
+	{ CCI_REG8(0x0e33), 0x18 },
+	{ CCI_REG8(0x0e42), 0x03 },
+	{ CCI_REG8(0x0e43), 0x80 },
+	{ CCI_REG8(0x0e44), 0x04 },
+	{ CCI_REG8(0x0e45), 0x00 },
+	{ CCI_REG8(0x0e4f), 0x04 },
+	{ CCI_REG8(0x057a), 0x20 },
+	{ CCI_REG8(0x0381), 0x7c },
+	{ CCI_REG8(0x0382), 0x9b },
+	{ CCI_REG8(0x0384), 0xfb },
+	{ CCI_REG8(0x0389), 0x38 },
+	{ CCI_REG8(0x038a), 0x03 },
+	{ CCI_REG8(0x0390), 0x6a },
+	{ CCI_REG8(0x0391), 0x0b },
+	{ CCI_REG8(0x0392), 0x60 },
+	{ CCI_REG8(0x0393), 0xc1 },
+	{ CCI_REG8(0x0396), 0xff },
+	{ CCI_REG8(0x0398), 0x62 },
+
+	/* cisctl reset */
+	{ CCI_REG8(0x031c), 0x80 },
+	{ CCI_REG8(0x03fe), 0x10 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x031c), 0x9f },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x031c), 0x80 },
+	{ CCI_REG8(0x03fe), 0x10 },
+	{ CCI_REG8(0x03fe), 0x00 },
+	{ CCI_REG8(0x031c), 0x9f },
+	{ CCI_REG8(0x0360), 0x01 },
+	{ CCI_REG8(0x0360), 0x00 },
+	{ CCI_REG8(0x0316), 0x09 },
+	{ CCI_REG8(0x0a67), 0x80 },
+	{ CCI_REG8(0x0313), 0x00 },
+	{ CCI_REG8(0x0a53), 0x0e },
+	{ CCI_REG8(0x0a65), 0x17 },
+	{ CCI_REG8(0x0a68), 0xa1 },
+	{ CCI_REG8(0x0a58), 0x00 },
+	{ CCI_REG8(0x0ace), 0x0c },
+	{ CCI_REG8(0x00a4), 0x00 },
+	{ CCI_REG8(0x00a5), 0x01 },
+	{ CCI_REG8(0x00a7), 0x09 },
+	{ CCI_REG8(0x00a8), 0x9c },
+	{ CCI_REG8(0x00a9), 0x0c },
+	{ CCI_REG8(0x00aa), 0xd0 },
+	{ CCI_REG8(0x0a8a), 0x00 },
+	{ CCI_REG8(0x0a8b), 0xe0 },
+	{ CCI_REG8(0x0a8c), 0x13 },
+	{ CCI_REG8(0x0a8d), 0xe8 },
+	{ CCI_REG8(0x0a90), 0x0a },
+	{ CCI_REG8(0x0a91), 0x10 },
+	{ CCI_REG8(0x0a92), 0xf8 },
+	{ CCI_REG8(0x0a71), 0xf2 },
+	{ CCI_REG8(0x0a72), 0x12 },
+	{ CCI_REG8(0x0a73), 0x64 },
+	{ CCI_REG8(0x0a75), 0x41 },
+	{ CCI_REG8(0x0a70), 0x07 },
+	{ CCI_REG8(0x0313), 0x80 },
+
+	/* ISP */
+	{ CCI_REG8(0x00a0), 0x01 },
+	{ CCI_REG8(0x0080), 0xd2 },
+	{ CCI_REG8(0x0081), 0x3f },
+	{ CCI_REG8(0x0087), 0x51 },
+	{ CCI_REG8(0x0089), 0x03 },
+	{ CCI_REG8(0x009b), 0x40 },
+	{ CCI_REG8(0x05a0), 0x82 },
+	{ CCI_REG8(0x05ac), 0x00 },
+	{ CCI_REG8(0x05ad), 0x01 },
+	{ CCI_REG8(0x05ae), 0x00 },
+	{ CCI_REG8(0x0800), 0x0a },
+	{ CCI_REG8(0x0801), 0x14 },
+	{ CCI_REG8(0x0802), 0x28 },
+	{ CCI_REG8(0x0803), 0x34 },
+	{ CCI_REG8(0x0804), 0x0e },
+	{ CCI_REG8(0x0805), 0x33 },
+	{ CCI_REG8(0x0806), 0x03 },
+	{ CCI_REG8(0x0807), 0x8a },
+	{ CCI_REG8(0x0808), 0x50 },
+	{ CCI_REG8(0x0809), 0x00 },
+	{ CCI_REG8(0x080a), 0x34 },
+	{ CCI_REG8(0x080b), 0x03 },
+	{ CCI_REG8(0x080c), 0x26 },
+	{ CCI_REG8(0x080d), 0x03 },
+	{ CCI_REG8(0x080e), 0x18 },
+	{ CCI_REG8(0x080f), 0x03 },
+	{ CCI_REG8(0x0810), 0x10 },
+	{ CCI_REG8(0x0811), 0x03 },
+	{ CCI_REG8(0x0812), 0x00 },
+	{ CCI_REG8(0x0813), 0x00 },
+	{ CCI_REG8(0x0814), 0x01 },
+	{ CCI_REG8(0x0815), 0x00 },
+	{ CCI_REG8(0x0816), 0x01 },
+	{ CCI_REG8(0x0817), 0x00 },
+	{ CCI_REG8(0x0818), 0x00 },
+	{ CCI_REG8(0x0819), 0x0a },
+	{ CCI_REG8(0x081a), 0x01 },
+	{ CCI_REG8(0x081b), 0x6c },
+	{ CCI_REG8(0x081c), 0x00 },
+	{ CCI_REG8(0x081d), 0x0b },
+	{ CCI_REG8(0x081e), 0x02 },
+	{ CCI_REG8(0x081f), 0x00 },
+	{ CCI_REG8(0x0820), 0x00 },
+	{ CCI_REG8(0x0821), 0x0c },
+	{ CCI_REG8(0x0822), 0x02 },
+	{ CCI_REG8(0x0823), 0xd9 },
+	{ CCI_REG8(0x0824), 0x00 },
+	{ CCI_REG8(0x0825), 0x0d },
+	{ CCI_REG8(0x0826), 0x03 },
+	{ CCI_REG8(0x0827), 0xf0 },
+	{ CCI_REG8(0x0828), 0x00 },
+	{ CCI_REG8(0x0829), 0x0e },
+	{ CCI_REG8(0x082a), 0x05 },
+	{ CCI_REG8(0x082b), 0x94 },
+	{ CCI_REG8(0x082c), 0x09 },
+	{ CCI_REG8(0x082d), 0x6e },
+	{ CCI_REG8(0x082e), 0x07 },
+	{ CCI_REG8(0x082f), 0xe6 },
+	{ CCI_REG8(0x0830), 0x10 },
+	{ CCI_REG8(0x0831), 0x0e },
+	{ CCI_REG8(0x0832), 0x0b },
+	{ CCI_REG8(0x0833), 0x2c },
+	{ CCI_REG8(0x0834), 0x14 },
+	{ CCI_REG8(0x0835), 0xae },
+	{ CCI_REG8(0x0836), 0x0f },
+	{ CCI_REG8(0x0837), 0xc4 },
+	{ CCI_REG8(0x0838), 0x18 },
+	{ CCI_REG8(0x0839), 0x0e },
+	{ CCI_REG8(0x05ac), 0x01 },
+	{ CCI_REG8(0x059a), 0x00 },
+	{ CCI_REG8(0x059b), 0x00 },
+	{ CCI_REG8(0x059c), 0x01 },
+	{ CCI_REG8(0x0598), 0x00 },
+	{ CCI_REG8(0x0597), 0x14 },
+	{ CCI_REG8(0x05ab), 0x09 },
+	{ CCI_REG8(0x05a4), 0x02 },
+	{ CCI_REG8(0x05a3), 0x05 },
+	{ CCI_REG8(0x05a0), 0xc2 },
+	{ CCI_REG8(0x0207), 0xc4 },
+
+	/* GAIN */
+	{ CCI_REG8(0x0208), 0x01 },
+	{ CCI_REG8(0x0209), 0x72 },
+	{ CCI_REG8(0x0204), 0x04 },
+	{ CCI_REG8(0x0205), 0x00 },
+
+	{ CCI_REG8(0x0040), 0x22 },
+	{ CCI_REG8(0x0041), 0x20 },
+	{ CCI_REG8(0x0043), 0x10 },
+	{ CCI_REG8(0x0044), 0x00 },
+	{ CCI_REG8(0x0046), 0x08 },
+	{ CCI_REG8(0x0047), 0xf0 },
+	{ CCI_REG8(0x0048), 0x0f },
+	{ CCI_REG8(0x004b), 0x0f },
+	{ CCI_REG8(0x004c), 0x00 },
+	{ CCI_REG8(0x0050), 0x5c },
+	{ CCI_REG8(0x0051), 0x44 },
+	{ CCI_REG8(0x005b), 0x03 },
+	{ CCI_REG8(0x00c0), 0x00 },
+	{ CCI_REG8(0x00c1), 0x80 },
+	{ CCI_REG8(0x00c2), 0x31 },
+	{ CCI_REG8(0x00c3), 0x00 },
+	{ CCI_REG8(0x0460), 0x04 },
+	{ CCI_REG8(0x0462), 0x08 },
+	{ CCI_REG8(0x0464), 0x0e },
+	{ CCI_REG8(0x0466), 0x0a },
+	{ CCI_REG8(0x0468), 0x12 },
+	{ CCI_REG8(0x046a), 0x12 },
+	{ CCI_REG8(0x046c), 0x10 },
+	{ CCI_REG8(0x046e), 0x0c },
+	{ CCI_REG8(0x0461), 0x03 },
+	{ CCI_REG8(0x0463), 0x03 },
+	{ CCI_REG8(0x0465), 0x03 },
+	{ CCI_REG8(0x0467), 0x03 },
+	{ CCI_REG8(0x0469), 0x04 },
+	{ CCI_REG8(0x046b), 0x04 },
+	{ CCI_REG8(0x046d), 0x04 },
+	{ CCI_REG8(0x046f), 0x04 },
+	{ CCI_REG8(0x0470), 0x04 },
+	{ CCI_REG8(0x0472), 0x10 },
+	{ CCI_REG8(0x0474), 0x26 },
+	{ CCI_REG8(0x0476), 0x38 },
+	{ CCI_REG8(0x0478), 0x20 },
+	{ CCI_REG8(0x047a), 0x30 },
+	{ CCI_REG8(0x047c), 0x38 },
+	{ CCI_REG8(0x047e), 0x60 },
+	{ CCI_REG8(0x0471), 0x05 },
+	{ CCI_REG8(0x0473), 0x05 },
+	{ CCI_REG8(0x0475), 0x05 },
+	{ CCI_REG8(0x0477), 0x05 },
+	{ CCI_REG8(0x0479), 0x04 },
+	{ CCI_REG8(0x047b), 0x04 },
+	{ CCI_REG8(0x047d), 0x04 },
+	{ CCI_REG8(0x047f), 0x04 },
+};
+
+struct gc08a3_mode {
+	u32 width;
+	u32 height;
+	const struct gc08a3_reg_list reg_list;
+
+	u32 hts; /* Horizontal timining size */
+	u32 vts_def; /* Default vertical timining size */
+	u32 vts_min; /* Min vertical timining size */
+};
+
+/* Declare modes in order, from biggest to smallest height. */
+static const struct gc08a3_mode gc08a3_modes[] = {
+	{
+		/* 3264*2448@30fps */
+		.width = GC08A3_NATIVE_WIDTH,
+		.height = GC08A3_NATIVE_HEIGHT,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3264x2448),
+			.regs = mode_3264x2448,
+		},
+		.hts = 3640,
+		.vts_def = 2548,
+		.vts_min = 2548,
+	},
+	{
+		/* 1920*1080@60fps */
+		.width = 1920,
+		.height = 1080,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1920x1080),
+			.regs = mode_1920x1080,
+		},
+		.hts = 3640,
+		.vts_def = 1276,
+		.vts_min = 1276,
+	},
+};
+
+static inline struct gc08a3 *to_gc08a3(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct gc08a3, sd);
+}
+
+static int gc08a3_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct gc08a3 *gc08a3 = to_gc08a3(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(gc08a3_supply_name),
+				    gc08a3->supplies);
+	if (ret < 0) {
+		dev_err(gc08a3->dev, "failed to enable regulators: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(gc08a3->xclk);
+	if (ret < 0) {
+		regulator_bulk_disable(ARRAY_SIZE(gc08a3_supply_name),
+				       gc08a3->supplies);
+		dev_err(gc08a3->dev, "clk prepare enable failed\n");
+		return ret;
+	}
+
+	fsleep(GC08A3_SLEEP_US);
+
+	gpiod_set_value_cansleep(gc08a3->reset_gpio, 0);
+	fsleep(GC08A3_SLEEP_US);
+
+	return 0;
+}
+
+static int gc08a3_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct gc08a3 *gc08a3 = to_gc08a3(sd);
+
+	clk_disable_unprepare(gc08a3->xclk);
+	gpiod_set_value_cansleep(gc08a3->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(gc08a3_supply_name),
+			       gc08a3->supplies);
+
+	return 0;
+}
+
+static int gc08a3_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = GC08A3_MBUS_CODE;
+
+	return 0;
+}
+
+static int gc08a3_enum_frame_size(struct v4l2_subdev *subdev,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->code != GC08A3_MBUS_CODE)
+		return -EINVAL;
+
+	if (fse->index >= ARRAY_SIZE(gc08a3_modes))
+		return -EINVAL;
+
+	fse->min_width = gc08a3_modes[fse->index].width;
+	fse->max_width = gc08a3_modes[fse->index].width;
+	fse->min_height = gc08a3_modes[fse->index].height;
+	fse->max_height = gc08a3_modes[fse->index].height;
+
+	return 0;
+}
+
+static int gc08a3_update_cur_mode_controls(struct gc08a3 *gc08a3,
+					   const struct gc08a3_mode *mode)
+{
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ret = __v4l2_ctrl_modify_range(gc08a3->vblank,
+				       mode->vts_min - mode->height,
+				       GC08A3_VTS_MAX - mode->height, 1,
+				       mode->vts_def - mode->height);
+	if (ret) {
+		dev_err(gc08a3->dev, "VB ctrl range update failed\n");
+		return ret;
+	}
+
+	h_blank = mode->hts - mode->width;
+	ret = __v4l2_ctrl_modify_range(gc08a3->hblank, h_blank, h_blank, 1,
+				       h_blank);
+	if (ret) {
+		dev_err(gc08a3->dev, "HB ctrl range update failed\n");
+		return ret;
+	}
+
+	exposure_max = mode->vts_def - GC08A3_EXP_MARGIN;
+	ret = __v4l2_ctrl_modify_range(gc08a3->exposure, GC08A3_EXP_MIN,
+				       exposure_max, GC08A3_EXP_STEP,
+				       exposure_max);
+	if (ret) {
+		dev_err(gc08a3->dev, "exposure ctrl range update failed\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static void gc08a3_update_pad_format(struct gc08a3 *gc08a3,
+				     const struct gc08a3_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = GC08A3_MBUS_CODE;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int gc08a3_set_format(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt)
+{
+	struct gc08a3 *gc08a3 = to_gc08a3(sd);
+	struct v4l2_mbus_framefmt *mbus_fmt;
+	struct v4l2_rect *crop;
+	const struct gc08a3_mode *mode;
+
+	mode = v4l2_find_nearest_size(gc08a3_modes, ARRAY_SIZE(gc08a3_modes),
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	/* update crop info to subdev state */
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	crop->width = mode->width;
+	crop->height = mode->height;
+
+	/* update fmt info to subdev state */
+	gc08a3_update_pad_format(gc08a3, mode, &fmt->format);
+	mbus_fmt = v4l2_subdev_state_get_format(state, 0);
+	*mbus_fmt = fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	gc08a3->cur_mode = mode;
+	gc08a3_update_cur_mode_controls(gc08a3, mode);
+
+	return 0;
+}
+
+static int gc08a3_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_state_get_crop(state, 0);
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = GC08A3_NATIVE_WIDTH;
+		sel->r.height = GC08A3_NATIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int gc08a3_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.pad = 0,
+		.format = {
+			.code = GC08A3_MBUS_CODE,
+			.width = gc08a3_modes[0].width,
+			.height = gc08a3_modes[0].height,
+		},
+	};
+
+	gc08a3_set_format(sd, state, &fmt);
+
+	return 0;
+}
+
+static int gc08a3_set_ctrl_hflip(struct gc08a3 *gc08a3, u32 ctrl_val)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(gc08a3->regmap, GC08A3_FLIP_REG, &val, NULL);
+	if (ret) {
+		dev_err(gc08a3->dev, "read hflip register failed: %d\n", ret);
+		return ret;
+	}
+
+	return cci_update_bits(gc08a3->regmap, GC08A3_FLIP_REG,
+			       GC08A3_FLIP_H_MASK,
+			       ctrl_val ? GC08A3_FLIP_H_MASK : 0, NULL);
+}
+
+static int gc08a3_set_ctrl_vflip(struct gc08a3 *gc08a3, u32 ctrl_val)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(gc08a3->regmap, GC08A3_FLIP_REG, &val, NULL);
+	if (ret) {
+		dev_err(gc08a3->dev, "read vflip register failed: %d\n", ret);
+		return ret;
+	}
+
+	return cci_update_bits(gc08a3->regmap, GC08A3_FLIP_REG,
+			       GC08A3_FLIP_V_MASK,
+			       ctrl_val ? GC08A3_FLIP_V_MASK : 0, NULL);
+}
+
+static int gc08a3_test_pattern(struct gc08a3 *gc08a3, u32 pattern_menu)
+{
+	u32 pattern;
+	int ret;
+
+	if (pattern_menu) {
+		switch (pattern_menu) {
+		case 1:
+			pattern = 0x00;
+			break;
+		case 2:
+			pattern = 0x10;
+			break;
+		case 3:
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			pattern = pattern_menu + 1;
+			break;
+		default:
+			pattern = 0x00;
+			break;
+		}
+
+		ret = cci_write(gc08a3->regmap, GC08A3_REG_TEST_PATTERN_IDX,
+				pattern, NULL);
+		if (ret)
+			return ret;
+
+		return cci_write(gc08a3->regmap, GC08A3_REG_TEST_PATTERN_EN,
+				 GC08A3_TEST_PATTERN_EN, NULL);
+	} else {
+		return cci_write(gc08a3->regmap, GC08A3_REG_TEST_PATTERN_EN,
+				 0x00, NULL);
+	}
+}
+
+static int gc08a3_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct gc08a3 *gc08a3 =
+		container_of(ctrl->handler, struct gc08a3, ctrls);
+	int ret = 0;
+	s64 exposure_max;
+	struct v4l2_subdev_state *state;
+	const struct v4l2_mbus_framefmt *format;
+
+	state = v4l2_subdev_get_locked_active_state(&gc08a3->sd);
+	format = v4l2_subdev_state_get_format(state, 0);
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = format->height + ctrl->val - GC08A3_EXP_MARGIN;
+		__v4l2_ctrl_modify_range(gc08a3->exposure,
+					 gc08a3->exposure->minimum,
+					 exposure_max, gc08a3->exposure->step,
+					 exposure_max);
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is on for streaming.
+	 */
+	if (!pm_runtime_get_if_active(gc08a3->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		ret = cci_write(gc08a3->regmap, GC08A3_EXP_REG,
+				ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = cci_write(gc08a3->regmap, GC08A3_AGAIN_REG,
+				ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_VBLANK:
+		ret = cci_write(gc08a3->regmap, GC08A3_FRAME_LENGTH_REG,
+				gc08a3->cur_mode->height + ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_HFLIP:
+		ret = gc08a3_set_ctrl_hflip(gc08a3, ctrl->val);
+		break;
+
+	case V4L2_CID_VFLIP:
+		ret = gc08a3_set_ctrl_vflip(gc08a3, ctrl->val);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = gc08a3_test_pattern(gc08a3, ctrl->val);
+		break;
+
+	default:
+		break;
+	}
+
+	pm_runtime_put(gc08a3->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops gc08a3_ctrl_ops = {
+	.s_ctrl = gc08a3_set_ctrl,
+};
+
+static int gc08a3_start_streaming(struct gc08a3 *gc08a3)
+{
+	const struct gc08a3_mode *mode;
+	const struct gc08a3_reg_list *reg_list;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(gc08a3->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = cci_multi_reg_write(gc08a3->regmap,
+				  mode_table_common,
+				  ARRAY_SIZE(mode_table_common), NULL);
+	if (ret)
+		goto err_rpm_put;
+
+	mode = gc08a3->cur_mode;
+	reg_list = &mode->reg_list;
+	ret = cci_multi_reg_write(gc08a3->regmap,
+				  reg_list->regs, reg_list->num_of_regs, NULL);
+	if (ret < 0)
+		goto err_rpm_put;
+
+	ret = __v4l2_ctrl_handler_setup(&gc08a3->ctrls);
+	if (ret < 0) {
+		dev_err(gc08a3->dev, "could not sync v4l2 controls\n");
+		goto err_rpm_put;
+	}
+
+	ret = cci_write(gc08a3->regmap, GC08A3_STREAMING_REG, 1, NULL);
+	if (ret < 0) {
+		dev_err(gc08a3->dev, "write STREAMING_REG failed: %d\n", ret);
+		goto err_rpm_put;
+	}
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(gc08a3->dev);
+	return ret;
+}
+
+static int gc08a3_stop_streaming(struct gc08a3 *gc08a3)
+{
+	int ret;
+
+	ret = cci_write(gc08a3->regmap, GC08A3_STREAMING_REG, 0, NULL);
+	if (ret < 0)
+		dev_err(gc08a3->dev, "could not sent stop streaming %d\n", ret);
+
+	pm_runtime_put(gc08a3->dev);
+	return ret;
+}
+
+static int gc08a3_s_stream(struct v4l2_subdev *subdev, int enable)
+{
+	struct gc08a3 *gc08a3 = to_gc08a3(subdev);
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	state = v4l2_subdev_lock_and_get_active_state(subdev);
+
+	if (enable)
+		ret = gc08a3_start_streaming(gc08a3);
+	else
+		ret = gc08a3_stop_streaming(gc08a3);
+
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct v4l2_subdev_video_ops gc08a3_video_ops = {
+	.s_stream = gc08a3_s_stream,
+};
+
+static const struct v4l2_subdev_pad_ops gc08a3_subdev_pad_ops = {
+	.enum_mbus_code = gc08a3_enum_mbus_code,
+	.enum_frame_size = gc08a3_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = gc08a3_set_format,
+	.get_selection = gc08a3_get_selection,
+};
+
+static const struct v4l2_subdev_ops gc08a3_subdev_ops = {
+	.video = &gc08a3_video_ops,
+	.pad = &gc08a3_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops gc08a3_internal_ops = {
+	.init_state = gc08a3_init_state,
+};
+
+static int gc08a3_get_regulators(struct device *dev, struct gc08a3 *gc08a3)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(gc08a3_supply_name); i++)
+		gc08a3->supplies[i].supply = gc08a3_supply_name[i];
+
+	return devm_regulator_bulk_get(dev, ARRAY_SIZE(gc08a3_supply_name),
+				       gc08a3->supplies);
+}
+
+static int gc08a3_parse_fwnode(struct gc08a3 *gc08a3)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	int ret;
+	struct device *dev = gc08a3->dev;
+
+	endpoint =
+		fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
+						FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!endpoint) {
+		dev_err(dev, "endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
+	if (ret) {
+		dev_err(dev, "parsing endpoint node failed\n");
+		goto done;
+	}
+
+	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       gc08a3_link_freq_menu_items,
+				       ARRAY_SIZE(gc08a3_link_freq_menu_items),
+				       &gc08a3->link_freq_bitmap);
+	if (ret)
+		goto done;
+
+done:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	fwnode_handle_put(endpoint);
+	return ret;
+}
+
+static u64 gc08a3_to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate =
+		gc08a3_link_freq_menu_items[f_index] * 2 * GC08A3_DATA_LANES;
+
+	return div_u64(pixel_rate, GC08A3_RGB_DEPTH);
+}
+
+static int gc08a3_init_controls(struct gc08a3 *gc08a3)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&gc08a3->sd);
+	const struct gc08a3_mode *mode = &gc08a3_modes[0];
+	const struct v4l2_ctrl_ops *ops = &gc08a3_ctrl_ops;
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 exposure_max, h_blank;
+	int ret;
+
+	ctrl_hdlr = &gc08a3->ctrls;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 9);
+	if (ret)
+		return ret;
+
+	gc08a3->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &gc08a3_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	gc08a3->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &gc08a3_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &gc08a3->hflip);
+
+	gc08a3->link_freq =
+	v4l2_ctrl_new_int_menu(ctrl_hdlr,
+			       &gc08a3_ctrl_ops,
+			       V4L2_CID_LINK_FREQ,
+			       ARRAY_SIZE(gc08a3_link_freq_menu_items) - 1,
+			       0,
+			       gc08a3_link_freq_menu_items);
+	if (gc08a3->link_freq)
+		gc08a3->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	gc08a3->pixel_rate =
+		v4l2_ctrl_new_std(ctrl_hdlr,
+				  &gc08a3_ctrl_ops,
+				  V4L2_CID_PIXEL_RATE, 0,
+				  gc08a3_to_pixel_rate(0),
+				  1,
+				  gc08a3_to_pixel_rate(0));
+
+	gc08a3->vblank =
+		v4l2_ctrl_new_std(ctrl_hdlr,
+				  &gc08a3_ctrl_ops, V4L2_CID_VBLANK,
+				  mode->vts_min - mode->height,
+				  GC08A3_VTS_MAX - mode->height, 1,
+				  mode->vts_def - mode->height);
+
+	h_blank = mode->hts - mode->width;
+	gc08a3->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &gc08a3_ctrl_ops,
+					   V4L2_CID_HBLANK, h_blank, h_blank, 1,
+					   h_blank);
+	if (gc08a3->hblank)
+		gc08a3->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &gc08a3_ctrl_ops,
+			  V4L2_CID_ANALOGUE_GAIN, GC08A3_AGAIN_MIN,
+			  GC08A3_AGAIN_MAX, GC08A3_AGAIN_STEP,
+			  GC08A3_AGAIN_MIN);
+
+	exposure_max = mode->vts_def - GC08A3_EXP_MARGIN;
+	gc08a3->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &gc08a3_ctrl_ops,
+					     V4L2_CID_EXPOSURE, GC08A3_EXP_MIN,
+					     exposure_max, GC08A3_EXP_STEP,
+					     exposure_max);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &gc08a3_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(gc08a3_test_pattern_menu) - 1,
+				     0, 0, gc08a3_test_pattern_menu);
+
+	/* register properties to fwnode (e.g. rotation, orientation) */
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error_ctrls;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, ops, &props);
+	if (ret)
+		goto error_ctrls;
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		goto error_ctrls;
+	}
+
+	gc08a3->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+error_ctrls:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static int gc08a3_identify_module(struct gc08a3 *gc08a3)
+{
+	u64 val;
+	int ret;
+
+	ret = cci_read(gc08a3->regmap, GC08A3_REG_CHIP_ID, &val, NULL);
+	if (ret) {
+		dev_err(gc08a3->dev, "failed to read chip id");
+		return ret;
+	}
+
+	if (val != GC08A3_CHIP_ID) {
+		dev_err(gc08a3->dev, "chip id mismatch: 0x%x!=0x%llx",
+			GC08A3_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int gc08a3_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct gc08a3 *gc08a3;
+	int ret;
+
+	gc08a3 = devm_kzalloc(dev, sizeof(*gc08a3), GFP_KERNEL);
+	if (!gc08a3)
+		return -ENOMEM;
+
+	gc08a3->dev = dev;
+
+	ret = gc08a3_parse_fwnode(gc08a3);
+	if (ret)
+		return ret;
+
+	gc08a3->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(gc08a3->regmap))
+		return dev_err_probe(dev, PTR_ERR(gc08a3->regmap),
+				     "failed to init CCI\n");
+
+	gc08a3->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(gc08a3->xclk))
+		return dev_err_probe(dev, PTR_ERR(gc08a3->xclk),
+				     "failed to get xclk\n");
+
+	ret = clk_set_rate(gc08a3->xclk, GC08A3_DEFAULT_CLK_FREQ);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to set xclk frequency\n");
+
+	ret = gc08a3_get_regulators(dev, gc08a3);
+	if (ret < 0)
+		return dev_err_probe(dev, ret,
+				     "failed to get regulators\n");
+
+	gc08a3->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(gc08a3->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(gc08a3->reset_gpio),
+				     "failed to get gpio\n");
+
+	v4l2_i2c_subdev_init(&gc08a3->sd, client, &gc08a3_subdev_ops);
+	gc08a3->sd.internal_ops = &gc08a3_internal_ops;
+	gc08a3->cur_mode = &gc08a3_modes[0];
+
+	ret = gc08a3_power_on(gc08a3->dev);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to sensor power on\n");
+
+	ret = gc08a3_identify_module(gc08a3);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+		goto err_power_off;
+	}
+
+	ret = gc08a3_init_controls(gc08a3);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto err_power_off;
+	}
+
+	gc08a3->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	gc08a3->pad.flags = MEDIA_PAD_FL_SOURCE;
+	gc08a3->sd.dev = &client->dev;
+	gc08a3->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	ret = media_entity_pads_init(&gc08a3->sd.entity, 1, &gc08a3->pad);
+	if (ret < 0) {
+		dev_err(dev, "could not register media entity\n");
+		goto err_v4l2_ctrl_handler_free;
+	}
+
+	gc08a3->sd.state_lock = gc08a3->ctrls.lock;
+	ret = v4l2_subdev_init_finalize(&gc08a3->sd);
+	if (ret < 0) {
+		dev_err(dev, "v4l2 subdev init error: %d\n", ret);
+		goto err_media_entity_cleanup;
+	}
+
+	pm_runtime_set_active(gc08a3->dev);
+	pm_runtime_enable(gc08a3->dev);
+	pm_runtime_set_autosuspend_delay(gc08a3->dev, 1000);
+	pm_runtime_use_autosuspend(gc08a3->dev);
+	pm_runtime_idle(gc08a3->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&gc08a3->sd);
+	if (ret < 0) {
+		dev_err(dev, "could not register v4l2 device\n");
+		goto err_rpm;
+	}
+
+	return 0;
+
+err_rpm:
+	pm_runtime_disable(gc08a3->dev);
+	v4l2_subdev_cleanup(&gc08a3->sd);
+
+err_media_entity_cleanup:
+	media_entity_cleanup(&gc08a3->sd.entity);
+
+err_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(gc08a3->sd.ctrl_handler);
+
+err_power_off:
+	gc08a3_power_off(gc08a3->dev);
+
+	return ret;
+}
+
+static void gc08a3_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct gc08a3 *gc08a3 = to_gc08a3(sd);
+
+	v4l2_async_unregister_subdev(&gc08a3->sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&gc08a3->sd.entity);
+	v4l2_ctrl_handler_free(&gc08a3->ctrls);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		gc08a3_power_off(gc08a3->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static const struct of_device_id gc08a3_of_match[] = {
+	{ .compatible = "galaxycore,gc08a3" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, gc08a3_of_match);
+
+static DEFINE_RUNTIME_DEV_PM_OPS(gc08a3_pm_ops,
+				 gc08a3_power_off,
+				 gc08a3_power_on,
+				 NULL);
+
+static struct i2c_driver gc08a3_i2c_driver = {
+	.driver = {
+		.of_match_table = gc08a3_of_match,
+		.pm = pm_ptr(&gc08a3_pm_ops),
+		.name  = "gc08a3",
+	},
+	.probe  = gc08a3_probe,
+	.remove = gc08a3_remove,
+};
+module_i2c_driver(gc08a3_i2c_driver);
+
+MODULE_DESCRIPTION("GalaxyCore gc08a3 Camera driver");
+MODULE_AUTHOR("Zhi Mao <zhi.mao@mediatek.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/gc2145.c b/drivers/media/i2c/gc2145.c
index bef7b0e056a8..ba02161d46e7 100644
--- a/drivers/media/i2c/gc2145.c
+++ b/drivers/media/i2c/gc2145.c
@@ -21,7 +21,6 @@
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mediabus.h>
 
@@ -68,8 +67,7 @@
 #define GC2145_DPHY_CLK_DELAY		BIT(4)
 #define GC2145_DPHY_LANE0_DELAY		BIT(5)
 #define GC2145_DPHY_LANE1_DELAY		BIT(6)
-#define GC2145_REG_FIFO_FULL_LVL_LOW	CCI_REG8(0x04)
-#define GC2145_REG_FIFO_FULL_LVL_HIGH	CCI_REG8(0x05)
+#define GC2145_REG_FIFO_FULL_LVL	CCI_REG16_LE(0x04)
 #define GC2145_REG_FIFO_MODE		CCI_REG8(0x06)
 #define GC2145_FIFO_MODE_READ_GATE	BIT(3)
 #define GC2145_FIFO_MODE_MIPI_CLK_MODULE	BIT(7)
@@ -79,8 +77,7 @@
 #define GC2145_CSI2_MODE_MIPI_EN	BIT(4)
 #define GC2145_CSI2_MODE_EN		BIT(7)
 #define GC2145_REG_MIPI_DT	CCI_REG8(0x11)
-#define GC2145_REG_LWC_LOW	CCI_REG8(0x12)
-#define GC2145_REG_LWC_HIGH	CCI_REG8(0x13)
+#define GC2145_REG_LWC		CCI_REG16_LE(0x12)
 #define GC2145_REG_DPHY_MODE	CCI_REG8(0x15)
 #define GC2145_DPHY_MODE_TRIGGER_PROG	BIT(4)
 #define GC2145_REG_FIFO_GATE_MODE	CCI_REG8(0x17)
@@ -542,45 +539,82 @@ static const struct gc2145_mode supported_modes[] = {
 /**
  * struct gc2145_format - GC2145 pixel format description
  * @code: media bus (MBUS) associated code
+ * @colorspace: V4L2 colorspace
  * @datatype: MIPI CSI2 data type
  * @output_fmt: GC2145 output format
  * @switch_bit: GC2145 first/second switch
+ * @row_col_switch: GC2145 switch row and/or column
  */
 struct gc2145_format {
 	unsigned int code;
+	unsigned int colorspace;
 	unsigned char datatype;
 	unsigned char output_fmt;
 	bool switch_bit;
+	unsigned char row_col_switch;
 };
 
 /* All supported formats */
 static const struct gc2145_format supported_formats[] = {
 	{
 		.code		= MEDIA_BUS_FMT_UYVY8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.datatype	= MIPI_CSI2_DT_YUV422_8B,
 		.output_fmt	= 0x00,
 	},
 	{
 		.code		= MEDIA_BUS_FMT_VYUY8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.datatype	= MIPI_CSI2_DT_YUV422_8B,
 		.output_fmt	= 0x01,
 	},
 	{
 		.code		= MEDIA_BUS_FMT_YUYV8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.datatype	= MIPI_CSI2_DT_YUV422_8B,
 		.output_fmt	= 0x02,
 	},
 	{
 		.code		= MEDIA_BUS_FMT_YVYU8_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.datatype	= MIPI_CSI2_DT_YUV422_8B,
 		.output_fmt	= 0x03,
 	},
 	{
 		.code		= MEDIA_BUS_FMT_RGB565_1X16,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
 		.datatype	= MIPI_CSI2_DT_RGB565,
 		.output_fmt	= 0x06,
 		.switch_bit	= true,
 	},
+	{
+		.code           = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.colorspace     = V4L2_COLORSPACE_RAW,
+		.datatype       = MIPI_CSI2_DT_RAW8,
+		.output_fmt     = 0x17,
+		.row_col_switch = GC2145_SYNC_MODE_COL_SWITCH,
+	},
+	{
+		.code           = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.colorspace     = V4L2_COLORSPACE_RAW,
+		.datatype       = MIPI_CSI2_DT_RAW8,
+		.output_fmt     = 0x17,
+		.row_col_switch = GC2145_SYNC_MODE_COL_SWITCH | GC2145_SYNC_MODE_ROW_SWITCH,
+	},
+	{
+		.code           = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.colorspace     = V4L2_COLORSPACE_RAW,
+		.datatype       = MIPI_CSI2_DT_RAW8,
+		.output_fmt     = 0x17,
+		.row_col_switch = 0,
+	},
+	{
+		.code           = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.colorspace     = V4L2_COLORSPACE_RAW,
+		.datatype       = MIPI_CSI2_DT_RAW8,
+		.output_fmt     = 0x17,
+		.row_col_switch = GC2145_SYNC_MODE_ROW_SWITCH,
+	},
 };
 
 struct gc2145_ctrls {
@@ -641,7 +675,8 @@ gc2145_get_format_code(struct gc2145 *gc2145, u32 code)
 
 static void gc2145_update_pad_format(struct gc2145 *gc2145,
 				     const struct gc2145_mode *mode,
-				     struct v4l2_mbus_framefmt *fmt, u32 code)
+				     struct v4l2_mbus_framefmt *fmt, u32 code,
+				     u32 colorspace)
 {
 	fmt->code = code;
 	fmt->width = mode->width;
@@ -663,7 +698,8 @@ static int gc2145_init_state(struct v4l2_subdev *sd,
 	/* Initialize pad format */
 	format = v4l2_subdev_state_get_format(state, 0);
 	gc2145_update_pad_format(gc2145, &supported_modes[0], format,
-				 MEDIA_BUS_FMT_RGB565_1X16);
+				 MEDIA_BUS_FMT_RGB565_1X16,
+				 V4L2_COLORSPACE_SRGB);
 
 	/* Initialize crop rectangle. */
 	crop = v4l2_subdev_state_get_crop(state, 0);
@@ -754,7 +790,13 @@ static int gc2145_set_pad_format(struct v4l2_subdev *sd,
 				      width, height,
 				      fmt->format.width, fmt->format.height);
 
-	gc2145_update_pad_format(gc2145, mode, &fmt->format, gc2145_fmt->code);
+	/* In RAW mode, VGA is not possible so use 720p instead */
+	if (gc2145_fmt->colorspace == V4L2_COLORSPACE_RAW &&
+	    mode == &supported_modes[GC2145_MODE_640X480])
+		mode = &supported_modes[GC2145_MODE_1280X720];
+
+	gc2145_update_pad_format(gc2145, mode, &fmt->format, gc2145_fmt->code,
+				 gc2145_fmt->colorspace);
 	framefmt = v4l2_subdev_state_get_format(sd_state, fmt->pad);
 	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 		gc2145->mode = mode;
@@ -811,9 +853,12 @@ static int gc2145_config_mipi_mode(struct gc2145 *gc2145,
 	 * For RAW8, LWC = image width
 	 * For RAW10, LWC = image width * 1.25
 	 */
-	lwc = gc2145->mode->width * 2;
-	cci_write(gc2145->regmap, GC2145_REG_LWC_HIGH, lwc >> 8, &ret);
-	cci_write(gc2145->regmap, GC2145_REG_LWC_LOW, lwc & 0xff, &ret);
+	if (gc2145_format->colorspace != V4L2_COLORSPACE_RAW)
+		lwc = gc2145->mode->width * 2;
+	else
+		lwc = gc2145->mode->width;
+
+	cci_write(gc2145->regmap, GC2145_REG_LWC, lwc, &ret);
 
 	/*
 	 * Adjust the MIPI FIFO Full Level
@@ -821,21 +866,25 @@ static int gc2145_config_mipi_mode(struct gc2145 *gc2145,
 	 * 1280x720 / 1600x1200 (aka no scaler) non RAW: 0x0001
 	 * 1600x1200 RAW: 0x0190
 	 */
-	if (gc2145->mode->width == 1280 || gc2145->mode->width == 1600)
-		fifo_full_lvl = 0x0001;
-	else
+	if (gc2145_format->colorspace != V4L2_COLORSPACE_RAW) {
+		if (gc2145->mode->width == 1280 || gc2145->mode->width == 1600)
+			fifo_full_lvl = 0x0001;
+		else
+			fifo_full_lvl = 0x0190;
+	} else {
 		fifo_full_lvl = 0x0190;
+	}
 
-	cci_write(gc2145->regmap, GC2145_REG_FIFO_FULL_LVL_HIGH,
-		  fifo_full_lvl >> 8, &ret);
-	cci_write(gc2145->regmap, GC2145_REG_FIFO_FULL_LVL_LOW,
-		  fifo_full_lvl & 0xff, &ret);
+	cci_write(gc2145->regmap, GC2145_REG_FIFO_FULL_LVL,
+		  fifo_full_lvl, &ret);
 
 	/*
 	 * Set the FIFO gate mode / MIPI wdiv set:
 	 * 0xf1 in case of RAW mode and 0xf0 otherwise
 	 */
-	cci_write(gc2145->regmap, GC2145_REG_FIFO_GATE_MODE, 0xf0, &ret);
+	cci_write(gc2145->regmap, GC2145_REG_FIFO_GATE_MODE,
+		  gc2145_format->colorspace == V4L2_COLORSPACE_RAW ?
+		  0xf1 : 0xf0, &ret);
 
 	/* Set the MIPI data type */
 	cci_write(gc2145->regmap, GC2145_REG_MIPI_DT,
@@ -849,9 +898,11 @@ static int gc2145_config_mipi_mode(struct gc2145 *gc2145,
 	return ret;
 }
 
-static int gc2145_start_streaming(struct gc2145 *gc2145,
-				  struct v4l2_subdev_state *state)
+static int gc2145_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
 {
+	struct gc2145 *gc2145 = to_gc2145(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&gc2145->sd);
 	const struct gc2145_format *gc2145_format;
 	struct v4l2_mbus_framefmt *fmt;
@@ -883,6 +934,10 @@ static int gc2145_start_streaming(struct gc2145 *gc2145,
 			GC2145_BYPASS_MODE_SWITCH,
 			gc2145_format->switch_bit ? GC2145_BYPASS_MODE_SWITCH
 						  : 0, &ret);
+	cci_update_bits(gc2145->regmap, GC2145_REG_SYNC_MODE,
+			GC2145_SYNC_MODE_COL_SWITCH |
+			GC2145_SYNC_MODE_ROW_SWITCH,
+			gc2145_format->row_col_switch, &ret);
 	if (ret) {
 		dev_err(&client->dev, "%s failed to write regs\n", __func__);
 		goto err_rpm_put;
@@ -913,8 +968,11 @@ err_rpm_put:
 	return ret;
 }
 
-static void gc2145_stop_streaming(struct gc2145 *gc2145)
+static int gc2145_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
 {
+	struct gc2145 *gc2145 = to_gc2145(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&gc2145->sd);
 	int ret = 0;
 
@@ -929,22 +987,6 @@ static void gc2145_stop_streaming(struct gc2145 *gc2145)
 
 	pm_runtime_mark_last_busy(&client->dev);
 	pm_runtime_put_autosuspend(&client->dev);
-}
-
-static int gc2145_set_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct gc2145 *gc2145 = to_gc2145(sd);
-	struct v4l2_subdev_state *state;
-	int ret = 0;
-
-	state = v4l2_subdev_lock_and_get_active_state(sd);
-
-	if (enable)
-		ret = gc2145_start_streaming(gc2145, state);
-	else
-		gc2145_stop_streaming(gc2145);
-
-	v4l2_subdev_unlock_state(state);
 
 	return ret;
 }
@@ -1069,13 +1111,8 @@ static const u8 test_pattern_val[] = {
 	GC2145_TEST_UNIFORM | GC2145_TEST_BLACK,
 };
 
-static const struct v4l2_subdev_core_ops gc2145_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
 static const struct v4l2_subdev_video_ops gc2145_video_ops = {
-	.s_stream = gc2145_set_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_pad_ops gc2145_pad_ops = {
@@ -1084,10 +1121,11 @@ static const struct v4l2_subdev_pad_ops gc2145_pad_ops = {
 	.set_fmt = gc2145_set_pad_format,
 	.get_selection = gc2145_get_selection,
 	.enum_frame_size = gc2145_enum_frame_size,
+	.enable_streams = gc2145_enable_streams,
+	.disable_streams = gc2145_disable_streams,
 };
 
 static const struct v4l2_subdev_ops gc2145_subdev_ops = {
-	.core = &gc2145_core_ops,
 	.video = &gc2145_video_ops,
 	.pad = &gc2145_pad_ops,
 };
@@ -1353,8 +1391,7 @@ static int gc2145_probe(struct i2c_client *client)
 		goto error_power_off;
 
 	/* Initialize subdev */
-	gc2145->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	gc2145->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	gc2145->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	/* Initialize source pad */
diff --git a/drivers/media/i2c/hi556.c b/drivers/media/i2c/hi556.c
index 38c77d515786..aed258211b8a 100644
--- a/drivers/media/i2c/hi556.c
+++ b/drivers/media/i2c/hi556.c
@@ -1,12 +1,15 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2019 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -633,10 +636,15 @@ struct hi556 {
 	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
 
+	/* GPIOs, clocks, etc. */
+	struct gpio_desc *reset_gpio;
+	struct clk *clk;
+	struct regulator *avdd;
+
 	/* Current mode */
 	const struct hi556_mode *cur_mode;
 
-	/* To serialize asynchronus callbacks */
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 
 	/* True if the device has been identified */
@@ -711,7 +719,7 @@ static int hi556_write_reg_list(struct hi556 *hi556,
 				      r_list->regs[i].val);
 		if (ret) {
 			dev_err_ratelimited(&client->dev,
-					    "failed to write reg 0x%4.4x. error = %d",
+					    "failed to write reg 0x%4.4x. error = %d\n",
 					    r_list->regs[i].address, ret);
 			return ret;
 		}
@@ -918,7 +926,7 @@ static int hi556_identify_module(struct hi556 *hi556)
 		return ret;
 
 	if (val != HI556_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%x",
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
 			HI556_CHIP_ID, val);
 		return -ENXIO;
 	}
@@ -994,14 +1002,14 @@ static int hi556_start_streaming(struct hi556 *hi556)
 	reg_list = &link_freq_configs[link_freq_index].reg_list;
 	ret = hi556_write_reg_list(hi556, reg_list);
 	if (ret) {
-		dev_err(&client->dev, "failed to set plls");
+		dev_err(&client->dev, "failed to set plls\n");
 		return ret;
 	}
 
 	reg_list = &hi556->cur_mode->reg_list;
 	ret = hi556_write_reg_list(hi556, reg_list);
 	if (ret) {
-		dev_err(&client->dev, "failed to set mode");
+		dev_err(&client->dev, "failed to set mode\n");
 		return ret;
 	}
 
@@ -1013,7 +1021,7 @@ static int hi556_start_streaming(struct hi556 *hi556)
 			      HI556_REG_VALUE_16BIT, HI556_MODE_STREAMING);
 
 	if (ret) {
-		dev_err(&client->dev, "failed to set stream");
+		dev_err(&client->dev, "failed to set stream\n");
 		return ret;
 	}
 
@@ -1026,7 +1034,7 @@ static void hi556_stop_streaming(struct hi556 *hi556)
 
 	if (hi556_write_reg(hi556, HI556_REG_MODE_SELECT,
 			    HI556_REG_VALUE_16BIT, HI556_MODE_STANDBY))
-		dev_err(&client->dev, "failed to set stream");
+		dev_err(&client->dev, "failed to set stream\n");
 }
 
 static int hi556_set_stream(struct v4l2_subdev *sd, int enable)
@@ -1045,7 +1053,6 @@ static int hi556_set_stream(struct v4l2_subdev *sd, int enable)
 
 		ret = hi556_start_streaming(hi556);
 		if (ret) {
-			enable = 0;
 			hi556_stop_streaming(hi556);
 			pm_runtime_put(&client->dev);
 		}
@@ -1206,38 +1213,41 @@ static int hi556_check_hwcfg(struct device *dev)
 	int ret = 0;
 	unsigned int i, j;
 
-	if (!fwnode)
-		return -ENXIO;
+	/*
+	 * Sometimes the fwnode graph is initialized by the bridge driver,
+	 * wait for this.
+	 */
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return dev_err_probe(dev, ret, "parsing endpoint failed\n");
 
 	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
 	if (ret) {
-		dev_err(dev, "can't get clock frequency");
-		return ret;
+		dev_err(dev, "can't get clock frequency\n");
+		goto check_hwcfg_error;
 	}
 
 	if (mclk != HI556_MCLK) {
-		dev_err(dev, "external clock %d is not supported", mclk);
-		return -EINVAL;
+		dev_err(dev, "external clock %d is not supported\n", mclk);
+		ret = -EINVAL;
+		goto check_hwcfg_error;
 	}
 
-	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
-	if (!ep)
-		return -ENXIO;
-
-	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
-	fwnode_handle_put(ep);
-	if (ret)
-		return ret;
-
 	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 2) {
-		dev_err(dev, "number of CSI2 data lanes %d is not supported",
+		dev_err(dev, "number of CSI2 data lanes %d is not supported\n",
 			bus_cfg.bus.mipi_csi2.num_data_lanes);
 		ret = -EINVAL;
 		goto check_hwcfg_error;
 	}
 
 	if (!bus_cfg.nr_of_link_frequencies) {
-		dev_err(dev, "no link frequencies defined");
+		dev_err(dev, "no link frequencies defined\n");
 		ret = -EINVAL;
 		goto check_hwcfg_error;
 	}
@@ -1250,7 +1260,7 @@ static int hi556_check_hwcfg(struct device *dev)
 		}
 
 		if (j == bus_cfg.nr_of_link_frequencies) {
-			dev_err(dev, "no link frequency %lld supported",
+			dev_err(dev, "no link frequency %lld supported\n",
 				link_freq_menu_items[i]);
 			ret = -EINVAL;
 			goto check_hwcfg_error;
@@ -1275,6 +1285,47 @@ static void hi556_remove(struct i2c_client *client)
 	mutex_destroy(&hi556->mutex);
 }
 
+static int hi556_suspend(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hi556 *hi556 = to_hi556(sd);
+	int ret;
+
+	gpiod_set_value_cansleep(hi556->reset_gpio, 1);
+
+	ret = regulator_disable(hi556->avdd);
+	if (ret) {
+		dev_err(dev, "failed to disable avdd: %d\n", ret);
+		gpiod_set_value_cansleep(hi556->reset_gpio, 0);
+		return ret;
+	}
+
+	clk_disable_unprepare(hi556->clk);
+	return 0;
+}
+
+static int hi556_resume(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct hi556 *hi556 = to_hi556(sd);
+	int ret;
+
+	ret = clk_prepare_enable(hi556->clk);
+	if (ret)
+		return ret;
+
+	ret = regulator_enable(hi556->avdd);
+	if (ret) {
+		dev_err(dev, "failed to enable avdd: %d\n", ret);
+		clk_disable_unprepare(hi556->clk);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(hi556->reset_gpio, 0);
+	usleep_range(5000, 5500);
+	return 0;
+}
+
 static int hi556_probe(struct i2c_client *client)
 {
 	struct hi556 *hi556;
@@ -1282,11 +1333,8 @@ static int hi556_probe(struct i2c_client *client)
 	int ret;
 
 	ret = hi556_check_hwcfg(&client->dev);
-	if (ret) {
-		dev_err(&client->dev, "failed to check HW configuration: %d",
-			ret);
+	if (ret)
 		return ret;
-	}
 
 	hi556 = devm_kzalloc(&client->dev, sizeof(*hi556), GFP_KERNEL);
 	if (!hi556)
@@ -1294,12 +1342,35 @@ static int hi556_probe(struct i2c_client *client)
 
 	v4l2_i2c_subdev_init(&hi556->sd, client, &hi556_subdev_ops);
 
+	hi556->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(hi556->reset_gpio))
+		return dev_err_probe(&client->dev, PTR_ERR(hi556->reset_gpio),
+				     "failed to get reset GPIO\n");
+
+	hi556->clk = devm_clk_get_optional(&client->dev, "clk");
+	if (IS_ERR(hi556->clk))
+		return dev_err_probe(&client->dev, PTR_ERR(hi556->clk),
+				     "failed to get clock\n");
+
+	/* The regulator core will provide a "dummy" regulator if necessary */
+	hi556->avdd = devm_regulator_get(&client->dev, "avdd");
+	if (IS_ERR(hi556->avdd))
+		return dev_err_probe(&client->dev, PTR_ERR(hi556->avdd),
+				     "failed to get avdd regulator\n");
+
 	full_power = acpi_dev_state_d0(&client->dev);
 	if (full_power) {
+		/* Ensure non ACPI managed resources are enabled */
+		ret = hi556_resume(&client->dev);
+		if (ret)
+			return dev_err_probe(&client->dev, ret,
+					     "failed to power on sensor\n");
+
 		ret = hi556_identify_module(hi556);
 		if (ret) {
-			dev_err(&client->dev, "failed to find sensor: %d", ret);
-			return ret;
+			dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+			goto probe_error_power_off;
 		}
 	}
 
@@ -1307,7 +1378,7 @@ static int hi556_probe(struct i2c_client *client)
 	hi556->cur_mode = &supported_modes[0];
 	ret = hi556_init_controls(hi556);
 	if (ret) {
-		dev_err(&client->dev, "failed to init controls: %d", ret);
+		dev_err(&client->dev, "failed to init controls: %d\n", ret);
 		goto probe_error_v4l2_ctrl_handler_free;
 	}
 
@@ -1318,13 +1389,13 @@ static int hi556_probe(struct i2c_client *client)
 	hi556->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&hi556->sd.entity, 1, &hi556->pad);
 	if (ret) {
-		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		dev_err(&client->dev, "failed to init entity pads: %d\n", ret);
 		goto probe_error_v4l2_ctrl_handler_free;
 	}
 
 	ret = v4l2_async_register_subdev_sensor(&hi556->sd);
 	if (ret < 0) {
-		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d\n",
 			ret);
 		goto probe_error_media_entity_cleanup;
 	}
@@ -1344,9 +1415,16 @@ probe_error_v4l2_ctrl_handler_free:
 	v4l2_ctrl_handler_free(hi556->sd.ctrl_handler);
 	mutex_destroy(&hi556->mutex);
 
+probe_error_power_off:
+	if (full_power)
+		hi556_suspend(&client->dev);
+
 	return ret;
 }
 
+static DEFINE_RUNTIME_DEV_PM_OPS(hi556_pm_ops, hi556_suspend, hi556_resume,
+				 NULL);
+
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id hi556_acpi_ids[] = {
 	{"INT3537"},
@@ -1360,6 +1438,7 @@ static struct i2c_driver hi556_i2c_driver = {
 	.driver = {
 		.name = "hi556",
 		.acpi_match_table = ACPI_PTR(hi556_acpi_ids),
+		.pm = pm_sleep_ptr(&hi556_pm_ops),
 	},
 	.probe = hi556_probe,
 	.remove = hi556_remove,
diff --git a/drivers/media/i2c/hi846.c b/drivers/media/i2c/hi846.c
index 9c565ec033d4..172772decd3d 100644
--- a/drivers/media/i2c/hi846.c
+++ b/drivers/media/i2c/hi846.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2021 Purism SPC
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/gpio/consumer.h>
@@ -1851,7 +1851,7 @@ static int hi846_get_selection(struct v4l2_subdev *sd,
 		mutex_lock(&hi846->mutex);
 		switch (sel->which) {
 		case V4L2_SUBDEV_FORMAT_TRY:
-			v4l2_subdev_state_get_crop(sd_state, sel->pad);
+			sel->r = *v4l2_subdev_state_get_crop(sd_state, sel->pad);
 			break;
 		case V4L2_SUBDEV_FORMAT_ACTIVE:
 			sel->r = hi846->cur_mode->crop;
diff --git a/drivers/media/i2c/hi847.c b/drivers/media/i2c/hi847.c
index 72c60747a839..546833f5b5f5 100644
--- a/drivers/media/i2c/hi847.c
+++ b/drivers/media/i2c/hi847.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2022 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
diff --git a/drivers/media/i2c/imx208.c b/drivers/media/i2c/imx208.c
index 639e05340dbb..2b5a6ce7b1ae 100644
--- a/drivers/media/i2c/imx208.c
+++ b/drivers/media/i2c/imx208.c
@@ -8,7 +8,7 @@
 #include <linux/pm_runtime.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #define IMX208_REG_MODE_SELECT		0x0100
 #define IMX208_MODE_STANDBY		0x00
@@ -814,7 +814,7 @@ out_unlock:
 }
 
 static ssize_t otp_read(struct file *filp, struct kobject *kobj,
-			struct bin_attribute *bin_attr,
+			const struct bin_attribute *bin_attr,
 			char *buf, loff_t off, size_t count)
 {
 	struct i2c_client *client = to_i2c_client(kobj_to_dev(kobj));
diff --git a/drivers/media/i2c/imx214.c b/drivers/media/i2c/imx214.c
index 10b6ad66d126..dd7bc45523d8 100644
--- a/drivers/media/i2c/imx214.c
+++ b/drivers/media/i2c/imx214.c
@@ -15,26 +15,186 @@
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 #include <media/media-entity.h>
+#include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
-#define IMX214_REG_MODE_SELECT		0x0100
+/* Chip ID */
+#define IMX214_REG_CHIP_ID		CCI_REG16(0x0016)
+#define IMX214_CHIP_ID			0x0214
+
+#define IMX214_REG_MODE_SELECT		CCI_REG8(0x0100)
 #define IMX214_MODE_STANDBY		0x00
 #define IMX214_MODE_STREAMING		0x01
 
+#define IMX214_REG_FAST_STANDBY_CTRL	CCI_REG8(0x0106)
+
 #define IMX214_DEFAULT_CLK_FREQ	24000000
-#define IMX214_DEFAULT_LINK_FREQ 480000000
+#define IMX214_DEFAULT_LINK_FREQ	600000000
+/* Keep wrong link frequency for backward compatibility */
+#define IMX214_DEFAULT_LINK_FREQ_LEGACY	480000000
 #define IMX214_DEFAULT_PIXEL_RATE ((IMX214_DEFAULT_LINK_FREQ * 8LL) / 10)
 #define IMX214_FPS 30
-#define IMX214_MBUS_CODE MEDIA_BUS_FMT_SRGGB10_1X10
+
+/* V-TIMING internal */
+#define IMX214_REG_FRM_LENGTH_LINES	CCI_REG16(0x0340)
+#define IMX214_VTS_MAX			0xffff
+
+#define IMX214_VBLANK_MIN		890
+
+/* HBLANK control - read only */
+#define IMX214_PPL_DEFAULT		5008
 
 /* Exposure control */
-#define IMX214_REG_EXPOSURE		0x0202
-#define IMX214_EXPOSURE_MIN		0
-#define IMX214_EXPOSURE_MAX		3184
+#define IMX214_REG_EXPOSURE		CCI_REG16(0x0202)
+#define IMX214_EXPOSURE_OFFSET		10
+#define IMX214_EXPOSURE_MIN		1
 #define IMX214_EXPOSURE_STEP		1
 #define IMX214_EXPOSURE_DEFAULT		3184
+#define IMX214_REG_EXPOSURE_RATIO	CCI_REG8(0x0222)
+#define IMX214_REG_SHORT_EXPOSURE	CCI_REG16(0x0224)
+
+/* Analog gain control */
+#define IMX214_REG_ANALOG_GAIN		CCI_REG16(0x0204)
+#define IMX214_REG_SHORT_ANALOG_GAIN	CCI_REG16(0x0216)
+#define IMX214_ANA_GAIN_MIN		0
+#define IMX214_ANA_GAIN_MAX		448
+#define IMX214_ANA_GAIN_STEP		1
+#define IMX214_ANA_GAIN_DEFAULT		0x0
+
+/* Digital gain control */
+#define IMX214_REG_DIG_GAIN_GREENR	CCI_REG16(0x020e)
+#define IMX214_REG_DIG_GAIN_RED		CCI_REG16(0x0210)
+#define IMX214_REG_DIG_GAIN_BLUE	CCI_REG16(0x0212)
+#define IMX214_REG_DIG_GAIN_GREENB	CCI_REG16(0x0214)
+#define IMX214_DGTL_GAIN_MIN		0x0100
+#define IMX214_DGTL_GAIN_MAX		0x0fff
+#define IMX214_DGTL_GAIN_DEFAULT	0x0100
+#define IMX214_DGTL_GAIN_STEP		1
+
+#define IMX214_REG_ORIENTATION		CCI_REG8(0x0101)
+
+#define IMX214_REG_MASK_CORR_FRAMES	CCI_REG8(0x0105)
+#define IMX214_CORR_FRAMES_TRANSMIT	0
+#define IMX214_CORR_FRAMES_MASK		1
+
+#define IMX214_REG_CSI_DATA_FORMAT	CCI_REG16(0x0112)
+#define IMX214_CSI_DATA_FORMAT_RAW8	0x0808
+#define IMX214_CSI_DATA_FORMAT_RAW10	0x0A0A
+#define IMX214_CSI_DATA_FORMAT_COMP6	0x0A06
+#define IMX214_CSI_DATA_FORMAT_COMP8	0x0A08
+
+#define IMX214_REG_CSI_LANE_MODE	CCI_REG8(0x0114)
+#define IMX214_CSI_2_LANE_MODE		1
+#define IMX214_CSI_4_LANE_MODE		3
+
+#define IMX214_REG_EXCK_FREQ		CCI_REG16(0x0136)
+#define IMX214_EXCK_FREQ(n)		((n) * 256)	/* n expressed in MHz */
+
+#define IMX214_REG_TEMP_SENSOR_CONTROL	CCI_REG8(0x0138)
+
+#define IMX214_REG_HDR_MODE		CCI_REG8(0x0220)
+#define IMX214_HDR_MODE_OFF		0
+#define IMX214_HDR_MODE_ON		1
+
+#define IMX214_REG_HDR_RES_REDUCTION	CCI_REG8(0x0221)
+#define IMX214_HDR_RES_REDU_THROUGH	0x11
+#define IMX214_HDR_RES_REDU_2_BINNING	0x22
+
+/* PLL settings */
+#define IMX214_REG_VTPXCK_DIV		CCI_REG8(0x0301)
+#define IMX214_REG_VTSYCK_DIV		CCI_REG8(0x0303)
+#define IMX214_REG_PREPLLCK_VT_DIV	CCI_REG8(0x0305)
+#define IMX214_REG_PLL_VT_MPY		CCI_REG16(0x0306)
+#define IMX214_REG_OPPXCK_DIV		CCI_REG8(0x0309)
+#define IMX214_REG_OPSYCK_DIV		CCI_REG8(0x030b)
+#define IMX214_REG_PLL_MULT_DRIV	CCI_REG8(0x0310)
+#define IMX214_PLL_SINGLE		0
+#define IMX214_PLL_DUAL			1
+
+#define IMX214_REG_LINE_LENGTH_PCK	CCI_REG16(0x0342)
+#define IMX214_REG_X_ADD_STA		CCI_REG16(0x0344)
+#define IMX214_REG_Y_ADD_STA		CCI_REG16(0x0346)
+#define IMX214_REG_X_ADD_END		CCI_REG16(0x0348)
+#define IMX214_REG_Y_ADD_END		CCI_REG16(0x034a)
+#define IMX214_REG_X_OUTPUT_SIZE	CCI_REG16(0x034c)
+#define IMX214_REG_Y_OUTPUT_SIZE	CCI_REG16(0x034e)
+#define IMX214_REG_X_EVEN_INC		CCI_REG8(0x0381)
+#define IMX214_REG_X_ODD_INC		CCI_REG8(0x0383)
+#define IMX214_REG_Y_EVEN_INC		CCI_REG8(0x0385)
+#define IMX214_REG_Y_ODD_INC		CCI_REG8(0x0387)
+
+#define IMX214_REG_SCALE_MODE		CCI_REG8(0x0401)
+#define IMX214_SCALE_NONE		0
+#define IMX214_SCALE_HORIZONTAL		1
+#define IMX214_SCALE_FULL		2
+#define IMX214_REG_SCALE_M		CCI_REG16(0x0404)
+
+#define IMX214_REG_DIG_CROP_X_OFFSET	CCI_REG16(0x0408)
+#define IMX214_REG_DIG_CROP_Y_OFFSET	CCI_REG16(0x040a)
+#define IMX214_REG_DIG_CROP_WIDTH	CCI_REG16(0x040c)
+#define IMX214_REG_DIG_CROP_HEIGHT	CCI_REG16(0x040e)
+
+#define IMX214_REG_REQ_LINK_BIT_RATE	CCI_REG32(0x0820)
+#define IMX214_LINK_BIT_RATE_MBPS(n)	((n) << 16)
+
+/* Binning mode */
+#define IMX214_REG_BINNING_MODE		CCI_REG8(0x0900)
+#define IMX214_BINNING_NONE		0
+#define IMX214_BINNING_ENABLE		1
+#define IMX214_REG_BINNING_TYPE		CCI_REG8(0x0901)
+#define IMX214_REG_BINNING_WEIGHTING	CCI_REG8(0x0902)
+#define IMX214_BINNING_AVERAGE		0x00
+#define IMX214_BINNING_SUMMED		0x01
+#define IMX214_BINNING_BAYER		0x02
+
+#define IMX214_REG_SING_DEF_CORR_EN	CCI_REG8(0x0b06)
+#define IMX214_SING_DEF_CORR_OFF	0
+#define IMX214_SING_DEF_CORR_ON		1
+
+/* AWB control */
+#define IMX214_REG_ABS_GAIN_GREENR	CCI_REG16(0x0b8e)
+#define IMX214_REG_ABS_GAIN_RED		CCI_REG16(0x0b90)
+#define IMX214_REG_ABS_GAIN_BLUE	CCI_REG16(0x0b92)
+#define IMX214_REG_ABS_GAIN_GREENB	CCI_REG16(0x0b94)
+
+#define IMX214_REG_RMSC_NR_MODE		CCI_REG8(0x3001)
+#define IMX214_REG_STATS_OUT_EN		CCI_REG8(0x3013)
+#define IMX214_STATS_OUT_OFF		0
+#define IMX214_STATS_OUT_ON		1
+
+/* Chroma noise reduction */
+#define IMX214_REG_NML_NR_EN		CCI_REG8(0x30a2)
+#define IMX214_NML_NR_OFF		0
+#define IMX214_NML_NR_ON		1
+
+#define IMX214_REG_EBD_SIZE_V		CCI_REG8(0x5041)
+#define IMX214_EBD_NO			0
+#define IMX214_EBD_4_LINE		4
+
+#define IMX214_REG_RG_STATS_LMT		CCI_REG16(0x6d12)
+#define IMX214_RG_STATS_LMT_10_BIT	0x03FF
+#define IMX214_RG_STATS_LMT_14_BIT	0x3FFF
+
+#define IMX214_REG_ATR_FAST_MOVE	CCI_REG8(0x9300)
+
+/* Test Pattern Control */
+#define IMX214_REG_TEST_PATTERN		CCI_REG16(0x0600)
+#define IMX214_TEST_PATTERN_DISABLE	0
+#define IMX214_TEST_PATTERN_SOLID_COLOR	1
+#define IMX214_TEST_PATTERN_COLOR_BARS	2
+#define IMX214_TEST_PATTERN_GREY_COLOR	3
+#define IMX214_TEST_PATTERN_PN9		4
+
+/* Test pattern colour components */
+#define IMX214_REG_TESTP_RED		CCI_REG16(0x0602)
+#define IMX214_REG_TESTP_GREENR		CCI_REG16(0x0604)
+#define IMX214_REG_TESTP_BLUE		CCI_REG16(0x0606)
+#define IMX214_REG_TESTP_GREENB		CCI_REG16(0x0608)
+#define IMX214_TESTP_COLOUR_MIN		0
+#define IMX214_TESTP_COLOUR_MAX		0x03ff
+#define IMX214_TESTP_COLOUR_STEP	1
 
 /* IMX214 native and active pixel array size */
 #define IMX214_NATIVE_WIDTH		4224U
@@ -52,6 +212,38 @@ static const char * const imx214_supply_name[] = {
 
 #define IMX214_NUM_SUPPLIES ARRAY_SIZE(imx214_supply_name)
 
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 imx214_mbus_formats[] = {
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+};
+
+static const char * const imx214_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bars",
+	"Solid Color",
+	"Grey Color Bars",
+	"PN9"
+};
+
+static const int imx214_test_pattern_val[] = {
+	IMX214_TEST_PATTERN_DISABLE,
+	IMX214_TEST_PATTERN_COLOR_BARS,
+	IMX214_TEST_PATTERN_SOLID_COLOR,
+	IMX214_TEST_PATTERN_GREY_COLOR,
+	IMX214_TEST_PATTERN_PN9,
+};
+
 struct imx214 {
 	struct device *dev;
 	struct clk *xclk;
@@ -59,365 +251,262 @@ struct imx214 {
 
 	struct v4l2_subdev sd;
 	struct media_pad pad;
-	struct v4l2_mbus_framefmt fmt;
-	struct v4l2_rect crop;
 
 	struct v4l2_ctrl_handler ctrls;
 	struct v4l2_ctrl *pixel_rate;
 	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
 	struct v4l2_ctrl *unit_size;
+	struct {
+		struct v4l2_ctrl *hflip;
+		struct v4l2_ctrl *vflip;
+	};
 
 	struct regulator_bulk_data	supplies[IMX214_NUM_SUPPLIES];
 
 	struct gpio_desc *enable_gpio;
-
-	/*
-	 * Serialize control access, get/set format, get selection
-	 * and start streaming.
-	 */
-	struct mutex mutex;
-};
-
-struct reg_8 {
-	u16 addr;
-	u8 val;
-};
-
-enum {
-	IMX214_TABLE_WAIT_MS = 0,
-	IMX214_TABLE_END,
-	IMX214_MAX_RETRIES,
-	IMX214_WAIT_MS
 };
 
 /*From imx214_mode_tbls.h*/
-static const struct reg_8 mode_4096x2304[] = {
-	{0x0114, 0x03},
-	{0x0220, 0x00},
-	{0x0221, 0x11},
-	{0x0222, 0x01},
-	{0x0340, 0x0C},
-	{0x0341, 0x7A},
-	{0x0342, 0x13},
-	{0x0343, 0x90},
-	{0x0344, 0x00},
-	{0x0345, 0x38},
-	{0x0346, 0x01},
-	{0x0347, 0x98},
-	{0x0348, 0x10},
-	{0x0349, 0x37},
-	{0x034A, 0x0A},
-	{0x034B, 0x97},
-	{0x0381, 0x01},
-	{0x0383, 0x01},
-	{0x0385, 0x01},
-	{0x0387, 0x01},
-	{0x0900, 0x00},
-	{0x0901, 0x00},
-	{0x0902, 0x00},
-	{0x3000, 0x35},
-	{0x3054, 0x01},
-	{0x305C, 0x11},
-
-	{0x0112, 0x0A},
-	{0x0113, 0x0A},
-	{0x034C, 0x10},
-	{0x034D, 0x00},
-	{0x034E, 0x09},
-	{0x034F, 0x00},
-	{0x0401, 0x00},
-	{0x0404, 0x00},
-	{0x0405, 0x10},
-	{0x0408, 0x00},
-	{0x0409, 0x00},
-	{0x040A, 0x00},
-	{0x040B, 0x00},
-	{0x040C, 0x10},
-	{0x040D, 0x00},
-	{0x040E, 0x09},
-	{0x040F, 0x00},
-
-	{0x0301, 0x05},
-	{0x0303, 0x02},
-	{0x0305, 0x03},
-	{0x0306, 0x00},
-	{0x0307, 0x96},
-	{0x0309, 0x0A},
-	{0x030B, 0x01},
-	{0x0310, 0x00},
-
-	{0x0820, 0x12},
-	{0x0821, 0xC0},
-	{0x0822, 0x00},
-	{0x0823, 0x00},
-
-	{0x3A03, 0x09},
-	{0x3A04, 0x50},
-	{0x3A05, 0x01},
-
-	{0x0B06, 0x01},
-	{0x30A2, 0x00},
-
-	{0x30B4, 0x00},
-
-	{0x3A02, 0xFF},
-
-	{0x3011, 0x00},
-	{0x3013, 0x01},
-
-	{0x0202, 0x0C},
-	{0x0203, 0x70},
-	{0x0224, 0x01},
-	{0x0225, 0xF4},
-
-	{0x0204, 0x00},
-	{0x0205, 0x00},
-	{0x020E, 0x01},
-	{0x020F, 0x00},
-	{0x0210, 0x01},
-	{0x0211, 0x00},
-	{0x0212, 0x01},
-	{0x0213, 0x00},
-	{0x0214, 0x01},
-	{0x0215, 0x00},
-	{0x0216, 0x00},
-	{0x0217, 0x00},
-
-	{0x4170, 0x00},
-	{0x4171, 0x10},
-	{0x4176, 0x00},
-	{0x4177, 0x3C},
-	{0xAE20, 0x04},
-	{0xAE21, 0x5C},
-
-	{IMX214_TABLE_WAIT_MS, 10},
-	{0x0138, 0x01},
-	{IMX214_TABLE_END, 0x00}
+static const struct cci_reg_sequence mode_4096x2304[] = {
+	{ IMX214_REG_HDR_MODE, IMX214_HDR_MODE_OFF },
+	{ IMX214_REG_HDR_RES_REDUCTION, IMX214_HDR_RES_REDU_THROUGH },
+	{ IMX214_REG_EXPOSURE_RATIO, 1 },
+	{ IMX214_REG_X_ADD_STA, 56 },
+	{ IMX214_REG_Y_ADD_STA, 408 },
+	{ IMX214_REG_X_ADD_END, 4151 },
+	{ IMX214_REG_Y_ADD_END, 2711 },
+	{ IMX214_REG_X_EVEN_INC, 1 },
+	{ IMX214_REG_X_ODD_INC, 1 },
+	{ IMX214_REG_Y_EVEN_INC, 1 },
+	{ IMX214_REG_Y_ODD_INC, 1 },
+	{ IMX214_REG_BINNING_MODE, IMX214_BINNING_NONE },
+	{ IMX214_REG_BINNING_TYPE, 0 },
+	{ IMX214_REG_BINNING_WEIGHTING, IMX214_BINNING_AVERAGE },
+	{ CCI_REG8(0x3000), 0x35 },
+	{ CCI_REG8(0x3054), 0x01 },
+	{ CCI_REG8(0x305C), 0x11 },
+
+	{ IMX214_REG_CSI_DATA_FORMAT, IMX214_CSI_DATA_FORMAT_RAW10 },
+	{ IMX214_REG_X_OUTPUT_SIZE, 4096 },
+	{ IMX214_REG_Y_OUTPUT_SIZE, 2304 },
+	{ IMX214_REG_SCALE_MODE, IMX214_SCALE_NONE },
+	{ IMX214_REG_SCALE_M, 2 },
+	{ IMX214_REG_DIG_CROP_X_OFFSET, 0 },
+	{ IMX214_REG_DIG_CROP_Y_OFFSET, 0 },
+	{ IMX214_REG_DIG_CROP_WIDTH, 4096 },
+	{ IMX214_REG_DIG_CROP_HEIGHT, 2304 },
+
+	{ IMX214_REG_VTPXCK_DIV, 5 },
+	{ IMX214_REG_VTSYCK_DIV, 2 },
+	{ IMX214_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX214_REG_PLL_VT_MPY, 150 },
+	{ IMX214_REG_OPPXCK_DIV, 10 },
+	{ IMX214_REG_OPSYCK_DIV, 1 },
+	{ IMX214_REG_PLL_MULT_DRIV, IMX214_PLL_SINGLE },
+
+	{ IMX214_REG_REQ_LINK_BIT_RATE, IMX214_LINK_BIT_RATE_MBPS(4800) },
+
+	{ CCI_REG8(0x3A03), 0x09 },
+	{ CCI_REG8(0x3A04), 0x50 },
+	{ CCI_REG8(0x3A05), 0x01 },
+
+	{ IMX214_REG_SING_DEF_CORR_EN, IMX214_SING_DEF_CORR_ON },
+	{ IMX214_REG_NML_NR_EN, IMX214_NML_NR_OFF },
+
+	{ CCI_REG8(0x30B4), 0x00 },
+
+	{ CCI_REG8(0x3A02), 0xFF },
+
+	{ CCI_REG8(0x3011), 0x00 },
+	{ IMX214_REG_STATS_OUT_EN, IMX214_STATS_OUT_ON },
+
+	{ IMX214_REG_SHORT_EXPOSURE, 500 },
+
+	{ CCI_REG8(0x4170), 0x00 },
+	{ CCI_REG8(0x4171), 0x10 },
+	{ CCI_REG8(0x4176), 0x00 },
+	{ CCI_REG8(0x4177), 0x3C },
+	{ CCI_REG8(0xAE20), 0x04 },
+	{ CCI_REG8(0xAE21), 0x5C },
 };
 
-static const struct reg_8 mode_1920x1080[] = {
-	{0x0114, 0x03},
-	{0x0220, 0x00},
-	{0x0221, 0x11},
-	{0x0222, 0x01},
-	{0x0340, 0x0C},
-	{0x0341, 0x7A},
-	{0x0342, 0x13},
-	{0x0343, 0x90},
-	{0x0344, 0x04},
-	{0x0345, 0x78},
-	{0x0346, 0x03},
-	{0x0347, 0xFC},
-	{0x0348, 0x0B},
-	{0x0349, 0xF7},
-	{0x034A, 0x08},
-	{0x034B, 0x33},
-	{0x0381, 0x01},
-	{0x0383, 0x01},
-	{0x0385, 0x01},
-	{0x0387, 0x01},
-	{0x0900, 0x00},
-	{0x0901, 0x00},
-	{0x0902, 0x00},
-	{0x3000, 0x35},
-	{0x3054, 0x01},
-	{0x305C, 0x11},
-
-	{0x0112, 0x0A},
-	{0x0113, 0x0A},
-	{0x034C, 0x07},
-	{0x034D, 0x80},
-	{0x034E, 0x04},
-	{0x034F, 0x38},
-	{0x0401, 0x00},
-	{0x0404, 0x00},
-	{0x0405, 0x10},
-	{0x0408, 0x00},
-	{0x0409, 0x00},
-	{0x040A, 0x00},
-	{0x040B, 0x00},
-	{0x040C, 0x07},
-	{0x040D, 0x80},
-	{0x040E, 0x04},
-	{0x040F, 0x38},
-
-	{0x0301, 0x05},
-	{0x0303, 0x02},
-	{0x0305, 0x03},
-	{0x0306, 0x00},
-	{0x0307, 0x96},
-	{0x0309, 0x0A},
-	{0x030B, 0x01},
-	{0x0310, 0x00},
-
-	{0x0820, 0x12},
-	{0x0821, 0xC0},
-	{0x0822, 0x00},
-	{0x0823, 0x00},
-
-	{0x3A03, 0x04},
-	{0x3A04, 0xF8},
-	{0x3A05, 0x02},
-
-	{0x0B06, 0x01},
-	{0x30A2, 0x00},
-
-	{0x30B4, 0x00},
-
-	{0x3A02, 0xFF},
-
-	{0x3011, 0x00},
-	{0x3013, 0x01},
-
-	{0x0202, 0x0C},
-	{0x0203, 0x70},
-	{0x0224, 0x01},
-	{0x0225, 0xF4},
-
-	{0x0204, 0x00},
-	{0x0205, 0x00},
-	{0x020E, 0x01},
-	{0x020F, 0x00},
-	{0x0210, 0x01},
-	{0x0211, 0x00},
-	{0x0212, 0x01},
-	{0x0213, 0x00},
-	{0x0214, 0x01},
-	{0x0215, 0x00},
-	{0x0216, 0x00},
-	{0x0217, 0x00},
-
-	{0x4170, 0x00},
-	{0x4171, 0x10},
-	{0x4176, 0x00},
-	{0x4177, 0x3C},
-	{0xAE20, 0x04},
-	{0xAE21, 0x5C},
-
-	{IMX214_TABLE_WAIT_MS, 10},
-	{0x0138, 0x01},
-	{IMX214_TABLE_END, 0x00}
+static const struct cci_reg_sequence mode_1920x1080[] = {
+	{ IMX214_REG_HDR_MODE, IMX214_HDR_MODE_OFF },
+	{ IMX214_REG_HDR_RES_REDUCTION, IMX214_HDR_RES_REDU_THROUGH },
+	{ IMX214_REG_EXPOSURE_RATIO, 1 },
+	{ IMX214_REG_X_ADD_STA, 1144 },
+	{ IMX214_REG_Y_ADD_STA, 1020 },
+	{ IMX214_REG_X_ADD_END, 3063 },
+	{ IMX214_REG_Y_ADD_END, 2099 },
+	{ IMX214_REG_X_EVEN_INC, 1 },
+	{ IMX214_REG_X_ODD_INC, 1 },
+	{ IMX214_REG_Y_EVEN_INC, 1 },
+	{ IMX214_REG_Y_ODD_INC, 1 },
+	{ IMX214_REG_BINNING_MODE, IMX214_BINNING_NONE },
+	{ IMX214_REG_BINNING_TYPE, 0 },
+	{ IMX214_REG_BINNING_WEIGHTING, IMX214_BINNING_AVERAGE },
+	{ CCI_REG8(0x3000), 0x35 },
+	{ CCI_REG8(0x3054), 0x01 },
+	{ CCI_REG8(0x305C), 0x11 },
+
+	{ IMX214_REG_CSI_DATA_FORMAT, IMX214_CSI_DATA_FORMAT_RAW10 },
+	{ IMX214_REG_X_OUTPUT_SIZE, 1920 },
+	{ IMX214_REG_Y_OUTPUT_SIZE, 1080 },
+	{ IMX214_REG_SCALE_MODE, IMX214_SCALE_NONE },
+	{ IMX214_REG_SCALE_M, 2 },
+	{ IMX214_REG_DIG_CROP_X_OFFSET, 0 },
+	{ IMX214_REG_DIG_CROP_Y_OFFSET, 0 },
+	{ IMX214_REG_DIG_CROP_WIDTH, 1920 },
+	{ IMX214_REG_DIG_CROP_HEIGHT, 1080 },
+
+	{ IMX214_REG_VTPXCK_DIV, 5 },
+	{ IMX214_REG_VTSYCK_DIV, 2 },
+	{ IMX214_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX214_REG_PLL_VT_MPY, 150 },
+	{ IMX214_REG_OPPXCK_DIV, 10 },
+	{ IMX214_REG_OPSYCK_DIV, 1 },
+	{ IMX214_REG_PLL_MULT_DRIV, IMX214_PLL_SINGLE },
+
+	{ IMX214_REG_REQ_LINK_BIT_RATE, IMX214_LINK_BIT_RATE_MBPS(4800) },
+
+	{ CCI_REG8(0x3A03), 0x04 },
+	{ CCI_REG8(0x3A04), 0xF8 },
+	{ CCI_REG8(0x3A05), 0x02 },
+
+	{ IMX214_REG_SING_DEF_CORR_EN, IMX214_SING_DEF_CORR_ON },
+	{ IMX214_REG_NML_NR_EN, IMX214_NML_NR_OFF },
+
+	{ CCI_REG8(0x30B4), 0x00 },
+
+	{ CCI_REG8(0x3A02), 0xFF },
+
+	{ CCI_REG8(0x3011), 0x00 },
+	{ IMX214_REG_STATS_OUT_EN, IMX214_STATS_OUT_ON },
+
+	{ IMX214_REG_SHORT_EXPOSURE, 500 },
+
+	{ CCI_REG8(0x4170), 0x00 },
+	{ CCI_REG8(0x4171), 0x10 },
+	{ CCI_REG8(0x4176), 0x00 },
+	{ CCI_REG8(0x4177), 0x3C },
+	{ CCI_REG8(0xAE20), 0x04 },
+	{ CCI_REG8(0xAE21), 0x5C },
 };
 
-static const struct reg_8 mode_table_common[] = {
+static const struct cci_reg_sequence mode_table_common[] = {
 	/* software reset */
 
 	/* software standby settings */
-	{0x0100, 0x00},
+	{ IMX214_REG_MODE_SELECT, IMX214_MODE_STANDBY },
 
 	/* ATR setting */
-	{0x9300, 0x02},
+	{ IMX214_REG_ATR_FAST_MOVE, 2 },
 
 	/* external clock setting */
-	{0x0136, 0x18},
-	{0x0137, 0x00},
+	{ IMX214_REG_EXCK_FREQ, IMX214_EXCK_FREQ(IMX214_DEFAULT_CLK_FREQ / 1000000) },
 
 	/* global setting */
 	/* basic config */
-	{0x0101, 0x00},
-	{0x0105, 0x01},
-	{0x0106, 0x01},
-	{0x4550, 0x02},
-	{0x4601, 0x00},
-	{0x4642, 0x05},
-	{0x6227, 0x11},
-	{0x6276, 0x00},
-	{0x900E, 0x06},
-	{0xA802, 0x90},
-	{0xA803, 0x11},
-	{0xA804, 0x62},
-	{0xA805, 0x77},
-	{0xA806, 0xAE},
-	{0xA807, 0x34},
-	{0xA808, 0xAE},
-	{0xA809, 0x35},
-	{0xA80A, 0x62},
-	{0xA80B, 0x83},
-	{0xAE33, 0x00},
+	{ IMX214_REG_MASK_CORR_FRAMES, IMX214_CORR_FRAMES_MASK },
+	{ IMX214_REG_FAST_STANDBY_CTRL, 1 },
+	{ IMX214_REG_LINE_LENGTH_PCK, IMX214_PPL_DEFAULT },
+	{ CCI_REG8(0x4550), 0x02 },
+	{ CCI_REG8(0x4601), 0x00 },
+	{ CCI_REG8(0x4642), 0x05 },
+	{ CCI_REG8(0x6227), 0x11 },
+	{ CCI_REG8(0x6276), 0x00 },
+	{ CCI_REG8(0x900E), 0x06 },
+	{ CCI_REG8(0xA802), 0x90 },
+	{ CCI_REG8(0xA803), 0x11 },
+	{ CCI_REG8(0xA804), 0x62 },
+	{ CCI_REG8(0xA805), 0x77 },
+	{ CCI_REG8(0xA806), 0xAE },
+	{ CCI_REG8(0xA807), 0x34 },
+	{ CCI_REG8(0xA808), 0xAE },
+	{ CCI_REG8(0xA809), 0x35 },
+	{ CCI_REG8(0xA80A), 0x62 },
+	{ CCI_REG8(0xA80B), 0x83 },
+	{ CCI_REG8(0xAE33), 0x00 },
 
 	/* analog setting */
-	{0x4174, 0x00},
-	{0x4175, 0x11},
-	{0x4612, 0x29},
-	{0x461B, 0x12},
-	{0x461F, 0x06},
-	{0x4635, 0x07},
-	{0x4637, 0x30},
-	{0x463F, 0x18},
-	{0x4641, 0x0D},
-	{0x465B, 0x12},
-	{0x465F, 0x11},
-	{0x4663, 0x11},
-	{0x4667, 0x0F},
-	{0x466F, 0x0F},
-	{0x470E, 0x09},
-	{0x4909, 0xAB},
-	{0x490B, 0x95},
-	{0x4915, 0x5D},
-	{0x4A5F, 0xFF},
-	{0x4A61, 0xFF},
-	{0x4A73, 0x62},
-	{0x4A85, 0x00},
-	{0x4A87, 0xFF},
+	{ CCI_REG8(0x4174), 0x00 },
+	{ CCI_REG8(0x4175), 0x11 },
+	{ CCI_REG8(0x4612), 0x29 },
+	{ CCI_REG8(0x461B), 0x12 },
+	{ CCI_REG8(0x461F), 0x06 },
+	{ CCI_REG8(0x4635), 0x07 },
+	{ CCI_REG8(0x4637), 0x30 },
+	{ CCI_REG8(0x463F), 0x18 },
+	{ CCI_REG8(0x4641), 0x0D },
+	{ CCI_REG8(0x465B), 0x12 },
+	{ CCI_REG8(0x465F), 0x11 },
+	{ CCI_REG8(0x4663), 0x11 },
+	{ CCI_REG8(0x4667), 0x0F },
+	{ CCI_REG8(0x466F), 0x0F },
+	{ CCI_REG8(0x470E), 0x09 },
+	{ CCI_REG8(0x4909), 0xAB },
+	{ CCI_REG8(0x490B), 0x95 },
+	{ CCI_REG8(0x4915), 0x5D },
+	{ CCI_REG8(0x4A5F), 0xFF },
+	{ CCI_REG8(0x4A61), 0xFF },
+	{ CCI_REG8(0x4A73), 0x62 },
+	{ CCI_REG8(0x4A85), 0x00 },
+	{ CCI_REG8(0x4A87), 0xFF },
 
 	/* embedded data */
-	{0x5041, 0x04},
-	{0x583C, 0x04},
-	{0x620E, 0x04},
-	{0x6EB2, 0x01},
-	{0x6EB3, 0x00},
-	{0x9300, 0x02},
+	{ IMX214_REG_EBD_SIZE_V, IMX214_EBD_4_LINE },
+	{ CCI_REG8(0x583C), 0x04 },
+	{ CCI_REG8(0x620E), 0x04 },
+	{ CCI_REG8(0x6EB2), 0x01 },
+	{ CCI_REG8(0x6EB3), 0x00 },
+	{ IMX214_REG_ATR_FAST_MOVE, 2 },
 
 	/* imagequality */
 	/* HDR setting */
-	{0x3001, 0x07},
-	{0x6D12, 0x3F},
-	{0x6D13, 0xFF},
-	{0x9344, 0x03},
-	{0x9706, 0x10},
-	{0x9707, 0x03},
-	{0x9708, 0x03},
-	{0x9E04, 0x01},
-	{0x9E05, 0x00},
-	{0x9E0C, 0x01},
-	{0x9E0D, 0x02},
-	{0x9E24, 0x00},
-	{0x9E25, 0x8C},
-	{0x9E26, 0x00},
-	{0x9E27, 0x94},
-	{0x9E28, 0x00},
-	{0x9E29, 0x96},
+	{ IMX214_REG_RMSC_NR_MODE, 0x07 },
+	{ IMX214_REG_RG_STATS_LMT, IMX214_RG_STATS_LMT_14_BIT },
+	{ CCI_REG8(0x9344), 0x03 },
+	{ CCI_REG8(0x9706), 0x10 },
+	{ CCI_REG8(0x9707), 0x03 },
+	{ CCI_REG8(0x9708), 0x03 },
+	{ CCI_REG8(0x9E04), 0x01 },
+	{ CCI_REG8(0x9E05), 0x00 },
+	{ CCI_REG8(0x9E0C), 0x01 },
+	{ CCI_REG8(0x9E0D), 0x02 },
+	{ CCI_REG8(0x9E24), 0x00 },
+	{ CCI_REG8(0x9E25), 0x8C },
+	{ CCI_REG8(0x9E26), 0x00 },
+	{ CCI_REG8(0x9E27), 0x94 },
+	{ CCI_REG8(0x9E28), 0x00 },
+	{ CCI_REG8(0x9E29), 0x96 },
 
 	/* CNR parameter setting */
-	{0x69DB, 0x01},
+	{ CCI_REG8(0x69DB), 0x01 },
 
 	/* Moire reduction */
-	{0x6957, 0x01},
+	{ CCI_REG8(0x6957), 0x01 },
 
 	/* image enhancement */
-	{0x6987, 0x17},
-	{0x698A, 0x03},
-	{0x698B, 0x03},
+	{ CCI_REG8(0x6987), 0x17 },
+	{ CCI_REG8(0x698A), 0x03 },
+	{ CCI_REG8(0x698B), 0x03 },
 
 	/* white balanace */
-	{0x0B8E, 0x01},
-	{0x0B8F, 0x00},
-	{0x0B90, 0x01},
-	{0x0B91, 0x00},
-	{0x0B92, 0x01},
-	{0x0B93, 0x00},
-	{0x0B94, 0x01},
-	{0x0B95, 0x00},
+	{ IMX214_REG_ABS_GAIN_GREENR, 0x0100 },
+	{ IMX214_REG_ABS_GAIN_RED, 0x0100 },
+	{ IMX214_REG_ABS_GAIN_BLUE, 0x0100 },
+	{ IMX214_REG_ABS_GAIN_GREENB, 0x0100 },
 
 	/* ATR setting */
-	{0x6E50, 0x00},
-	{0x6E51, 0x32},
-	{0x9340, 0x00},
-	{0x9341, 0x3C},
-	{0x9342, 0x03},
-	{0x9343, 0xFF},
-	{IMX214_TABLE_END, 0x00}
+	{ CCI_REG8(0x6E50), 0x00 },
+	{ CCI_REG8(0x6E51), 0x32 },
+	{ CCI_REG8(0x9340), 0x00 },
+	{ CCI_REG8(0x9341), 0x3C },
+	{ CCI_REG8(0x9342), 0x03 },
+	{ CCI_REG8(0x9343), 0xFF },
 };
 
 /*
@@ -427,16 +516,25 @@ static const struct reg_8 mode_table_common[] = {
 static const struct imx214_mode {
 	u32 width;
 	u32 height;
-	const struct reg_8 *reg_table;
+
+	/* V-timing */
+	unsigned int vts_def;
+
+	unsigned int num_of_regs;
+	const struct cci_reg_sequence *reg_table;
 } imx214_modes[] = {
 	{
 		.width = 4096,
 		.height = 2304,
+		.vts_def = 3194,
+		.num_of_regs = ARRAY_SIZE(mode_4096x2304),
 		.reg_table = mode_4096x2304,
 	},
 	{
 		.width = 1920,
 		.height = 1080,
+		.vts_def = 3194,
+		.num_of_regs = ARRAY_SIZE(mode_1920x1080),
 		.reg_table = mode_1920x1080,
 	},
 };
@@ -490,14 +588,42 @@ static int __maybe_unused imx214_power_off(struct device *dev)
 	return 0;
 }
 
+/* Get bayer order based on flip setting. */
+static u32 imx214_get_format_code(struct imx214 *imx214)
+{
+	unsigned int i;
+
+	i = (imx214->vflip->val ? 2 : 0) | (imx214->hflip->val ? 1 : 0);
+
+	return imx214_mbus_formats[i];
+}
+
+static void imx214_update_pad_format(struct imx214 *imx214,
+				     const struct imx214_mode *mode,
+				     struct v4l2_mbus_framefmt *fmt, u32 code)
+{
+	fmt->code = imx214_get_format_code(imx214);
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
+	fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
+							  fmt->colorspace,
+							  fmt->ycbcr_enc);
+	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
+}
+
 static int imx214_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_state *sd_state,
 				 struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (code->index > 0)
+	struct imx214 *imx214 = to_imx214(sd);
+
+	if (code->index >= (ARRAY_SIZE(imx214_mbus_formats) / 4))
 		return -EINVAL;
 
-	code->code = IMX214_MBUS_CODE;
+	code->code = imx214_get_format_code(imx214);
 
 	return 0;
 }
@@ -506,7 +632,11 @@ static int imx214_enum_frame_size(struct v4l2_subdev *subdev,
 				  struct v4l2_subdev_state *sd_state,
 				  struct v4l2_subdev_frame_size_enum *fse)
 {
-	if (fse->code != IMX214_MBUS_CODE)
+	struct imx214 *imx214 = to_imx214(subdev);
+	u32 code;
+
+	code = imx214_get_format_code(imx214);
+	if (fse->code != code)
 		return -EINVAL;
 
 	if (fse->index >= ARRAY_SIZE(imx214_modes))
@@ -549,52 +679,6 @@ static const struct v4l2_subdev_core_ops imx214_core_ops = {
 #endif
 };
 
-static struct v4l2_mbus_framefmt *
-__imx214_get_pad_format(struct imx214 *imx214,
-			struct v4l2_subdev_state *sd_state,
-			unsigned int pad,
-			enum v4l2_subdev_format_whence which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_state_get_format(sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &imx214->fmt;
-	default:
-		return NULL;
-	}
-}
-
-static int imx214_get_format(struct v4l2_subdev *sd,
-			     struct v4l2_subdev_state *sd_state,
-			     struct v4l2_subdev_format *format)
-{
-	struct imx214 *imx214 = to_imx214(sd);
-
-	mutex_lock(&imx214->mutex);
-	format->format = *__imx214_get_pad_format(imx214, sd_state,
-						  format->pad,
-						  format->which);
-	mutex_unlock(&imx214->mutex);
-
-	return 0;
-}
-
-static struct v4l2_rect *
-__imx214_get_pad_crop(struct imx214 *imx214,
-		      struct v4l2_subdev_state *sd_state,
-		      unsigned int pad, enum v4l2_subdev_format_whence which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_state_get_crop(sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &imx214->crop;
-	default:
-		return NULL;
-	}
-}
-
 static int imx214_set_format(struct v4l2_subdev *sd,
 			     struct v4l2_subdev_state *sd_state,
 			     struct v4l2_subdev_format *format)
@@ -604,34 +688,48 @@ static int imx214_set_format(struct v4l2_subdev *sd,
 	struct v4l2_rect *__crop;
 	const struct imx214_mode *mode;
 
-	mutex_lock(&imx214->mutex);
-
-	__crop = __imx214_get_pad_crop(imx214, sd_state, format->pad,
-				       format->which);
-
 	mode = v4l2_find_nearest_size(imx214_modes,
 				      ARRAY_SIZE(imx214_modes), width, height,
 				      format->format.width,
 				      format->format.height);
 
-	__crop->width = mode->width;
-	__crop->height = mode->height;
+	imx214_update_pad_format(imx214, mode, &format->format,
+				 format->format.code);
+	__format = v4l2_subdev_state_get_format(sd_state, 0);
 
-	__format = __imx214_get_pad_format(imx214, sd_state, format->pad,
-					   format->which);
-	__format->width = __crop->width;
-	__format->height = __crop->height;
-	__format->code = IMX214_MBUS_CODE;
-	__format->field = V4L2_FIELD_NONE;
-	__format->colorspace = V4L2_COLORSPACE_SRGB;
-	__format->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(__format->colorspace);
-	__format->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
-				__format->colorspace, __format->ycbcr_enc);
-	__format->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(__format->colorspace);
+	*__format = format->format;
 
-	format->format = *__format;
+	__crop = v4l2_subdev_state_get_crop(sd_state, 0);
+	__crop->width = mode->width;
+	__crop->height = mode->height;
 
-	mutex_unlock(&imx214->mutex);
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+		int exposure_max;
+		int exposure_def;
+		int hblank;
+
+		/* Update blank limits */
+		__v4l2_ctrl_modify_range(imx214->vblank, IMX214_VBLANK_MIN,
+					 IMX214_VTS_MAX - mode->height, 2,
+					 mode->vts_def - mode->height);
+
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = mode->vts_def - IMX214_EXPOSURE_OFFSET;
+		exposure_def = min(exposure_max, IMX214_EXPOSURE_DEFAULT);
+		__v4l2_ctrl_modify_range(imx214->exposure,
+					 imx214->exposure->minimum,
+					 exposure_max, imx214->exposure->step,
+					 exposure_def);
+
+		/*
+		 * Currently PPL is fixed to IMX214_PPL_DEFAULT, so hblank
+		 * depends on mode->width only, and is not changeable in any
+		 * way other than changing the mode.
+		 */
+		hblank = IMX214_PPL_DEFAULT - mode->width;
+		__v4l2_ctrl_modify_range(imx214->hblank, hblank, hblank, 1,
+					 hblank);
+	}
 
 	return 0;
 }
@@ -640,14 +738,9 @@ static int imx214_get_selection(struct v4l2_subdev *sd,
 				struct v4l2_subdev_state *sd_state,
 				struct v4l2_subdev_selection *sel)
 {
-	struct imx214 *imx214 = to_imx214(sd);
-
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP:
-		mutex_lock(&imx214->mutex);
-		sel->r = *__imx214_get_pad_crop(imx214, sd_state, sel->pad,
-						sel->which);
-		mutex_unlock(&imx214->mutex);
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
 		return 0;
 
 	case V4L2_SEL_TGT_NATIVE_SIZE:
@@ -675,6 +768,7 @@ static int imx214_entity_init_state(struct v4l2_subdev *subdev,
 	struct v4l2_subdev_format fmt = { };
 
 	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
+	fmt.format.code = MEDIA_BUS_FMT_SRGGB10_1X10;
 	fmt.format.width = imx214_modes[0].width;
 	fmt.format.height = imx214_modes[0].height;
 
@@ -683,12 +777,41 @@ static int imx214_entity_init_state(struct v4l2_subdev *subdev,
 	return 0;
 }
 
+static int imx214_update_digital_gain(struct imx214 *imx214, u32 val)
+{
+	int ret = 0;
+
+	cci_write(imx214->regmap, IMX214_REG_DIG_GAIN_GREENR, val, &ret);
+	cci_write(imx214->regmap, IMX214_REG_DIG_GAIN_RED, val, &ret);
+	cci_write(imx214->regmap, IMX214_REG_DIG_GAIN_BLUE, val, &ret);
+	cci_write(imx214->regmap, IMX214_REG_DIG_GAIN_GREENB, val, &ret);
+
+	return ret;
+}
+
 static int imx214_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct imx214 *imx214 = container_of(ctrl->handler,
 					     struct imx214, ctrls);
-	u8 vals[2];
-	int ret;
+	const struct v4l2_mbus_framefmt *format = NULL;
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		int exposure_max, exposure_def;
+
+		state = v4l2_subdev_get_locked_active_state(&imx214->sd);
+		format = v4l2_subdev_state_get_format(state, 0);
+
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max =
+			format->height + ctrl->val - IMX214_EXPOSURE_OFFSET;
+		exposure_def = min(exposure_max, IMX214_EXPOSURE_DEFAULT);
+		__v4l2_ctrl_modify_range(imx214->exposure,
+					 imx214->exposure->minimum,
+					 exposure_max, imx214->exposure->step,
+					 exposure_def);
+	}
 
 	/*
 	 * Applying V4L2 control value only happens
@@ -698,15 +821,47 @@ static int imx214_set_ctrl(struct v4l2_ctrl *ctrl)
 		return 0;
 
 	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		cci_write(imx214->regmap, IMX214_REG_ANALOG_GAIN,
+			  ctrl->val, &ret);
+		cci_write(imx214->regmap, IMX214_REG_SHORT_ANALOG_GAIN,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = imx214_update_digital_gain(imx214, ctrl->val);
+		break;
 	case V4L2_CID_EXPOSURE:
-		vals[1] = ctrl->val;
-		vals[0] = ctrl->val >> 8;
-		ret = regmap_bulk_write(imx214->regmap, IMX214_REG_EXPOSURE, vals, 2);
-		if (ret < 0)
-			dev_err(imx214->dev, "Error %d\n", ret);
-		ret = 0;
+		cci_write(imx214->regmap, IMX214_REG_EXPOSURE, ctrl->val, &ret);
+		break;
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		cci_write(imx214->regmap, IMX214_REG_ORIENTATION,
+			  imx214->hflip->val | imx214->vflip->val << 1, &ret);
+		break;
+	case V4L2_CID_VBLANK:
+		cci_write(imx214->regmap, IMX214_REG_FRM_LENGTH_LINES,
+			  format->height + ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		cci_write(imx214->regmap, IMX214_REG_TEST_PATTERN,
+			  imx214_test_pattern_val[ctrl->val], &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_RED:
+		cci_write(imx214->regmap, IMX214_REG_TESTP_RED,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		cci_write(imx214->regmap, IMX214_REG_TESTP_GREENR,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		cci_write(imx214->regmap, IMX214_REG_TESTP_BLUE,
+			  ctrl->val, &ret);
+		break;
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		cci_write(imx214->regmap, IMX214_REG_TESTP_GREENB,
+			  ctrl->val, &ret);
 		break;
-
 	default:
 		ret = -EINVAL;
 	}
@@ -729,16 +884,19 @@ static int imx214_ctrls_init(struct imx214 *imx214)
 		.width = 1120,
 		.height = 1120,
 	};
+	const struct imx214_mode *mode = &imx214_modes[0];
 	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl_handler *ctrl_hdlr;
-	int ret;
+	int exposure_max, exposure_def;
+	int hblank;
+	int i, ret;
 
 	ret = v4l2_fwnode_device_parse(imx214->dev, &props);
 	if (ret < 0)
 		return ret;
 
 	ctrl_hdlr = &imx214->ctrls;
-	ret = v4l2_ctrl_handler_init(&imx214->ctrls, 6);
+	ret = v4l2_ctrl_handler_init(&imx214->ctrls, 13);
 	if (ret)
 		return ret;
 
@@ -764,17 +922,75 @@ static int imx214_ctrls_init(struct imx214 *imx214)
 	 *
 	 * Yours sincerely, Ricardo.
 	 */
+
+	/* Initial vblank/hblank/exposure parameters based on current mode */
+	imx214->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
+					   V4L2_CID_VBLANK, IMX214_VBLANK_MIN,
+					   IMX214_VTS_MAX - mode->height, 2,
+					   mode->vts_def - mode->height);
+
+	hblank = IMX214_PPL_DEFAULT - mode->width;
+	imx214->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
+					   V4L2_CID_HBLANK, hblank, hblank,
+					   1, hblank);
+	if (imx214->hblank)
+		imx214->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	exposure_max = mode->vts_def - IMX214_EXPOSURE_OFFSET;
+	exposure_def = min(exposure_max, IMX214_EXPOSURE_DEFAULT);
 	imx214->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
 					     V4L2_CID_EXPOSURE,
 					     IMX214_EXPOSURE_MIN,
-					     IMX214_EXPOSURE_MAX,
+					     exposure_max,
 					     IMX214_EXPOSURE_STEP,
-					     IMX214_EXPOSURE_DEFAULT);
+					     exposure_def);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX214_ANA_GAIN_MIN, IMX214_ANA_GAIN_MAX,
+			  IMX214_ANA_GAIN_STEP, IMX214_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX214_DGTL_GAIN_MIN, IMX214_DGTL_GAIN_MAX,
+			  IMX214_DGTL_GAIN_STEP, IMX214_DGTL_GAIN_DEFAULT);
+
+	imx214->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (imx214->hflip)
+		imx214->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	imx214->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (imx214->vflip)
+		imx214->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_cluster(2, &imx214->hflip);
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx214_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx214_test_pattern_menu) - 1,
+				     0, 0, imx214_test_pattern_menu);
+	for (i = 0; i < 4; i++) {
+		/*
+		 * The assumption is that
+		 * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
+		 * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
+		 * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
+		 */
+		v4l2_ctrl_new_std(ctrl_hdlr, &imx214_ctrl_ops,
+				  V4L2_CID_TEST_PATTERN_RED + i,
+				  IMX214_TESTP_COLOUR_MIN,
+				  IMX214_TESTP_COLOUR_MAX,
+				  IMX214_TESTP_COLOUR_STEP,
+				  IMX214_TESTP_COLOUR_MAX);
+		/* The "Solid color" pattern is white by default */
+	}
 
 	imx214->unit_size = v4l2_ctrl_new_std_compound(ctrl_hdlr,
 				NULL,
 				V4L2_CID_UNIT_CELL_SIZE,
-				v4l2_ctrl_ptr_create((void *)&unit_size));
+				v4l2_ctrl_ptr_create((void *)&unit_size),
+				v4l2_ctrl_ptr_create(NULL),
+				v4l2_ctrl_ptr_create(NULL));
 
 	v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx214_ctrl_ops, &props);
 
@@ -790,76 +1006,52 @@ static int imx214_ctrls_init(struct imx214 *imx214)
 	return 0;
 };
 
-#define MAX_CMD 4
-static int imx214_write_table(struct imx214 *imx214,
-			      const struct reg_8 table[])
-{
-	u8 vals[MAX_CMD];
-	int i;
-	int ret;
-
-	for (; table->addr != IMX214_TABLE_END ; table++) {
-		if (table->addr == IMX214_TABLE_WAIT_MS) {
-			usleep_range(table->val * 1000,
-				     table->val * 1000 + 500);
-			continue;
-		}
-
-		for (i = 0; i < MAX_CMD; i++) {
-			if (table[i].addr != (table[0].addr + i))
-				break;
-			vals[i] = table[i].val;
-		}
-
-		ret = regmap_bulk_write(imx214->regmap, table->addr, vals, i);
-
-		if (ret) {
-			dev_err(imx214->dev, "write_table error: %d\n", ret);
-			return ret;
-		}
-
-		table += i - 1;
-	}
-
-	return 0;
-}
-
 static int imx214_start_streaming(struct imx214 *imx214)
 {
+	const struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_subdev_state *state;
 	const struct imx214_mode *mode;
 	int ret;
 
-	mutex_lock(&imx214->mutex);
-	ret = imx214_write_table(imx214, mode_table_common);
+	ret = cci_multi_reg_write(imx214->regmap, mode_table_common,
+				  ARRAY_SIZE(mode_table_common), NULL);
 	if (ret < 0) {
 		dev_err(imx214->dev, "could not sent common table %d\n", ret);
-		goto error;
+		return ret;
 	}
 
-	mode = v4l2_find_nearest_size(imx214_modes,
-				ARRAY_SIZE(imx214_modes), width, height,
-				imx214->fmt.width, imx214->fmt.height);
-	ret = imx214_write_table(imx214, mode->reg_table);
+	ret = cci_write(imx214->regmap, IMX214_REG_CSI_LANE_MODE,
+			IMX214_CSI_4_LANE_MODE, NULL);
+	if (ret) {
+		dev_err(imx214->dev, "failed to configure lanes\n");
+		return ret;
+	}
+
+	state = v4l2_subdev_get_locked_active_state(&imx214->sd);
+	fmt = v4l2_subdev_state_get_format(state, 0);
+	mode = v4l2_find_nearest_size(imx214_modes, ARRAY_SIZE(imx214_modes),
+				      width, height, fmt->width, fmt->height);
+	ret = cci_multi_reg_write(imx214->regmap, mode->reg_table,
+				  mode->num_of_regs, NULL);
 	if (ret < 0) {
 		dev_err(imx214->dev, "could not sent mode table %d\n", ret);
-		goto error;
+		return ret;
 	}
+
+	usleep_range(10000, 10500);
+
+	cci_write(imx214->regmap, IMX214_REG_TEMP_SENSOR_CONTROL, 0x01, NULL);
+
 	ret = __v4l2_ctrl_handler_setup(&imx214->ctrls);
 	if (ret < 0) {
 		dev_err(imx214->dev, "could not sync v4l2 controls\n");
-		goto error;
+		return ret;
 	}
-	ret = regmap_write(imx214->regmap, IMX214_REG_MODE_SELECT, IMX214_MODE_STREAMING);
-	if (ret < 0) {
+	ret = cci_write(imx214->regmap, IMX214_REG_MODE_SELECT,
+			IMX214_MODE_STREAMING, NULL);
+	if (ret < 0)
 		dev_err(imx214->dev, "could not sent start table %d\n", ret);
-		goto error;
-	}
 
-	mutex_unlock(&imx214->mutex);
-	return 0;
-
-error:
-	mutex_unlock(&imx214->mutex);
 	return ret;
 }
 
@@ -867,7 +1059,8 @@ static int imx214_stop_streaming(struct imx214 *imx214)
 {
 	int ret;
 
-	ret = regmap_write(imx214->regmap, IMX214_REG_MODE_SELECT, IMX214_MODE_STANDBY);
+	ret = cci_write(imx214->regmap, IMX214_REG_MODE_SELECT,
+			IMX214_MODE_STANDBY, NULL);
 	if (ret < 0)
 		dev_err(imx214->dev, "could not sent stop table %d\n",	ret);
 
@@ -877,6 +1070,7 @@ static int imx214_stop_streaming(struct imx214 *imx214)
 static int imx214_s_stream(struct v4l2_subdev *subdev, int enable)
 {
 	struct imx214 *imx214 = to_imx214(subdev);
+	struct v4l2_subdev_state *state;
 	int ret;
 
 	if (enable) {
@@ -884,7 +1078,9 @@ static int imx214_s_stream(struct v4l2_subdev *subdev, int enable)
 		if (ret < 0)
 			return ret;
 
+		state = v4l2_subdev_lock_and_get_active_state(subdev);
 		ret = imx214_start_streaming(imx214);
+		v4l2_subdev_unlock_state(state);
 		if (ret < 0)
 			goto err_rpm_put;
 	} else {
@@ -918,12 +1114,22 @@ static int imx214_get_frame_interval(struct v4l2_subdev *subdev,
 	return 0;
 }
 
+/*
+ * Raw sensors should be using the VBLANK and HBLANK controls to determine
+ * the frame rate. However this driver was initially added using the
+ * [S|G|ENUM]_FRAME_INTERVAL ioctls with a fixed rate of 30fps.
+ * Retain the frame_interval ops for backwards compatibility, but they do
+ * nothing.
+ */
 static int imx214_enum_frame_interval(struct v4l2_subdev *subdev,
 				struct v4l2_subdev_state *sd_state,
 				struct v4l2_subdev_frame_interval_enum *fie)
 {
+	struct imx214 *imx214 = to_imx214(subdev);
 	const struct imx214_mode *mode;
 
+	dev_warn_once(imx214->dev, "frame_interval functions return an unreliable value for compatibility reasons. Use the VBLANK and HBLANK controls to determine the correct frame rate.\n");
+
 	if (fie->index != 0)
 		return -EINVAL;
 
@@ -931,7 +1137,7 @@ static int imx214_enum_frame_interval(struct v4l2_subdev *subdev,
 				ARRAY_SIZE(imx214_modes), width, height,
 				fie->width, fie->height);
 
-	fie->code = IMX214_MBUS_CODE;
+	fie->code = imx214_get_format_code(imx214);
 	fie->width = mode->width;
 	fie->height = mode->height;
 	fie->interval.numerator = 1;
@@ -948,7 +1154,7 @@ static const struct v4l2_subdev_pad_ops imx214_subdev_pad_ops = {
 	.enum_mbus_code = imx214_enum_mbus_code,
 	.enum_frame_size = imx214_enum_frame_size,
 	.enum_frame_interval = imx214_enum_frame_interval,
-	.get_fmt = imx214_get_format,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = imx214_set_format,
 	.get_selection = imx214_get_selection,
 	.get_frame_interval = imx214_get_frame_interval,
@@ -965,12 +1171,6 @@ static const struct v4l2_subdev_internal_ops imx214_internal_ops = {
 	.init_state = imx214_entity_init_state,
 };
 
-static const struct regmap_config sensor_regmap_config = {
-	.reg_bits = 16,
-	.val_bits = 8,
-	.cache_type = REGCACHE_MAPLE,
-};
-
 static int imx214_get_regulators(struct device *dev, struct imx214 *imx214)
 {
 	unsigned int i;
@@ -982,6 +1182,27 @@ static int imx214_get_regulators(struct device *dev, struct imx214 *imx214)
 				       imx214->supplies);
 }
 
+/* Verify chip ID */
+static int imx214_identify_module(struct imx214 *imx214)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&imx214->sd);
+	int ret;
+	u64 val;
+
+	ret = cci_read(imx214->regmap, IMX214_REG_CHIP_ID, &val, NULL);
+	if (ret)
+		return dev_err_probe(&client->dev, ret,
+				     "failed to read chip id %x\n",
+				     IMX214_CHIP_ID);
+
+	if (val != IMX214_CHIP_ID)
+		return dev_err_probe(&client->dev, -EIO,
+				     "chip id mismatch: %x!=%llx\n",
+				     IMX214_CHIP_ID, val);
+
+	return 0;
+}
+
 static int imx214_parse_fwnode(struct device *dev)
 {
 	struct fwnode_handle *endpoint;
@@ -992,28 +1213,42 @@ static int imx214_parse_fwnode(struct device *dev)
 	int ret;
 
 	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
-	if (!endpoint) {
-		dev_err(dev, "endpoint node not found\n");
-		return -EINVAL;
-	}
+	if (!endpoint)
+		return dev_err_probe(dev, -EINVAL, "endpoint node not found\n");
 
 	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &bus_cfg);
 	if (ret) {
-		dev_err(dev, "parsing endpoint node failed\n");
+		dev_err_probe(dev, ret, "parsing endpoint node failed\n");
 		goto done;
 	}
 
-	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
+	/* Check the number of MIPI CSI2 data lanes */
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 4) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "only 4 data lanes are currently supported\n");
+		goto done;
+	}
+
+	if (bus_cfg.nr_of_link_frequencies != 1)
+		dev_warn(dev, "Only one link-frequency supported, please review your DT. Continuing anyway\n");
+
+	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++) {
 		if (bus_cfg.link_frequencies[i] == IMX214_DEFAULT_LINK_FREQ)
 			break;
-
-	if (i == bus_cfg.nr_of_link_frequencies) {
-		dev_err(dev, "link-frequencies %d not supported, Please review your DT\n",
-			IMX214_DEFAULT_LINK_FREQ);
-		ret = -EINVAL;
-		goto done;
+		if (bus_cfg.link_frequencies[i] ==
+		    IMX214_DEFAULT_LINK_FREQ_LEGACY) {
+			dev_warn(dev,
+				 "link-frequencies %d not supported, please review your DT. Continuing anyway\n",
+				 IMX214_DEFAULT_LINK_FREQ);
+			break;
+		}
 	}
 
+	if (i == bus_cfg.nr_of_link_frequencies)
+		ret = dev_err_probe(dev, -EINVAL,
+				    "link-frequencies %d not supported, please review your DT\n",
+				    IMX214_DEFAULT_LINK_FREQ);
+
 done:
 	v4l2_fwnode_endpoint_free(&bus_cfg);
 	fwnode_handle_put(endpoint);
@@ -1037,34 +1272,28 @@ static int imx214_probe(struct i2c_client *client)
 	imx214->dev = dev;
 
 	imx214->xclk = devm_clk_get(dev, NULL);
-	if (IS_ERR(imx214->xclk)) {
-		dev_err(dev, "could not get xclk");
-		return PTR_ERR(imx214->xclk);
-	}
+	if (IS_ERR(imx214->xclk))
+		return dev_err_probe(dev, PTR_ERR(imx214->xclk),
+				     "failed to get xclk\n");
 
 	ret = clk_set_rate(imx214->xclk, IMX214_DEFAULT_CLK_FREQ);
-	if (ret) {
-		dev_err(dev, "could not set xclk frequency\n");
-		return ret;
-	}
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "failed to set xclk frequency\n");
 
 	ret = imx214_get_regulators(dev, imx214);
-	if (ret < 0) {
-		dev_err(dev, "cannot get regulators\n");
-		return ret;
-	}
+	if (ret < 0)
+		return dev_err_probe(dev, ret, "failed to get regulators\n");
 
 	imx214->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_LOW);
-	if (IS_ERR(imx214->enable_gpio)) {
-		dev_err(dev, "cannot get enable gpio\n");
-		return PTR_ERR(imx214->enable_gpio);
-	}
+	if (IS_ERR(imx214->enable_gpio))
+		return dev_err_probe(dev, PTR_ERR(imx214->enable_gpio),
+				     "failed to get enable gpio\n");
 
-	imx214->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
-	if (IS_ERR(imx214->regmap)) {
-		dev_err(dev, "regmap init failed\n");
-		return PTR_ERR(imx214->regmap);
-	}
+	imx214->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx214->regmap))
+		return dev_err_probe(dev, PTR_ERR(imx214->regmap),
+				     "failed to initialize CCI\n");
 
 	v4l2_i2c_subdev_init(&imx214->sd, client, &imx214_subdev_ops);
 	imx214->sd.internal_ops = &imx214_internal_ops;
@@ -1075,17 +1304,14 @@ static int imx214_probe(struct i2c_client *client)
 	 */
 	imx214_power_on(imx214->dev);
 
-	pm_runtime_set_active(imx214->dev);
-	pm_runtime_enable(imx214->dev);
-	pm_runtime_idle(imx214->dev);
+	ret = imx214_identify_module(imx214);
+	if (ret)
+		goto error_power_off;
 
 	ret = imx214_ctrls_init(imx214);
 	if (ret < 0)
 		goto error_power_off;
 
-	mutex_init(&imx214->mutex);
-	imx214->ctrls.lock = &imx214->mutex;
-
 	imx214->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	imx214->pad.flags = MEDIA_PAD_FL_SOURCE;
 	imx214->sd.dev = &client->dev;
@@ -1093,27 +1319,44 @@ static int imx214_probe(struct i2c_client *client)
 
 	ret = media_entity_pads_init(&imx214->sd.entity, 1, &imx214->pad);
 	if (ret < 0) {
-		dev_err(dev, "could not register media entity\n");
+		dev_err_probe(dev, ret, "failed to init entity pads\n");
 		goto free_ctrl;
 	}
 
-	imx214_entity_init_state(&imx214->sd, NULL);
+	imx214->sd.state_lock = imx214->ctrls.lock;
+	ret = v4l2_subdev_init_finalize(&imx214->sd);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "subdev init error\n");
+		goto free_entity;
+	}
+
+	pm_runtime_set_active(imx214->dev);
+	pm_runtime_enable(imx214->dev);
 
 	ret = v4l2_async_register_subdev_sensor(&imx214->sd);
 	if (ret < 0) {
-		dev_err(dev, "could not register v4l2 device\n");
-		goto free_entity;
+		dev_err_probe(dev, ret,
+			      "failed to register sensor sub-device\n");
+		goto error_subdev_cleanup;
 	}
 
+	pm_runtime_idle(imx214->dev);
+
 	return 0;
 
+error_subdev_cleanup:
+	pm_runtime_disable(imx214->dev);
+	pm_runtime_set_suspended(&client->dev);
+	v4l2_subdev_cleanup(&imx214->sd);
+
 free_entity:
 	media_entity_cleanup(&imx214->sd.entity);
+
 free_ctrl:
-	mutex_destroy(&imx214->mutex);
 	v4l2_ctrl_handler_free(&imx214->ctrls);
+
 error_power_off:
-	pm_runtime_disable(imx214->dev);
+	imx214_power_off(imx214->dev);
 
 	return ret;
 }
@@ -1124,13 +1367,14 @@ static void imx214_remove(struct i2c_client *client)
 	struct imx214 *imx214 = to_imx214(sd);
 
 	v4l2_async_unregister_subdev(&imx214->sd);
+	v4l2_subdev_cleanup(sd);
 	media_entity_cleanup(&imx214->sd.entity);
 	v4l2_ctrl_handler_free(&imx214->ctrls);
-
 	pm_runtime_disable(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
-
-	mutex_destroy(&imx214->mutex);
+	if (!pm_runtime_status_suspended(&client->dev)) {
+		imx214_power_off(imx214->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
 }
 
 static const struct of_device_id imx214_of_match[] = {
diff --git a/drivers/media/i2c/imx219.c b/drivers/media/i2c/imx219.c
index e17ef2e9d9d0..3b4f68543342 100644
--- a/drivers/media/i2c/imx219.c
+++ b/drivers/media/i2c/imx219.c
@@ -26,7 +26,6 @@
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mediabus.h>
 
@@ -71,15 +70,14 @@
 #define IMX219_EXPOSURE_MAX		65535
 
 /* V_TIMING internal */
-#define IMX219_REG_VTS			CCI_REG16(0x0160)
-#define IMX219_VTS_MAX			0xffff
-
-#define IMX219_VBLANK_MIN		4
-
-/* HBLANK control - read only */
-#define IMX219_PPL_DEFAULT		3448
-
+#define IMX219_REG_FRM_LENGTH_A		CCI_REG16(0x0160)
+#define IMX219_FLL_MAX			0xffff
+#define IMX219_VBLANK_MIN		32
 #define IMX219_REG_LINE_LENGTH_A	CCI_REG16(0x0162)
+#define IMX219_LLP_MIN			0x0d78
+#define IMX219_BINNED_LLP_MIN		0x0de8
+#define IMX219_LLP_MAX			0x7ff0
+
 #define IMX219_REG_X_ADD_STA_A		CCI_REG16(0x0164)
 #define IMX219_REG_X_ADD_END_A		CCI_REG16(0x0166)
 #define IMX219_REG_Y_ADD_STA_A		CCI_REG16(0x0168)
@@ -134,10 +132,11 @@
 
 /* Pixel rate is fixed for all the modes */
 #define IMX219_PIXEL_RATE		182400000
-#define IMX219_PIXEL_RATE_4LANE		280800000
+#define IMX219_PIXEL_RATE_4LANE		281600000
 
 #define IMX219_DEFAULT_LINK_FREQ	456000000
-#define IMX219_DEFAULT_LINK_FREQ_4LANE	363000000
+#define IMX219_DEFAULT_LINK_FREQ_4LANE_UNSUPPORTED	363000000
+#define IMX219_DEFAULT_LINK_FREQ_4LANE	364000000
 
 /* IMX219 native and active pixel array size. */
 #define IMX219_NATIVE_WIDTH		3296U
@@ -155,29 +154,20 @@ struct imx219_mode {
 	unsigned int height;
 
 	/* V-timing */
-	unsigned int vts_def;
+	unsigned int fll_def;
 };
 
 static const struct cci_reg_sequence imx219_common_regs[] = {
 	{ IMX219_REG_MODE_SELECT, 0x00 },	/* Mode Select */
 
 	/* To Access Addresses 3000-5fff, send the following commands */
-	{ CCI_REG8(0x30eb), 0x0c },
 	{ CCI_REG8(0x30eb), 0x05 },
+	{ CCI_REG8(0x30eb), 0x0c },
 	{ CCI_REG8(0x300a), 0xff },
 	{ CCI_REG8(0x300b), 0xff },
 	{ CCI_REG8(0x30eb), 0x05 },
 	{ CCI_REG8(0x30eb), 0x09 },
 
-	/* PLL Clock Table */
-	{ IMX219_REG_VTPXCK_DIV, 5 },
-	{ IMX219_REG_VTSYCK_DIV, 1 },
-	{ IMX219_REG_PREPLLCK_VT_DIV, 3 },	/* 0x03 = AUTO set */
-	{ IMX219_REG_PREPLLCK_OP_DIV, 3 },	/* 0x03 = AUTO set */
-	{ IMX219_REG_PLL_VT_MPY, 57 },
-	{ IMX219_REG_OPSYCK_DIV, 1 },
-	{ IMX219_REG_PLL_OP_MPY, 114 },
-
 	/* Undocumented registers */
 	{ CCI_REG8(0x455e), 0x00 },
 	{ CCI_REG8(0x471e), 0x4b },
@@ -193,7 +183,6 @@ static const struct cci_reg_sequence imx219_common_regs[] = {
 	{ CCI_REG8(0x479b), 0x0e },
 
 	/* Frame Bank Register Group "A" */
-	{ IMX219_REG_LINE_LENGTH_A, 3448 },
 	{ IMX219_REG_X_ODD_INC_A, 1 },
 	{ IMX219_REG_Y_ODD_INC_A, 1 },
 
@@ -202,12 +191,45 @@ static const struct cci_reg_sequence imx219_common_regs[] = {
 	{ IMX219_REG_EXCK_FREQ, IMX219_EXCK_FREQ(IMX219_XCLK_FREQ / 1000000) },
 };
 
+static const struct cci_reg_sequence imx219_2lane_regs[] = {
+	/* PLL Clock Table */
+	{ IMX219_REG_VTPXCK_DIV, 5 },
+	{ IMX219_REG_VTSYCK_DIV, 1 },
+	{ IMX219_REG_PREPLLCK_VT_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PREPLLCK_OP_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PLL_VT_MPY, 57 },
+	{ IMX219_REG_OPSYCK_DIV, 1 },
+	{ IMX219_REG_PLL_OP_MPY, 114 },
+
+	/* 2-Lane CSI Mode */
+	{ IMX219_REG_CSI_LANE_MODE, IMX219_CSI_2_LANE_MODE },
+};
+
+static const struct cci_reg_sequence imx219_4lane_regs[] = {
+	/* PLL Clock Table */
+	{ IMX219_REG_VTPXCK_DIV, 5 },
+	{ IMX219_REG_VTSYCK_DIV, 1 },
+	{ IMX219_REG_PREPLLCK_VT_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PREPLLCK_OP_DIV, 3 },	/* 0x03 = AUTO set */
+	{ IMX219_REG_PLL_VT_MPY, 88 },
+	{ IMX219_REG_OPSYCK_DIV, 1 },
+	{ IMX219_REG_PLL_OP_MPY, 91 },
+
+	/* 4-Lane CSI Mode */
+	{ IMX219_REG_CSI_LANE_MODE, IMX219_CSI_4_LANE_MODE },
+};
+
 static const s64 imx219_link_freq_menu[] = {
 	IMX219_DEFAULT_LINK_FREQ,
 };
 
 static const s64 imx219_link_freq_4lane_menu[] = {
 	IMX219_DEFAULT_LINK_FREQ_4LANE,
+	/*
+	 * This will never be advertised to userspace, but will be used for
+	 * v4l2_link_freq_to_bitmap
+	 */
+	IMX219_DEFAULT_LINK_FREQ_4LANE_UNSUPPORTED,
 };
 
 static const char * const imx219_test_pattern_menu[] = {
@@ -290,25 +312,25 @@ static const struct imx219_mode supported_modes[] = {
 		/* 8MPix 15fps mode */
 		.width = 3280,
 		.height = 2464,
-		.vts_def = 3526,
+		.fll_def = 3526,
 	},
 	{
 		/* 1080P 30fps cropped */
 		.width = 1920,
 		.height = 1080,
-		.vts_def = 1763,
+		.fll_def = 1763,
 	},
 	{
-		/* 2x2 binned 30fps mode */
+		/* 2x2 binned 60fps mode */
 		.width = 1640,
 		.height = 1232,
-		.vts_def = 1763,
+		.fll_def = 1707,
 	},
 	{
-		/* 640x480 30fps mode */
+		/* 640x480 60fps mode */
 		.width = 640,
 		.height = 480,
-		.vts_def = 1763,
+		.fll_def = 1707,
 	},
 };
 
@@ -360,6 +382,62 @@ static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
 	return imx219_mbus_formats[i];
 }
 
+static u32 imx219_get_format_bpp(const struct v4l2_mbus_framefmt *format)
+{
+	switch (format->code) {
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+		return 8;
+
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	default:
+		return 10;
+	}
+}
+
+static void imx219_get_binning(struct v4l2_subdev_state *state, u8 *bin_h,
+			       u8 *bin_v)
+{
+	const struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+	u32 hbin = crop->width / format->width;
+	u32 vbin = crop->height / format->height;
+
+	*bin_h = IMX219_BINNING_NONE;
+	*bin_v = IMX219_BINNING_NONE;
+
+	/*
+	 * Use analog binning only if both dimensions are binned, as it crops
+	 * the other dimension.
+	 */
+	if (hbin == 2 && vbin == 2) {
+		*bin_h = IMX219_BINNING_X2_ANALOG;
+		*bin_v = IMX219_BINNING_X2_ANALOG;
+
+		return;
+	}
+
+	if (hbin == 2)
+		*bin_h = IMX219_BINNING_X2;
+	if (vbin == 2)
+		*bin_v = IMX219_BINNING_X2;
+}
+
+static inline u32 imx219_get_rate_factor(struct v4l2_subdev_state *state)
+{
+	u8 bin_h, bin_v;
+
+	imx219_get_binning(state, &bin_h, &bin_v);
+
+	return (bin_h & bin_v) == IMX219_BINNING_X2_ANALOG ? 2 : 1;
+}
+
 /* -----------------------------------------------------------------------------
  * Controls
  */
@@ -371,10 +449,12 @@ static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	const struct v4l2_mbus_framefmt *format;
 	struct v4l2_subdev_state *state;
+	u32 rate_factor;
 	int ret = 0;
 
 	state = v4l2_subdev_get_locked_active_state(&imx219->sd);
 	format = v4l2_subdev_state_get_format(state, 0);
+	rate_factor = imx219_get_rate_factor(state);
 
 	if (ctrl->id == V4L2_CID_VBLANK) {
 		int exposure_max, exposure_def;
@@ -403,7 +483,7 @@ static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 	case V4L2_CID_EXPOSURE:
 		cci_write(imx219->regmap, IMX219_REG_EXPOSURE,
-			  ctrl->val, &ret);
+			  ctrl->val / rate_factor, &ret);
 		break;
 	case V4L2_CID_DIGITAL_GAIN:
 		cci_write(imx219->regmap, IMX219_REG_DIGITAL_GAIN,
@@ -419,8 +499,12 @@ static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 			  imx219->hflip->val | imx219->vflip->val << 1, &ret);
 		break;
 	case V4L2_CID_VBLANK:
-		cci_write(imx219->regmap, IMX219_REG_VTS,
-			  format->height + ctrl->val, &ret);
+		cci_write(imx219->regmap, IMX219_REG_FRM_LENGTH_A,
+			  (format->height + ctrl->val) / rate_factor, &ret);
+		break;
+	case V4L2_CID_HBLANK:
+		cci_write(imx219->regmap, IMX219_REG_LINE_LENGTH_A,
+			  format->width + ctrl->val, &ret);
 		break;
 	case V4L2_CID_TEST_PATTERN_RED:
 		cci_write(imx219->regmap, IMX219_REG_TESTP_RED,
@@ -467,7 +551,7 @@ static int imx219_init_controls(struct imx219 *imx219)
 	const struct imx219_mode *mode = &supported_modes[0];
 	struct v4l2_ctrl_handler *ctrl_hdlr;
 	struct v4l2_fwnode_device_properties props;
-	int exposure_max, exposure_def, hblank;
+	int exposure_max, exposure_def;
 	int i, ret;
 
 	ctrl_hdlr = &imx219->ctrl_handler;
@@ -491,18 +575,17 @@ static int imx219_init_controls(struct imx219 *imx219)
 	if (imx219->link_freq)
 		imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-	/* Initial vblank/hblank/exposure parameters based on current mode */
+	/* Initial blanking and exposure. Limits are updated during set_fmt */
 	imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
 					   V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
-					   IMX219_VTS_MAX - mode->height, 1,
-					   mode->vts_def - mode->height);
-	hblank = IMX219_PPL_DEFAULT - mode->width;
+					   IMX219_FLL_MAX - mode->height, 1,
+					   mode->fll_def - mode->height);
 	imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
-					   V4L2_CID_HBLANK, hblank, hblank,
-					   1, hblank);
-	if (imx219->hblank)
-		imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-	exposure_max = mode->vts_def - 4;
+					   V4L2_CID_HBLANK,
+					   IMX219_LLP_MIN - mode->width,
+					   IMX219_LLP_MAX - mode->width, 1,
+					   IMX219_LLP_MIN - mode->width);
+	exposure_max = mode->fll_def - 4;
 	exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 		exposure_max : IMX219_EXPOSURE_DEFAULT;
 	imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
@@ -551,8 +634,7 @@ static int imx219_init_controls(struct imx219 *imx219)
 
 	if (ctrl_hdlr->error) {
 		ret = ctrl_hdlr->error;
-		dev_err(&client->dev, "%s control init failed (%d)\n",
-			__func__, ret);
+		dev_err_probe(&client->dev, ret, "Control init failed\n");
 		goto error;
 	}
 
@@ -589,29 +671,13 @@ static int imx219_set_framefmt(struct imx219 *imx219,
 {
 	const struct v4l2_mbus_framefmt *format;
 	const struct v4l2_rect *crop;
-	unsigned int bpp;
-	u64 bin_h, bin_v;
+	u8 bin_h, bin_v;
+	u32 bpp;
 	int ret = 0;
 
 	format = v4l2_subdev_state_get_format(state, 0);
 	crop = v4l2_subdev_state_get_crop(state, 0);
-
-	switch (format->code) {
-	case MEDIA_BUS_FMT_SRGGB8_1X8:
-	case MEDIA_BUS_FMT_SGRBG8_1X8:
-	case MEDIA_BUS_FMT_SGBRG8_1X8:
-	case MEDIA_BUS_FMT_SBGGR8_1X8:
-		bpp = 8;
-		break;
-
-	case MEDIA_BUS_FMT_SRGGB10_1X10:
-	case MEDIA_BUS_FMT_SGRBG10_1X10:
-	case MEDIA_BUS_FMT_SGBRG10_1X10:
-	case MEDIA_BUS_FMT_SBGGR10_1X10:
-	default:
-		bpp = 10;
-		break;
-	}
+	bpp = imx219_get_format_bpp(format);
 
 	cci_write(imx219->regmap, IMX219_REG_X_ADD_STA_A,
 		  crop->left - IMX219_PIXEL_ARRAY_LEFT, &ret);
@@ -622,26 +688,7 @@ static int imx219_set_framefmt(struct imx219 *imx219,
 	cci_write(imx219->regmap, IMX219_REG_Y_ADD_END_A,
 		  crop->top - IMX219_PIXEL_ARRAY_TOP + crop->height - 1, &ret);
 
-	switch (crop->width / format->width) {
-	case 1:
-	default:
-		bin_h = IMX219_BINNING_NONE;
-		break;
-	case 2:
-		bin_h = bpp == 8 ? IMX219_BINNING_X2_ANALOG : IMX219_BINNING_X2;
-		break;
-	}
-
-	switch (crop->height / format->height) {
-	case 1:
-	default:
-		bin_v = IMX219_BINNING_NONE;
-		break;
-	case 2:
-		bin_v = bpp == 8 ? IMX219_BINNING_X2_ANALOG : IMX219_BINNING_X2;
-		break;
-	}
-
+	imx219_get_binning(state, &bin_h, &bin_v);
 	cci_write(imx219->regmap, IMX219_REG_BINNING_MODE_H, bin_h, &ret);
 	cci_write(imx219->regmap, IMX219_REG_BINNING_MODE_V, bin_v, &ret);
 
@@ -664,14 +711,18 @@ static int imx219_set_framefmt(struct imx219 *imx219,
 
 static int imx219_configure_lanes(struct imx219 *imx219)
 {
-	return cci_write(imx219->regmap, IMX219_REG_CSI_LANE_MODE,
-			 imx219->lanes == 2 ? IMX219_CSI_2_LANE_MODE :
-			 IMX219_CSI_4_LANE_MODE, NULL);
+	/* Write the appropriate PLL settings for the number of MIPI lanes */
+	return cci_multi_reg_write(imx219->regmap,
+				  imx219->lanes == 2 ? imx219_2lane_regs : imx219_4lane_regs,
+				  imx219->lanes == 2 ? ARRAY_SIZE(imx219_2lane_regs) :
+				  ARRAY_SIZE(imx219_4lane_regs), NULL);
 };
 
-static int imx219_start_streaming(struct imx219 *imx219,
-				  struct v4l2_subdev_state *state)
+static int imx219_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
 {
+	struct imx219 *imx219 = to_imx219(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	int ret;
 
@@ -720,12 +771,16 @@ static int imx219_start_streaming(struct imx219 *imx219,
 	return 0;
 
 err_rpm_put:
-	pm_runtime_put(&client->dev);
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
 	return ret;
 }
 
-static void imx219_stop_streaming(struct imx219 *imx219)
+static int imx219_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
 {
+	struct imx219 *imx219 = to_imx219(sd);
 	struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 	int ret;
 
@@ -738,23 +793,9 @@ static void imx219_stop_streaming(struct imx219 *imx219)
 	__v4l2_ctrl_grab(imx219->vflip, false);
 	__v4l2_ctrl_grab(imx219->hflip, false);
 
-	pm_runtime_put(&client->dev);
-}
-
-static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct imx219 *imx219 = to_imx219(sd);
-	struct v4l2_subdev_state *state;
-	int ret = 0;
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
 
-	state = v4l2_subdev_lock_and_get_active_state(sd);
-
-	if (enable)
-		ret = imx219_start_streaming(imx219, state);
-	else
-		imx219_stop_streaming(imx219);
-
-	v4l2_subdev_unlock_state(state);
 	return ret;
 }
 
@@ -817,7 +858,11 @@ static int imx219_set_pad_format(struct v4l2_subdev *sd,
 	const struct imx219_mode *mode;
 	struct v4l2_mbus_framefmt *format;
 	struct v4l2_rect *crop;
-	unsigned int bin_h, bin_v;
+	u8 bin_h, bin_v;
+	u32 prev_line_len;
+
+	format = v4l2_subdev_state_get_format(state, 0);
+	prev_line_len = format->width + imx219->hblank->val;
 
 	mode = v4l2_find_nearest_size(supported_modes,
 				      ARRAY_SIZE(supported_modes),
@@ -825,8 +870,6 @@ static int imx219_set_pad_format(struct v4l2_subdev *sd,
 				      fmt->format.width, fmt->format.height);
 
 	imx219_update_pad_format(imx219, mode, &fmt->format, fmt->format.code);
-
-	format = v4l2_subdev_state_get_format(state, 0);
 	*format = fmt->format;
 
 	/*
@@ -845,30 +888,51 @@ static int imx219_set_pad_format(struct v4l2_subdev *sd,
 	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 		int exposure_max;
 		int exposure_def;
-		int hblank;
+		int hblank, llp_min;
+		int pixel_rate;
 
 		/* Update limits and set FPS to default */
 		__v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
-					 IMX219_VTS_MAX - mode->height, 1,
-					 mode->vts_def - mode->height);
+					 IMX219_FLL_MAX - mode->height, 1,
+					 mode->fll_def - mode->height);
 		__v4l2_ctrl_s_ctrl(imx219->vblank,
-				   mode->vts_def - mode->height);
+				   mode->fll_def - mode->height);
 		/* Update max exposure while meeting expected vblanking */
-		exposure_max = mode->vts_def - 4;
+		exposure_max = mode->fll_def - 4;
 		exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 			exposure_max : IMX219_EXPOSURE_DEFAULT;
 		__v4l2_ctrl_modify_range(imx219->exposure,
 					 imx219->exposure->minimum,
 					 exposure_max, imx219->exposure->step,
 					 exposure_def);
+
+		/*
+		 * With analog binning the default minimum line length of 3448
+		 * can cause artefacts with RAW10 formats, because the ADC
+		 * operates on two lines together. So we switch to a higher
+		 * minimum of 3560.
+		 */
+		imx219_get_binning(state, &bin_h, &bin_v);
+		llp_min = (bin_h & bin_v) == IMX219_BINNING_X2_ANALOG ?
+				  IMX219_BINNED_LLP_MIN : IMX219_LLP_MIN;
+		__v4l2_ctrl_modify_range(imx219->hblank, llp_min - mode->width,
+					 IMX219_LLP_MAX - mode->width, 1,
+					 llp_min - mode->width);
 		/*
-		 * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
-		 * depends on mode->width only, and is not changeble in any
-		 * way other than changing the mode.
+		 * Retain PPL setting from previous mode so that the
+		 * line time does not change on a mode change.
+		 * Limits have to be recomputed as the controls define
+		 * the blanking only, so PPL values need to have the
+		 * mode width subtracted.
 		 */
-		hblank = IMX219_PPL_DEFAULT - mode->width;
-		__v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1,
-					 hblank);
+		hblank = prev_line_len - mode->width;
+		__v4l2_ctrl_s_ctrl(imx219->hblank, hblank);
+
+		/* Scale the pixel rate based on the mode specific factor */
+		pixel_rate = imx219_get_pixel_rate(imx219) *
+			     imx219_get_rate_factor(state);
+		__v4l2_ctrl_modify_range(imx219->pixel_rate, pixel_rate,
+					 pixel_rate, 1, pixel_rate);
 	}
 
 	return 0;
@@ -879,10 +943,9 @@ static int imx219_get_selection(struct v4l2_subdev *sd,
 				struct v4l2_subdev_selection *sel)
 {
 	switch (sel->target) {
-	case V4L2_SEL_TGT_CROP: {
+	case V4L2_SEL_TGT_CROP:
 		sel->r = *v4l2_subdev_state_get_crop(state, 0);
 		return 0;
-	}
 
 	case V4L2_SEL_TGT_NATIVE_SIZE:
 		sel->r.top = 0;
@@ -923,13 +986,8 @@ static int imx219_init_state(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static const struct v4l2_subdev_core_ops imx219_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
 static const struct v4l2_subdev_video_ops imx219_video_ops = {
-	.s_stream = imx219_set_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
@@ -938,10 +996,11 @@ static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
 	.set_fmt = imx219_set_pad_format,
 	.get_selection = imx219_get_selection,
 	.enum_frame_size = imx219_enum_frame_size,
+	.enable_streams = imx219_enable_streams,
+	.disable_streams = imx219_disable_streams,
 };
 
 static const struct v4l2_subdev_ops imx219_subdev_ops = {
-	.core = &imx219_core_ops,
 	.video = &imx219_video_ops,
 	.pad = &imx219_pad_ops,
 };
@@ -1024,17 +1083,15 @@ static int imx219_identify_module(struct imx219 *imx219)
 	u64 val;
 
 	ret = cci_read(imx219->regmap, IMX219_REG_CHIP_ID, &val, NULL);
-	if (ret) {
-		dev_err(&client->dev, "failed to read chip id %x\n",
-			IMX219_CHIP_ID);
-		return ret;
-	}
+	if (ret)
+		return dev_err_probe(&client->dev, ret,
+				     "failed to read chip id %x\n",
+				     IMX219_CHIP_ID);
 
-	if (val != IMX219_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%llx\n",
-			IMX219_CHIP_ID, val);
-		return -EIO;
-	}
+	if (val != IMX219_CHIP_ID)
+		return dev_err_probe(&client->dev, -EIO,
+				     "chip id mismatch: %x!=%llx\n",
+				     IMX219_CHIP_ID, val);
 
 	return 0;
 }
@@ -1045,43 +1102,62 @@ static int imx219_check_hwcfg(struct device *dev, struct imx219 *imx219)
 	struct v4l2_fwnode_endpoint ep_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY
 	};
+	unsigned long link_freq_bitmap;
 	int ret = -EINVAL;
 
 	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
-	if (!endpoint) {
-		dev_err(dev, "endpoint node not found\n");
-		return -EINVAL;
-	}
+	if (!endpoint)
+		return dev_err_probe(dev, -EINVAL, "endpoint node not found\n");
 
 	if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
-		dev_err(dev, "could not parse endpoint\n");
+		dev_err_probe(dev, -EINVAL, "could not parse endpoint\n");
 		goto error_out;
 	}
 
 	/* Check the number of MIPI CSI2 data lanes */
 	if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2 &&
 	    ep_cfg.bus.mipi_csi2.num_data_lanes != 4) {
-		dev_err(dev, "only 2 or 4 data lanes are currently supported\n");
+		dev_err_probe(dev, -EINVAL,
+			      "only 2 or 4 data lanes are currently supported\n");
 		goto error_out;
 	}
 	imx219->lanes = ep_cfg.bus.mipi_csi2.num_data_lanes;
 
 	/* Check the link frequency set in device tree */
-	if (!ep_cfg.nr_of_link_frequencies) {
-		dev_err(dev, "link-frequency property not found in DT\n");
-		goto error_out;
+	switch (imx219->lanes) {
+	case 2:
+		ret = v4l2_link_freq_to_bitmap(dev,
+					       ep_cfg.link_frequencies,
+					       ep_cfg.nr_of_link_frequencies,
+					       imx219_link_freq_menu,
+					       ARRAY_SIZE(imx219_link_freq_menu),
+					       &link_freq_bitmap);
+		break;
+	case 4:
+		ret = v4l2_link_freq_to_bitmap(dev,
+					       ep_cfg.link_frequencies,
+					       ep_cfg.nr_of_link_frequencies,
+					       imx219_link_freq_4lane_menu,
+					       ARRAY_SIZE(imx219_link_freq_4lane_menu),
+					       &link_freq_bitmap);
+
+		if (!ret && (link_freq_bitmap & BIT(1))) {
+			dev_warn(dev, "Link frequency of %d not supported, but has been incorrectly advertised previously\n",
+				 IMX219_DEFAULT_LINK_FREQ_4LANE_UNSUPPORTED);
+			dev_warn(dev, "Using link frequency of %d\n",
+				 IMX219_DEFAULT_LINK_FREQ_4LANE);
+			link_freq_bitmap |= BIT(0);
+		}
+		break;
 	}
 
-	if (ep_cfg.nr_of_link_frequencies != 1 ||
-	   (ep_cfg.link_frequencies[0] != ((imx219->lanes == 2) ?
-	    IMX219_DEFAULT_LINK_FREQ : IMX219_DEFAULT_LINK_FREQ_4LANE))) {
-		dev_err(dev, "Link frequency not supported: %lld\n",
-			ep_cfg.link_frequencies[0]);
-		goto error_out;
+	if (ret || !(link_freq_bitmap & BIT(0))) {
+		ret = -EINVAL;
+		dev_err_probe(dev, -EINVAL,
+			      "Link frequency not supported: %lld\n",
+			      ep_cfg.link_frequencies[0]);
 	}
 
-	ret = 0;
-
 error_out:
 	v4l2_fwnode_endpoint_free(&ep_cfg);
 	fwnode_handle_put(endpoint);
@@ -1107,31 +1183,25 @@ static int imx219_probe(struct i2c_client *client)
 		return -EINVAL;
 
 	imx219->regmap = devm_cci_regmap_init_i2c(client, 16);
-	if (IS_ERR(imx219->regmap)) {
-		ret = PTR_ERR(imx219->regmap);
-		dev_err(dev, "failed to initialize CCI: %d\n", ret);
-		return ret;
-	}
+	if (IS_ERR(imx219->regmap))
+		return dev_err_probe(dev, PTR_ERR(imx219->regmap),
+				     "failed to initialize CCI\n");
 
 	/* Get system clock (xclk) */
 	imx219->xclk = devm_clk_get(dev, NULL);
-	if (IS_ERR(imx219->xclk)) {
-		dev_err(dev, "failed to get xclk\n");
-		return PTR_ERR(imx219->xclk);
-	}
+	if (IS_ERR(imx219->xclk))
+		return dev_err_probe(dev, PTR_ERR(imx219->xclk),
+				     "failed to get xclk\n");
 
 	imx219->xclk_freq = clk_get_rate(imx219->xclk);
-	if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
-		dev_err(dev, "xclk frequency not supported: %d Hz\n",
-			imx219->xclk_freq);
-		return -EINVAL;
-	}
+	if (imx219->xclk_freq != IMX219_XCLK_FREQ)
+		return dev_err_probe(dev, -EINVAL,
+				     "xclk frequency not supported: %d Hz\n",
+				     imx219->xclk_freq);
 
 	ret = imx219_get_regulators(imx219);
-	if (ret) {
-		dev_err(dev, "failed to get regulators\n");
-		return ret;
-	}
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to get regulators\n");
 
 	/* Request optional enable pin */
 	imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset",
@@ -1174,8 +1244,7 @@ static int imx219_probe(struct i2c_client *client)
 		goto error_power_off;
 
 	/* Initialize subdev */
-	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			    V4L2_SUBDEV_FL_HAS_EVENTS;
+	imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	/* Initialize source pad */
@@ -1183,32 +1252,37 @@ static int imx219_probe(struct i2c_client *client)
 
 	ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
 	if (ret) {
-		dev_err(dev, "failed to init entity pads: %d\n", ret);
+		dev_err_probe(dev, ret, "failed to init entity pads\n");
 		goto error_handler_free;
 	}
 
 	imx219->sd.state_lock = imx219->ctrl_handler.lock;
 	ret = v4l2_subdev_init_finalize(&imx219->sd);
 	if (ret < 0) {
-		dev_err(dev, "subdev init error: %d\n", ret);
+		dev_err_probe(dev, ret, "subdev init error\n");
 		goto error_media_entity;
 	}
 
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+
 	ret = v4l2_async_register_subdev_sensor(&imx219->sd);
 	if (ret < 0) {
-		dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
+		dev_err_probe(dev, ret,
+			      "failed to register sensor sub-device\n");
 		goto error_subdev_cleanup;
 	}
 
-	/* Enable runtime PM and turn off the device */
-	pm_runtime_set_active(dev);
-	pm_runtime_enable(dev);
 	pm_runtime_idle(dev);
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
 
 	return 0;
 
 error_subdev_cleanup:
 	v4l2_subdev_cleanup(&imx219->sd);
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
 
 error_media_entity:
 	media_entity_cleanup(&imx219->sd.entity);
@@ -1233,9 +1307,10 @@ static void imx219_remove(struct i2c_client *client)
 	imx219_free_controls(imx219);
 
 	pm_runtime_disable(&client->dev);
-	if (!pm_runtime_status_suspended(&client->dev))
+	if (!pm_runtime_status_suspended(&client->dev)) {
 		imx219_power_off(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
 }
 
 static const struct of_device_id imx219_dt_ids[] = {
diff --git a/drivers/media/i2c/imx258.c b/drivers/media/i2c/imx258.c
index a577afb530b7..9e30fce1f223 100644
--- a/drivers/media/i2c/imx258.c
+++ b/drivers/media/i2c/imx258.c
@@ -7,97 +7,150 @@
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
-#define IMX258_REG_VALUE_08BIT		1
-#define IMX258_REG_VALUE_16BIT		2
-
-#define IMX258_REG_MODE_SELECT		0x0100
+#define IMX258_REG_MODE_SELECT		CCI_REG8(0x0100)
 #define IMX258_MODE_STANDBY		0x00
 #define IMX258_MODE_STREAMING		0x01
 
+#define IMX258_REG_RESET		CCI_REG8(0x0103)
+
 /* Chip ID */
-#define IMX258_REG_CHIP_ID		0x0016
+#define IMX258_REG_CHIP_ID		CCI_REG16(0x0016)
 #define IMX258_CHIP_ID			0x0258
 
 /* V_TIMING internal */
 #define IMX258_VTS_30FPS		0x0c50
 #define IMX258_VTS_30FPS_2K		0x0638
 #define IMX258_VTS_30FPS_VGA		0x034c
-#define IMX258_VTS_MAX			0xffff
-
-/*Frame Length Line*/
-#define IMX258_FLL_MIN			0x08a6
-#define IMX258_FLL_MAX			0xffff
-#define IMX258_FLL_STEP			1
-#define IMX258_FLL_DEFAULT		0x0c98
+#define IMX258_VTS_MAX			65525
 
 /* HBLANK control - read only */
 #define IMX258_PPL_DEFAULT		5352
 
 /* Exposure control */
-#define IMX258_REG_EXPOSURE		0x0202
+#define IMX258_REG_EXPOSURE		CCI_REG16(0x0202)
+#define IMX258_EXPOSURE_OFFSET		10
 #define IMX258_EXPOSURE_MIN		4
 #define IMX258_EXPOSURE_STEP		1
 #define IMX258_EXPOSURE_DEFAULT		0x640
-#define IMX258_EXPOSURE_MAX		65535
+#define IMX258_EXPOSURE_MAX		(IMX258_VTS_MAX - IMX258_EXPOSURE_OFFSET)
 
 /* Analog gain control */
-#define IMX258_REG_ANALOG_GAIN		0x0204
+#define IMX258_REG_ANALOG_GAIN		CCI_REG16(0x0204)
 #define IMX258_ANA_GAIN_MIN		0
 #define IMX258_ANA_GAIN_MAX		480
 #define IMX258_ANA_GAIN_STEP		1
 #define IMX258_ANA_GAIN_DEFAULT		0x0
 
 /* Digital gain control */
-#define IMX258_REG_GR_DIGITAL_GAIN	0x020e
-#define IMX258_REG_R_DIGITAL_GAIN	0x0210
-#define IMX258_REG_B_DIGITAL_GAIN	0x0212
-#define IMX258_REG_GB_DIGITAL_GAIN	0x0214
+#define IMX258_REG_GR_DIGITAL_GAIN	CCI_REG16(0x020e)
+#define IMX258_REG_R_DIGITAL_GAIN	CCI_REG16(0x0210)
+#define IMX258_REG_B_DIGITAL_GAIN	CCI_REG16(0x0212)
+#define IMX258_REG_GB_DIGITAL_GAIN	CCI_REG16(0x0214)
 #define IMX258_DGTL_GAIN_MIN		0
 #define IMX258_DGTL_GAIN_MAX		4096	/* Max = 0xFFF */
 #define IMX258_DGTL_GAIN_DEFAULT	1024
 #define IMX258_DGTL_GAIN_STEP		1
 
 /* HDR control */
-#define IMX258_REG_HDR			0x0220
+#define IMX258_REG_HDR			CCI_REG8(0x0220)
 #define IMX258_HDR_ON			BIT(0)
-#define IMX258_REG_HDR_RATIO		0x0222
+#define IMX258_REG_HDR_RATIO		CCI_REG8(0x0222)
 #define IMX258_HDR_RATIO_MIN		0
 #define IMX258_HDR_RATIO_MAX		5
 #define IMX258_HDR_RATIO_STEP		1
 #define IMX258_HDR_RATIO_DEFAULT	0x0
 
 /* Test Pattern Control */
-#define IMX258_REG_TEST_PATTERN		0x0600
+#define IMX258_REG_TEST_PATTERN		CCI_REG16(0x0600)
+
+#define IMX258_CLK_BLANK_STOP		CCI_REG8(0x4040)
 
 /* Orientation */
-#define REG_MIRROR_FLIP_CONTROL		0x0101
-#define REG_CONFIG_MIRROR_FLIP		0x03
-#define REG_CONFIG_FLIP_TEST_PATTERN	0x02
+#define REG_MIRROR_FLIP_CONTROL		CCI_REG8(0x0101)
+#define REG_CONFIG_MIRROR_HFLIP		0x01
+#define REG_CONFIG_MIRROR_VFLIP		0x02
+
+/* IMX258 native and active pixel array size. */
+#define IMX258_NATIVE_WIDTH		4224U
+#define IMX258_NATIVE_HEIGHT		3192U
+#define IMX258_PIXEL_ARRAY_LEFT		8U
+#define IMX258_PIXEL_ARRAY_TOP		16U
+#define IMX258_PIXEL_ARRAY_WIDTH	4208U
+#define IMX258_PIXEL_ARRAY_HEIGHT	3120U
+
+/* regs */
+#define IMX258_REG_PLL_MULT_DRIV                  CCI_REG8(0x0310)
+#define IMX258_REG_IVTPXCK_DIV                    CCI_REG8(0x0301)
+#define IMX258_REG_IVTSYCK_DIV                    CCI_REG8(0x0303)
+#define IMX258_REG_PREPLLCK_VT_DIV                CCI_REG8(0x0305)
+#define IMX258_REG_IOPPXCK_DIV                    CCI_REG8(0x0309)
+#define IMX258_REG_IOPSYCK_DIV                    CCI_REG8(0x030b)
+#define IMX258_REG_PREPLLCK_OP_DIV                CCI_REG8(0x030d)
+#define IMX258_REG_PHASE_PIX_OUTEN                CCI_REG8(0x3030)
+#define IMX258_REG_PDPIX_DATA_RATE                CCI_REG8(0x3032)
+#define IMX258_REG_SCALE_MODE                     CCI_REG8(0x0401)
+#define IMX258_REG_SCALE_MODE_EXT                 CCI_REG8(0x3038)
+#define IMX258_REG_AF_WINDOW_MODE                 CCI_REG8(0x7bcd)
+#define IMX258_REG_FRM_LENGTH_CTL                 CCI_REG8(0x0350)
+#define IMX258_REG_CSI_LANE_MODE                  CCI_REG8(0x0114)
+#define IMX258_REG_X_EVN_INC                      CCI_REG8(0x0381)
+#define IMX258_REG_X_ODD_INC                      CCI_REG8(0x0383)
+#define IMX258_REG_Y_EVN_INC                      CCI_REG8(0x0385)
+#define IMX258_REG_Y_ODD_INC                      CCI_REG8(0x0387)
+#define IMX258_REG_BINNING_MODE                   CCI_REG8(0x0900)
+#define IMX258_REG_BINNING_TYPE_V                 CCI_REG8(0x0901)
+#define IMX258_REG_FORCE_FD_SUM                   CCI_REG8(0x300d)
+#define IMX258_REG_DIG_CROP_X_OFFSET              CCI_REG16(0x0408)
+#define IMX258_REG_DIG_CROP_Y_OFFSET              CCI_REG16(0x040a)
+#define IMX258_REG_DIG_CROP_IMAGE_WIDTH           CCI_REG16(0x040c)
+#define IMX258_REG_DIG_CROP_IMAGE_HEIGHT          CCI_REG16(0x040e)
+#define IMX258_REG_SCALE_M                        CCI_REG16(0x0404)
+#define IMX258_REG_X_OUT_SIZE                     CCI_REG16(0x034c)
+#define IMX258_REG_Y_OUT_SIZE                     CCI_REG16(0x034e)
+#define IMX258_REG_X_ADD_STA                      CCI_REG16(0x0344)
+#define IMX258_REG_Y_ADD_STA                      CCI_REG16(0x0346)
+#define IMX258_REG_X_ADD_END                      CCI_REG16(0x0348)
+#define IMX258_REG_Y_ADD_END                      CCI_REG16(0x034a)
+#define IMX258_REG_EXCK_FREQ                      CCI_REG16(0x0136)
+#define IMX258_REG_CSI_DT_FMT                     CCI_REG16(0x0112)
+#define IMX258_REG_LINE_LENGTH_PCK                CCI_REG16(0x0342)
+#define IMX258_REG_SCALE_M_EXT                    CCI_REG16(0x303a)
+#define IMX258_REG_FRM_LENGTH_LINES               CCI_REG16(0x0340)
+#define IMX258_REG_FINE_INTEG_TIME                CCI_REG8(0x0200)
+#define IMX258_REG_PLL_IVT_MPY                    CCI_REG16(0x0306)
+#define IMX258_REG_PLL_IOP_MPY                    CCI_REG16(0x030e)
+#define IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H       CCI_REG16(0x0820)
+#define IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L       CCI_REG16(0x0822)
 
-/* Input clock frequency in Hz */
-#define IMX258_INPUT_CLOCK_FREQ		19200000
+struct imx258_reg_list {
+	u32 num_of_regs;
+	const struct cci_reg_sequence *regs;
+};
 
-struct imx258_reg {
-	u16 address;
-	u8 val;
+struct imx258_link_cfg {
+	unsigned int lf_to_pix_rate_factor;
+	struct imx258_reg_list reg_list;
 };
 
-struct imx258_reg_list {
-	u32 num_of_regs;
-	const struct imx258_reg *regs;
+enum {
+	IMX258_2_LANE_MODE,
+	IMX258_4_LANE_MODE,
+	IMX258_LANE_CONFIGS,
 };
 
 /* Link frequency config */
 struct imx258_link_freq_config {
 	u32 pixels_per_line;
 
-	/* PLL registers for this link frequency */
-	struct imx258_reg_list reg_list;
+	/* Configuration for this link frequency / num lanes selection */
+	struct imx258_link_cfg link_cfg[IMX258_LANE_CONFIGS];
 };
 
 /* Mode : resolution and related config&values */
@@ -115,400 +168,307 @@ struct imx258_mode {
 	u32 link_freq_index;
 	/* Default register values */
 	struct imx258_reg_list reg_list;
+
+	/* Analog crop rectangle */
+	struct v4l2_rect crop;
+};
+
+/*
+ * 4208x3120 @ 30 fps needs 1267Mbps/lane, 4 lanes.
+ * To avoid further computation of clock settings, adopt the same per
+ * lane data rate when using 2 lanes, thus allowing a maximum of 15fps.
+ */
+static const struct cci_reg_sequence mipi_1267mbps_19_2mhz_2l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1333 },
+	{ IMX258_REG_IVTPXCK_DIV, 10 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX258_REG_PLL_IVT_MPY, 198 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 1 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 1267 * 2 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
+};
+
+static const struct cci_reg_sequence mipi_1267mbps_19_2mhz_4l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1333 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX258_REG_PLL_IVT_MPY, 198 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 3 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 1267 * 4 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
+};
+
+static const struct cci_reg_sequence mipi_1272mbps_24mhz_2l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1800 },
+	{ IMX258_REG_IVTPXCK_DIV, 10 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 4 },
+	{ IMX258_REG_PLL_IVT_MPY, 212 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 1 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 1272 * 2 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
+};
+
+static const struct cci_reg_sequence mipi_1272mbps_24mhz_4l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1800 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 4 },
+	{ IMX258_REG_PLL_IVT_MPY, 212 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 3 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 1272 * 4 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
+};
+
+static const struct cci_reg_sequence mipi_640mbps_19_2mhz_2l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1333 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX258_REG_PLL_IVT_MPY, 100 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 1 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 640 * 2 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
+};
+
+static const struct cci_reg_sequence mipi_640mbps_19_2mhz_4l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1333 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 3 },
+	{ IMX258_REG_PLL_IVT_MPY, 100 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 3 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 640 * 4 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
 };
 
-/* 4208x3118 needs 1267Mbps/lane, 4 lanes */
-static const struct imx258_reg mipi_data_rate_1267mbps[] = {
-	{ 0x0301, 0x05 },
-	{ 0x0303, 0x02 },
-	{ 0x0305, 0x03 },
-	{ 0x0306, 0x00 },
-	{ 0x0307, 0xC6 },
-	{ 0x0309, 0x0A },
-	{ 0x030B, 0x01 },
-	{ 0x030D, 0x02 },
-	{ 0x030E, 0x00 },
-	{ 0x030F, 0xD8 },
-	{ 0x0310, 0x00 },
-	{ 0x0820, 0x13 },
-	{ 0x0821, 0x4C },
-	{ 0x0822, 0xCC },
-	{ 0x0823, 0xCC },
+static const struct cci_reg_sequence mipi_642mbps_24mhz_2l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1800 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 4 },
+	{ IMX258_REG_PLL_IVT_MPY, 107 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 1 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 642 * 2 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
 };
 
-static const struct imx258_reg mipi_data_rate_640mbps[] = {
-	{ 0x0301, 0x05 },
-	{ 0x0303, 0x02 },
-	{ 0x0305, 0x03 },
-	{ 0x0306, 0x00 },
-	{ 0x0307, 0x64 },
-	{ 0x0309, 0x0A },
-	{ 0x030B, 0x01 },
-	{ 0x030D, 0x02 },
-	{ 0x030E, 0x00 },
-	{ 0x030F, 0xD8 },
-	{ 0x0310, 0x00 },
-	{ 0x0820, 0x0A },
-	{ 0x0821, 0x00 },
-	{ 0x0822, 0x00 },
-	{ 0x0823, 0x00 },
+static const struct cci_reg_sequence mipi_642mbps_24mhz_4l[] = {
+	{ IMX258_REG_EXCK_FREQ, 0x1800 },
+	{ IMX258_REG_IVTPXCK_DIV, 5 },
+	{ IMX258_REG_IVTSYCK_DIV, 2 },
+	{ IMX258_REG_PREPLLCK_VT_DIV, 4 },
+	{ IMX258_REG_PLL_IVT_MPY, 107 },
+	{ IMX258_REG_IOPPXCK_DIV, 10 },
+	{ IMX258_REG_IOPSYCK_DIV, 1 },
+	{ IMX258_REG_PREPLLCK_OP_DIV, 2 },
+	{ IMX258_REG_PLL_IOP_MPY, 216 },
+	{ IMX258_REG_PLL_MULT_DRIV, 0 },
+
+	{ IMX258_REG_CSI_LANE_MODE, 3 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_H, 642 * 4 },
+	{ IMX258_REG_REQ_LINK_BIT_RATE_MBPS_L, 0 },
 };
 
-static const struct imx258_reg mode_4208x3118_regs[] = {
-	{ 0x0136, 0x13 },
-	{ 0x0137, 0x33 },
-	{ 0x3051, 0x00 },
-	{ 0x3052, 0x00 },
-	{ 0x4E21, 0x14 },
-	{ 0x6B11, 0xCF },
-	{ 0x7FF0, 0x08 },
-	{ 0x7FF1, 0x0F },
-	{ 0x7FF2, 0x08 },
-	{ 0x7FF3, 0x1B },
-	{ 0x7FF4, 0x23 },
-	{ 0x7FF5, 0x60 },
-	{ 0x7FF6, 0x00 },
-	{ 0x7FF7, 0x01 },
-	{ 0x7FF8, 0x00 },
-	{ 0x7FF9, 0x78 },
-	{ 0x7FFA, 0x00 },
-	{ 0x7FFB, 0x00 },
-	{ 0x7FFC, 0x00 },
-	{ 0x7FFD, 0x00 },
-	{ 0x7FFE, 0x00 },
-	{ 0x7FFF, 0x03 },
-	{ 0x7F76, 0x03 },
-	{ 0x7F77, 0xFE },
-	{ 0x7FA8, 0x03 },
-	{ 0x7FA9, 0xFE },
-	{ 0x7B24, 0x81 },
-	{ 0x7B25, 0x00 },
-	{ 0x6564, 0x07 },
-	{ 0x6B0D, 0x41 },
-	{ 0x653D, 0x04 },
-	{ 0x6B05, 0x8C },
-	{ 0x6B06, 0xF9 },
-	{ 0x6B08, 0x65 },
-	{ 0x6B09, 0xFC },
-	{ 0x6B0A, 0xCF },
-	{ 0x6B0B, 0xD2 },
-	{ 0x6700, 0x0E },
-	{ 0x6707, 0x0E },
-	{ 0x9104, 0x00 },
-	{ 0x4648, 0x7F },
-	{ 0x7420, 0x00 },
-	{ 0x7421, 0x1C },
-	{ 0x7422, 0x00 },
-	{ 0x7423, 0xD7 },
-	{ 0x5F04, 0x00 },
-	{ 0x5F05, 0xED },
-	{ 0x0112, 0x0A },
-	{ 0x0113, 0x0A },
-	{ 0x0114, 0x03 },
-	{ 0x0342, 0x14 },
-	{ 0x0343, 0xE8 },
-	{ 0x0340, 0x0C },
-	{ 0x0341, 0x50 },
-	{ 0x0344, 0x00 },
-	{ 0x0345, 0x00 },
-	{ 0x0346, 0x00 },
-	{ 0x0347, 0x00 },
-	{ 0x0348, 0x10 },
-	{ 0x0349, 0x6F },
-	{ 0x034A, 0x0C },
-	{ 0x034B, 0x2E },
-	{ 0x0381, 0x01 },
-	{ 0x0383, 0x01 },
-	{ 0x0385, 0x01 },
-	{ 0x0387, 0x01 },
-	{ 0x0900, 0x00 },
-	{ 0x0901, 0x11 },
-	{ 0x0401, 0x00 },
-	{ 0x0404, 0x00 },
-	{ 0x0405, 0x10 },
-	{ 0x0408, 0x00 },
-	{ 0x0409, 0x00 },
-	{ 0x040A, 0x00 },
-	{ 0x040B, 0x00 },
-	{ 0x040C, 0x10 },
-	{ 0x040D, 0x70 },
-	{ 0x040E, 0x0C },
-	{ 0x040F, 0x30 },
-	{ 0x3038, 0x00 },
-	{ 0x303A, 0x00 },
-	{ 0x303B, 0x10 },
-	{ 0x300D, 0x00 },
-	{ 0x034C, 0x10 },
-	{ 0x034D, 0x70 },
-	{ 0x034E, 0x0C },
-	{ 0x034F, 0x30 },
-	{ 0x0350, 0x01 },
-	{ 0x0202, 0x0C },
-	{ 0x0203, 0x46 },
-	{ 0x0204, 0x00 },
-	{ 0x0205, 0x00 },
-	{ 0x020E, 0x01 },
-	{ 0x020F, 0x00 },
-	{ 0x0210, 0x01 },
-	{ 0x0211, 0x00 },
-	{ 0x0212, 0x01 },
-	{ 0x0213, 0x00 },
-	{ 0x0214, 0x01 },
-	{ 0x0215, 0x00 },
-	{ 0x7BCD, 0x00 },
-	{ 0x94DC, 0x20 },
-	{ 0x94DD, 0x20 },
-	{ 0x94DE, 0x20 },
-	{ 0x95DC, 0x20 },
-	{ 0x95DD, 0x20 },
-	{ 0x95DE, 0x20 },
-	{ 0x7FB0, 0x00 },
-	{ 0x9010, 0x3E },
-	{ 0x9419, 0x50 },
-	{ 0x941B, 0x50 },
-	{ 0x9519, 0x50 },
-	{ 0x951B, 0x50 },
-	{ 0x3030, 0x00 },
-	{ 0x3032, 0x00 },
-	{ 0x0220, 0x00 },
+static const struct cci_reg_sequence mode_common_regs[] = {
+	{ CCI_REG8(0x3051), 0x00 },
+	{ CCI_REG8(0x6B11), 0xCF },
+	{ CCI_REG8(0x7FF0), 0x08 },
+	{ CCI_REG8(0x7FF1), 0x0F },
+	{ CCI_REG8(0x7FF2), 0x08 },
+	{ CCI_REG8(0x7FF3), 0x1B },
+	{ CCI_REG8(0x7FF4), 0x23 },
+	{ CCI_REG8(0x7FF5), 0x60 },
+	{ CCI_REG8(0x7FF6), 0x00 },
+	{ CCI_REG8(0x7FF7), 0x01 },
+	{ CCI_REG8(0x7FF8), 0x00 },
+	{ CCI_REG8(0x7FF9), 0x78 },
+	{ CCI_REG8(0x7FFA), 0x00 },
+	{ CCI_REG8(0x7FFB), 0x00 },
+	{ CCI_REG8(0x7FFC), 0x00 },
+	{ CCI_REG8(0x7FFD), 0x00 },
+	{ CCI_REG8(0x7FFE), 0x00 },
+	{ CCI_REG8(0x7FFF), 0x03 },
+	{ CCI_REG8(0x7F76), 0x03 },
+	{ CCI_REG8(0x7F77), 0xFE },
+	{ CCI_REG8(0x7FA8), 0x03 },
+	{ CCI_REG8(0x7FA9), 0xFE },
+	{ CCI_REG8(0x7B24), 0x81 },
+	{ CCI_REG8(0x6564), 0x07 },
+	{ CCI_REG8(0x6B0D), 0x41 },
+	{ CCI_REG8(0x653D), 0x04 },
+	{ CCI_REG8(0x6B05), 0x8C },
+	{ CCI_REG8(0x6B06), 0xF9 },
+	{ CCI_REG8(0x6B08), 0x65 },
+	{ CCI_REG8(0x6B09), 0xFC },
+	{ CCI_REG8(0x6B0A), 0xCF },
+	{ CCI_REG8(0x6B0B), 0xD2 },
+	{ CCI_REG8(0x6700), 0x0E },
+	{ CCI_REG8(0x6707), 0x0E },
+	{ CCI_REG8(0x9104), 0x00 },
+	{ CCI_REG8(0x4648), 0x7F },
+	{ CCI_REG8(0x7420), 0x00 },
+	{ CCI_REG8(0x7421), 0x1C },
+	{ CCI_REG8(0x7422), 0x00 },
+	{ CCI_REG8(0x7423), 0xD7 },
+	{ CCI_REG8(0x5F04), 0x00 },
+	{ CCI_REG8(0x5F05), 0xED },
+	{IMX258_REG_CSI_DT_FMT, 0x0a0a},
+	{IMX258_REG_LINE_LENGTH_PCK, 5352},
+	{IMX258_REG_X_ADD_STA, 0},
+	{IMX258_REG_Y_ADD_STA, 0},
+	{IMX258_REG_X_ADD_END, 4207},
+	{IMX258_REG_Y_ADD_END, 3119},
+	{IMX258_REG_X_EVN_INC, 1},
+	{IMX258_REG_X_ODD_INC, 1},
+	{IMX258_REG_Y_EVN_INC, 1},
+	{IMX258_REG_Y_ODD_INC, 1},
+	{IMX258_REG_DIG_CROP_X_OFFSET, 0},
+	{IMX258_REG_DIG_CROP_Y_OFFSET, 0},
+	{IMX258_REG_DIG_CROP_IMAGE_WIDTH, 4208},
+	{IMX258_REG_SCALE_MODE_EXT, 0},
+	{IMX258_REG_SCALE_M_EXT, 16},
+	{IMX258_REG_FORCE_FD_SUM, 0},
+	{IMX258_REG_FRM_LENGTH_CTL, 0},
+	{IMX258_REG_ANALOG_GAIN, 0},
+	{IMX258_REG_GR_DIGITAL_GAIN, 256},
+	{IMX258_REG_R_DIGITAL_GAIN, 256},
+	{IMX258_REG_B_DIGITAL_GAIN, 256},
+	{IMX258_REG_GB_DIGITAL_GAIN, 256},
+	{IMX258_REG_AF_WINDOW_MODE, 0},
+	{ CCI_REG8(0x94DC), 0x20 },
+	{ CCI_REG8(0x94DD), 0x20 },
+	{ CCI_REG8(0x94DE), 0x20 },
+	{ CCI_REG8(0x95DC), 0x20 },
+	{ CCI_REG8(0x95DD), 0x20 },
+	{ CCI_REG8(0x95DE), 0x20 },
+	{ CCI_REG8(0x7FB0), 0x00 },
+	{ CCI_REG8(0x9010), 0x3E },
+	{ CCI_REG8(0x9419), 0x50 },
+	{ CCI_REG8(0x941B), 0x50 },
+	{ CCI_REG8(0x9519), 0x50 },
+	{ CCI_REG8(0x951B), 0x50 },
+	{IMX258_REG_PHASE_PIX_OUTEN, 0},
+	{IMX258_REG_PDPIX_DATA_RATE, 0},
+	{IMX258_REG_HDR, 0},
 };
 
-static const struct imx258_reg mode_2104_1560_regs[] = {
-	{ 0x0136, 0x13 },
-	{ 0x0137, 0x33 },
-	{ 0x3051, 0x00 },
-	{ 0x3052, 0x00 },
-	{ 0x4E21, 0x14 },
-	{ 0x6B11, 0xCF },
-	{ 0x7FF0, 0x08 },
-	{ 0x7FF1, 0x0F },
-	{ 0x7FF2, 0x08 },
-	{ 0x7FF3, 0x1B },
-	{ 0x7FF4, 0x23 },
-	{ 0x7FF5, 0x60 },
-	{ 0x7FF6, 0x00 },
-	{ 0x7FF7, 0x01 },
-	{ 0x7FF8, 0x00 },
-	{ 0x7FF9, 0x78 },
-	{ 0x7FFA, 0x00 },
-	{ 0x7FFB, 0x00 },
-	{ 0x7FFC, 0x00 },
-	{ 0x7FFD, 0x00 },
-	{ 0x7FFE, 0x00 },
-	{ 0x7FFF, 0x03 },
-	{ 0x7F76, 0x03 },
-	{ 0x7F77, 0xFE },
-	{ 0x7FA8, 0x03 },
-	{ 0x7FA9, 0xFE },
-	{ 0x7B24, 0x81 },
-	{ 0x7B25, 0x00 },
-	{ 0x6564, 0x07 },
-	{ 0x6B0D, 0x41 },
-	{ 0x653D, 0x04 },
-	{ 0x6B05, 0x8C },
-	{ 0x6B06, 0xF9 },
-	{ 0x6B08, 0x65 },
-	{ 0x6B09, 0xFC },
-	{ 0x6B0A, 0xCF },
-	{ 0x6B0B, 0xD2 },
-	{ 0x6700, 0x0E },
-	{ 0x6707, 0x0E },
-	{ 0x9104, 0x00 },
-	{ 0x4648, 0x7F },
-	{ 0x7420, 0x00 },
-	{ 0x7421, 0x1C },
-	{ 0x7422, 0x00 },
-	{ 0x7423, 0xD7 },
-	{ 0x5F04, 0x00 },
-	{ 0x5F05, 0xED },
-	{ 0x0112, 0x0A },
-	{ 0x0113, 0x0A },
-	{ 0x0114, 0x03 },
-	{ 0x0342, 0x14 },
-	{ 0x0343, 0xE8 },
-	{ 0x0340, 0x06 },
-	{ 0x0341, 0x38 },
-	{ 0x0344, 0x00 },
-	{ 0x0345, 0x00 },
-	{ 0x0346, 0x00 },
-	{ 0x0347, 0x00 },
-	{ 0x0348, 0x10 },
-	{ 0x0349, 0x6F },
-	{ 0x034A, 0x0C },
-	{ 0x034B, 0x2E },
-	{ 0x0381, 0x01 },
-	{ 0x0383, 0x01 },
-	{ 0x0385, 0x01 },
-	{ 0x0387, 0x01 },
-	{ 0x0900, 0x01 },
-	{ 0x0901, 0x12 },
-	{ 0x0401, 0x01 },
-	{ 0x0404, 0x00 },
-	{ 0x0405, 0x20 },
-	{ 0x0408, 0x00 },
-	{ 0x0409, 0x02 },
-	{ 0x040A, 0x00 },
-	{ 0x040B, 0x00 },
-	{ 0x040C, 0x10 },
-	{ 0x040D, 0x6A },
-	{ 0x040E, 0x06 },
-	{ 0x040F, 0x18 },
-	{ 0x3038, 0x00 },
-	{ 0x303A, 0x00 },
-	{ 0x303B, 0x10 },
-	{ 0x300D, 0x00 },
-	{ 0x034C, 0x08 },
-	{ 0x034D, 0x38 },
-	{ 0x034E, 0x06 },
-	{ 0x034F, 0x18 },
-	{ 0x0350, 0x01 },
-	{ 0x0202, 0x06 },
-	{ 0x0203, 0x2E },
-	{ 0x0204, 0x00 },
-	{ 0x0205, 0x00 },
-	{ 0x020E, 0x01 },
-	{ 0x020F, 0x00 },
-	{ 0x0210, 0x01 },
-	{ 0x0211, 0x00 },
-	{ 0x0212, 0x01 },
-	{ 0x0213, 0x00 },
-	{ 0x0214, 0x01 },
-	{ 0x0215, 0x00 },
-	{ 0x7BCD, 0x01 },
-	{ 0x94DC, 0x20 },
-	{ 0x94DD, 0x20 },
-	{ 0x94DE, 0x20 },
-	{ 0x95DC, 0x20 },
-	{ 0x95DD, 0x20 },
-	{ 0x95DE, 0x20 },
-	{ 0x7FB0, 0x00 },
-	{ 0x9010, 0x3E },
-	{ 0x9419, 0x50 },
-	{ 0x941B, 0x50 },
-	{ 0x9519, 0x50 },
-	{ 0x951B, 0x50 },
-	{ 0x3030, 0x00 },
-	{ 0x3032, 0x00 },
-	{ 0x0220, 0x00 },
+static const struct cci_reg_sequence mode_4208x3120_regs[] = {
+	{IMX258_REG_BINNING_MODE, 0},
+	{IMX258_REG_BINNING_TYPE_V, 0x11},
+	{IMX258_REG_SCALE_MODE, 0},
+	{IMX258_REG_SCALE_M, 16},
+	{IMX258_REG_DIG_CROP_IMAGE_HEIGHT, 3120},
+	{IMX258_REG_X_OUT_SIZE, 4208},
+	{IMX258_REG_Y_OUT_SIZE, 3120},
 };
 
-static const struct imx258_reg mode_1048_780_regs[] = {
-	{ 0x0136, 0x13 },
-	{ 0x0137, 0x33 },
-	{ 0x3051, 0x00 },
-	{ 0x3052, 0x00 },
-	{ 0x4E21, 0x14 },
-	{ 0x6B11, 0xCF },
-	{ 0x7FF0, 0x08 },
-	{ 0x7FF1, 0x0F },
-	{ 0x7FF2, 0x08 },
-	{ 0x7FF3, 0x1B },
-	{ 0x7FF4, 0x23 },
-	{ 0x7FF5, 0x60 },
-	{ 0x7FF6, 0x00 },
-	{ 0x7FF7, 0x01 },
-	{ 0x7FF8, 0x00 },
-	{ 0x7FF9, 0x78 },
-	{ 0x7FFA, 0x00 },
-	{ 0x7FFB, 0x00 },
-	{ 0x7FFC, 0x00 },
-	{ 0x7FFD, 0x00 },
-	{ 0x7FFE, 0x00 },
-	{ 0x7FFF, 0x03 },
-	{ 0x7F76, 0x03 },
-	{ 0x7F77, 0xFE },
-	{ 0x7FA8, 0x03 },
-	{ 0x7FA9, 0xFE },
-	{ 0x7B24, 0x81 },
-	{ 0x7B25, 0x00 },
-	{ 0x6564, 0x07 },
-	{ 0x6B0D, 0x41 },
-	{ 0x653D, 0x04 },
-	{ 0x6B05, 0x8C },
-	{ 0x6B06, 0xF9 },
-	{ 0x6B08, 0x65 },
-	{ 0x6B09, 0xFC },
-	{ 0x6B0A, 0xCF },
-	{ 0x6B0B, 0xD2 },
-	{ 0x6700, 0x0E },
-	{ 0x6707, 0x0E },
-	{ 0x9104, 0x00 },
-	{ 0x4648, 0x7F },
-	{ 0x7420, 0x00 },
-	{ 0x7421, 0x1C },
-	{ 0x7422, 0x00 },
-	{ 0x7423, 0xD7 },
-	{ 0x5F04, 0x00 },
-	{ 0x5F05, 0xED },
-	{ 0x0112, 0x0A },
-	{ 0x0113, 0x0A },
-	{ 0x0114, 0x03 },
-	{ 0x0342, 0x14 },
-	{ 0x0343, 0xE8 },
-	{ 0x0340, 0x03 },
-	{ 0x0341, 0x4C },
-	{ 0x0344, 0x00 },
-	{ 0x0345, 0x00 },
-	{ 0x0346, 0x00 },
-	{ 0x0347, 0x00 },
-	{ 0x0348, 0x10 },
-	{ 0x0349, 0x6F },
-	{ 0x034A, 0x0C },
-	{ 0x034B, 0x2E },
-	{ 0x0381, 0x01 },
-	{ 0x0383, 0x01 },
-	{ 0x0385, 0x01 },
-	{ 0x0387, 0x01 },
-	{ 0x0900, 0x01 },
-	{ 0x0901, 0x14 },
-	{ 0x0401, 0x01 },
-	{ 0x0404, 0x00 },
-	{ 0x0405, 0x40 },
-	{ 0x0408, 0x00 },
-	{ 0x0409, 0x06 },
-	{ 0x040A, 0x00 },
-	{ 0x040B, 0x00 },
-	{ 0x040C, 0x10 },
-	{ 0x040D, 0x64 },
-	{ 0x040E, 0x03 },
-	{ 0x040F, 0x0C },
-	{ 0x3038, 0x00 },
-	{ 0x303A, 0x00 },
-	{ 0x303B, 0x10 },
-	{ 0x300D, 0x00 },
-	{ 0x034C, 0x04 },
-	{ 0x034D, 0x18 },
-	{ 0x034E, 0x03 },
-	{ 0x034F, 0x0C },
-	{ 0x0350, 0x01 },
-	{ 0x0202, 0x03 },
-	{ 0x0203, 0x42 },
-	{ 0x0204, 0x00 },
-	{ 0x0205, 0x00 },
-	{ 0x020E, 0x01 },
-	{ 0x020F, 0x00 },
-	{ 0x0210, 0x01 },
-	{ 0x0211, 0x00 },
-	{ 0x0212, 0x01 },
-	{ 0x0213, 0x00 },
-	{ 0x0214, 0x01 },
-	{ 0x0215, 0x00 },
-	{ 0x7BCD, 0x00 },
-	{ 0x94DC, 0x20 },
-	{ 0x94DD, 0x20 },
-	{ 0x94DE, 0x20 },
-	{ 0x95DC, 0x20 },
-	{ 0x95DD, 0x20 },
-	{ 0x95DE, 0x20 },
-	{ 0x7FB0, 0x00 },
-	{ 0x9010, 0x3E },
-	{ 0x9419, 0x50 },
-	{ 0x941B, 0x50 },
-	{ 0x9519, 0x50 },
-	{ 0x951B, 0x50 },
-	{ 0x3030, 0x00 },
-	{ 0x3032, 0x00 },
-	{ 0x0220, 0x00 },
+static const struct cci_reg_sequence mode_2104_1560_regs[] = {
+	{IMX258_REG_BINNING_MODE, 1},
+	{IMX258_REG_BINNING_TYPE_V, 0x12},
+	{IMX258_REG_SCALE_MODE, 1},
+	{IMX258_REG_SCALE_M, 32},
+	{IMX258_REG_DIG_CROP_IMAGE_HEIGHT, 1560},
+	{IMX258_REG_X_OUT_SIZE, 2104},
+	{IMX258_REG_Y_OUT_SIZE, 1560},
+};
+
+static const struct cci_reg_sequence mode_1048_780_regs[] = {
+	{IMX258_REG_BINNING_MODE, 1},
+	{IMX258_REG_BINNING_TYPE_V, 0x14},
+	{IMX258_REG_SCALE_MODE, 1},
+	{IMX258_REG_SCALE_M, 64},
+	{IMX258_REG_DIG_CROP_IMAGE_HEIGHT, 780},
+	{IMX258_REG_X_OUT_SIZE, 1048},
+	{IMX258_REG_Y_OUT_SIZE, 780},
+};
+
+struct imx258_variant_cfg {
+	const struct cci_reg_sequence *regs;
+	unsigned int num_regs;
+};
+
+static const struct cci_reg_sequence imx258_cfg_regs[] = {
+	{ CCI_REG8(0x3052), 0x00 },
+	{ CCI_REG8(0x4E21), 0x14 },
+	{ CCI_REG8(0x7B25), 0x00 },
+};
+
+static const struct imx258_variant_cfg imx258_cfg = {
+	.regs = imx258_cfg_regs,
+	.num_regs = ARRAY_SIZE(imx258_cfg_regs),
+};
+
+static const struct cci_reg_sequence imx258_pdaf_cfg_regs[] = {
+	{ CCI_REG8(0x3052), 0x01 },
+	{ CCI_REG8(0x4E21), 0x10 },
+	{ CCI_REG8(0x7B25), 0x01 },
+};
+
+static const struct imx258_variant_cfg imx258_pdaf_cfg = {
+	.regs = imx258_pdaf_cfg_regs,
+	.num_regs = ARRAY_SIZE(imx258_pdaf_cfg_regs),
+};
+
+/*
+ * The supported formats.
+ * This table MUST contain 4 entries per format, to cover the various flip
+ * combinations in the order
+ * - no flip
+ * - h flip
+ * - v flip
+ * - h&v flips
+ */
+static const u32 codes[] = {
+	/* 10-bit modes. */
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SBGGR10_1X10
 };
 
 static const char * const imx258_test_pattern_menu[] = {
@@ -519,9 +479,15 @@ static const char * const imx258_test_pattern_menu[] = {
 	"Pseudorandom Sequence (PN9)",
 };
 
-/* Configurations for supported link frequencies */
-#define IMX258_LINK_FREQ_634MHZ	633600000ULL
-#define IMX258_LINK_FREQ_320MHZ	320000000ULL
+/* regulator supplies */
+static const char * const imx258_supply_name[] = {
+	/* Supplies can be enabled in any order */
+	"vana",  /* Analog (2.8V) supply */
+	"vdig",  /* Digital Core (1.2V) supply */
+	"vif",  /* IF (1.8V) supply */
+};
+
+#define IMX258_NUM_SUPPLIES ARRAY_SIZE(imx258_supply_name)
 
 enum {
 	IMX258_LINK_FREQ_1267MBPS,
@@ -529,37 +495,96 @@ enum {
 };
 
 /*
- * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
- * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ * Pixel rate does not necessarily relate to link frequency on this sensor as
+ * there is a FIFO between the pixel array pipeline and the MIPI serializer.
+ * The recommendation from Sony is that the pixel array is always run with a
+ * line length of 5352 pixels, which means that there is a large amount of
+ * blanking time for the 1048x780 mode. There is no need to replicate this
+ * blanking on the CSI2 bus, and the configuration of register 0x0301 allows the
+ * divider to be altered.
+ *
+ * The actual factor between link frequency and pixel rate is in the
+ * imx258_link_cfg, so use this to convert between the two.
+ * bits per pixel being 10, and D-PHY being DDR is assumed by this function, so
+ * the value is only the combination of number of lanes and pixel clock divider.
  */
-static u64 link_freq_to_pixel_rate(u64 f)
+static u64 link_freq_to_pixel_rate(u64 f, const struct imx258_link_cfg *link_cfg)
 {
-	f *= 2 * 4;
+	f *= 2 * link_cfg->lf_to_pix_rate_factor;
 	do_div(f, 10);
 
 	return f;
 }
 
 /* Menu items for LINK_FREQ V4L2 control */
-static const s64 link_freq_menu_items[] = {
-	IMX258_LINK_FREQ_634MHZ,
-	IMX258_LINK_FREQ_320MHZ,
+/* Configurations for supported link frequencies */
+static const s64 link_freq_menu_items_19_2[] = {
+	633600000ULL,
+	320000000ULL,
 };
 
+static const s64 link_freq_menu_items_24[] = {
+	636000000ULL,
+	321000000ULL,
+};
+
+#define REGS(_list) { .num_of_regs = ARRAY_SIZE(_list), .regs = _list, }
+
 /* Link frequency configs */
-static const struct imx258_link_freq_config link_freq_configs[] = {
+static const struct imx258_link_freq_config link_freq_configs_19_2[] = {
 	[IMX258_LINK_FREQ_1267MBPS] = {
 		.pixels_per_line = IMX258_PPL_DEFAULT,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1267mbps),
-			.regs = mipi_data_rate_1267mbps,
+		.link_cfg = {
+			[IMX258_2_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 2 * 2,
+				.reg_list = REGS(mipi_1267mbps_19_2mhz_2l),
+			},
+			[IMX258_4_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 4,
+				.reg_list = REGS(mipi_1267mbps_19_2mhz_4l),
+			},
 		}
 	},
 	[IMX258_LINK_FREQ_640MBPS] = {
 		.pixels_per_line = IMX258_PPL_DEFAULT,
-		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mipi_data_rate_640mbps),
-			.regs = mipi_data_rate_640mbps,
+		.link_cfg = {
+			[IMX258_2_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 2,
+				.reg_list = REGS(mipi_640mbps_19_2mhz_2l),
+			},
+			[IMX258_4_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 4,
+				.reg_list = REGS(mipi_640mbps_19_2mhz_4l),
+			},
+		}
+	},
+};
+
+static const struct imx258_link_freq_config link_freq_configs_24[] = {
+	[IMX258_LINK_FREQ_1267MBPS] = {
+		.pixels_per_line = IMX258_PPL_DEFAULT,
+		.link_cfg = {
+			[IMX258_2_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 2,
+				.reg_list = REGS(mipi_1272mbps_24mhz_2l),
+			},
+			[IMX258_4_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 4,
+				.reg_list = REGS(mipi_1272mbps_24mhz_4l),
+			},
+		}
+	},
+	[IMX258_LINK_FREQ_640MBPS] = {
+		.pixels_per_line = IMX258_PPL_DEFAULT,
+		.link_cfg = {
+			[IMX258_2_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 2 * 2,
+				.reg_list = REGS(mipi_642mbps_24mhz_2l),
+			},
+			[IMX258_4_LANE_MODE] = {
+				.lf_to_pix_rate_factor = 4,
+				.reg_list = REGS(mipi_642mbps_24mhz_4l),
+			},
 		}
 	},
 };
@@ -568,14 +593,20 @@ static const struct imx258_link_freq_config link_freq_configs[] = {
 static const struct imx258_mode supported_modes[] = {
 	{
 		.width = 4208,
-		.height = 3118,
+		.height = 3120,
 		.vts_def = IMX258_VTS_30FPS,
 		.vts_min = IMX258_VTS_30FPS,
 		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(mode_4208x3118_regs),
-			.regs = mode_4208x3118_regs,
+			.num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
+			.regs = mode_4208x3120_regs,
 		},
 		.link_freq_index = IMX258_LINK_FREQ_1267MBPS,
+		.crop = {
+			.left = IMX258_PIXEL_ARRAY_LEFT,
+			.top = IMX258_PIXEL_ARRAY_TOP,
+			.width = 4208,
+			.height = 3120,
+		},
 	},
 	{
 		.width = 2104,
@@ -587,6 +618,12 @@ static const struct imx258_mode supported_modes[] = {
 			.regs = mode_2104_1560_regs,
 		},
 		.link_freq_index = IMX258_LINK_FREQ_640MBPS,
+		.crop = {
+			.left = IMX258_PIXEL_ARRAY_LEFT,
+			.top = IMX258_PIXEL_ARRAY_TOP,
+			.width = 4208,
+			.height = 3120,
+		},
 	},
 	{
 		.width = 1048,
@@ -598,12 +635,21 @@ static const struct imx258_mode supported_modes[] = {
 			.regs = mode_1048_780_regs,
 		},
 		.link_freq_index = IMX258_LINK_FREQ_640MBPS,
+		.crop = {
+			.left = IMX258_PIXEL_ARRAY_LEFT,
+			.top = IMX258_PIXEL_ARRAY_TOP,
+			.width = 4208,
+			.height = 3120,
+		},
 	},
 };
 
 struct imx258 {
 	struct v4l2_subdev sd;
 	struct media_pad pad;
+	struct regmap *regmap;
+
+	const struct imx258_variant_cfg *variant_cfg;
 
 	struct v4l2_ctrl_handler ctrl_handler;
 	/* V4L2 Controls */
@@ -612,10 +658,18 @@ struct imx258 {
 	struct v4l2_ctrl *vblank;
 	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *hflip;
+	struct v4l2_ctrl *vflip;
 
 	/* Current mode */
 	const struct imx258_mode *cur_mode;
 
+	unsigned long link_freq_bitmap;
+	const struct imx258_link_freq_config *link_freq_configs;
+	const s64 *link_freq_menu_items;
+	unsigned int lane_mode_idx;
+	unsigned int csi2_flags;
+
 	/*
 	 * Mutex for serialized access:
 	 * Protect sensor module set pad format and start/stop streaming safely.
@@ -623,6 +677,7 @@ struct imx258 {
 	struct mutex mutex;
 
 	struct clk *clk;
+	struct regulator_bulk_data supplies[IMX258_NUM_SUPPLIES];
 };
 
 static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
@@ -630,120 +685,66 @@ static inline struct imx258 *to_imx258(struct v4l2_subdev *_sd)
 	return container_of(_sd, struct imx258, sd);
 }
 
-/* Read registers up to 2 at a time */
-static int imx258_read_reg(struct imx258 *imx258, u16 reg, u32 len, u32 *val)
+/* Get bayer order based on flip setting. */
+static u32 imx258_get_format_code(const struct imx258 *imx258)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
-	struct i2c_msg msgs[2];
-	u8 addr_buf[2] = { reg >> 8, reg & 0xff };
-	u8 data_buf[4] = { 0, };
-	int ret;
-
-	if (len > 4)
-		return -EINVAL;
-
-	/* Write register address */
-	msgs[0].addr = client->addr;
-	msgs[0].flags = 0;
-	msgs[0].len = ARRAY_SIZE(addr_buf);
-	msgs[0].buf = addr_buf;
-
-	/* Read data from register */
-	msgs[1].addr = client->addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].len = len;
-	msgs[1].buf = &data_buf[4 - len];
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
-
-	*val = get_unaligned_be32(data_buf);
-
-	return 0;
-}
-
-/* Write registers up to 2 at a time */
-static int imx258_write_reg(struct imx258 *imx258, u16 reg, u32 len, u32 val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
-	u8 buf[6];
-
-	if (len > 4)
-		return -EINVAL;
-
-	put_unaligned_be16(reg, buf);
-	put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
-
-	return 0;
-}
-
-/* Write a list of registers */
-static int imx258_write_regs(struct imx258 *imx258,
-			     const struct imx258_reg *regs, u32 len)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 	unsigned int i;
-	int ret;
 
-	for (i = 0; i < len; i++) {
-		ret = imx258_write_reg(imx258, regs[i].address, 1,
-					regs[i].val);
-		if (ret) {
-			dev_err_ratelimited(
-				&client->dev,
-				"Failed to write reg 0x%4.4x. error = %d\n",
-				regs[i].address, ret);
+	lockdep_assert_held(&imx258->mutex);
 
-			return ret;
-		}
-	}
+	i = (imx258->vflip->val ? 2 : 0) |
+	    (imx258->hflip->val ? 1 : 0);
 
-	return 0;
+	return codes[i];
 }
 
 /* Open sub-device */
 static int imx258_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
+	struct imx258 *imx258 = to_imx258(sd);
 	struct v4l2_mbus_framefmt *try_fmt =
 		v4l2_subdev_state_get_format(fh->state, 0);
+	struct v4l2_rect *try_crop;
 
 	/* Initialize try_fmt */
 	try_fmt->width = supported_modes[0].width;
 	try_fmt->height = supported_modes[0].height;
-	try_fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	try_fmt->code = imx258_get_format_code(imx258);
 	try_fmt->field = V4L2_FIELD_NONE;
 
+	/* Initialize try_crop */
+	try_crop = v4l2_subdev_state_get_crop(fh->state, 0);
+	try_crop->left = IMX258_PIXEL_ARRAY_LEFT;
+	try_crop->top = IMX258_PIXEL_ARRAY_TOP;
+	try_crop->width = IMX258_PIXEL_ARRAY_WIDTH;
+	try_crop->height = IMX258_PIXEL_ARRAY_HEIGHT;
+
 	return 0;
 }
 
-static int imx258_update_digital_gain(struct imx258 *imx258, u32 len, u32 val)
+static int imx258_update_digital_gain(struct imx258 *imx258, u32 val)
 {
-	int ret;
+	int ret = 0;
 
-	ret = imx258_write_reg(imx258, IMX258_REG_GR_DIGITAL_GAIN,
-				IMX258_REG_VALUE_16BIT,
-				val);
-	if (ret)
-		return ret;
-	ret = imx258_write_reg(imx258, IMX258_REG_GB_DIGITAL_GAIN,
-				IMX258_REG_VALUE_16BIT,
-				val);
-	if (ret)
-		return ret;
-	ret = imx258_write_reg(imx258, IMX258_REG_R_DIGITAL_GAIN,
-				IMX258_REG_VALUE_16BIT,
-				val);
-	if (ret)
-		return ret;
-	ret = imx258_write_reg(imx258, IMX258_REG_B_DIGITAL_GAIN,
-				IMX258_REG_VALUE_16BIT,
-				val);
-	if (ret)
-		return ret;
-	return 0;
+	cci_write(imx258->regmap, IMX258_REG_GR_DIGITAL_GAIN, val, &ret);
+	cci_write(imx258->regmap, IMX258_REG_GB_DIGITAL_GAIN, val, &ret);
+	cci_write(imx258->regmap, IMX258_REG_R_DIGITAL_GAIN, val, &ret);
+	cci_write(imx258->regmap, IMX258_REG_B_DIGITAL_GAIN, val, &ret);
+
+	return ret;
+}
+
+static void imx258_adjust_exposure_range(struct imx258 *imx258)
+{
+	int exposure_max, exposure_def;
+
+	/* Honour the VBLANK limits when setting exposure. */
+	exposure_max = imx258->cur_mode->height + imx258->vblank->val -
+		       IMX258_EXPOSURE_OFFSET;
+	exposure_def = min(exposure_max, imx258->exposure->val);
+	__v4l2_ctrl_modify_range(imx258->exposure, imx258->exposure->minimum,
+				 exposure_max, imx258->exposure->step,
+				 exposure_def);
 }
 
 static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
@@ -754,6 +755,13 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
 	int ret = 0;
 
 	/*
+	 * The VBLANK control may change the limits of usable exposure, so check
+	 * and adjust if necessary.
+	 */
+	if (ctrl->id == V4L2_CID_VBLANK)
+		imx258_adjust_exposure_range(imx258);
+
+	/*
 	 * Applying V4L2 control value only happens
 	 * when power is up for streaming
 	 */
@@ -762,44 +770,46 @@ static int imx258_set_ctrl(struct v4l2_ctrl *ctrl)
 
 	switch (ctrl->id) {
 	case V4L2_CID_ANALOGUE_GAIN:
-		ret = imx258_write_reg(imx258, IMX258_REG_ANALOG_GAIN,
-				IMX258_REG_VALUE_16BIT,
-				ctrl->val);
+		ret = cci_write(imx258->regmap, IMX258_REG_ANALOG_GAIN,
+				ctrl->val, NULL);
 		break;
 	case V4L2_CID_EXPOSURE:
-		ret = imx258_write_reg(imx258, IMX258_REG_EXPOSURE,
-				IMX258_REG_VALUE_16BIT,
-				ctrl->val);
+		ret = cci_write(imx258->regmap, IMX258_REG_EXPOSURE,
+				ctrl->val, NULL);
 		break;
 	case V4L2_CID_DIGITAL_GAIN:
-		ret = imx258_update_digital_gain(imx258, IMX258_REG_VALUE_16BIT,
-				ctrl->val);
+		ret = imx258_update_digital_gain(imx258, ctrl->val);
 		break;
 	case V4L2_CID_TEST_PATTERN:
-		ret = imx258_write_reg(imx258, IMX258_REG_TEST_PATTERN,
-				IMX258_REG_VALUE_16BIT,
-				ctrl->val);
-		ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
-				IMX258_REG_VALUE_08BIT,
-				!ctrl->val ? REG_CONFIG_MIRROR_FLIP :
-				REG_CONFIG_FLIP_TEST_PATTERN);
+		ret = cci_write(imx258->regmap, IMX258_REG_TEST_PATTERN,
+				ctrl->val, NULL);
 		break;
 	case V4L2_CID_WIDE_DYNAMIC_RANGE:
 		if (!ctrl->val) {
-			ret = imx258_write_reg(imx258, IMX258_REG_HDR,
-					       IMX258_REG_VALUE_08BIT,
-					       IMX258_HDR_RATIO_MIN);
+			ret = cci_write(imx258->regmap, IMX258_REG_HDR,
+					IMX258_HDR_RATIO_MIN, NULL);
 		} else {
-			ret = imx258_write_reg(imx258, IMX258_REG_HDR,
-					       IMX258_REG_VALUE_08BIT,
-					       IMX258_HDR_ON);
+			ret = cci_write(imx258->regmap, IMX258_REG_HDR,
+					IMX258_HDR_ON, NULL);
 			if (ret)
 				break;
-			ret = imx258_write_reg(imx258, IMX258_REG_HDR_RATIO,
-					       IMX258_REG_VALUE_08BIT,
-					       BIT(IMX258_HDR_RATIO_MAX));
+			ret = cci_write(imx258->regmap, IMX258_REG_HDR_RATIO,
+					BIT(IMX258_HDR_RATIO_MAX), NULL);
 		}
 		break;
+	case V4L2_CID_VBLANK:
+		ret = cci_write(imx258->regmap, IMX258_REG_FRM_LENGTH_LINES,
+				imx258->cur_mode->height + ctrl->val, NULL);
+		break;
+	case V4L2_CID_VFLIP:
+	case V4L2_CID_HFLIP:
+		ret = cci_write(imx258->regmap, REG_MIRROR_FLIP_CONTROL,
+				(imx258->hflip->val ?
+				 REG_CONFIG_MIRROR_HFLIP : 0) |
+				(imx258->vflip->val ?
+				 REG_CONFIG_MIRROR_VFLIP : 0),
+				NULL);
+		break;
 	default:
 		dev_info(&client->dev,
 			 "ctrl(id:0x%x,val:0x%x) is not handled\n",
@@ -821,11 +831,13 @@ static int imx258_enum_mbus_code(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_state *sd_state,
 				  struct v4l2_subdev_mbus_code_enum *code)
 {
-	/* Only one bayer order(GRBG) is supported */
+	struct imx258 *imx258 = to_imx258(sd);
+
+	/* Only one bayer format (10 bit) is supported */
 	if (code->index > 0)
 		return -EINVAL;
 
-	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	code->code = imx258_get_format_code(imx258);
 
 	return 0;
 }
@@ -834,10 +846,11 @@ static int imx258_enum_frame_size(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_state *sd_state,
 				  struct v4l2_subdev_frame_size_enum *fse)
 {
+	struct imx258 *imx258 = to_imx258(sd);
 	if (fse->index >= ARRAY_SIZE(supported_modes))
 		return -EINVAL;
 
-	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+	if (fse->code != imx258_get_format_code(imx258))
 		return -EINVAL;
 
 	fse->min_width = supported_modes[fse->index].width;
@@ -848,12 +861,13 @@ static int imx258_enum_frame_size(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static void imx258_update_pad_format(const struct imx258_mode *mode,
+static void imx258_update_pad_format(struct imx258 *imx258,
+				     const struct imx258_mode *mode,
 				     struct v4l2_subdev_format *fmt)
 {
 	fmt->format.width = mode->width;
 	fmt->format.height = mode->height;
-	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.code = imx258_get_format_code(imx258);
 	fmt->format.field = V4L2_FIELD_NONE;
 }
 
@@ -865,7 +879,7 @@ static int __imx258_get_pad_format(struct imx258 *imx258,
 		fmt->format = *v4l2_subdev_state_get_format(sd_state,
 							    fmt->pad);
 	else
-		imx258_update_pad_format(imx258->cur_mode, fmt);
+		imx258_update_pad_format(imx258, imx258->cur_mode, fmt);
 
 	return 0;
 }
@@ -889,8 +903,10 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_format *fmt)
 {
 	struct imx258 *imx258 = to_imx258(sd);
-	const struct imx258_mode *mode;
+	const struct imx258_link_freq_config *link_freq_cfgs;
+	const struct imx258_link_cfg *link_cfg;
 	struct v4l2_mbus_framefmt *framefmt;
+	const struct imx258_mode *mode;
 	s32 vblank_def;
 	s32 vblank_min;
 	s64 h_blank;
@@ -899,13 +915,12 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 
 	mutex_lock(&imx258->mutex);
 
-	/* Only one raw bayer(GBRG) order is supported */
-	fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->format.code = imx258_get_format_code(imx258);
 
 	mode = v4l2_find_nearest_size(supported_modes,
 		ARRAY_SIZE(supported_modes), width, height,
 		fmt->format.width, fmt->format.height);
-	imx258_update_pad_format(mode, fmt);
+	imx258_update_pad_format(imx258, mode, fmt);
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		framefmt = v4l2_subdev_state_get_format(sd_state, fmt->pad);
 		*framefmt = fmt->format;
@@ -913,9 +928,14 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 		imx258->cur_mode = mode;
 		__v4l2_ctrl_s_ctrl(imx258->link_freq, mode->link_freq_index);
 
-		link_freq = link_freq_menu_items[mode->link_freq_index];
-		pixel_rate = link_freq_to_pixel_rate(link_freq);
-		__v4l2_ctrl_s_ctrl_int64(imx258->pixel_rate, pixel_rate);
+		link_freq = imx258->link_freq_menu_items[mode->link_freq_index];
+		link_freq_cfgs =
+			&imx258->link_freq_configs[mode->link_freq_index];
+
+		link_cfg = &link_freq_cfgs->link_cfg[imx258->lane_mode_idx];
+		pixel_rate = link_freq_to_pixel_rate(link_freq, link_cfg);
+		__v4l2_ctrl_modify_range(imx258->pixel_rate, pixel_rate,
+					 pixel_rate, 1, pixel_rate);
 		/* Update limits and set FPS to default */
 		vblank_def = imx258->cur_mode->vts_def -
 			     imx258->cur_mode->height;
@@ -927,7 +947,7 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 			vblank_def);
 		__v4l2_ctrl_s_ctrl(imx258->vblank, vblank_def);
 		h_blank =
-			link_freq_configs[mode->link_freq_index].pixels_per_line
+			imx258->link_freq_configs[mode->link_freq_index].pixels_per_line
 			 - imx258->cur_mode->width;
 		__v4l2_ctrl_modify_range(imx258->hblank, h_blank,
 					 h_blank, 1, h_blank);
@@ -938,48 +958,125 @@ static int imx258_set_pad_format(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static const struct v4l2_rect *
+__imx258_get_pad_crop(struct imx258 *imx258,
+		      struct v4l2_subdev_state *sd_state,
+		      unsigned int pad, enum v4l2_subdev_format_whence which)
+{
+	switch (which) {
+	case V4L2_SUBDEV_FORMAT_TRY:
+		return v4l2_subdev_state_get_crop(sd_state, pad);
+	case V4L2_SUBDEV_FORMAT_ACTIVE:
+		return &imx258->cur_mode->crop;
+	}
+
+	return NULL;
+}
+
+static int imx258_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		struct imx258 *imx258 = to_imx258(sd);
+
+		mutex_lock(&imx258->mutex);
+		sel->r = *__imx258_get_pad_crop(imx258, sd_state, sel->pad,
+						sel->which);
+		mutex_unlock(&imx258->mutex);
+
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = IMX258_NATIVE_WIDTH;
+		sel->r.height = IMX258_NATIVE_HEIGHT;
+
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = IMX258_PIXEL_ARRAY_LEFT;
+		sel->r.top = IMX258_PIXEL_ARRAY_TOP;
+		sel->r.width = IMX258_PIXEL_ARRAY_WIDTH;
+		sel->r.height = IMX258_PIXEL_ARRAY_HEIGHT;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
 /* Start streaming */
 static int imx258_start_streaming(struct imx258 *imx258)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 	const struct imx258_reg_list *reg_list;
+	const struct imx258_link_freq_config *link_freq_cfg;
 	int ret, link_freq_index;
 
+	ret = cci_write(imx258->regmap, IMX258_REG_RESET, 0x01, NULL);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to reset sensor\n", __func__);
+		return ret;
+	}
+
+	/* 12ms is required from poweron to standby */
+	fsleep(12000);
+
 	/* Setup PLL */
 	link_freq_index = imx258->cur_mode->link_freq_index;
-	reg_list = &link_freq_configs[link_freq_index].reg_list;
-	ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+	link_freq_cfg = &imx258->link_freq_configs[link_freq_index];
+
+	reg_list = &link_freq_cfg->link_cfg[imx258->lane_mode_idx].reg_list;
+	ret = cci_multi_reg_write(imx258->regmap, reg_list->regs, reg_list->num_of_regs, NULL);
 	if (ret) {
 		dev_err(&client->dev, "%s failed to set plls\n", __func__);
 		return ret;
 	}
 
-	/* Apply default values of current mode */
-	reg_list = &imx258->cur_mode->reg_list;
-	ret = imx258_write_regs(imx258, reg_list->regs, reg_list->num_of_regs);
+	ret = cci_multi_reg_write(imx258->regmap, mode_common_regs,
+				  ARRAY_SIZE(mode_common_regs), NULL);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		dev_err(&client->dev, "%s failed to set common regs\n", __func__);
 		return ret;
 	}
 
-	/* Set Orientation be 180 degree */
-	ret = imx258_write_reg(imx258, REG_MIRROR_FLIP_CONTROL,
-			       IMX258_REG_VALUE_08BIT, REG_CONFIG_MIRROR_FLIP);
+	ret = cci_multi_reg_write(imx258->regmap, imx258->variant_cfg->regs,
+				  imx258->variant_cfg->num_regs, NULL);
 	if (ret) {
-		dev_err(&client->dev, "%s failed to set orientation\n",
+		dev_err(&client->dev, "%s failed to set variant config\n",
 			__func__);
 		return ret;
 	}
 
+	ret = cci_write(imx258->regmap, IMX258_CLK_BLANK_STOP,
+			!!(imx258->csi2_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK),
+			NULL);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set clock lane mode\n", __func__);
+		return ret;
+	}
+
+	/* Apply default values of current mode */
+	reg_list = &imx258->cur_mode->reg_list;
+	ret = cci_multi_reg_write(imx258->regmap, reg_list->regs, reg_list->num_of_regs, NULL);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set mode\n", __func__);
+		return ret;
+	}
+
 	/* Apply customized values from user */
 	ret =  __v4l2_ctrl_handler_setup(imx258->sd.ctrl_handler);
 	if (ret)
 		return ret;
 
 	/* set stream on register */
-	return imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
-				IMX258_REG_VALUE_08BIT,
-				IMX258_MODE_STREAMING);
+	return cci_write(imx258->regmap, IMX258_REG_MODE_SELECT,
+			 IMX258_MODE_STREAMING, NULL);
 }
 
 /* Stop streaming */
@@ -989,8 +1086,8 @@ static int imx258_stop_streaming(struct imx258 *imx258)
 	int ret;
 
 	/* set stream off register */
-	ret = imx258_write_reg(imx258, IMX258_REG_MODE_SELECT,
-		IMX258_REG_VALUE_08BIT, IMX258_MODE_STANDBY);
+	ret = cci_write(imx258->regmap, IMX258_REG_MODE_SELECT,
+			IMX258_MODE_STANDBY, NULL);
 	if (ret)
 		dev_err(&client->dev, "%s failed to set stream\n", __func__);
 
@@ -1007,9 +1104,19 @@ static int imx258_power_on(struct device *dev)
 	struct imx258 *imx258 = to_imx258(sd);
 	int ret;
 
+	ret = regulator_bulk_enable(IMX258_NUM_SUPPLIES,
+				    imx258->supplies);
+	if (ret) {
+		dev_err(dev, "%s: failed to enable regulators\n",
+			__func__);
+		return ret;
+	}
+
 	ret = clk_prepare_enable(imx258->clk);
-	if (ret)
+	if (ret) {
 		dev_err(dev, "failed to enable clock\n");
+		regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
+	}
 
 	return ret;
 }
@@ -1020,6 +1127,7 @@ static int imx258_power_off(struct device *dev)
 	struct imx258 *imx258 = to_imx258(sd);
 
 	clk_disable_unprepare(imx258->clk);
+	regulator_bulk_disable(IMX258_NUM_SUPPLIES, imx258->supplies);
 
 	return 0;
 }
@@ -1066,10 +1174,10 @@ static int imx258_identify_module(struct imx258 *imx258)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
 	int ret;
-	u32 val;
+	u64 val;
 
-	ret = imx258_read_reg(imx258, IMX258_REG_CHIP_ID,
-			      IMX258_REG_VALUE_16BIT, &val);
+	ret = cci_read(imx258->regmap, IMX258_REG_CHIP_ID,
+		       &val, NULL);
 	if (ret) {
 		dev_err(&client->dev, "failed to read chip id %x\n",
 			IMX258_CHIP_ID);
@@ -1077,7 +1185,7 @@ static int imx258_identify_module(struct imx258 *imx258)
 	}
 
 	if (val != IMX258_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+		dev_err(&client->dev, "chip id mismatch: %x!=%llx\n",
 			IMX258_CHIP_ID, val);
 		return -EIO;
 	}
@@ -1094,6 +1202,7 @@ static const struct v4l2_subdev_pad_ops imx258_pad_ops = {
 	.get_fmt = imx258_get_pad_format,
 	.set_fmt = imx258_set_pad_format,
 	.enum_frame_size = imx258_enum_frame_size,
+	.get_selection = imx258_get_selection,
 };
 
 static const struct v4l2_subdev_ops imx258_subdev_ops = {
@@ -1109,13 +1218,13 @@ static const struct v4l2_subdev_internal_ops imx258_internal_ops = {
 static int imx258_init_controls(struct imx258 *imx258)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&imx258->sd);
+	const struct imx258_link_freq_config *link_freq_cfgs;
 	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl_handler *ctrl_hdlr;
-	struct v4l2_ctrl *vflip, *hflip;
+	const struct imx258_link_cfg *link_cfg;
 	s64 vblank_def;
 	s64 vblank_min;
-	s64 pixel_rate_min;
-	s64 pixel_rate_max;
+	s64 pixel_rate;
 	int ret;
 
 	ctrl_hdlr = &imx258->ctrl_handler;
@@ -1128,32 +1237,33 @@ static int imx258_init_controls(struct imx258 *imx258)
 	imx258->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
 				&imx258_ctrl_ops,
 				V4L2_CID_LINK_FREQ,
-				ARRAY_SIZE(link_freq_menu_items) - 1,
+				ARRAY_SIZE(link_freq_menu_items_19_2) - 1,
 				0,
-				link_freq_menu_items);
+				imx258->link_freq_menu_items);
 
 	if (imx258->link_freq)
 		imx258->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-	/* The driver only supports one bayer order and flips by default. */
-	hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
-				  V4L2_CID_HFLIP, 1, 1, 1, 1);
-	if (hflip)
-		hflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	imx258->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+					  V4L2_CID_HFLIP, 0, 1, 1, 1);
+	if (imx258->hflip)
+		imx258->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	imx258->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
+					  V4L2_CID_VFLIP, 0, 1, 1, 1);
+	if (imx258->vflip)
+		imx258->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 
-	vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
-				  V4L2_CID_VFLIP, 1, 1, 1, 1);
-	if (vflip)
-		vflip->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	link_freq_cfgs = &imx258->link_freq_configs[0];
+	link_cfg = link_freq_cfgs[imx258->lane_mode_idx].link_cfg;
+	pixel_rate = link_freq_to_pixel_rate(imx258->link_freq_menu_items[0],
+					     link_cfg);
 
-	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
-	pixel_rate_min = link_freq_to_pixel_rate(link_freq_menu_items[1]);
 	/* By default, PIXEL_RATE is read only */
 	imx258->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx258_ctrl_ops,
 				V4L2_CID_PIXEL_RATE,
-				pixel_rate_min, pixel_rate_max,
-				1, pixel_rate_max);
-
+				pixel_rate, pixel_rate,
+				1, pixel_rate);
 
 	vblank_def = imx258->cur_mode->vts_def - imx258->cur_mode->height;
 	vblank_min = imx258->cur_mode->vts_min - imx258->cur_mode->height;
@@ -1163,9 +1273,6 @@ static int imx258_init_controls(struct imx258 *imx258)
 				IMX258_VTS_MAX - imx258->cur_mode->height, 1,
 				vblank_def);
 
-	if (imx258->vblank)
-		imx258->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-
 	imx258->hblank = v4l2_ctrl_new_std(
 				ctrl_hdlr, &imx258_ctrl_ops, V4L2_CID_HBLANK,
 				IMX258_PPL_DEFAULT - imx258->cur_mode->width,
@@ -1232,9 +1339,25 @@ static void imx258_free_controls(struct imx258 *imx258)
 	mutex_destroy(&imx258->mutex);
 }
 
+static int imx258_get_regulators(struct imx258 *imx258,
+				 struct i2c_client *client)
+{
+	unsigned int i;
+
+	for (i = 0; i < IMX258_NUM_SUPPLIES; i++)
+		imx258->supplies[i].supply = imx258_supply_name[i];
+
+	return devm_regulator_bulk_get(&client->dev,
+				    IMX258_NUM_SUPPLIES, imx258->supplies);
+}
+
 static int imx258_probe(struct i2c_client *client)
 {
 	struct imx258 *imx258;
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint ep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
 	int ret;
 	u32 val = 0;
 
@@ -1242,6 +1365,18 @@ static int imx258_probe(struct i2c_client *client)
 	if (!imx258)
 		return -ENOMEM;
 
+	imx258->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx258->regmap)) {
+		ret = PTR_ERR(imx258->regmap);
+		dev_err(&client->dev, "failed to initialize CCI: %d\n", ret);
+		return ret;
+	}
+
+	ret = imx258_get_regulators(imx258, client);
+	if (ret)
+		return dev_err_probe(&client->dev, ret,
+				     "failed to get regulators\n");
+
 	imx258->clk = devm_clk_get_optional(&client->dev, NULL);
 	if (IS_ERR(imx258->clk))
 		return dev_err_probe(&client->dev, PTR_ERR(imx258->clk),
@@ -1254,18 +1389,74 @@ static int imx258_probe(struct i2c_client *client)
 	} else {
 		val = clk_get_rate(imx258->clk);
 	}
-	if (val != IMX258_INPUT_CLOCK_FREQ) {
-		dev_err(&client->dev, "input clock frequency not supported\n");
+
+	switch (val) {
+	case 19200000:
+		imx258->link_freq_configs = link_freq_configs_19_2;
+		imx258->link_freq_menu_items = link_freq_menu_items_19_2;
+		break;
+	case 24000000:
+		imx258->link_freq_configs = link_freq_configs_24;
+		imx258->link_freq_menu_items = link_freq_menu_items_24;
+		break;
+	default:
+		dev_err(&client->dev, "input clock frequency of %u not supported\n",
+			val);
 		return -EINVAL;
 	}
 
+	endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev), NULL);
+	if (!endpoint) {
+		dev_err(&client->dev, "Endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(&client->dev, "Parsing endpoint node failed\n");
+		return ret;
+	}
+
+	ret = v4l2_link_freq_to_bitmap(&client->dev,
+				       ep.link_frequencies,
+				       ep.nr_of_link_frequencies,
+				       imx258->link_freq_menu_items,
+				       ARRAY_SIZE(link_freq_menu_items_19_2),
+				       &imx258->link_freq_bitmap);
+	if (ret) {
+		dev_err(&client->dev, "Link frequency not supported\n");
+		goto error_endpoint_free;
+	}
+
+	/* Get number of data lanes */
+	switch (ep.bus.mipi_csi2.num_data_lanes) {
+	case 2:
+		imx258->lane_mode_idx = IMX258_2_LANE_MODE;
+		break;
+	case 4:
+		imx258->lane_mode_idx = IMX258_4_LANE_MODE;
+		break;
+	default:
+		dev_err(&client->dev, "Invalid data lanes: %u\n",
+			ep.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto error_endpoint_free;
+	}
+
+	imx258->csi2_flags = ep.bus.mipi_csi2.flags;
+
+	imx258->variant_cfg = device_get_match_data(&client->dev);
+	if (!imx258->variant_cfg)
+		imx258->variant_cfg = &imx258_cfg;
+
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&imx258->sd, client, &imx258_subdev_ops);
 
 	/* Will be powered off via pm_runtime_idle */
 	ret = imx258_power_on(&client->dev);
 	if (ret)
-		return ret;
+		goto error_endpoint_free;
 
 	/* Check module identity */
 	ret = imx258_identify_module(imx258);
@@ -1298,6 +1489,7 @@ static int imx258_probe(struct i2c_client *client)
 	pm_runtime_set_active(&client->dev);
 	pm_runtime_enable(&client->dev);
 	pm_runtime_idle(&client->dev);
+	v4l2_fwnode_endpoint_free(&ep);
 
 	return 0;
 
@@ -1310,6 +1502,9 @@ error_handler_free:
 error_identify:
 	imx258_power_off(&client->dev);
 
+error_endpoint_free:
+	v4l2_fwnode_endpoint_free(&ep);
+
 	return ret;
 }
 
@@ -1342,7 +1537,8 @@ MODULE_DEVICE_TABLE(acpi, imx258_acpi_ids);
 #endif
 
 static const struct of_device_id imx258_dt_ids[] = {
-	{ .compatible = "sony,imx258" },
+	{ .compatible = "sony,imx258", .data = &imx258_cfg },
+	{ .compatible = "sony,imx258-pdaf", .data = &imx258_pdaf_cfg },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, imx258_dt_ids);
diff --git a/drivers/media/i2c/imx274.c b/drivers/media/i2c/imx274.c
index 3800de974e8a..a2b824986027 100644
--- a/drivers/media/i2c/imx274.c
+++ b/drivers/media/i2c/imx274.c
@@ -1949,7 +1949,7 @@ static const struct of_device_id imx274_of_id_table[] = {
 MODULE_DEVICE_TABLE(of, imx274_of_id_table);
 
 static const struct i2c_device_id imx274_id[] = {
-	{ "IMX274", 0 },
+	{ "IMX274" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, imx274_id);
diff --git a/drivers/media/i2c/imx283.c b/drivers/media/i2c/imx283.c
new file mode 100644
index 000000000000..da618c8cbadc
--- /dev/null
+++ b/drivers/media/i2c/imx283.c
@@ -0,0 +1,1624 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * V4L2 Support for the IMX283
+ *
+ * Diagonal 15.86 mm (Type 1) CMOS Image Sensor with Square Pixel for Color
+ * Cameras.
+ *
+ * Copyright (C) 2024 Ideas on Board Oy.
+ *
+ * Based on Sony IMX283 driver prepared by Will Whang
+ *
+ * Based on Sony imx477 camera driver
+ * Copyright (C) 2019-2020 Raspberry Pi (Trading) Ltd
+ */
+
+#include <linux/array_size.h>
+#include <linux/bitops.h>
+#include <linux/container_of.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/minmax.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/types.h>
+#include <linux/units.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mediabus.h>
+
+/* Chip ID */
+#define IMX283_REG_CHIP_ID		CCI_REG8(0x3000)
+#define IMX283_CHIP_ID			0x0b	// Default power on state
+
+#define IMX283_REG_STANDBY		CCI_REG8(0x3000)
+#define   IMX283_ACTIVE			0
+#define   IMX283_STANDBY		BIT(0)
+#define   IMX283_STBLOGIC		BIT(1)
+#define   IMX283_STBMIPI		BIT(2)
+#define   IMX283_STBDV			BIT(3)
+#define   IMX283_SLEEP			BIT(4)
+
+#define IMX283_REG_CLAMP		CCI_REG8(0x3001)
+#define   IMX283_CLPSQRST		BIT(4)
+
+#define IMX283_REG_PLSTMG08		CCI_REG8(0x3003)
+#define   IMX283_PLSTMG08_VAL		0x77
+
+#define IMX283_REG_MDSEL1		CCI_REG8(0x3004)
+#define IMX283_REG_MDSEL2		CCI_REG8(0x3005)
+#define IMX283_REG_MDSEL3		CCI_REG8(0x3006)
+#define   IMX283_MDSEL3_VCROP_EN	BIT(5)
+#define IMX283_REG_MDSEL4		CCI_REG8(0x3007)
+#define   IMX283_MDSEL4_VCROP_EN	(BIT(4) | BIT(6))
+
+#define IMX283_REG_SVR			CCI_REG16_LE(0x3009)
+
+#define IMX283_REG_HTRIMMING		CCI_REG8(0x300b)
+#define   IMX283_MDVREV			BIT(0) /* VFLIP */
+#define   IMX283_HTRIMMING_EN		BIT(4)
+
+#define IMX283_REG_VWINPOS		CCI_REG16_LE(0x300f)
+#define IMX283_REG_VWIDCUT		CCI_REG16_LE(0x3011)
+
+#define IMX283_REG_MDSEL7		CCI_REG16_LE(0x3013)
+
+/* CSI Clock Configuration */
+#define IMX283_REG_TCLKPOST		CCI_REG8(0x3018)
+#define IMX283_REG_THSPREPARE		CCI_REG8(0x301a)
+#define IMX283_REG_THSZERO		CCI_REG8(0x301c)
+#define IMX283_REG_THSTRAIL		CCI_REG8(0x301e)
+#define IMX283_REG_TCLKTRAIL		CCI_REG8(0x3020)
+#define IMX283_REG_TCLKPREPARE		CCI_REG8(0x3022)
+#define IMX283_REG_TCLKZERO		CCI_REG16_LE(0x3024)
+#define IMX283_REG_TLPX			CCI_REG8(0x3026)
+#define IMX283_REG_THSEXIT		CCI_REG8(0x3028)
+#define IMX283_REG_TCLKPRE		CCI_REG8(0x302a)
+#define IMX283_REG_SYSMODE		CCI_REG8(0x3104)
+
+#define IMX283_REG_Y_OUT_SIZE		CCI_REG16_LE(0x302f)
+#define IMX283_REG_WRITE_VSIZE		CCI_REG16_LE(0x3031)
+#define IMX283_REG_OB_SIZE_V		CCI_REG8(0x3033)
+
+/* HMAX internal HBLANK */
+#define IMX283_REG_HMAX			CCI_REG16_LE(0x3036)
+#define IMX283_HMAX_MAX			(BIT(16) - 1)
+
+/* VMAX internal VBLANK */
+#define IMX283_REG_VMAX			CCI_REG24_LE(0x3038)
+#define   IMX283_VMAX_MAX		(BIT(16) - 1)
+
+/* SHR internal */
+#define IMX283_REG_SHR			CCI_REG16_LE(0x303b)
+#define   IMX283_SHR_MIN		11
+
+/*
+ * Analog gain control
+ *  Gain [dB] = -20log{(2048 - value [10:0]) /2048}
+ *  Range: 0dB to approximately +27dB
+ */
+#define IMX283_REG_ANALOG_GAIN		CCI_REG16_LE(0x3042)
+#define   IMX283_ANA_GAIN_MIN		0
+#define   IMX283_ANA_GAIN_MAX		1957
+#define   IMX283_ANA_GAIN_STEP		1
+#define   IMX283_ANA_GAIN_DEFAULT	0x0
+
+/*
+ * Digital gain control
+ *  Gain [dB] = value * 6
+ *  Range: 0dB to +18db
+ */
+#define IMX283_REG_DIGITAL_GAIN		CCI_REG8(0x3044)
+#define IMX283_DGTL_GAIN_MIN		0
+#define IMX283_DGTL_GAIN_MAX		3
+#define IMX283_DGTL_GAIN_DEFAULT	0
+#define IMX283_DGTL_GAIN_STEP		1
+
+#define IMX283_REG_HTRIMMING_START	CCI_REG16_LE(0x3058)
+#define IMX283_REG_HTRIMMING_END	CCI_REG16_LE(0x305a)
+
+#define IMX283_REG_MDSEL18		CCI_REG16_LE(0x30f6)
+
+/* Master Mode Operation Control */
+#define IMX283_REG_XMSTA		CCI_REG8(0x3105)
+#define   IMX283_XMSTA			BIT(0)
+
+#define IMX283_REG_SYNCDRV		CCI_REG8(0x3107)
+#define   IMX283_SYNCDRV_XHS_XVS	(0xa0 | 0x02)
+#define   IMX283_SYNCDRV_HIZ		(0xa0 | 0x03)
+
+/* PLL Standby */
+#define IMX283_REG_STBPL		CCI_REG8(0x320b)
+#define  IMX283_STBPL_NORMAL		0x00
+#define  IMX283_STBPL_STANDBY		0x03
+
+/* Input Frequency Setting */
+#define IMX283_REG_PLRD1		CCI_REG8(0x36c1)
+#define IMX283_REG_PLRD2		CCI_REG16_LE(0x36c2)
+#define IMX283_REG_PLRD3		CCI_REG8(0x36f7)
+#define IMX283_REG_PLRD4		CCI_REG8(0x36f8)
+
+#define IMX283_REG_PLSTMG02		CCI_REG8(0x36aa)
+#define   IMX283_PLSTMG02_VAL		0x00
+
+#define IMX283_REG_EBD_X_OUT_SIZE	CCI_REG16_LE(0x3a54)
+
+/* Test pattern generator */
+#define IMX283_REG_TPG_CTRL		CCI_REG8(0x3156)
+#define   IMX283_TPG_CTRL_CLKEN		BIT(0)
+#define   IMX283_TPG_CTRL_PATEN		BIT(4)
+
+#define IMX283_REG_TPG_PAT		CCI_REG8(0x3157)
+#define   IMX283_TPG_PAT_ALL_000	0x00
+#define   IMX283_TPG_PAT_ALL_FFF	0x01
+#define   IMX283_TPG_PAT_ALL_555	0x02
+#define   IMX283_TPG_PAT_ALL_AAA	0x03
+#define   IMX283_TPG_PAT_H_COLOR_BARS	0x0a
+#define   IMX283_TPG_PAT_V_COLOR_BARS	0x0b
+
+/* Exposure control */
+#define IMX283_EXPOSURE_MIN		52
+#define IMX283_EXPOSURE_STEP		1
+#define IMX283_EXPOSURE_DEFAULT		1000
+#define IMX283_EXPOSURE_MAX		49865
+
+#define IMAGE_PAD			0
+
+#define IMX283_XCLR_MIN_DELAY_US	(1 * USEC_PER_MSEC)
+#define IMX283_XCLR_DELAY_RANGE_US	(1 * USEC_PER_MSEC)
+
+/* IMX283 native and active pixel array size. */
+static const struct v4l2_rect imx283_native_area = {
+	.top = 0,
+	.left = 0,
+	.width = 5592,
+	.height = 3710,
+};
+
+static const struct v4l2_rect imx283_active_area = {
+	.top = 40,
+	.left = 108,
+	.width = 5472,
+	.height = 3648,
+};
+
+struct imx283_reg_list {
+	unsigned int num_of_regs;
+	const struct cci_reg_sequence *regs;
+};
+
+/* Mode : resolution and related config values */
+struct imx283_mode {
+	unsigned int mode;
+
+	/* Bits per pixel */
+	unsigned int bpp;
+
+	/* Frame width */
+	unsigned int width;
+
+	/* Frame height */
+	unsigned int height;
+
+	/*
+	 * Minimum horizontal timing in pixel-units
+	 *
+	 * Note that HMAX is written in 72MHz units, and the datasheet assumes a
+	 * 720MHz link frequency. Convert datasheet values with the following:
+	 *
+	 * For 12 bpp modes (480Mbps) convert with:
+	 *   hmax = [hmax in 72MHz units] * 480 / 72
+	 *
+	 * For 10 bpp modes (576Mbps) convert with:
+	 *   hmax = [hmax in 72MHz units] * 576 / 72
+	 */
+	u32 min_hmax;
+
+	/* minimum V-timing in lines */
+	u32 min_vmax;
+
+	/* default H-timing */
+	u32 default_hmax;
+
+	/* default V-timing */
+	u32 default_vmax;
+
+	/* minimum SHR */
+	u32 min_shr;
+
+	/*
+	 * Per-mode vertical crop constants used to calculate values
+	 * of IMX283REG_WIDCUT and IMX283_REG_VWINPOS.
+	 */
+	u32 veff;
+	u32 vst;
+	u32 vct;
+
+	/* Horizontal and vertical binning ratio */
+	u8 hbin_ratio;
+	u8 vbin_ratio;
+
+	/* Optical Blanking */
+	u32 horizontal_ob;
+	u32 vertical_ob;
+
+	/* Analog crop rectangle. */
+	struct v4l2_rect crop;
+};
+
+struct imx283_input_frequency {
+	unsigned int mhz;
+	unsigned int reg_count;
+	struct cci_reg_sequence regs[4];
+};
+
+static const struct imx283_input_frequency imx283_frequencies[] = {
+	{
+		.mhz = 6 * HZ_PER_MHZ,
+		.reg_count = 4,
+		.regs = {
+			{ IMX283_REG_PLRD1, 0x00 },
+			{ IMX283_REG_PLRD2, 0x00f0 },
+			{ IMX283_REG_PLRD3, 0x00 },
+			{ IMX283_REG_PLRD4, 0xc0 },
+		},
+	},
+	{
+		.mhz = 12 * HZ_PER_MHZ,
+		.reg_count = 4,
+		.regs = {
+			{ IMX283_REG_PLRD1, 0x01 },
+			{ IMX283_REG_PLRD2, 0x00f0 },
+			{ IMX283_REG_PLRD3, 0x01 },
+			{ IMX283_REG_PLRD4, 0xc0 },
+		},
+	},
+	{
+		.mhz = 18 * HZ_PER_MHZ,
+		.reg_count = 4,
+		.regs = {
+			{ IMX283_REG_PLRD1, 0x01 },
+			{ IMX283_REG_PLRD2, 0x00a0 },
+			{ IMX283_REG_PLRD3, 0x01 },
+			{ IMX283_REG_PLRD4, 0x80 },
+		},
+	},
+	{
+		.mhz = 24 * HZ_PER_MHZ,
+		.reg_count = 4,
+		.regs = {
+			{ IMX283_REG_PLRD1, 0x02 },
+			{ IMX283_REG_PLRD2, 0x00f0 },
+			{ IMX283_REG_PLRD3, 0x02 },
+			{ IMX283_REG_PLRD4, 0xc0 },
+		},
+	},
+};
+
+enum imx283_modes {
+	IMX283_MODE_0,
+	IMX283_MODE_1,
+	IMX283_MODE_1A,
+	IMX283_MODE_1S,
+	IMX283_MODE_2,
+	IMX283_MODE_2A,
+	IMX283_MODE_3,
+	IMX283_MODE_4,
+	IMX283_MODE_5,
+	IMX283_MODE_6,
+};
+
+struct imx283_readout_mode {
+	u8 mdsel1;
+	u8 mdsel2;
+	u8 mdsel3;
+	u8 mdsel4;
+};
+
+static const struct imx283_readout_mode imx283_readout_modes[] = {
+	/* All pixel scan modes */
+	[IMX283_MODE_0] = { 0x04, 0x03, 0x10, 0x00 }, /* 12 bit */
+	[IMX283_MODE_1] = { 0x04, 0x01, 0x00, 0x00 }, /* 10 bit */
+	[IMX283_MODE_1A] = { 0x04, 0x01, 0x20, 0x50 }, /* 10 bit */
+	[IMX283_MODE_1S] = { 0x04, 0x41, 0x20, 0x50 }, /* 10 bit */
+
+	/* Horizontal / Vertical 2/2-line binning */
+	[IMX283_MODE_2] = { 0x0d, 0x11, 0x50, 0x00 }, /* 12 bit */
+	[IMX283_MODE_2A] = { 0x0d, 0x11, 0x70, 0x50 }, /* 12 bit */
+
+	/* Horizontal / Vertical 3/3-line binning */
+	[IMX283_MODE_3] = { 0x1e, 0x18, 0x10, 0x00 }, /* 12 bit */
+
+	/* Vertical 2/9 subsampling, horizontal 3 binning cropping */
+	[IMX283_MODE_4] = { 0x29, 0x18, 0x30, 0x50 }, /* 12 bit */
+
+	/* Vertical 2/19 subsampling binning, horizontal 3 binning */
+	[IMX283_MODE_5] = { 0x2d, 0x18, 0x10, 0x00 }, /* 12 bit */
+
+	/* Vertical 2 binning horizontal 2/4, subsampling 16:9 cropping */
+	[IMX283_MODE_6] = { 0x18, 0x21, 0x00, 0x09 }, /* 10 bit */
+
+	/*
+	 * New modes should make sure the offset period is complied.
+	 * See imx283_exposure() for reference.
+	 */
+};
+
+static const struct cci_reg_sequence mipi_data_rate_1440Mbps[] = {
+	/* The default register settings provide the 1440Mbps rate */
+	{ CCI_REG8(0x36c5), 0x00 }, /* Undocumented */
+	{ CCI_REG8(0x3ac4), 0x00 }, /* Undocumented */
+
+	{ IMX283_REG_STBPL, 0x00 },
+	{ IMX283_REG_TCLKPOST, 0xa7 },
+	{ IMX283_REG_THSPREPARE, 0x6f },
+	{ IMX283_REG_THSZERO, 0x9f },
+	{ IMX283_REG_THSTRAIL, 0x5f },
+	{ IMX283_REG_TCLKTRAIL, 0x5f },
+	{ IMX283_REG_TCLKPREPARE, 0x6f },
+	{ IMX283_REG_TCLKZERO, 0x017f },
+	{ IMX283_REG_TLPX, 0x4f },
+	{ IMX283_REG_THSEXIT, 0x47 },
+	{ IMX283_REG_TCLKPRE, 0x07 },
+	{ IMX283_REG_SYSMODE, 0x02 },
+};
+
+static const struct cci_reg_sequence mipi_data_rate_720Mbps[] = {
+	/* Undocumented Additions "For 720MBps" Setting */
+	{ CCI_REG8(0x36c5), 0x01 }, /* Undocumented */
+	{ CCI_REG8(0x3ac4), 0x01 }, /* Undocumented */
+
+	{ IMX283_REG_STBPL, 0x00 },
+	{ IMX283_REG_TCLKPOST, 0x77 },
+	{ IMX283_REG_THSPREPARE, 0x37 },
+	{ IMX283_REG_THSZERO, 0x67 },
+	{ IMX283_REG_THSTRAIL, 0x37 },
+	{ IMX283_REG_TCLKTRAIL, 0x37 },
+	{ IMX283_REG_TCLKPREPARE, 0x37 },
+	{ IMX283_REG_TCLKZERO, 0xdf },
+	{ IMX283_REG_TLPX, 0x2f },
+	{ IMX283_REG_THSEXIT, 0x47 },
+	{ IMX283_REG_TCLKPRE, 0x0f },
+	{ IMX283_REG_SYSMODE, 0x02 },
+};
+
+static const s64 link_frequencies[] = {
+	720 * HZ_PER_MHZ, /* 1440 Mbps lane data rate */
+	360 * HZ_PER_MHZ, /* 720 Mbps data lane rate */
+};
+
+static const struct imx283_reg_list link_freq_reglist[] = {
+	{ /* 720 MHz */
+		.num_of_regs = ARRAY_SIZE(mipi_data_rate_1440Mbps),
+		.regs = mipi_data_rate_1440Mbps,
+	},
+	{ /* 360 MHz */
+		.num_of_regs = ARRAY_SIZE(mipi_data_rate_720Mbps),
+		.regs = mipi_data_rate_720Mbps,
+	},
+};
+
+/* Mode configs */
+static const struct imx283_mode supported_modes_12bit[] = {
+	{
+		/* 20MPix 21.40 fps readout mode 0 */
+		.mode = IMX283_MODE_0,
+		.bpp = 12,
+		.width = 5472,
+		.height = 3648,
+		.min_hmax = 5914, /* 887 @ 480MHz/72MHz */
+		.min_vmax = 3793, /* Lines */
+
+		.veff = 3694,
+		.vst = 0,
+		.vct = 0,
+
+		.hbin_ratio = 1,
+		.vbin_ratio = 1,
+
+		/* 20.00 FPS */
+		.default_hmax = 6000, /* 900 @ 480MHz/72MHz */
+		.default_vmax = 4000,
+
+		.min_shr = 11,
+		.horizontal_ob = 96,
+		.vertical_ob = 16,
+		.crop = {
+			.top = 40,
+			.left = 108,
+			.width = 5472,
+			.height = 3648,
+		},
+	},
+	{
+		/*
+		 * Readout mode 2 : 2/2 binned mode (2736x1824)
+		 */
+		.mode = IMX283_MODE_2,
+		.bpp = 12,
+		.width = 2736,
+		.height = 1824,
+		.min_hmax = 2414, /* Pixels (362 * 480MHz/72MHz + padding) */
+		.min_vmax = 3840, /* Lines */
+
+		/* 50.00 FPS */
+		.default_hmax = 2500, /* 375 @ 480MHz/72Mhz */
+		.default_vmax = 3840,
+
+		.veff = 1824,
+		.vst = 0,
+		.vct = 0,
+
+		.hbin_ratio = 2,
+		.vbin_ratio = 2,
+
+		.min_shr = 12,
+		.horizontal_ob = 48,
+		.vertical_ob = 4,
+
+		.crop = {
+			.top = 40,
+			.left = 108,
+			.width = 5472,
+			.height = 3648,
+		},
+	},
+	{
+		/*
+		 * Readout mode 3 : 3/3 binned mode (1824x1216)
+		 */
+		.mode = IMX283_MODE_3,
+		.bpp = 12,
+		.width = 1824,
+		.height = 1216,
+		.min_hmax = 1894, /* Pixels (284 * 480MHz/72MHz + padding) */
+		.min_vmax = 4200, /* Lines */
+
+		/* 60.00 fps */
+		.default_hmax = 1900, /* 285 @ 480MHz/72Mhz */
+		.default_vmax = 4200,
+
+		.veff = 1234,
+		.vst = 0,
+		.vct = 0,
+
+		.hbin_ratio = 3,
+		.vbin_ratio = 3,
+
+		.min_shr = 16,
+		.horizontal_ob = 32,
+		.vertical_ob = 4,
+
+		.crop = {
+			.top = 40,
+			.left = 108,
+			.width = 5472,
+			.height = 3648,
+		},
+	},
+};
+
+static const struct imx283_mode supported_modes_10bit[] = {
+	{
+		/* 20MPix 25.48 fps readout mode 1 */
+		.mode = IMX283_MODE_1,
+		.bpp = 10,
+		.width = 5472,
+		.height = 3648,
+		.min_hmax = 5960, /* 745 @ 576MHz / 72MHz */
+		.min_vmax = 3793,
+
+		/* 25.00 FPS */
+		.default_hmax = 6000, /* 750 @ 576MHz / 72MHz */
+		.default_vmax = 3840,
+
+		.min_shr = 10,
+		.horizontal_ob = 96,
+		.vertical_ob = 16,
+		.crop = {
+			.top = 40,
+			.left = 108,
+			.width = 5472,
+			.height = 3648,
+		},
+	},
+};
+
+static const u32 imx283_mbus_codes[] = {
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+};
+
+/* regulator supplies */
+static const char *const imx283_supply_name[] = {
+	"vadd", /* Analog (2.9V) supply */
+	"vdd1", /* Supply Voltage 2 (1.8V) supply */
+	"vdd2", /* Supply Voltage 3 (1.2V) supply */
+};
+
+struct imx283 {
+	struct device *dev;
+	struct regmap *cci;
+
+	const struct imx283_input_frequency *freq;
+
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+
+	struct clk *xclk;
+
+	struct gpio_desc *reset_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(imx283_supply_name)];
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *vflip;
+
+	unsigned long link_freq_bitmap;
+
+	u16 hmax;
+	u32 vmax;
+};
+
+static inline struct imx283 *to_imx283(struct v4l2_subdev *sd)
+{
+	return container_of_const(sd, struct imx283, sd);
+}
+
+static inline void get_mode_table(unsigned int code,
+				  const struct imx283_mode **mode_list,
+				  unsigned int *num_modes)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+	case MEDIA_BUS_FMT_SGRBG12_1X12:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+		*mode_list = supported_modes_12bit;
+		*num_modes = ARRAY_SIZE(supported_modes_12bit);
+		break;
+
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		*mode_list = supported_modes_10bit;
+		*num_modes = ARRAY_SIZE(supported_modes_10bit);
+		break;
+	default:
+		*mode_list = NULL;
+		*num_modes = 0;
+		break;
+	}
+}
+
+/* Calculate the Pixel Rate based on the current mode */
+static u64 imx283_pixel_rate(struct imx283 *imx283,
+			     const struct imx283_mode *mode)
+{
+	u64 link_frequency = link_frequencies[__ffs(imx283->link_freq_bitmap)];
+	unsigned int bpp = mode->bpp;
+	const unsigned int ddr = 2; /* Double Data Rate */
+	const unsigned int lanes = 4; /* Only 4 lane support */
+	u64 numerator = link_frequency * ddr * lanes;
+
+	do_div(numerator, bpp);
+
+	return numerator;
+}
+
+/* Convert from a variable pixel_rate to 72 MHz clock cycles */
+static u64 imx283_internal_clock(unsigned int pixel_rate, unsigned int pixels)
+{
+	/*
+	 * Determine the following operation without overflow:
+	 *    pixels = 72 Mhz / pixel_rate
+	 *
+	 * The internal clock at 72MHz and Pixel Rate (between 240 and 576MHz)
+	 * can easily overflow this calculation, so pre-divide to simplify.
+	 */
+	const u32 iclk_pre = 72;
+	const u32 pclk_pre = pixel_rate / HZ_PER_MHZ;
+	u64 numerator = pixels * iclk_pre;
+
+	do_div(numerator, pclk_pre);
+
+	return numerator;
+}
+
+/* Internal clock (72MHz) to Pixel Rate clock (Variable) */
+static u64 imx283_iclk_to_pix(unsigned int pixel_rate, unsigned int cycles)
+{
+	/*
+	 * Determine the following operation without overflow:
+	 *    cycles * pixel_rate / 72 MHz
+	 *
+	 * The internal clock at 72MHz and Pixel Rate (between 240 and 576MHz)
+	 * can easily overflow this calculation, so pre-divide to simplify.
+	 */
+	const u32 iclk_pre = 72;
+	const u32 pclk_pre = pixel_rate / HZ_PER_MHZ;
+	u64 numerator = cycles * pclk_pre;
+
+	do_div(numerator, iclk_pre);
+
+	return numerator;
+}
+
+/* Determine the exposure based on current hmax, vmax and a given SHR */
+static u32 imx283_exposure(struct imx283 *imx283,
+			   const struct imx283_mode *mode, u64 shr)
+{
+	u32 svr = 0; /* SVR feature is not currently supported */
+	u32 offset;
+	u64 numerator;
+
+	/* Number of clocks per internal offset period */
+	offset = mode->mode == IMX283_MODE_0 ? 209 : 157;
+	numerator = (imx283->vmax * (svr + 1) - shr) * imx283->hmax + offset;
+
+	do_div(numerator, imx283->hmax);
+
+	return clamp(numerator, 0, U32_MAX);
+}
+
+static void imx283_exposure_limits(struct imx283 *imx283,
+				   const struct imx283_mode *mode,
+				   s64 *min_exposure, s64 *max_exposure)
+{
+	u32 svr = 0; /* SVR feature is not currently supported */
+	u64 min_shr = mode->min_shr;
+	/* Global Shutter is not supported */
+	u64 max_shr = (svr + 1) * imx283->vmax - 4;
+
+	max_shr = min(max_shr, BIT(16) - 1);
+
+	*min_exposure = imx283_exposure(imx283, mode, max_shr);
+	*max_exposure = imx283_exposure(imx283, mode, min_shr);
+}
+
+/*
+ * Integration Time [s] = [ {VMAX x (SVR + 1) – (SHR)} x HMAX + offset ]
+ *                      / [ 72 x 10^6 ]
+ */
+static u32 imx283_shr(struct imx283 *imx283, const struct imx283_mode *mode,
+		      u32 exposure)
+{
+	u32 svr = 0; /* SVR feature is not currently supported */
+	u32 offset;
+	u64 temp;
+
+	/* Number of clocks per internal offset period */
+	offset = mode->mode == IMX283_MODE_0 ? 209 : 157;
+	temp = ((u64)exposure * imx283->hmax - offset);
+	do_div(temp, imx283->hmax);
+
+	return (imx283->vmax * (svr + 1) - temp);
+}
+
+static const char * const imx283_tpg_menu[] = {
+	"Disabled",
+	"All 000h",
+	"All FFFh",
+	"All 555h",
+	"All AAAh",
+	"Horizontal color bars",
+	"Vertical color bars",
+};
+
+static const int imx283_tpg_val[] = {
+	IMX283_TPG_PAT_ALL_000,
+	IMX283_TPG_PAT_ALL_000,
+	IMX283_TPG_PAT_ALL_FFF,
+	IMX283_TPG_PAT_ALL_555,
+	IMX283_TPG_PAT_ALL_AAA,
+	IMX283_TPG_PAT_H_COLOR_BARS,
+	IMX283_TPG_PAT_V_COLOR_BARS,
+};
+
+static int imx283_update_test_pattern(struct imx283 *imx283, u32 pattern_index)
+{
+	int ret;
+
+	if (pattern_index >= ARRAY_SIZE(imx283_tpg_val))
+		return -EINVAL;
+
+	if (!pattern_index)
+		return cci_write(imx283->cci, IMX283_REG_TPG_CTRL, 0x00, NULL);
+
+	ret = cci_write(imx283->cci, IMX283_REG_TPG_PAT,
+			imx283_tpg_val[pattern_index], NULL);
+	if (ret)
+		return ret;
+
+	return cci_write(imx283->cci, IMX283_REG_TPG_CTRL,
+			 IMX283_TPG_CTRL_CLKEN | IMX283_TPG_CTRL_PATEN, NULL);
+}
+
+static int imx283_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct imx283 *imx283 = container_of(ctrl->handler, struct imx283,
+					     ctrl_handler);
+	const struct imx283_mode *mode;
+	struct v4l2_mbus_framefmt *fmt;
+	const struct imx283_mode *mode_list;
+	struct v4l2_subdev_state *state;
+	unsigned int num_modes;
+	u64 shr, pixel_rate;
+	int ret = 0;
+
+	state = v4l2_subdev_get_locked_active_state(&imx283->sd);
+	fmt = v4l2_subdev_state_get_format(state, 0);
+
+	get_mode_table(fmt->code, &mode_list, &num_modes);
+	mode = v4l2_find_nearest_size(mode_list, num_modes, width, height,
+				      fmt->width, fmt->height);
+
+	/*
+	 * The VBLANK control may change the limits of usable exposure, so check
+	 * and adjust if necessary.
+	 */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Honour the VBLANK limits when setting exposure. */
+		s64 current_exposure, max_exposure, min_exposure;
+
+		imx283->vmax = mode->height + ctrl->val;
+
+		imx283_exposure_limits(imx283, mode,
+				       &min_exposure, &max_exposure);
+
+		current_exposure = imx283->exposure->val;
+		current_exposure = clamp(current_exposure, min_exposure,
+					 max_exposure);
+
+		__v4l2_ctrl_modify_range(imx283->exposure, min_exposure,
+					 max_exposure, 1, current_exposure);
+	}
+
+	/*
+	 * Applying V4L2 control value only happens
+	 * when power is up for streaming
+	 */
+	if (!pm_runtime_get_if_active(imx283->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE:
+		shr = imx283_shr(imx283, mode, ctrl->val);
+		dev_dbg(imx283->dev, "V4L2_CID_EXPOSURE : %d - SHR: %lld\n",
+			ctrl->val, shr);
+		ret = cci_write(imx283->cci, IMX283_REG_SHR, shr, NULL);
+		break;
+
+	case V4L2_CID_HBLANK:
+		pixel_rate = imx283_pixel_rate(imx283, mode);
+		imx283->hmax = imx283_internal_clock(pixel_rate, mode->width + ctrl->val);
+		dev_dbg(imx283->dev, "V4L2_CID_HBLANK : %d  HMAX : %u\n",
+			ctrl->val, imx283->hmax);
+		ret = cci_write(imx283->cci, IMX283_REG_HMAX, imx283->hmax, NULL);
+		break;
+
+	case V4L2_CID_VBLANK:
+		imx283->vmax = mode->height + ctrl->val;
+		dev_dbg(imx283->dev, "V4L2_CID_VBLANK : %d  VMAX : %u\n",
+			ctrl->val, imx283->vmax);
+		ret = cci_write(imx283->cci, IMX283_REG_VMAX, imx283->vmax, NULL);
+		break;
+
+	case V4L2_CID_ANALOGUE_GAIN:
+		ret = cci_write(imx283->cci, IMX283_REG_ANALOG_GAIN, ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		ret = cci_write(imx283->cci, IMX283_REG_DIGITAL_GAIN, ctrl->val, NULL);
+		break;
+
+	case V4L2_CID_VFLIP:
+		/*
+		 * VFLIP is managed by BIT(0) of IMX283_REG_HTRIMMING address, hence
+		 * both need to be set simultaneously.
+		 */
+		if (ctrl->val) {
+			cci_write(imx283->cci, IMX283_REG_HTRIMMING,
+				  IMX283_HTRIMMING_EN | IMX283_MDVREV, &ret);
+		} else {
+			cci_write(imx283->cci, IMX283_REG_HTRIMMING,
+				  IMX283_HTRIMMING_EN, &ret);
+		}
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = imx283_update_test_pattern(imx283, ctrl->val);
+		break;
+
+	default:
+		dev_err(imx283->dev, "ctrl(id:0x%x, val:0x%x) is not handled\n",
+			ctrl->id, ctrl->val);
+		break;
+	}
+
+	pm_runtime_put(imx283->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops imx283_ctrl_ops = {
+	.s_ctrl = imx283_set_ctrl,
+};
+
+static int imx283_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(imx283_mbus_codes))
+		return -EINVAL;
+
+	code->code = imx283_mbus_codes[code->index];
+
+	return 0;
+}
+
+static int imx283_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct imx283_mode *mode_list;
+	unsigned int num_modes;
+
+	get_mode_table(fse->code, &mode_list, &num_modes);
+
+	if (fse->index >= num_modes)
+		return -EINVAL;
+
+	fse->min_width = mode_list[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = mode_list[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void imx283_update_image_pad_format(struct imx283 *imx283,
+					   const struct imx283_mode *mode,
+					   struct v4l2_mbus_framefmt *format)
+{
+	format->width = mode->width;
+	format->height = mode->height;
+	format->field = V4L2_FIELD_NONE;
+	format->colorspace = V4L2_COLORSPACE_RAW;
+	format->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	format->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	format->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int imx283_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state)
+{
+	struct imx283 *imx283 = to_imx283(sd);
+	struct v4l2_mbus_framefmt *format;
+	const struct imx283_mode *mode;
+	struct v4l2_rect *crop;
+
+	/* Initialize try_fmt */
+	format = v4l2_subdev_state_get_format(state, IMAGE_PAD);
+
+	mode = &supported_modes_12bit[0];
+	format->code = MEDIA_BUS_FMT_SRGGB12_1X12;
+	imx283_update_image_pad_format(imx283, mode, format);
+
+	/* Initialize crop rectangle to mode default */
+	crop = v4l2_subdev_state_get_crop(state, IMAGE_PAD);
+	*crop = mode->crop;
+
+	return 0;
+}
+
+static void imx283_set_framing_limits(struct imx283 *imx283,
+				      const struct imx283_mode *mode)
+{
+	u64 pixel_rate = imx283_pixel_rate(imx283, mode);
+	u64 min_hblank, max_hblank, def_hblank;
+
+	/* Initialise hmax and vmax for exposure calculations */
+	imx283->hmax = imx283_internal_clock(pixel_rate, mode->default_hmax);
+	imx283->vmax = mode->default_vmax;
+
+	/*
+	 * Horizontal Blanking
+	 * Convert the HMAX_MAX (72MHz) to Pixel rate values for HBLANK_MAX
+	 */
+	min_hblank = mode->min_hmax - mode->width;
+	max_hblank = imx283_iclk_to_pix(pixel_rate, IMX283_HMAX_MAX) - mode->width;
+	def_hblank = mode->default_hmax - mode->width;
+	__v4l2_ctrl_modify_range(imx283->hblank, min_hblank, max_hblank, 1,
+				 def_hblank);
+	__v4l2_ctrl_s_ctrl(imx283->hblank, def_hblank);
+
+	/* Vertical Blanking */
+	__v4l2_ctrl_modify_range(imx283->vblank, mode->min_vmax - mode->height,
+				 IMX283_VMAX_MAX - mode->height, 1,
+				 mode->default_vmax - mode->height);
+	__v4l2_ctrl_s_ctrl(imx283->vblank, mode->default_vmax - mode->height);
+}
+
+static int imx283_set_pad_format(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_format *fmt)
+{
+	struct v4l2_mbus_framefmt *format;
+	const struct imx283_mode *mode;
+	struct imx283 *imx283 = to_imx283(sd);
+	const struct imx283_mode *mode_list;
+	unsigned int num_modes;
+
+	get_mode_table(fmt->format.code, &mode_list, &num_modes);
+
+	mode = v4l2_find_nearest_size(mode_list, num_modes, width, height,
+				      fmt->format.width, fmt->format.height);
+
+	fmt->format.width = mode->width;
+	fmt->format.height = mode->height;
+	fmt->format.field = V4L2_FIELD_NONE;
+	fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+	fmt->format.ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->format.quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	fmt->format.xfer_func = V4L2_XFER_FUNC_NONE;
+
+	format = v4l2_subdev_state_get_format(sd_state, 0);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		imx283_set_framing_limits(imx283, mode);
+
+	*format = fmt->format;
+
+	return 0;
+}
+
+static int imx283_standby_cancel(struct imx283 *imx283)
+{
+	unsigned int link_freq_idx;
+	int ret = 0;
+
+	cci_write(imx283->cci, IMX283_REG_STANDBY,
+		  IMX283_STBLOGIC | IMX283_STBDV, &ret);
+
+	/* Configure PLL clocks based on the xclk */
+	cci_multi_reg_write(imx283->cci, imx283->freq->regs,
+			    imx283->freq->reg_count, &ret);
+
+	dev_dbg(imx283->dev, "Using clk freq %ld MHz",
+		imx283->freq->mhz / HZ_PER_MHZ);
+
+	/* Initialise communication */
+	cci_write(imx283->cci, IMX283_REG_PLSTMG08, IMX283_PLSTMG08_VAL, &ret);
+	cci_write(imx283->cci, IMX283_REG_PLSTMG02, IMX283_PLSTMG02_VAL, &ret);
+
+	/* Enable PLL */
+	cci_write(imx283->cci, IMX283_REG_STBPL, IMX283_STBPL_NORMAL, &ret);
+
+	/* Configure the MIPI link speed */
+	link_freq_idx = __ffs(imx283->link_freq_bitmap);
+	cci_multi_reg_write(imx283->cci, link_freq_reglist[link_freq_idx].regs,
+			    link_freq_reglist[link_freq_idx].num_of_regs,
+			    &ret);
+
+	/* 1st Stabilisation period of 1 ms or more */
+	usleep_range(1000, 2000);
+
+	/* Activate */
+	cci_write(imx283->cci, IMX283_REG_STANDBY, IMX283_ACTIVE, &ret);
+
+	/* 2nd Stabilisation period of 19ms or more */
+	usleep_range(19000, 20000);
+
+	cci_write(imx283->cci, IMX283_REG_CLAMP, IMX283_CLPSQRST, &ret);
+	cci_write(imx283->cci, IMX283_REG_XMSTA, 0, &ret);
+	cci_write(imx283->cci, IMX283_REG_SYNCDRV, IMX283_SYNCDRV_XHS_XVS, &ret);
+
+	return ret;
+}
+
+/* Start streaming */
+static int imx283_start_streaming(struct imx283 *imx283,
+				  struct v4l2_subdev_state *state)
+{
+	const struct imx283_readout_mode *readout;
+	const struct imx283_mode *mode;
+	const struct v4l2_mbus_framefmt *fmt;
+	const struct imx283_mode *mode_list;
+	unsigned int num_modes;
+	u32 v_widcut;
+	s32 v_pos;
+	u32 write_v_size;
+	u32 y_out_size;
+	int ret = 0;
+
+	fmt = v4l2_subdev_state_get_format(state, 0);
+	get_mode_table(fmt->code, &mode_list, &num_modes);
+	mode = v4l2_find_nearest_size(mode_list, num_modes, width, height,
+				      fmt->width, fmt->height);
+
+	ret = imx283_standby_cancel(imx283);
+	if (ret) {
+		dev_err(imx283->dev, "failed to cancel standby\n");
+		return ret;
+	}
+
+	/*
+	 * Set the readout mode registers.
+	 * MDSEL3 and MDSEL4 are updated to enable Arbitrary Vertical Cropping.
+	 */
+	readout = &imx283_readout_modes[mode->mode];
+	cci_write(imx283->cci, IMX283_REG_MDSEL1, readout->mdsel1, &ret);
+	cci_write(imx283->cci, IMX283_REG_MDSEL2, readout->mdsel2, &ret);
+	cci_write(imx283->cci, IMX283_REG_MDSEL3,
+		  readout->mdsel3 | IMX283_MDSEL3_VCROP_EN, &ret);
+	cci_write(imx283->cci, IMX283_REG_MDSEL4,
+		  readout->mdsel4 | IMX283_MDSEL4_VCROP_EN, &ret);
+
+	/* Mode 1S specific entries from the Readout Drive Mode Tables */
+	if (mode->mode == IMX283_MODE_1S) {
+		cci_write(imx283->cci, IMX283_REG_MDSEL7, 0x01, &ret);
+		cci_write(imx283->cci, IMX283_REG_MDSEL18, 0x1098, &ret);
+	}
+
+	if (ret) {
+		dev_err(imx283->dev, "failed to set readout\n");
+		return ret;
+	}
+
+	/* Initialise SVR. Unsupported for now - Always 0 */
+	cci_write(imx283->cci, IMX283_REG_SVR, 0x00, &ret);
+
+	dev_dbg(imx283->dev, "Mode: Size %d x %d\n", mode->width, mode->height);
+	dev_dbg(imx283->dev, "Analogue Crop (in the mode) (%d,%d)/%ux%u\n",
+		mode->crop.left,
+		mode->crop.top,
+		mode->crop.width,
+		mode->crop.height);
+
+	y_out_size = mode->crop.height / mode->vbin_ratio;
+	write_v_size = y_out_size + mode->vertical_ob;
+	/*
+	 * cropping start position = (VWINPOS – Vst) × 2
+	 * cropping width = Veff – (VWIDCUT – Vct) × 2
+	 */
+	v_pos = imx283->vflip->val ?
+		((-mode->crop.top / mode->vbin_ratio) / 2) + mode->vst :
+		((mode->crop.top / mode->vbin_ratio) / 2)  + mode->vst;
+	v_widcut = ((mode->veff - y_out_size) / 2) + mode->vct;
+
+	cci_write(imx283->cci, IMX283_REG_Y_OUT_SIZE, y_out_size, &ret);
+	cci_write(imx283->cci, IMX283_REG_WRITE_VSIZE, write_v_size, &ret);
+	cci_write(imx283->cci, IMX283_REG_VWIDCUT, v_widcut, &ret);
+	cci_write(imx283->cci, IMX283_REG_VWINPOS, v_pos, &ret);
+
+	cci_write(imx283->cci, IMX283_REG_OB_SIZE_V, mode->vertical_ob, &ret);
+
+	/* TODO: Validate mode->crop is fully contained within imx283_native_area */
+	cci_write(imx283->cci, IMX283_REG_HTRIMMING_START, mode->crop.left, &ret);
+	cci_write(imx283->cci, IMX283_REG_HTRIMMING_END,
+		  mode->crop.left + mode->crop.width, &ret);
+
+	/* Disable embedded data */
+	cci_write(imx283->cci, IMX283_REG_EBD_X_OUT_SIZE, 0, &ret);
+
+	/* Apply customized values from controls (HMAX/VMAX/SHR) */
+	ret =  __v4l2_ctrl_handler_setup(imx283->sd.ctrl_handler);
+
+	return ret;
+}
+
+static int imx283_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
+{
+	struct imx283 *imx283 = to_imx283(sd);
+	int ret;
+
+	if (pad != IMAGE_PAD)
+		return -EINVAL;
+
+	ret = pm_runtime_get_sync(imx283->dev);
+	if (ret < 0) {
+		pm_runtime_put_noidle(imx283->dev);
+		return ret;
+	}
+
+	ret = imx283_start_streaming(imx283, state);
+	if (ret)
+		goto err_rpm_put;
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_mark_last_busy(imx283->dev);
+	pm_runtime_put_autosuspend(imx283->dev);
+
+	return ret;
+}
+
+static int imx283_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
+{
+	struct imx283 *imx283 = to_imx283(sd);
+	int ret;
+
+	if (pad != IMAGE_PAD)
+		return -EINVAL;
+
+	ret = cci_write(imx283->cci, IMX283_REG_STANDBY, IMX283_STBLOGIC, NULL);
+	if (ret)
+		dev_err(imx283->dev, "Failed to stop stream\n");
+
+	pm_runtime_mark_last_busy(imx283->dev);
+	pm_runtime_put_autosuspend(imx283->dev);
+
+	return ret;
+}
+
+/* Power/clock management functions */
+static int imx283_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx283 *imx283 = to_imx283(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(imx283_supply_name),
+				    imx283->supplies);
+	if (ret) {
+		dev_err(imx283->dev, "failed to enable regulators\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(imx283->xclk);
+	if (ret) {
+		dev_err(imx283->dev, "failed to enable clock\n");
+		goto reg_off;
+	}
+
+	gpiod_set_value_cansleep(imx283->reset_gpio, 0);
+
+	usleep_range(IMX283_XCLR_MIN_DELAY_US,
+		     IMX283_XCLR_MIN_DELAY_US + IMX283_XCLR_DELAY_RANGE_US);
+
+	return 0;
+
+reg_off:
+	regulator_bulk_disable(ARRAY_SIZE(imx283_supply_name), imx283->supplies);
+	return ret;
+}
+
+static int imx283_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct imx283 *imx283 = to_imx283(sd);
+
+	gpiod_set_value_cansleep(imx283->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(imx283_supply_name), imx283->supplies);
+	clk_disable_unprepare(imx283->xclk);
+
+	return 0;
+}
+
+static int imx283_get_regulators(struct imx283 *imx283)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx283_supply_name); i++)
+		imx283->supplies[i].supply = imx283_supply_name[i];
+
+	return devm_regulator_bulk_get(imx283->dev,
+				       ARRAY_SIZE(imx283_supply_name),
+				       imx283->supplies);
+}
+
+/* Verify chip ID */
+static int imx283_identify_module(struct imx283 *imx283)
+{
+	int ret;
+	u64 val;
+
+	ret = cci_read(imx283->cci, IMX283_REG_CHIP_ID, &val, NULL);
+	if (ret) {
+		dev_err(imx283->dev, "failed to read chip id %x, with error %d\n",
+			IMX283_CHIP_ID, ret);
+		return ret;
+	}
+
+	if (val != IMX283_CHIP_ID) {
+		dev_err(imx283->dev, "chip id mismatch: %x!=%llx\n",
+			IMX283_CHIP_ID, val);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int imx283_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP: {
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
+		return 0;
+	}
+
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+		sel->r = imx283_native_area;
+		return 0;
+
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r = imx283_active_area;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_subdev_video_ops imx283_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_pad_ops imx283_pad_ops = {
+	.enum_mbus_code = imx283_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = imx283_set_pad_format,
+	.get_selection = imx283_get_selection,
+	.enum_frame_size = imx283_enum_frame_size,
+	.enable_streams = imx283_enable_streams,
+	.disable_streams = imx283_disable_streams,
+};
+
+static const struct v4l2_subdev_internal_ops imx283_internal_ops = {
+	.init_state = imx283_init_state,
+};
+
+static const struct v4l2_subdev_ops imx283_subdev_ops = {
+	.video = &imx283_video_ops,
+	.pad = &imx283_pad_ops,
+};
+
+/* Initialize control handlers */
+static int imx283_init_controls(struct imx283 *imx283)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	struct v4l2_fwnode_device_properties props;
+	struct v4l2_ctrl *link_freq;
+	const struct imx283_mode *mode = &supported_modes_12bit[0];
+	u64 min_hblank, max_hblank, def_hblank;
+	u64 pixel_rate;
+	int ret;
+
+	ctrl_hdlr = &imx283->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 16);
+	if (ret)
+		return ret;
+
+	/*
+	 * Create the controls here, but mode specific limits are setup
+	 * in the imx283_set_framing_limits() call below.
+	 */
+
+	/* By default, PIXEL_RATE is read only */
+	pixel_rate = imx283_pixel_rate(imx283, mode);
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops,
+			  V4L2_CID_PIXEL_RATE, pixel_rate,
+			  pixel_rate, 1, pixel_rate);
+
+	link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx283_ctrl_ops,
+					   V4L2_CID_LINK_FREQ,
+					   __fls(imx283->link_freq_bitmap),
+					   __ffs(imx283->link_freq_bitmap),
+					   link_frequencies);
+	if (link_freq)
+		link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* Initialise vblank/hblank/exposure based on the current mode. */
+	imx283->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops,
+					   V4L2_CID_VBLANK,
+					   mode->min_vmax - mode->height,
+					   IMX283_VMAX_MAX, 1,
+					   mode->default_vmax - mode->height);
+
+	min_hblank = mode->min_hmax - mode->width;
+	max_hblank = imx283_iclk_to_pix(pixel_rate, IMX283_HMAX_MAX) - mode->width;
+	def_hblank = mode->default_hmax - mode->width;
+	imx283->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops,
+					   V4L2_CID_HBLANK, min_hblank, max_hblank,
+					   1, def_hblank);
+
+	imx283->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops,
+					     V4L2_CID_EXPOSURE,
+					     IMX283_EXPOSURE_MIN,
+					     IMX283_EXPOSURE_MAX,
+					     IMX283_EXPOSURE_STEP,
+					     IMX283_EXPOSURE_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  IMX283_ANA_GAIN_MIN, IMX283_ANA_GAIN_MAX,
+			  IMX283_ANA_GAIN_STEP, IMX283_ANA_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  IMX283_DGTL_GAIN_MIN, IMX283_DGTL_GAIN_MAX,
+			  IMX283_DGTL_GAIN_STEP, IMX283_DGTL_GAIN_DEFAULT);
+
+	imx283->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx283_ctrl_ops, V4L2_CID_VFLIP,
+					  0, 1, 1, 0);
+	if (imx283->vflip)
+		imx283->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx283_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(imx283_tpg_menu) - 1,
+				     0, 0, imx283_tpg_menu);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		dev_err(imx283->dev, "control init failed (%d)\n", ret);
+		goto error;
+	}
+
+	ret = v4l2_fwnode_device_parse(imx283->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx283_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
+	imx283->sd.ctrl_handler = ctrl_hdlr;
+
+	mutex_lock(imx283->ctrl_handler.lock);
+
+	/* Setup exposure and frame/line length limits. */
+	imx283_set_framing_limits(imx283, mode);
+
+	mutex_unlock(imx283->ctrl_handler.lock);
+
+	return 0;
+
+error:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+
+	return ret;
+}
+
+static int imx283_parse_endpoint(struct imx283 *imx283)
+{
+	struct fwnode_handle *fwnode;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	int ret;
+
+	fwnode = dev_fwnode(imx283->dev);
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep) {
+		dev_err(imx283->dev, "Failed to get next endpoint\n");
+		return -ENXIO;
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return ret;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 4) {
+		dev_err(imx283->dev,
+			"number of CSI2 data lanes %d is not supported\n",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto done_endpoint_free;
+	}
+
+	ret = v4l2_link_freq_to_bitmap(imx283->dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       link_frequencies, ARRAY_SIZE(link_frequencies),
+				       &imx283->link_freq_bitmap);
+
+done_endpoint_free:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+};
+
+static int imx283_probe(struct i2c_client *client)
+{
+	struct imx283 *imx283;
+	unsigned int i;
+	unsigned int xclk_freq;
+	int ret;
+
+	imx283 = devm_kzalloc(&client->dev, sizeof(*imx283), GFP_KERNEL);
+	if (!imx283)
+		return -ENOMEM;
+
+	imx283->dev = &client->dev;
+
+	v4l2_i2c_subdev_init(&imx283->sd, client, &imx283_subdev_ops);
+
+	imx283->cci = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx283->cci)) {
+		ret = PTR_ERR(imx283->cci);
+		dev_err(imx283->dev, "failed to initialize CCI: %d\n", ret);
+		return ret;
+	}
+
+	/* Get system clock (xclk) */
+	imx283->xclk = devm_clk_get(imx283->dev, NULL);
+	if (IS_ERR(imx283->xclk)) {
+		return dev_err_probe(imx283->dev, PTR_ERR(imx283->xclk),
+				     "failed to get xclk\n");
+	}
+
+	xclk_freq = clk_get_rate(imx283->xclk);
+	for (i = 0; i < ARRAY_SIZE(imx283_frequencies); i++) {
+		if (xclk_freq == imx283_frequencies[i].mhz) {
+			imx283->freq = &imx283_frequencies[i];
+			break;
+		}
+	}
+	if (!imx283->freq) {
+		dev_err(imx283->dev, "xclk frequency unsupported: %d Hz\n", xclk_freq);
+		return -EINVAL;
+	}
+
+	ret = imx283_get_regulators(imx283);
+	if (ret) {
+		return dev_err_probe(imx283->dev, ret,
+				"failed to get regulators\n");
+	}
+
+	ret = imx283_parse_endpoint(imx283);
+	if (ret) {
+		dev_err(imx283->dev, "failed to parse endpoint configuration\n");
+		return ret;
+	}
+
+	/* Request optional enable pin */
+	imx283->reset_gpio = devm_gpiod_get_optional(imx283->dev, "reset",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(imx283->reset_gpio))
+		return dev_err_probe(imx283->dev, PTR_ERR(imx283->reset_gpio),
+				     "failed to get reset GPIO\n");
+
+	/*
+	 * The sensor must be powered for imx283_identify_module()
+	 * to be able to read the CHIP_ID register
+	 */
+	ret = imx283_power_on(imx283->dev);
+	if (ret)
+		return ret;
+
+	ret = imx283_identify_module(imx283);
+	if (ret)
+		goto error_power_off;
+
+	/*
+	 * Enable runtime PM with autosuspend. As the device has been powered
+	 * manually, mark it as active, and increase the usage count without
+	 * resuming the device.
+	 */
+	pm_runtime_set_active(imx283->dev);
+	pm_runtime_get_noresume(imx283->dev);
+	pm_runtime_enable(imx283->dev);
+	pm_runtime_set_autosuspend_delay(imx283->dev, 1000);
+	pm_runtime_use_autosuspend(imx283->dev);
+
+	/* This needs the pm runtime to be registered. */
+	ret = imx283_init_controls(imx283);
+	if (ret)
+		goto error_pm;
+
+	/* Initialize subdev */
+	imx283->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	imx283->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	imx283->sd.internal_ops = &imx283_internal_ops;
+
+	/* Initialize source pads */
+	imx283->pad.flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&imx283->sd.entity, 1, &imx283->pad);
+	if (ret) {
+		dev_err(imx283->dev, "failed to init entity pads: %d\n", ret);
+		goto error_handler_free;
+	}
+
+	imx283->sd.state_lock = imx283->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&imx283->sd);
+	if (ret < 0) {
+		dev_err(imx283->dev, "subdev init error: %d\n", ret);
+		goto error_media_entity;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&imx283->sd);
+	if (ret < 0) {
+		dev_err(imx283->dev, "failed to register sensor sub-device: %d\n", ret);
+		goto error_subdev_cleanup;
+	}
+
+	/*
+	 * Decrease the PM usage count. The device will get suspended after the
+	 * autosuspend delay, turning the power off.
+	 */
+	pm_runtime_mark_last_busy(imx283->dev);
+	pm_runtime_put_autosuspend(imx283->dev);
+
+	return 0;
+
+error_subdev_cleanup:
+	v4l2_subdev_cleanup(&imx283->sd);
+
+error_media_entity:
+	media_entity_cleanup(&imx283->sd.entity);
+
+error_handler_free:
+	v4l2_ctrl_handler_free(imx283->sd.ctrl_handler);
+
+error_pm:
+	pm_runtime_disable(imx283->dev);
+	pm_runtime_set_suspended(imx283->dev);
+error_power_off:
+	imx283_power_off(imx283->dev);
+
+	return ret;
+}
+
+static void imx283_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct imx283 *imx283 = to_imx283(sd);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(&imx283->sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(imx283->sd.ctrl_handler);
+
+	pm_runtime_disable(imx283->dev);
+	if (!pm_runtime_status_suspended(imx283->dev))
+		imx283_power_off(imx283->dev);
+	pm_runtime_set_suspended(imx283->dev);
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(imx283_pm_ops, imx283_power_off,
+				 imx283_power_on, NULL);
+
+static const struct of_device_id imx283_dt_ids[] = {
+	{ .compatible = "sony,imx283" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx283_dt_ids);
+
+static struct i2c_driver imx283_i2c_driver = {
+	.driver = {
+		.name = "imx283",
+		.pm = pm_ptr(&imx283_pm_ops),
+		.of_match_table	= imx283_dt_ids,
+	},
+	.probe = imx283_probe,
+	.remove = imx283_remove,
+};
+module_i2c_driver(imx283_i2c_driver);
+
+MODULE_AUTHOR("Will Whang <will@willwhang.com>");
+MODULE_AUTHOR("Kieran Bingham <kieran.bingham@ideasonboard.com>");
+MODULE_AUTHOR("Umang Jain <umang.jain@ideasonboard.com>");
+MODULE_DESCRIPTION("Sony IMX283 Sensor Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/imx290.c b/drivers/media/i2c/imx290.c
index 4150e6e4b9a6..fbf7eba3d71d 100644
--- a/drivers/media/i2c/imx290.c
+++ b/drivers/media/i2c/imx290.c
@@ -18,13 +18,12 @@
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/media-entity.h>
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
@@ -78,7 +77,6 @@
 #define IMX290_ADBIT2					CCI_REG8(0x317c)
 #define IMX290_ADBIT2_10BIT				0x12
 #define IMX290_ADBIT2_12BIT				0x00
-#define IMX290_CHIP_ID					CCI_REG16_LE(0x319a)
 #define IMX290_ADBIT3					CCI_REG8(0x31ec)
 #define IMX290_ADBIT3_10BIT				0x37
 #define IMX290_ADBIT3_12BIT				0x0e
@@ -172,12 +170,15 @@ enum imx290_model {
 	IMX290_MODEL_IMX290LQR,
 	IMX290_MODEL_IMX290LLR,
 	IMX290_MODEL_IMX327LQR,
+	IMX290_MODEL_IMX462LQR,
+	IMX290_MODEL_IMX462LLR,
 };
 
 struct imx290_model_info {
 	enum imx290_colour_variant colour_variant;
 	const struct cci_reg_sequence *init_regs;
 	size_t init_regs_num;
+	unsigned int max_analog_gain;
 	const char *name;
 };
 
@@ -269,7 +270,6 @@ static const struct cci_reg_sequence imx290_global_init_settings[] = {
 	{ IMX290_WINWV, 1097 },
 	{ IMX290_XSOUTSEL, IMX290_XSOUTSEL_XVSOUTSEL_VSYNC |
 			   IMX290_XSOUTSEL_XHSOUTSEL_HSYNC },
-	{ CCI_REG8(0x3011), 0x02 },
 	{ CCI_REG8(0x3012), 0x64 },
 	{ CCI_REG8(0x3013), 0x00 },
 };
@@ -277,6 +277,51 @@ static const struct cci_reg_sequence imx290_global_init_settings[] = {
 static const struct cci_reg_sequence imx290_global_init_settings_290[] = {
 	{ CCI_REG8(0x300f), 0x00 },
 	{ CCI_REG8(0x3010), 0x21 },
+	{ CCI_REG8(0x3011), 0x00 },
+	{ CCI_REG8(0x3016), 0x09 },
+	{ CCI_REG8(0x3070), 0x02 },
+	{ CCI_REG8(0x3071), 0x11 },
+	{ CCI_REG8(0x309b), 0x10 },
+	{ CCI_REG8(0x309c), 0x22 },
+	{ CCI_REG8(0x30a2), 0x02 },
+	{ CCI_REG8(0x30a6), 0x20 },
+	{ CCI_REG8(0x30a8), 0x20 },
+	{ CCI_REG8(0x30aa), 0x20 },
+	{ CCI_REG8(0x30ac), 0x20 },
+	{ CCI_REG8(0x30b0), 0x43 },
+	{ CCI_REG8(0x3119), 0x9e },
+	{ CCI_REG8(0x311c), 0x1e },
+	{ CCI_REG8(0x311e), 0x08 },
+	{ CCI_REG8(0x3128), 0x05 },
+	{ CCI_REG8(0x313d), 0x83 },
+	{ CCI_REG8(0x3150), 0x03 },
+	{ CCI_REG8(0x317e), 0x00 },
+	{ CCI_REG8(0x32b8), 0x50 },
+	{ CCI_REG8(0x32b9), 0x10 },
+	{ CCI_REG8(0x32ba), 0x00 },
+	{ CCI_REG8(0x32bb), 0x04 },
+	{ CCI_REG8(0x32c8), 0x50 },
+	{ CCI_REG8(0x32c9), 0x10 },
+	{ CCI_REG8(0x32ca), 0x00 },
+	{ CCI_REG8(0x32cb), 0x04 },
+	{ CCI_REG8(0x332c), 0xd3 },
+	{ CCI_REG8(0x332d), 0x10 },
+	{ CCI_REG8(0x332e), 0x0d },
+	{ CCI_REG8(0x3358), 0x06 },
+	{ CCI_REG8(0x3359), 0xe1 },
+	{ CCI_REG8(0x335a), 0x11 },
+	{ CCI_REG8(0x3360), 0x1e },
+	{ CCI_REG8(0x3361), 0x61 },
+	{ CCI_REG8(0x3362), 0x10 },
+	{ CCI_REG8(0x33b0), 0x50 },
+	{ CCI_REG8(0x33b2), 0x1a },
+	{ CCI_REG8(0x33b3), 0x04 },
+};
+
+static const struct cci_reg_sequence imx290_global_init_settings_462[] = {
+	{ CCI_REG8(0x300f), 0x00 },
+	{ CCI_REG8(0x3010), 0x21 },
+	{ CCI_REG8(0x3011), 0x02 },
 	{ CCI_REG8(0x3016), 0x09 },
 	{ CCI_REG8(0x3070), 0x02 },
 	{ CCI_REG8(0x3071), 0x11 },
@@ -330,6 +375,7 @@ static const struct cci_reg_sequence xclk_regs[][IMX290_NUM_CLK_REGS] = {
 };
 
 static const struct cci_reg_sequence imx290_global_init_settings_327[] = {
+	{ CCI_REG8(0x3011), 0x02 },
 	{ CCI_REG8(0x309e), 0x4A },
 	{ CCI_REG8(0x309f), 0x4A },
 	{ CCI_REG8(0x313b), 0x61 },
@@ -878,14 +924,10 @@ static int imx290_ctrl_init(struct imx290 *imx290)
 	 * up to 72.0dB (240) add further digital gain. Limit the range to
 	 * analog gain only, support for digital gain can be added separately
 	 * if needed.
-	 *
-	 * The IMX327 and IMX462 are largely compatible with the IMX290, but
-	 * have an analog gain range of 0.0dB to 29.4dB and 42dB of digital
-	 * gain. When support for those sensors gets added to the driver, the
-	 * gain control should be adjusted accordingly.
 	 */
 	v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
-			  V4L2_CID_ANALOGUE_GAIN, 0, 100, 1, 0);
+			  V4L2_CID_ANALOGUE_GAIN, 0,
+			  imx290->model->max_analog_gain, 1, 0);
 
 	/*
 	 * Correct range will be determined through imx290_ctrl_update setting
@@ -1211,11 +1253,6 @@ static int imx290_entity_init_state(struct v4l2_subdev *subdev,
 	return 0;
 }
 
-static const struct v4l2_subdev_core_ops imx290_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
 static const struct v4l2_subdev_video_ops imx290_video_ops = {
 	.s_stream = imx290_set_stream,
 };
@@ -1229,7 +1266,6 @@ static const struct v4l2_subdev_pad_ops imx290_pad_ops = {
 };
 
 static const struct v4l2_subdev_ops imx290_subdev_ops = {
-	.core = &imx290_core_ops,
 	.video = &imx290_video_ops,
 	.pad = &imx290_pad_ops,
 };
@@ -1250,11 +1286,20 @@ static int imx290_subdev_init(struct imx290 *imx290)
 
 	imx290->current_mode = &imx290_modes_ptr(imx290)[0];
 
+	/*
+	 * After linking the subdev with the imx290 instance, we are allowed to
+	 * use the pm_runtime functions. Decrease the PM usage count. The device
+	 * will get suspended after the autosuspend delay, turning the power
+	 * off. However, the communication happening in imx290_ctrl_update()
+	 * will already be prevented even before the delay.
+	 */
 	v4l2_i2c_subdev_init(&imx290->sd, client, &imx290_subdev_ops);
-	imx290->sd.internal_ops = &imx290_internal_ops;
-	imx290->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-			    V4L2_SUBDEV_FL_HAS_EVENTS;
 	imx290->sd.dev = imx290->dev;
+	pm_runtime_mark_last_busy(imx290->dev);
+	pm_runtime_put_autosuspend(imx290->dev);
+
+	imx290->sd.internal_ops = &imx290_internal_ops;
+	imx290->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	imx290->sd.entity.ops = &imx290_subdev_entity_ops;
 	imx290->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
@@ -1440,20 +1485,37 @@ static const struct imx290_model_info imx290_models[] = {
 		.colour_variant = IMX290_VARIANT_COLOUR,
 		.init_regs = imx290_global_init_settings_290,
 		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_290),
+		.max_analog_gain = 100,
 		.name = "imx290",
 	},
 	[IMX290_MODEL_IMX290LLR] = {
 		.colour_variant = IMX290_VARIANT_MONO,
 		.init_regs = imx290_global_init_settings_290,
 		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_290),
+		.max_analog_gain = 100,
 		.name = "imx290",
 	},
 	[IMX290_MODEL_IMX327LQR] = {
 		.colour_variant = IMX290_VARIANT_COLOUR,
 		.init_regs = imx290_global_init_settings_327,
 		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_327),
+		.max_analog_gain = 98,
 		.name = "imx327",
 	},
+	[IMX290_MODEL_IMX462LQR] = {
+		.colour_variant = IMX290_VARIANT_COLOUR,
+		.init_regs = imx290_global_init_settings_462,
+		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_462),
+		.max_analog_gain = 98,
+		.name = "imx462",
+	},
+	[IMX290_MODEL_IMX462LLR] = {
+		.colour_variant = IMX290_VARIANT_MONO,
+		.init_regs = imx290_global_init_settings_462,
+		.init_regs_num = ARRAY_SIZE(imx290_global_init_settings_462),
+		.max_analog_gain = 98,
+		.name = "imx462",
+	},
 };
 
 static int imx290_parse_dt(struct imx290 *imx290)
@@ -1580,6 +1642,16 @@ static int imx290_probe(struct i2c_client *client)
 	pm_runtime_set_autosuspend_delay(dev, 1000);
 	pm_runtime_use_autosuspend(dev);
 
+	/*
+	 * Make sure the sensor is available, in STANDBY and not streaming
+	 * before the V4L2 subdev is initialized.
+	 */
+	ret = imx290_stop_streaming(imx290);
+	if (ret) {
+		ret = dev_err_probe(dev, ret, "Could not initialize device\n");
+		goto err_pm;
+	}
+
 	/* Initialize the V4L2 subdev. */
 	ret = imx290_subdev_init(imx290);
 	if (ret)
@@ -1599,13 +1671,6 @@ static int imx290_probe(struct i2c_client *client)
 		goto err_subdev;
 	}
 
-	/*
-	 * Decrease the PM usage count. The device will get suspended after the
-	 * autosuspend delay, turning the power off.
-	 */
-	pm_runtime_mark_last_busy(dev);
-	pm_runtime_put_autosuspend(dev);
-
 	return 0;
 
 err_subdev:
@@ -1649,6 +1714,12 @@ static const struct of_device_id imx290_of_match[] = {
 	}, {
 		.compatible = "sony,imx327lqr",
 		.data = &imx290_models[IMX290_MODEL_IMX327LQR],
+	}, {
+		.compatible = "sony,imx462lqr",
+		.data = &imx290_models[IMX290_MODEL_IMX462LQR],
+	}, {
+		.compatible = "sony,imx462llr",
+		.data = &imx290_models[IMX290_MODEL_IMX462LLR],
 	},
 	{ /* sentinel */ },
 };
diff --git a/drivers/media/i2c/imx296.c b/drivers/media/i2c/imx296.c
index 83149fa729c4..f3bec16b527c 100644
--- a/drivers/media/i2c/imx296.c
+++ b/drivers/media/i2c/imx296.c
@@ -954,6 +954,8 @@ static int imx296_identify_model(struct imx296 *sensor)
 		return ret;
 	}
 
+	usleep_range(2000, 5000);
+
 	ret = imx296_read(sensor, IMX296_SENSOR_INFO);
 	if (ret < 0) {
 		dev_err(sensor->dev, "failed to read sensor information (%d)\n",
diff --git a/drivers/media/i2c/imx319.c b/drivers/media/i2c/imx319.c
index 8fe3933f3146..701840f4a5cc 100644
--- a/drivers/media/i2c/imx319.c
+++ b/drivers/media/i2c/imx319.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (C) 2018 Intel Corporation
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
@@ -2442,17 +2442,19 @@ static int imx319_probe(struct i2c_client *client)
 	if (full_power)
 		pm_runtime_set_active(&client->dev);
 	pm_runtime_enable(&client->dev);
-	pm_runtime_idle(&client->dev);
 
 	ret = v4l2_async_register_subdev_sensor(&imx319->sd);
 	if (ret < 0)
 		goto error_media_entity_pm;
 
+	pm_runtime_idle(&client->dev);
+
 	return 0;
 
 error_media_entity_pm:
 	pm_runtime_disable(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
+	if (full_power)
+		pm_runtime_set_suspended(&client->dev);
 	media_entity_cleanup(&imx319->sd.entity);
 
 error_handler_free:
@@ -2474,7 +2476,8 @@ static void imx319_remove(struct i2c_client *client)
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 
 	pm_runtime_disable(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		pm_runtime_set_suspended(&client->dev);
 
 	mutex_destroy(&imx319->mutex);
 }
diff --git a/drivers/media/i2c/imx334.c b/drivers/media/i2c/imx334.c
index 40863d87d341..846b9928d4e8 100644
--- a/drivers/media/i2c/imx334.c
+++ b/drivers/media/i2c/imx334.c
@@ -4,7 +4,7 @@
  *
  * Copyright (C) 2021 Intel Corporation
  */
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -12,77 +12,125 @@
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
 
+#include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
 /* Streaming Mode */
-#define IMX334_REG_MODE_SELECT	0x3000
-#define IMX334_MODE_STANDBY	0x01
-#define IMX334_MODE_STREAMING	0x00
+#define IMX334_REG_MODE_SELECT		CCI_REG8(0x3000)
+#define IMX334_MODE_STANDBY		0x01
+#define IMX334_MODE_STREAMING		0x00
 
 /* Lines per frame */
-#define IMX334_REG_LPFR		0x3030
+#define IMX334_REG_VMAX			CCI_REG24_LE(0x3030)
+
+#define IMX334_REG_HMAX			CCI_REG16_LE(0x3034)
+
+#define IMX334_REG_OPB_SIZE_V		CCI_REG8(0x304c)
+#define IMX334_REG_ADBIT		CCI_REG8(0x3050)
+#define IMX334_REG_MDBIT		CCI_REG8(0x319d)
+#define IMX334_REG_ADBIT1		CCI_REG16_LE(0x341c)
+#define IMX334_REG_Y_OUT_SIZE		CCI_REG16_LE(0x3308)
+#define IMX334_REG_XVS_XHS_OUTSEL	CCI_REG8(0x31a0)
+#define IMX334_REG_XVS_XHS_DRV		CCI_REG8(0x31a1)
 
 /* Chip ID */
-#define IMX334_REG_ID		0x3044
-#define IMX334_ID		0x1e
+#define IMX334_REG_ID			CCI_REG8(0x3044)
+#define IMX334_ID			0x1e
 
 /* Exposure control */
-#define IMX334_REG_SHUTTER	0x3058
-#define IMX334_EXPOSURE_MIN	1
-#define IMX334_EXPOSURE_OFFSET	5
-#define IMX334_EXPOSURE_STEP	1
-#define IMX334_EXPOSURE_DEFAULT	0x0648
+#define IMX334_REG_SHUTTER		CCI_REG24_LE(0x3058)
+#define IMX334_EXPOSURE_MIN		1
+#define IMX334_EXPOSURE_OFFSET		5
+#define IMX334_EXPOSURE_STEP		1
+#define IMX334_EXPOSURE_DEFAULT		0x0648
+
+#define IMX334_REG_LANEMODE		CCI_REG8(0x3a01)
+#define IMX334_CSI_4_LANE_MODE		3
+#define IMX334_CSI_8_LANE_MODE		7
+
+/* Window cropping Settings */
+#define IMX334_REG_AREA3_ST_ADR_1	CCI_REG16_LE(0x3074)
+#define IMX334_REG_AREA3_ST_ADR_2	CCI_REG16_LE(0x308e)
+#define IMX334_REG_UNREAD_PARAM5	CCI_REG16_LE(0x30b6)
+#define IMX334_REG_AREA3_WIDTH_1	CCI_REG16_LE(0x3076)
+#define IMX334_REG_AREA3_WIDTH_2	CCI_REG16_LE(0x3090)
+#define IMX334_REG_BLACK_OFSET_ADR	CCI_REG16_LE(0x30c6)
+#define IMX334_REG_UNRD_LINE_MAX	CCI_REG16_LE(0x30ce)
+#define IMX334_REG_UNREAD_ED_ADR	CCI_REG16_LE(0x30d8)
+#define IMX334_REG_UNREAD_PARAM6	CCI_REG16_LE(0x3116)
+
+#define IMX334_REG_VREVERSE		CCI_REG8(0x304f)
+#define IMX334_REG_HREVERSE		CCI_REG8(0x304e)
+
+/* Binning Settings */
+#define IMX334_REG_HADD_VADD		CCI_REG8(0x3199)
+#define IMX334_REG_VALID_EXPAND		CCI_REG8(0x31dd)
+#define IMX334_REG_TCYCLE		CCI_REG8(0x3300)
 
 /* Analog gain control */
-#define IMX334_REG_AGAIN	0x30e8
-#define IMX334_AGAIN_MIN	0
-#define IMX334_AGAIN_MAX	240
-#define IMX334_AGAIN_STEP	1
-#define IMX334_AGAIN_DEFAULT	0
+#define IMX334_REG_AGAIN		CCI_REG16_LE(0x30e8)
+#define IMX334_AGAIN_MIN		0
+#define IMX334_AGAIN_MAX		240
+#define IMX334_AGAIN_STEP		1
+#define IMX334_AGAIN_DEFAULT		0
 
 /* Group hold register */
-#define IMX334_REG_HOLD		0x3001
+#define IMX334_REG_HOLD			CCI_REG8(0x3001)
+
+#define IMX334_REG_MASTER_MODE		CCI_REG8(0x3002)
+#define IMX334_REG_WINMODE		CCI_REG8(0x3018)
+#define IMX334_REG_HTRIMMING_START	CCI_REG16_LE(0x302c)
+#define IMX334_REG_HNUM			CCI_REG16_LE(0x302e)
 
 /* Input clock rate */
-#define IMX334_INCLK_RATE	24000000
+#define IMX334_INCLK_RATE		24000000
+
+/* INCK Setting Register */
+#define IMX334_REG_BCWAIT_TIME		CCI_REG8(0x300c)
+#define IMX334_REG_CPWAIT_TIME		CCI_REG8(0x300d)
+#define IMX334_REG_INCKSEL1		CCI_REG16_LE(0x314c)
+#define IMX334_REG_INCKSEL2		CCI_REG8(0x315a)
+#define IMX334_REG_INCKSEL3		CCI_REG8(0x3168)
+#define IMX334_REG_INCKSEL4		CCI_REG8(0x316a)
+#define IMX334_REG_SYS_MODE		CCI_REG8(0x319e)
+
+#define IMX334_REG_TCLKPOST		CCI_REG16_LE(0x3a18)
+#define IMX334_REG_TCLKPREPARE		CCI_REG16_LE(0x3a1a)
+#define IMX334_REG_TCLKTRAIL		CCI_REG16_LE(0x3a1c)
+#define IMX334_REG_TCLKZERO		CCI_REG16_LE(0x3a1e)
+#define IMX334_REG_THSPREPARE		CCI_REG16_LE(0x3a20)
+#define IMX334_REG_THSZERO		CCI_REG16_LE(0x3a22)
+#define IMX334_REG_THSTRAIL		CCI_REG16_LE(0x3a24)
+#define IMX334_REG_THSEXIT		CCI_REG16_LE(0x3a26)
+#define IMX334_REG_TPLX			CCI_REG16_LE(0x3a28)
 
 /* CSI2 HW configuration */
-#define IMX334_LINK_FREQ_891M	891000000
-#define IMX334_LINK_FREQ_445M	445500000
-#define IMX334_NUM_DATA_LANES	4
+#define IMX334_LINK_FREQ_891M		891000000
+#define IMX334_LINK_FREQ_445M		445500000
+#define IMX334_NUM_DATA_LANES		4
 
-#define IMX334_REG_MIN		0x00
-#define IMX334_REG_MAX		0xfffff
+#define IMX334_REG_MIN			0x00
+#define IMX334_REG_MAX			0xfffff
 
 /* Test Pattern Control */
-#define IMX334_REG_TP		0x329e
-#define IMX334_TP_COLOR_HBARS	0xA
-#define IMX334_TP_COLOR_VBARS	0xB
+#define IMX334_REG_TP			CCI_REG8(0x329e)
+#define IMX334_TP_COLOR_HBARS		0xa
+#define IMX334_TP_COLOR_VBARS		0xb
 
-#define IMX334_TPG_EN_DOUT	0x329c
-#define IMX334_TP_ENABLE	0x1
-#define IMX334_TP_DISABLE	0x0
+#define IMX334_TPG_EN_DOUT		CCI_REG8(0x329c)
+#define IMX334_TP_ENABLE		0x1
+#define IMX334_TP_DISABLE		0x0
 
-#define IMX334_TPG_COLORW	0x32a0
-#define IMX334_TPG_COLORW_120P	0x13
+#define IMX334_TPG_COLORW		CCI_REG8(0x32a0)
+#define IMX334_TPG_COLORW_120P		0x13
 
-#define IMX334_TP_CLK_EN	0x3148
-#define IMX334_TP_CLK_EN_VAL	0x10
-#define IMX334_TP_CLK_DIS_VAL	0x0
+#define IMX334_TP_CLK_EN		CCI_REG8(0x3148)
+#define IMX334_TP_CLK_EN_VAL		0x10
+#define IMX334_TP_CLK_DIS_VAL		0x0
 
-#define IMX334_DIG_CLP_MODE	0x3280
-
-/**
- * struct imx334_reg - imx334 sensor register
- * @address: Register address
- * @val: Register value
- */
-struct imx334_reg {
-	u16 address;
-	u8 val;
-};
+#define IMX334_DIG_CLP_MODE		CCI_REG8(0x3280)
 
 /**
  * struct imx334_reg_list - imx334 sensor register list
@@ -91,7 +139,7 @@ struct imx334_reg {
  */
 struct imx334_reg_list {
 	u32 num_of_regs;
-	const struct imx334_reg *regs;
+	const struct cci_reg_sequence *regs;
 };
 
 /**
@@ -121,6 +169,7 @@ struct imx334_mode {
 /**
  * struct imx334 - imx334 sensor device structure
  * @dev: Pointer to generic device
+ * @cci: CCI register map
  * @client: Pointer to i2c client
  * @sd: V4L2 sub-device
  * @pad: Media pad. Only one pad supported
@@ -135,12 +184,12 @@ struct imx334_mode {
  * @again_ctrl: Pointer to analog gain control
  * @vblank: Vertical blanking in lines
  * @cur_mode: Pointer to current selected sensor mode
- * @mutex: Mutex for serializing sensor controls
  * @link_freq_bitmap: Menu bitmap for link_freq_ctrl
  * @cur_code: current selected format code
  */
 struct imx334 {
 	struct device *dev;
+	struct regmap *cci;
 	struct i2c_client *client;
 	struct v4l2_subdev sd;
 	struct media_pad pad;
@@ -157,7 +206,6 @@ struct imx334 {
 	};
 	u32 vblank;
 	const struct imx334_mode *cur_mode;
-	struct mutex mutex;
 	unsigned long link_freq_bitmap;
 	u32 cur_code;
 };
@@ -167,283 +215,183 @@ static const s64 link_freq[] = {
 	IMX334_LINK_FREQ_445M,
 };
 
+/* Sensor common mode registers values */
+static const struct cci_reg_sequence common_mode_regs[] = {
+	{ IMX334_REG_MODE_SELECT, IMX334_MODE_STANDBY },
+	{ IMX334_REG_WINMODE,		0x04 },
+	{ IMX334_REG_VMAX,		0x0008ca },
+	{ IMX334_REG_HMAX,		0x044c },
+	{ IMX334_REG_BLACK_OFSET_ADR,	0x0000 },
+	{ IMX334_REG_UNRD_LINE_MAX,	0x0000 },
+	{ IMX334_REG_OPB_SIZE_V,	0x00 },
+	{ IMX334_REG_HREVERSE,		0x00 },
+	{ IMX334_REG_VREVERSE,		0x00 },
+	{ IMX334_REG_UNREAD_PARAM5,	0x0000 },
+	{ IMX334_REG_UNREAD_PARAM6,	0x0008 },
+	{ IMX334_REG_XVS_XHS_OUTSEL,	0x20 },
+	{ IMX334_REG_XVS_XHS_DRV,	0x0f },
+	{ IMX334_REG_BCWAIT_TIME,	0x3b },
+	{ IMX334_REG_CPWAIT_TIME,	0x2a },
+	{ IMX334_REG_INCKSEL1,		0x0129 },
+	{ IMX334_REG_INCKSEL2,		0x06 },
+	{ IMX334_REG_INCKSEL3,		0xa0 },
+	{ IMX334_REG_INCKSEL4,		0x7e },
+	{ IMX334_REG_SYS_MODE,		0x02 },
+	{ IMX334_REG_HADD_VADD,		0x00 },
+	{ IMX334_REG_VALID_EXPAND,	0x03 },
+	{ IMX334_REG_TCYCLE,		0x00 },
+	{ IMX334_REG_TCLKPOST,		0x007f },
+	{ IMX334_REG_TCLKPREPARE,	0x0037 },
+	{ IMX334_REG_TCLKTRAIL,		0x0037 },
+	{ IMX334_REG_TCLKZERO,		0xf7 },
+	{ IMX334_REG_THSPREPARE,	0x002f },
+	{ CCI_REG8(0x3078), 0x02 },
+	{ CCI_REG8(0x3079), 0x00 },
+	{ CCI_REG8(0x307a), 0x00 },
+	{ CCI_REG8(0x307b), 0x00 },
+	{ CCI_REG8(0x3080), 0x02 },
+	{ CCI_REG8(0x3081), 0x00 },
+	{ CCI_REG8(0x3082), 0x00 },
+	{ CCI_REG8(0x3083), 0x00 },
+	{ CCI_REG8(0x3088), 0x02 },
+	{ CCI_REG8(0x3094), 0x00 },
+	{ CCI_REG8(0x3095), 0x00 },
+	{ CCI_REG8(0x3096), 0x00 },
+	{ CCI_REG8(0x309b), 0x02 },
+	{ CCI_REG8(0x309c), 0x00 },
+	{ CCI_REG8(0x309d), 0x00 },
+	{ CCI_REG8(0x309e), 0x00 },
+	{ CCI_REG8(0x30a4), 0x00 },
+	{ CCI_REG8(0x30a5), 0x00 },
+	{ CCI_REG8(0x3288), 0x21 },
+	{ CCI_REG8(0x328a), 0x02 },
+	{ CCI_REG8(0x3414), 0x05 },
+	{ CCI_REG8(0x3416), 0x18 },
+	{ CCI_REG8(0x35Ac), 0x0e },
+	{ CCI_REG8(0x3648), 0x01 },
+	{ CCI_REG8(0x364a), 0x04 },
+	{ CCI_REG8(0x364c), 0x04 },
+	{ CCI_REG8(0x3678), 0x01 },
+	{ CCI_REG8(0x367c), 0x31 },
+	{ CCI_REG8(0x367e), 0x31 },
+	{ CCI_REG8(0x3708), 0x02 },
+	{ CCI_REG8(0x3714), 0x01 },
+	{ CCI_REG8(0x3715), 0x02 },
+	{ CCI_REG8(0x3716), 0x02 },
+	{ CCI_REG8(0x3717), 0x02 },
+	{ CCI_REG8(0x371c), 0x3d },
+	{ CCI_REG8(0x371d), 0x3f },
+	{ CCI_REG8(0x372c), 0x00 },
+	{ CCI_REG8(0x372d), 0x00 },
+	{ CCI_REG8(0x372e), 0x46 },
+	{ CCI_REG8(0x372f), 0x00 },
+	{ CCI_REG8(0x3730), 0x89 },
+	{ CCI_REG8(0x3731), 0x00 },
+	{ CCI_REG8(0x3732), 0x08 },
+	{ CCI_REG8(0x3733), 0x01 },
+	{ CCI_REG8(0x3734), 0xfe },
+	{ CCI_REG8(0x3735), 0x05 },
+	{ CCI_REG8(0x375d), 0x00 },
+	{ CCI_REG8(0x375e), 0x00 },
+	{ CCI_REG8(0x375f), 0x61 },
+	{ CCI_REG8(0x3760), 0x06 },
+	{ CCI_REG8(0x3768), 0x1b },
+	{ CCI_REG8(0x3769), 0x1b },
+	{ CCI_REG8(0x376a), 0x1a },
+	{ CCI_REG8(0x376b), 0x19 },
+	{ CCI_REG8(0x376c), 0x18 },
+	{ CCI_REG8(0x376d), 0x14 },
+	{ CCI_REG8(0x376e), 0x0f },
+	{ CCI_REG8(0x3776), 0x00 },
+	{ CCI_REG8(0x3777), 0x00 },
+	{ CCI_REG8(0x3778), 0x46 },
+	{ CCI_REG8(0x3779), 0x00 },
+	{ CCI_REG8(0x377a), 0x08 },
+	{ CCI_REG8(0x377b), 0x01 },
+	{ CCI_REG8(0x377c), 0x45 },
+	{ CCI_REG8(0x377d), 0x01 },
+	{ CCI_REG8(0x377e), 0x23 },
+	{ CCI_REG8(0x377f), 0x02 },
+	{ CCI_REG8(0x3780), 0xd9 },
+	{ CCI_REG8(0x3781), 0x03 },
+	{ CCI_REG8(0x3782), 0xf5 },
+	{ CCI_REG8(0x3783), 0x06 },
+	{ CCI_REG8(0x3784), 0xa5 },
+	{ CCI_REG8(0x3788), 0x0f },
+	{ CCI_REG8(0x378a), 0xd9 },
+	{ CCI_REG8(0x378b), 0x03 },
+	{ CCI_REG8(0x378c), 0xeb },
+	{ CCI_REG8(0x378d), 0x05 },
+	{ CCI_REG8(0x378e), 0x87 },
+	{ CCI_REG8(0x378f), 0x06 },
+	{ CCI_REG8(0x3790), 0xf5 },
+	{ CCI_REG8(0x3792), 0x43 },
+	{ CCI_REG8(0x3794), 0x7a },
+	{ CCI_REG8(0x3796), 0xa1 },
+	{ CCI_REG8(0x37b0), 0x37 },
+	{ CCI_REG8(0x3e04), 0x0e },
+	{ IMX334_REG_AGAIN, 0x0050 },
+	{ IMX334_REG_MASTER_MODE, 0x00 },
+};
+
+/* Sensor mode registers for 640x480@30fps */
+static const struct cci_reg_sequence mode_640x480_regs[] = {
+	{ IMX334_REG_HTRIMMING_START,	0x0670 },
+	{ IMX334_REG_HNUM,		0x0280 },
+	{ IMX334_REG_AREA3_ST_ADR_1,	0x0748 },
+	{ IMX334_REG_AREA3_ST_ADR_2,	0x0749 },
+	{ IMX334_REG_AREA3_WIDTH_1,	0x01e0 },
+	{ IMX334_REG_AREA3_WIDTH_2,	0x01e0 },
+	{ IMX334_REG_Y_OUT_SIZE,	0x01e0 },
+	{ IMX334_REG_UNREAD_ED_ADR,	0x0b30 },
+};
+
+/* Sensor mode registers for 1280x720@30fps */
+static const struct cci_reg_sequence mode_1280x720_regs[] = {
+	{ IMX334_REG_HTRIMMING_START,	0x0530 },
+	{ IMX334_REG_HNUM,		0x0500 },
+	{ IMX334_REG_AREA3_ST_ADR_1,	0x0384 },
+	{ IMX334_REG_AREA3_ST_ADR_2,	0x0385 },
+	{ IMX334_REG_AREA3_WIDTH_1,	0x02d0 },
+	{ IMX334_REG_AREA3_WIDTH_2,	0x02d0 },
+	{ IMX334_REG_Y_OUT_SIZE,	0x02d0 },
+	{ IMX334_REG_UNREAD_ED_ADR,	0x0b30 },
+};
+
 /* Sensor mode registers for 1920x1080@30fps */
-static const struct imx334_reg mode_1920x1080_regs[] = {
-	{0x3000, 0x01},
-	{0x3018, 0x04},
-	{0x3030, 0xca},
-	{0x3031, 0x08},
-	{0x3032, 0x00},
-	{0x3034, 0x4c},
-	{0x3035, 0x04},
-	{0x302c, 0xf0},
-	{0x302d, 0x03},
-	{0x302e, 0x80},
-	{0x302f, 0x07},
-	{0x3074, 0xcc},
-	{0x3075, 0x02},
-	{0x308e, 0xcd},
-	{0x308f, 0x02},
-	{0x3076, 0x38},
-	{0x3077, 0x04},
-	{0x3090, 0x38},
-	{0x3091, 0x04},
-	{0x3308, 0x38},
-	{0x3309, 0x04},
-	{0x30C6, 0x00},
-	{0x30c7, 0x00},
-	{0x30ce, 0x00},
-	{0x30cf, 0x00},
-	{0x30d8, 0x18},
-	{0x30d9, 0x0a},
-	{0x304c, 0x00},
-	{0x304e, 0x00},
-	{0x304f, 0x00},
-	{0x3050, 0x00},
-	{0x30b6, 0x00},
-	{0x30b7, 0x00},
-	{0x3116, 0x08},
-	{0x3117, 0x00},
-	{0x31a0, 0x20},
-	{0x31a1, 0x0f},
-	{0x300c, 0x3b},
-	{0x300d, 0x29},
-	{0x314c, 0x29},
-	{0x314d, 0x01},
-	{0x315a, 0x06},
-	{0x3168, 0xa0},
-	{0x316a, 0x7e},
-	{0x319e, 0x02},
-	{0x3199, 0x00},
-	{0x319d, 0x00},
-	{0x31dd, 0x03},
-	{0x3300, 0x00},
-	{0x341c, 0xff},
-	{0x341d, 0x01},
-	{0x3a01, 0x03},
-	{0x3a18, 0x7f},
-	{0x3a19, 0x00},
-	{0x3a1a, 0x37},
-	{0x3a1b, 0x00},
-	{0x3a1c, 0x37},
-	{0x3a1d, 0x00},
-	{0x3a1e, 0xf7},
-	{0x3a1f, 0x00},
-	{0x3a20, 0x3f},
-	{0x3a21, 0x00},
-	{0x3a20, 0x6f},
-	{0x3a21, 0x00},
-	{0x3a20, 0x3f},
-	{0x3a21, 0x00},
-	{0x3a20, 0x5f},
-	{0x3a21, 0x00},
-	{0x3a20, 0x2f},
-	{0x3a21, 0x00},
-	{0x3078, 0x02},
-	{0x3079, 0x00},
-	{0x307a, 0x00},
-	{0x307b, 0x00},
-	{0x3080, 0x02},
-	{0x3081, 0x00},
-	{0x3082, 0x00},
-	{0x3083, 0x00},
-	{0x3088, 0x02},
-	{0x3094, 0x00},
-	{0x3095, 0x00},
-	{0x3096, 0x00},
-	{0x309b, 0x02},
-	{0x309c, 0x00},
-	{0x309d, 0x00},
-	{0x309e, 0x00},
-	{0x30a4, 0x00},
-	{0x30a5, 0x00},
-	{0x3288, 0x21},
-	{0x328a, 0x02},
-	{0x3414, 0x05},
-	{0x3416, 0x18},
-	{0x35Ac, 0x0e},
-	{0x3648, 0x01},
-	{0x364a, 0x04},
-	{0x364c, 0x04},
-	{0x3678, 0x01},
-	{0x367c, 0x31},
-	{0x367e, 0x31},
-	{0x3708, 0x02},
-	{0x3714, 0x01},
-	{0x3715, 0x02},
-	{0x3716, 0x02},
-	{0x3717, 0x02},
-	{0x371c, 0x3d},
-	{0x371d, 0x3f},
-	{0x372c, 0x00},
-	{0x372d, 0x00},
-	{0x372e, 0x46},
-	{0x372f, 0x00},
-	{0x3730, 0x89},
-	{0x3731, 0x00},
-	{0x3732, 0x08},
-	{0x3733, 0x01},
-	{0x3734, 0xfe},
-	{0x3735, 0x05},
-	{0x375d, 0x00},
-	{0x375e, 0x00},
-	{0x375f, 0x61},
-	{0x3760, 0x06},
-	{0x3768, 0x1b},
-	{0x3769, 0x1b},
-	{0x376a, 0x1a},
-	{0x376b, 0x19},
-	{0x376c, 0x18},
-	{0x376d, 0x14},
-	{0x376e, 0x0f},
-	{0x3776, 0x00},
-	{0x3777, 0x00},
-	{0x3778, 0x46},
-	{0x3779, 0x00},
-	{0x377a, 0x08},
-	{0x377b, 0x01},
-	{0x377c, 0x45},
-	{0x377d, 0x01},
-	{0x377e, 0x23},
-	{0x377f, 0x02},
-	{0x3780, 0xd9},
-	{0x3781, 0x03},
-	{0x3782, 0xf5},
-	{0x3783, 0x06},
-	{0x3784, 0xa5},
-	{0x3788, 0x0f},
-	{0x378a, 0xd9},
-	{0x378b, 0x03},
-	{0x378c, 0xeb},
-	{0x378d, 0x05},
-	{0x378e, 0x87},
-	{0x378f, 0x06},
-	{0x3790, 0xf5},
-	{0x3792, 0x43},
-	{0x3794, 0x7a},
-	{0x3796, 0xa1},
-	{0x37b0, 0x37},
-	{0x3e04, 0x0e},
-	{0x30e8, 0x50},
-	{0x30e9, 0x00},
-	{0x3e04, 0x0e},
-	{0x3002, 0x00},
+static const struct cci_reg_sequence mode_1920x1080_regs[] = {
+	{ IMX334_REG_HTRIMMING_START,	0x03f0 },
+	{ IMX334_REG_HNUM,		0x0780 },
+	{ IMX334_REG_AREA3_ST_ADR_1,	0x02cc },
+	{ IMX334_REG_AREA3_ST_ADR_2,	0x02cd },
+	{ IMX334_REG_AREA3_WIDTH_1,	0x0438 },
+	{ IMX334_REG_AREA3_WIDTH_2,	0x0438 },
+	{ IMX334_REG_Y_OUT_SIZE,	0x0438 },
+	{ IMX334_REG_UNREAD_ED_ADR,	0x0a18 },
 };
 
 /* Sensor mode registers for 3840x2160@30fps */
-static const struct imx334_reg mode_3840x2160_regs[] = {
-	{0x3000, 0x01},
-	{0x3002, 0x00},
-	{0x3018, 0x04},
-	{0x37b0, 0x36},
-	{0x304c, 0x00},
-	{0x300c, 0x3b},
-	{0x300d, 0x2a},
-	{0x3034, 0x26},
-	{0x3035, 0x02},
-	{0x314c, 0x29},
-	{0x314d, 0x01},
-	{0x315a, 0x02},
-	{0x3168, 0xa0},
-	{0x316a, 0x7e},
-	{0x3288, 0x21},
-	{0x328a, 0x02},
-	{0x302c, 0x3c},
-	{0x302d, 0x00},
-	{0x302e, 0x00},
-	{0x302f, 0x0f},
-	{0x3076, 0x70},
-	{0x3077, 0x08},
-	{0x3090, 0x70},
-	{0x3091, 0x08},
-	{0x30d8, 0x20},
-	{0x30d9, 0x12},
-	{0x3308, 0x70},
-	{0x3309, 0x08},
-	{0x3414, 0x05},
-	{0x3416, 0x18},
-	{0x35ac, 0x0e},
-	{0x3648, 0x01},
-	{0x364a, 0x04},
-	{0x364c, 0x04},
-	{0x3678, 0x01},
-	{0x367c, 0x31},
-	{0x367e, 0x31},
-	{0x3708, 0x02},
-	{0x3714, 0x01},
-	{0x3715, 0x02},
-	{0x3716, 0x02},
-	{0x3717, 0x02},
-	{0x371c, 0x3d},
-	{0x371d, 0x3f},
-	{0x372c, 0x00},
-	{0x372d, 0x00},
-	{0x372e, 0x46},
-	{0x372f, 0x00},
-	{0x3730, 0x89},
-	{0x3731, 0x00},
-	{0x3732, 0x08},
-	{0x3733, 0x01},
-	{0x3734, 0xfe},
-	{0x3735, 0x05},
-	{0x375d, 0x00},
-	{0x375e, 0x00},
-	{0x375f, 0x61},
-	{0x3760, 0x06},
-	{0x3768, 0x1b},
-	{0x3769, 0x1b},
-	{0x376a, 0x1a},
-	{0x376b, 0x19},
-	{0x376c, 0x18},
-	{0x376d, 0x14},
-	{0x376e, 0x0f},
-	{0x3776, 0x00},
-	{0x3777, 0x00},
-	{0x3778, 0x46},
-	{0x3779, 0x00},
-	{0x377a, 0x08},
-	{0x377b, 0x01},
-	{0x377c, 0x45},
-	{0x377d, 0x01},
-	{0x377e, 0x23},
-	{0x377f, 0x02},
-	{0x3780, 0xd9},
-	{0x3781, 0x03},
-	{0x3782, 0xf5},
-	{0x3783, 0x06},
-	{0x3784, 0xa5},
-	{0x3788, 0x0f},
-	{0x378a, 0xd9},
-	{0x378b, 0x03},
-	{0x378c, 0xeb},
-	{0x378d, 0x05},
-	{0x378e, 0x87},
-	{0x378f, 0x06},
-	{0x3790, 0xf5},
-	{0x3792, 0x43},
-	{0x3794, 0x7a},
-	{0x3796, 0xa1},
-	{0x3e04, 0x0e},
-	{0x319e, 0x00},
-	{0x3a00, 0x01},
-	{0x3a18, 0xbf},
-	{0x3a19, 0x00},
-	{0x3a1a, 0x67},
-	{0x3a1b, 0x00},
-	{0x3a1c, 0x6f},
-	{0x3a1d, 0x00},
-	{0x3a1e, 0xd7},
-	{0x3a1f, 0x01},
-	{0x3a20, 0x6f},
-	{0x3a21, 0x00},
-	{0x3a22, 0xcf},
-	{0x3a23, 0x00},
-	{0x3a24, 0x6f},
-	{0x3a25, 0x00},
-	{0x3a26, 0xb7},
-	{0x3a27, 0x00},
-	{0x3a28, 0x5f},
-	{0x3a29, 0x00},
+static const struct cci_reg_sequence mode_3840x2160_regs[] = {
+	{ IMX334_REG_HMAX,		0x0226 },
+	{ IMX334_REG_INCKSEL2,		0x02 },
+	{ IMX334_REG_HTRIMMING_START,	0x003c },
+	{ IMX334_REG_HNUM,		0x0f00 },
+	{ IMX334_REG_AREA3_ST_ADR_1,	0x00b0 },
+	{ IMX334_REG_AREA3_ST_ADR_2,	0x00b1 },
+	{ IMX334_REG_UNREAD_ED_ADR,	0x1220 },
+	{ IMX334_REG_AREA3_WIDTH_1,	0x0870 },
+	{ IMX334_REG_AREA3_WIDTH_2,	0x0870 },
+	{ IMX334_REG_Y_OUT_SIZE,	0x0870 },
+	{ IMX334_REG_SYS_MODE,		0x0100 },
+	{ IMX334_REG_TCLKPOST,		0x00bf },
+	{ IMX334_REG_TCLKPREPARE,	0x0067 },
+	{ IMX334_REG_TCLKTRAIL,		0x006f },
+	{ IMX334_REG_TCLKZERO,		0x1d7 },
+	{ IMX334_REG_THSPREPARE,	0x006f },
+	{ IMX334_REG_THSZERO,		0x00cf },
+	{ IMX334_REG_THSTRAIL,		0x006f },
+	{ IMX334_REG_THSEXIT,		0x00b7 },
+	{ IMX334_REG_TPLX,		0x005f },
 };
 
 static const char * const imx334_test_pattern_menu[] = {
@@ -458,18 +406,16 @@ static const int imx334_test_pattern_val[] = {
 	IMX334_TP_COLOR_VBARS,
 };
 
-static const struct imx334_reg raw10_framefmt_regs[] = {
-	{0x3050, 0x00},
-	{0x319d, 0x00},
-	{0x341c, 0xff},
-	{0x341d, 0x01},
+static const struct cci_reg_sequence raw10_framefmt_regs[] = {
+	{ IMX334_REG_ADBIT,  0x00 },
+	{ IMX334_REG_MDBIT,  0x00 },
+	{ IMX334_REG_ADBIT1, 0x01ff },
 };
 
-static const struct imx334_reg raw12_framefmt_regs[] = {
-	{0x3050, 0x01},
-	{0x319d, 0x01},
-	{0x341c, 0x47},
-	{0x341d, 0x00},
+static const struct cci_reg_sequence raw12_framefmt_regs[] = {
+	{ IMX334_REG_ADBIT,  0x01 },
+	{ IMX334_REG_MDBIT,  0x01 },
+	{ IMX334_REG_ADBIT1, 0x0047 },
 };
 
 static const u32 imx334_mbus_codes[] = {
@@ -505,6 +451,32 @@ static const struct imx334_mode supported_modes[] = {
 			.num_of_regs = ARRAY_SIZE(mode_1920x1080_regs),
 			.regs = mode_1920x1080_regs,
 		},
+	}, {
+		.width = 1280,
+		.height = 720,
+		.hblank = 2480,
+		.vblank = 1170,
+		.vblank_min = 45,
+		.vblank_max = 132840,
+		.pclk = 297000000,
+		.link_freq_idx = 1,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1280x720_regs),
+			.regs = mode_1280x720_regs,
+		},
+	}, {
+		.width = 640,
+		.height = 480,
+		.hblank = 2480,
+		.vblank = 1170,
+		.vblank_min = 45,
+		.vblank_max = 132840,
+		.pclk = 297000000,
+		.link_freq_idx = 1,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_640x480_regs),
+			.regs = mode_640x480_regs,
+		},
 	},
 };
 
@@ -520,101 +492,6 @@ static inline struct imx334 *to_imx334(struct v4l2_subdev *subdev)
 }
 
 /**
- * imx334_read_reg() - Read registers.
- * @imx334: pointer to imx334 device
- * @reg: register address
- * @len: length of bytes to read. Max supported bytes is 4
- * @val: pointer to register value to be filled.
- *
- * Big endian register addresses with little endian values.
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx334_read_reg(struct imx334 *imx334, u16 reg, u32 len, u32 *val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
-	struct i2c_msg msgs[2] = {0};
-	u8 addr_buf[2] = {0};
-	u8 data_buf[4] = {0};
-	int ret;
-
-	if (WARN_ON(len > 4))
-		return -EINVAL;
-
-	put_unaligned_be16(reg, addr_buf);
-
-	/* Write register address */
-	msgs[0].addr = client->addr;
-	msgs[0].flags = 0;
-	msgs[0].len = ARRAY_SIZE(addr_buf);
-	msgs[0].buf = addr_buf;
-
-	/* Read data from register */
-	msgs[1].addr = client->addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].len = len;
-	msgs[1].buf = data_buf;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
-
-	*val = get_unaligned_le32(data_buf);
-
-	return 0;
-}
-
-/**
- * imx334_write_reg() - Write register
- * @imx334: pointer to imx334 device
- * @reg: register address
- * @len: length of bytes. Max supported bytes is 4
- * @val: register value
- *
- * Big endian register addresses with little endian values.
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx334_write_reg(struct imx334 *imx334, u16 reg, u32 len, u32 val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx334->sd);
-	u8 buf[6] = {0};
-
-	if (WARN_ON(len > 4))
-		return -EINVAL;
-
-	put_unaligned_be16(reg, buf);
-	put_unaligned_le32(val, buf + 2);
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
-
-	return 0;
-}
-
-/**
- * imx334_write_regs() - Write a list of registers
- * @imx334: pointer to imx334 device
- * @regs: list of registers to be written
- * @len: length of registers array
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx334_write_regs(struct imx334 *imx334,
-			     const struct imx334_reg *regs, u32 len)
-{
-	unsigned int i;
-	int ret;
-
-	for (i = 0; i < len; i++) {
-		ret = imx334_write_reg(imx334, regs[i].address, 1, regs[i].val);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
-/**
  * imx334_update_controls() - Update control ranges based on streaming mode
  * @imx334: pointer to imx334 device
  * @mode: pointer to imx334_mode sensor mode
@@ -659,30 +536,23 @@ static int imx334_update_controls(struct imx334 *imx334,
 static int imx334_update_exp_gain(struct imx334 *imx334, u32 exposure, u32 gain)
 {
 	u32 lpfr, shutter;
-	int ret;
+	int ret_hold;
+	int ret = 0;
 
 	lpfr = imx334->vblank + imx334->cur_mode->height;
 	shutter = lpfr - exposure;
 
-	dev_dbg(imx334->dev, "Set long exp %u analog gain %u sh0 %u lpfr %u",
+	dev_dbg(imx334->dev, "Set long exp %u analog gain %u sh0 %u lpfr %u\n",
 		exposure, gain, shutter, lpfr);
 
-	ret = imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 1);
-	if (ret)
-		return ret;
-
-	ret = imx334_write_reg(imx334, IMX334_REG_LPFR, 3, lpfr);
-	if (ret)
-		goto error_release_group_hold;
-
-	ret = imx334_write_reg(imx334, IMX334_REG_SHUTTER, 3, shutter);
-	if (ret)
-		goto error_release_group_hold;
-
-	ret = imx334_write_reg(imx334, IMX334_REG_AGAIN, 1, gain);
+	cci_write(imx334->cci, IMX334_REG_HOLD, 1, &ret);
+	cci_write(imx334->cci, IMX334_REG_VMAX, lpfr, &ret);
+	cci_write(imx334->cci, IMX334_REG_SHUTTER, shutter, &ret);
+	cci_write(imx334->cci, IMX334_REG_AGAIN, gain, &ret);
 
-error_release_group_hold:
-	imx334_write_reg(imx334, IMX334_REG_HOLD, 1, 0);
+	ret_hold = cci_write(imx334->cci, IMX334_REG_HOLD, 0, NULL);
+	if (ret_hold)
+		return ret_hold;
 
 	return ret;
 }
@@ -707,11 +577,10 @@ static int imx334_set_ctrl(struct v4l2_ctrl *ctrl)
 	u32 exposure;
 	int ret;
 
-	switch (ctrl->id) {
-	case V4L2_CID_VBLANK:
+	if (ctrl->id == V4L2_CID_VBLANK) {
 		imx334->vblank = imx334->vblank_ctrl->val;
 
-		dev_dbg(imx334->dev, "Received vblank %u, new lpfr %u",
+		dev_dbg(imx334->dev, "Received vblank %u, new lpfr %u\n",
 			imx334->vblank,
 			imx334->vblank + imx334->cur_mode->height);
 
@@ -721,23 +590,32 @@ static int imx334_set_ctrl(struct v4l2_ctrl *ctrl)
 					       imx334->cur_mode->height -
 					       IMX334_EXPOSURE_OFFSET,
 					       1, IMX334_EXPOSURE_DEFAULT);
+		if (ret)
+			return ret;
+	}
+
+	/* Set controls only if sensor is in power on state */
+	if (!pm_runtime_get_if_in_use(imx334->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		exposure = imx334->exp_ctrl->val;
+		analog_gain = imx334->again_ctrl->val;
+
+		ret = imx334_update_exp_gain(imx334, exposure, analog_gain);
+
 		break;
 	case V4L2_CID_EXPOSURE:
 
-		/* Set controls only if sensor is in power on state */
-		if (!pm_runtime_get_if_in_use(imx334->dev))
-			return 0;
-
 		exposure = ctrl->val;
 		analog_gain = imx334->again_ctrl->val;
 
-		dev_dbg(imx334->dev, "Received exp %u analog gain %u",
+		dev_dbg(imx334->dev, "Received exp %u analog gain %u\n",
 			exposure, analog_gain);
 
 		ret = imx334_update_exp_gain(imx334, exposure, analog_gain);
 
-		pm_runtime_put(imx334->dev);
-
 		break;
 	case V4L2_CID_PIXEL_RATE:
 	case V4L2_CID_LINK_FREQ:
@@ -746,29 +624,31 @@ static int imx334_set_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 	case V4L2_CID_TEST_PATTERN:
 		if (ctrl->val) {
-			imx334_write_reg(imx334, IMX334_TP_CLK_EN, 1,
-					 IMX334_TP_CLK_EN_VAL);
-			imx334_write_reg(imx334, IMX334_DIG_CLP_MODE, 1, 0x0);
-			imx334_write_reg(imx334, IMX334_TPG_COLORW, 1,
-					 IMX334_TPG_COLORW_120P);
-			imx334_write_reg(imx334, IMX334_REG_TP, 1,
-					 imx334_test_pattern_val[ctrl->val]);
-			imx334_write_reg(imx334, IMX334_TPG_EN_DOUT, 1,
-					 IMX334_TP_ENABLE);
+			cci_write(imx334->cci, IMX334_TP_CLK_EN,
+				  IMX334_TP_CLK_EN_VAL, NULL);
+			cci_write(imx334->cci, IMX334_DIG_CLP_MODE, 0x0, NULL);
+			cci_write(imx334->cci, IMX334_TPG_COLORW,
+				  IMX334_TPG_COLORW_120P, NULL);
+			cci_write(imx334->cci, IMX334_REG_TP,
+				  imx334_test_pattern_val[ctrl->val], NULL);
+			cci_write(imx334->cci, IMX334_TPG_EN_DOUT,
+				  IMX334_TP_ENABLE, NULL);
 		} else {
-			imx334_write_reg(imx334, IMX334_DIG_CLP_MODE, 1, 0x1);
-			imx334_write_reg(imx334, IMX334_TP_CLK_EN, 1,
-					 IMX334_TP_CLK_DIS_VAL);
-			imx334_write_reg(imx334, IMX334_TPG_EN_DOUT, 1,
-					 IMX334_TP_DISABLE);
+			cci_write(imx334->cci, IMX334_DIG_CLP_MODE, 0x1, NULL);
+			cci_write(imx334->cci, IMX334_TP_CLK_EN,
+				  IMX334_TP_CLK_DIS_VAL, NULL);
+			cci_write(imx334->cci, IMX334_TPG_EN_DOUT,
+				  IMX334_TP_DISABLE, NULL);
 		}
 		ret = 0;
 		break;
 	default:
-		dev_err(imx334->dev, "Invalid control %d", ctrl->id);
+		dev_err(imx334->dev, "Invalid control %d\n", ctrl->id);
 		ret = -EINVAL;
 	}
 
+	pm_runtime_put(imx334->dev);
+
 	return ret;
 }
 
@@ -874,8 +754,6 @@ static int imx334_get_pad_format(struct v4l2_subdev *sd,
 {
 	struct imx334 *imx334 = to_imx334(sd);
 
-	mutex_lock(&imx334->mutex);
-
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		struct v4l2_mbus_framefmt *framefmt;
 
@@ -886,8 +764,6 @@ static int imx334_get_pad_format(struct v4l2_subdev *sd,
 		imx334_fill_pad_format(imx334, imx334->cur_mode, fmt);
 	}
 
-	mutex_unlock(&imx334->mutex);
-
 	return 0;
 }
 
@@ -907,8 +783,6 @@ static int imx334_set_pad_format(struct v4l2_subdev *sd,
 	const struct imx334_mode *mode;
 	int ret = 0;
 
-	mutex_lock(&imx334->mutex);
-
 	mode = v4l2_find_nearest_size(supported_modes,
 				      ARRAY_SIZE(supported_modes),
 				      width, height,
@@ -929,8 +803,6 @@ static int imx334_set_pad_format(struct v4l2_subdev *sd,
 			imx334->cur_mode = mode;
 	}
 
-	mutex_unlock(&imx334->mutex);
-
 	return ret;
 }
 
@@ -949,8 +821,6 @@ static int imx334_init_state(struct v4l2_subdev *sd,
 
 	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
 
-	mutex_lock(&imx334->mutex);
-
 	imx334_fill_pad_format(imx334, imx334->cur_mode, &fmt);
 
 	__v4l2_ctrl_modify_range(imx334->link_freq_ctrl, 0,
@@ -958,8 +828,6 @@ static int imx334_init_state(struct v4l2_subdev *sd,
 				 ~(imx334->link_freq_bitmap),
 				 __ffs(imx334->link_freq_bitmap));
 
-	mutex_unlock(&imx334->mutex);
-
 	return imx334_set_pad_format(sd, sd_state, &fmt);
 }
 
@@ -967,109 +835,113 @@ static int imx334_set_framefmt(struct imx334 *imx334)
 {
 	switch (imx334->cur_code) {
 	case MEDIA_BUS_FMT_SRGGB10_1X10:
-		return imx334_write_regs(imx334, raw10_framefmt_regs,
-					 ARRAY_SIZE(raw10_framefmt_regs));
+		return cci_multi_reg_write(imx334->cci, raw10_framefmt_regs,
+					ARRAY_SIZE(raw10_framefmt_regs), NULL);
+
 
 	case MEDIA_BUS_FMT_SRGGB12_1X12:
-		return imx334_write_regs(imx334, raw12_framefmt_regs,
-					 ARRAY_SIZE(raw12_framefmt_regs));
+		return cci_multi_reg_write(imx334->cci, raw12_framefmt_regs,
+					ARRAY_SIZE(raw12_framefmt_regs), NULL);
 	}
 
 	return -EINVAL;
 }
 
 /**
- * imx334_start_streaming() - Start sensor stream
- * @imx334: pointer to imx334 device
+ * imx334_enable_streams() - Enable specified streams for the sensor
+ * @sd: pointer to the V4L2 subdevice
+ * @state: pointer to the subdevice state
+ * @pad: pad number for which streams are enabled
+ * @streams_mask: bitmask specifying the streams to enable
  *
  * Return: 0 if successful, error code otherwise.
  */
-static int imx334_start_streaming(struct imx334 *imx334)
+static int imx334_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
 {
+	struct imx334 *imx334 = to_imx334(sd);
 	const struct imx334_reg_list *reg_list;
 	int ret;
 
+	ret = pm_runtime_resume_and_get(imx334->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = cci_multi_reg_write(imx334->cci, common_mode_regs,
+				  ARRAY_SIZE(common_mode_regs), NULL);
+	if (ret) {
+		dev_err(imx334->dev, "fail to write common registers\n");
+		goto err_rpm_put;
+	}
+
 	/* Write sensor mode registers */
 	reg_list = &imx334->cur_mode->reg_list;
-	ret = imx334_write_regs(imx334, reg_list->regs,
-				reg_list->num_of_regs);
+	ret = cci_multi_reg_write(imx334->cci, reg_list->regs,
+				  reg_list->num_of_regs, NULL);
 	if (ret) {
-		dev_err(imx334->dev, "fail to write initial registers");
-		return ret;
+		dev_err(imx334->dev, "fail to write initial registers\n");
+		goto err_rpm_put;
+	}
+
+	ret = cci_write(imx334->cci, IMX334_REG_LANEMODE,
+			IMX334_CSI_4_LANE_MODE, NULL);
+	if (ret) {
+		dev_err(imx334->dev, "failed to configure lanes\n");
+		goto err_rpm_put;
 	}
 
 	ret = imx334_set_framefmt(imx334);
 	if (ret) {
 		dev_err(imx334->dev, "%s failed to set frame format: %d\n",
 			__func__, ret);
-		return ret;
+		goto err_rpm_put;
 	}
 
 	/* Setup handler will write actual exposure and gain */
 	ret =  __v4l2_ctrl_handler_setup(imx334->sd.ctrl_handler);
 	if (ret) {
-		dev_err(imx334->dev, "fail to setup handler");
-		return ret;
+		dev_err(imx334->dev, "fail to setup handler\n");
+		goto err_rpm_put;
 	}
 
 	/* Start streaming */
-	ret = imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
-			       1, IMX334_MODE_STREAMING);
+	ret = cci_write(imx334->cci, IMX334_REG_MODE_SELECT,
+			IMX334_MODE_STREAMING, NULL);
 	if (ret) {
-		dev_err(imx334->dev, "fail to start streaming");
-		return ret;
+		dev_err(imx334->dev, "fail to start streaming\n");
+		goto err_rpm_put;
 	}
 
 	return 0;
-}
 
-/**
- * imx334_stop_streaming() - Stop sensor stream
- * @imx334: pointer to imx334 device
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx334_stop_streaming(struct imx334 *imx334)
-{
-	return imx334_write_reg(imx334, IMX334_REG_MODE_SELECT,
-				1, IMX334_MODE_STANDBY);
+err_rpm_put:
+	pm_runtime_put(imx334->dev);
+	return ret;
 }
 
 /**
- * imx334_set_stream() - Enable sensor streaming
- * @sd: pointer to imx334 subdevice
- * @enable: set to enable sensor streaming
+ * imx334_disable_streams() - Enable specified streams for the sensor
+ * @sd: pointer to the V4L2 subdevice
+ * @state: pointer to the subdevice state
+ * @pad: pad number for which streams are disabled
+ * @streams_mask: bitmask specifying the streams to disable
  *
  * Return: 0 if successful, error code otherwise.
  */
-static int imx334_set_stream(struct v4l2_subdev *sd, int enable)
+static int imx334_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
 {
 	struct imx334 *imx334 = to_imx334(sd);
 	int ret;
 
-	mutex_lock(&imx334->mutex);
-
-	if (enable) {
-		ret = pm_runtime_resume_and_get(imx334->dev);
-		if (ret < 0)
-			goto error_unlock;
-
-		ret = imx334_start_streaming(imx334);
-		if (ret)
-			goto error_power_off;
-	} else {
-		imx334_stop_streaming(imx334);
-		pm_runtime_put(imx334->dev);
-	}
-
-	mutex_unlock(&imx334->mutex);
-
-	return 0;
+	ret = cci_write(imx334->cci, IMX334_REG_MODE_SELECT,
+			IMX334_MODE_STANDBY, NULL);
+	if (ret)
+		dev_err(imx334->dev, "%s failed to stop stream\n", __func__);
 
-error_power_off:
 	pm_runtime_put(imx334->dev);
-error_unlock:
-	mutex_unlock(&imx334->mutex);
 
 	return ret;
 }
@@ -1083,14 +955,14 @@ error_unlock:
 static int imx334_detect(struct imx334 *imx334)
 {
 	int ret;
-	u32 val;
+	u64 val;
 
-	ret = imx334_read_reg(imx334, IMX334_REG_ID, 2, &val);
+	ret = cci_read(imx334->cci, IMX334_REG_ID, &val, NULL);
 	if (ret)
 		return ret;
 
 	if (val != IMX334_ID) {
-		dev_err(imx334->dev, "chip id mismatch: %x!=%x",
+		dev_err(imx334->dev, "chip id mismatch: %x!=%llx\n",
 			IMX334_ID, val);
 		return -ENXIO;
 	}
@@ -1120,24 +992,20 @@ static int imx334_parse_hw_config(struct imx334 *imx334)
 	/* Request optional reset pin */
 	imx334->reset_gpio = devm_gpiod_get_optional(imx334->dev, "reset",
 						     GPIOD_OUT_LOW);
-	if (IS_ERR(imx334->reset_gpio)) {
-		dev_err(imx334->dev, "failed to get reset gpio %ld",
-			PTR_ERR(imx334->reset_gpio));
-		return PTR_ERR(imx334->reset_gpio);
-	}
+	if (IS_ERR(imx334->reset_gpio))
+		return dev_err_probe(imx334->dev, PTR_ERR(imx334->reset_gpio),
+				     "failed to get reset gpio\n");
 
 	/* Get sensor input clock */
 	imx334->inclk = devm_clk_get(imx334->dev, NULL);
-	if (IS_ERR(imx334->inclk)) {
-		dev_err(imx334->dev, "could not get inclk");
-		return PTR_ERR(imx334->inclk);
-	}
+	if (IS_ERR(imx334->inclk))
+		return dev_err_probe(imx334->dev, PTR_ERR(imx334->inclk),
+					 "could not get inclk\n");
 
 	rate = clk_get_rate(imx334->inclk);
-	if (rate != IMX334_INCLK_RATE) {
-		dev_err(imx334->dev, "inclk frequency mismatch");
-		return -EINVAL;
-	}
+	if (rate != IMX334_INCLK_RATE)
+		return dev_err_probe(imx334->dev, -EINVAL,
+					 "inclk frequency mismatch\n");
 
 	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
 	if (!ep)
@@ -1150,7 +1018,7 @@ static int imx334_parse_hw_config(struct imx334 *imx334)
 
 	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX334_NUM_DATA_LANES) {
 		dev_err(imx334->dev,
-			"number of CSI2 data lanes %d is not supported",
+			"number of CSI2 data lanes %d is not supported\n",
 			bus_cfg.bus.mipi_csi2.num_data_lanes);
 		ret = -EINVAL;
 		goto done_endpoint_free;
@@ -1169,7 +1037,7 @@ done_endpoint_free:
 
 /* V4l2 subdevice ops */
 static const struct v4l2_subdev_video_ops imx334_video_ops = {
-	.s_stream = imx334_set_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_pad_ops imx334_pad_ops = {
@@ -1177,6 +1045,8 @@ static const struct v4l2_subdev_pad_ops imx334_pad_ops = {
 	.enum_frame_size = imx334_enum_frame_size,
 	.get_fmt = imx334_get_pad_format,
 	.set_fmt = imx334_set_pad_format,
+	.enable_streams = imx334_enable_streams,
+	.disable_streams = imx334_disable_streams,
 };
 
 static const struct v4l2_subdev_ops imx334_subdev_ops = {
@@ -1204,7 +1074,7 @@ static int imx334_power_on(struct device *dev)
 
 	ret = clk_prepare_enable(imx334->inclk);
 	if (ret) {
-		dev_err(imx334->dev, "fail to enable inclk");
+		dev_err(imx334->dev, "fail to enable inclk\n");
 		goto error_reset;
 	}
 
@@ -1253,9 +1123,6 @@ static int imx334_init_controls(struct imx334 *imx334)
 	if (ret)
 		return ret;
 
-	/* Serialize controls with sensor device */
-	ctrl_hdlr->lock = &imx334->mutex;
-
 	/* Initialize exposure and gain */
 	lpfr = mode->vblank + mode->height;
 	imx334->exp_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
@@ -1342,29 +1209,31 @@ static int imx334_probe(struct i2c_client *client)
 		return -ENOMEM;
 
 	imx334->dev = &client->dev;
+	imx334->cci = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx334->cci)) {
+		dev_err(imx334->dev, "Unable to initialize I2C\n");
+		return -ENODEV;
+	}
 
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&imx334->sd, client, &imx334_subdev_ops);
 	imx334->sd.internal_ops = &imx334_internal_ops;
 
 	ret = imx334_parse_hw_config(imx334);
-	if (ret) {
-		dev_err(imx334->dev, "HW configuration is not supported");
-		return ret;
-	}
-
-	mutex_init(&imx334->mutex);
+	if (ret)
+		return dev_err_probe(imx334->dev, ret,
+					"HW configuration is not supported\n");
 
 	ret = imx334_power_on(imx334->dev);
 	if (ret) {
-		dev_err(imx334->dev, "failed to power-on the sensor");
-		goto error_mutex_destroy;
+		dev_err_probe(imx334->dev, ret, "failed to power-on the sensor\n");
+		return ret;
 	}
 
 	/* Check module identity */
 	ret = imx334_detect(imx334);
 	if (ret) {
-		dev_err(imx334->dev, "failed to find sensor: %d", ret);
+		dev_err(imx334->dev, "failed to find sensor: %d\n", ret);
 		goto error_power_off;
 	}
 
@@ -1375,7 +1244,7 @@ static int imx334_probe(struct i2c_client *client)
 
 	ret = imx334_init_controls(imx334);
 	if (ret) {
-		dev_err(imx334->dev, "failed to init controls: %d", ret);
+		dev_err(imx334->dev, "failed to init controls: %d\n", ret);
 		goto error_power_off;
 	}
 
@@ -1387,31 +1256,44 @@ static int imx334_probe(struct i2c_client *client)
 	imx334->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&imx334->sd.entity, 1, &imx334->pad);
 	if (ret) {
-		dev_err(imx334->dev, "failed to init entity pads: %d", ret);
+		dev_err(imx334->dev, "failed to init entity pads: %d\n", ret);
 		goto error_handler_free;
 	}
 
-	ret = v4l2_async_register_subdev_sensor(&imx334->sd);
+	imx334->sd.state_lock = imx334->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&imx334->sd);
 	if (ret < 0) {
-		dev_err(imx334->dev,
-			"failed to register async subdev: %d", ret);
+		dev_err(imx334->dev, "subdev init error: %d\n", ret);
 		goto error_media_entity;
 	}
 
 	pm_runtime_set_active(imx334->dev);
 	pm_runtime_enable(imx334->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&imx334->sd);
+	if (ret < 0) {
+		dev_err(imx334->dev,
+			"failed to register async subdev: %d\n", ret);
+		goto error_subdev_cleanup;
+	}
+
 	pm_runtime_idle(imx334->dev);
 
 	return 0;
 
+error_subdev_cleanup:
+	v4l2_subdev_cleanup(&imx334->sd);
+	pm_runtime_disable(imx334->dev);
+	pm_runtime_set_suspended(imx334->dev);
+
 error_media_entity:
 	media_entity_cleanup(&imx334->sd.entity);
+
 error_handler_free:
 	v4l2_ctrl_handler_free(imx334->sd.ctrl_handler);
+
 error_power_off:
 	imx334_power_off(imx334->dev);
-error_mutex_destroy:
-	mutex_destroy(&imx334->mutex);
 
 	return ret;
 }
@@ -1425,16 +1307,17 @@ error_mutex_destroy:
 static void imx334_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
-	struct imx334 *imx334 = to_imx334(sd);
 
 	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
 	media_entity_cleanup(&sd->entity);
 	v4l2_ctrl_handler_free(sd->ctrl_handler);
 
 	pm_runtime_disable(&client->dev);
-	pm_runtime_suspended(&client->dev);
-
-	mutex_destroy(&imx334->mutex);
+	if (!pm_runtime_status_suspended(&client->dev)) {
+		imx334_power_off(&client->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
 }
 
 static const struct dev_pm_ops imx334_pm_ops = {
diff --git a/drivers/media/i2c/imx335.c b/drivers/media/i2c/imx335.c
index dab6d080bc4c..9b4db4cd4929 100644
--- a/drivers/media/i2c/imx335.c
+++ b/drivers/media/i2c/imx335.c
@@ -4,64 +4,115 @@
  *
  * Copyright (C) 2021 Intel Corporation
  */
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/regmap.h>
 
+#include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
 /* Streaming Mode */
-#define IMX335_REG_MODE_SELECT	0x3000
-#define IMX335_MODE_STANDBY	0x01
-#define IMX335_MODE_STREAMING	0x00
+#define IMX335_REG_MODE_SELECT		CCI_REG8(0x3000)
+#define IMX335_MODE_STANDBY		0x01
+#define IMX335_MODE_STREAMING		0x00
+
+/* Group hold register */
+#define IMX335_REG_HOLD			CCI_REG8(0x3001)
+
+#define IMX335_REG_MASTER_MODE		CCI_REG8(0x3002)
+#define IMX335_REG_BCWAIT_TIME		CCI_REG8(0x300c)
+#define IMX335_REG_CPWAIT_TIME		CCI_REG8(0x300d)
+#define IMX335_REG_WINMODE		CCI_REG8(0x3018)
+#define IMX335_REG_HTRIMMING_START	CCI_REG16_LE(0x302c)
+#define IMX335_REG_HNUM			CCI_REG16_LE(0x302e)
 
 /* Lines per frame */
-#define IMX335_REG_LPFR		0x3030
+#define IMX335_REG_VMAX			CCI_REG24_LE(0x3030)
 
-/* Chip ID */
-#define IMX335_REG_ID		0x3912
-#define IMX335_ID		0x00
-
-/* Exposure control */
-#define IMX335_REG_SHUTTER	0x3058
-#define IMX335_EXPOSURE_MIN	1
-#define IMX335_EXPOSURE_OFFSET	9
-#define IMX335_EXPOSURE_STEP	1
-#define IMX335_EXPOSURE_DEFAULT	0x0648
-
-/* Analog gain control */
-#define IMX335_REG_AGAIN	0x30e8
-#define IMX335_AGAIN_MIN	0
-#define IMX335_AGAIN_MAX	240
-#define IMX335_AGAIN_STEP	1
-#define IMX335_AGAIN_DEFAULT	0
+#define IMX335_REG_OPB_SIZE_V		CCI_REG8(0x304c)
+#define IMX335_REG_ADBIT		CCI_REG8(0x3050)
+#define IMX335_REG_Y_OUT_SIZE		CCI_REG16_LE(0x3056)
 
-/* Group hold register */
-#define IMX335_REG_HOLD		0x3001
+#define IMX335_REG_SHUTTER		CCI_REG24_LE(0x3058)
+#define IMX335_EXPOSURE_MIN		1
+#define IMX335_EXPOSURE_OFFSET		9
+#define IMX335_EXPOSURE_STEP		1
+#define IMX335_EXPOSURE_DEFAULT		0x0648
+
+#define IMX335_REG_AREA3_ST_ADR_1	CCI_REG16_LE(0x3074)
+#define IMX335_REG_AREA3_WIDTH_1	CCI_REG16_LE(0x3076)
+
+/* Analog and Digital gain control */
+#define IMX335_REG_GAIN			CCI_REG8(0x30e8)
+#define IMX335_AGAIN_MIN		0
+#define IMX335_AGAIN_MAX		100
+#define IMX335_AGAIN_STEP		1
+#define IMX335_AGAIN_DEFAULT		0
+
+#define IMX335_REG_TPG_TESTCLKEN	CCI_REG8(0x3148)
+
+#define IMX335_REG_INCLKSEL1		CCI_REG16_LE(0x314c)
+#define IMX335_REG_INCLKSEL2		CCI_REG8(0x315a)
+#define IMX335_REG_INCLKSEL3		CCI_REG8(0x3168)
+#define IMX335_REG_INCLKSEL4		CCI_REG8(0x316a)
+
+#define IMX335_REG_MDBIT		CCI_REG8(0x319d)
+#define IMX335_REG_SYSMODE		CCI_REG8(0x319e)
+
+#define IMX335_REG_XVS_XHS_DRV		CCI_REG8(0x31a1)
 
 /* Test pattern generator */
-#define IMX335_REG_TPG		0x329e
-#define IMX335_TPG_ALL_000	0
-#define IMX335_TPG_ALL_FFF	1
-#define IMX335_TPG_ALL_555	2
-#define IMX335_TPG_ALL_AAA	3
-#define IMX335_TPG_TOG_555_AAA	4
-#define IMX335_TPG_TOG_AAA_555	5
-#define IMX335_TPG_TOG_000_555	6
-#define IMX335_TPG_TOG_555_000	7
-#define IMX335_TPG_TOG_000_FFF	8
-#define IMX335_TPG_TOG_FFF_000	9
-#define IMX335_TPG_H_COLOR_BARS 10
-#define IMX335_TPG_V_COLOR_BARS 11
+#define IMX335_REG_TPG_DIG_CLP_MODE	CCI_REG8(0x3280)
+#define IMX335_REG_TPG_EN_DUOUT		CCI_REG8(0x329c)
+#define IMX335_REG_TPG			CCI_REG8(0x329e)
+#define IMX335_TPG_ALL_000		0
+#define IMX335_TPG_ALL_FFF		1
+#define IMX335_TPG_ALL_555		2
+#define IMX335_TPG_ALL_AAA		3
+#define IMX335_TPG_TOG_555_AAA		4
+#define IMX335_TPG_TOG_AAA_555		5
+#define IMX335_TPG_TOG_000_555		6
+#define IMX335_TPG_TOG_555_000		7
+#define IMX335_TPG_TOG_000_FFF		8
+#define IMX335_TPG_TOG_FFF_000		9
+#define IMX335_TPG_H_COLOR_BARS		10
+#define IMX335_TPG_V_COLOR_BARS		11
+#define IMX335_REG_TPG_COLORWIDTH	CCI_REG8(0x32a0)
+
+#define IMX335_REG_BLKLEVEL		CCI_REG16_LE(0x3302)
+
+#define IMX335_REG_WRJ_OPEN		CCI_REG8(0x336c)
+
+#define IMX335_REG_ADBIT1		CCI_REG16_LE(0x341c)
+
+/* Chip ID */
+#define IMX335_REG_ID			CCI_REG8(0x3912)
+#define IMX335_ID			0x00
+
+/* Data Lanes */
+#define IMX335_REG_LANEMODE		CCI_REG8(0x3a01)
+#define IMX335_2LANE			1
+#define IMX335_4LANE			3
+
+#define IMX335_REG_TCLKPOST		CCI_REG16_LE(0x3a18)
+#define IMX335_REG_TCLKPREPARE		CCI_REG16_LE(0x3a1a)
+#define IMX335_REG_TCLK_TRAIL		CCI_REG16_LE(0x3a1c)
+#define IMX335_REG_TCLK_ZERO		CCI_REG16_LE(0x3a1e)
+#define IMX335_REG_THS_PREPARE		CCI_REG16_LE(0x3a20)
+#define IMX335_REG_THS_ZERO		CCI_REG16_LE(0x3a22)
+#define IMX335_REG_THS_TRAIL		CCI_REG16_LE(0x3a24)
+#define IMX335_REG_THS_EXIT		CCI_REG16_LE(0x3a26)
+#define IMX335_REG_TPLX			CCI_REG16_LE(0x3a28)
 
 /* Input clock rate */
-#define IMX335_INCLK_RATE	24000000
+#define IMX335_INCLK_RATE		24000000
 
 /* CSI2 HW configuration */
 #define IMX335_LINK_FREQ_594MHz		594000000LL
@@ -69,9 +120,6 @@
 
 #define IMX335_NUM_DATA_LANES	4
 
-#define IMX335_REG_MIN		0x00
-#define IMX335_REG_MAX		0xfffff
-
 /* IMX335 native and active pixel array size. */
 #define IMX335_NATIVE_WIDTH		2616U
 #define IMX335_NATIVE_HEIGHT		1964U
@@ -81,23 +129,13 @@
 #define IMX335_PIXEL_ARRAY_HEIGHT	1944U
 
 /**
- * struct imx335_reg - imx335 sensor register
- * @address: Register address
- * @val: Register value
- */
-struct imx335_reg {
-	u16 address;
-	u8 val;
-};
-
-/**
  * struct imx335_reg_list - imx335 sensor register list
  * @num_of_regs: Number of registers in the list
  * @regs: Pointer to register list
  */
 struct imx335_reg_list {
 	u32 num_of_regs;
-	const struct imx335_reg *regs;
+	const struct cci_reg_sequence *regs;
 };
 
 static const char * const imx335_supply_name[] = {
@@ -138,6 +176,7 @@ struct imx335_mode {
  * @pad: Media pad. Only one pad supported
  * @reset_gpio: Sensor reset gpio
  * @supplies: Regulator supplies to handle power control
+ * @cci: CCI register map
  * @inclk: Sensor input clock
  * @ctrl_handler: V4L2 control handler
  * @link_freq_ctrl: Pointer to link frequency control
@@ -147,6 +186,7 @@ struct imx335_mode {
  * @exp_ctrl: Pointer to exposure control
  * @again_ctrl: Pointer to analog gain control
  * @vblank: Vertical blanking in lines
+ * @lane_mode: Mode for number of connected data lanes
  * @cur_mode: Pointer to current selected sensor mode
  * @mutex: Mutex for serializing sensor controls
  * @link_freq_bitmap: Menu bitmap for link_freq_ctrl
@@ -159,6 +199,7 @@ struct imx335 {
 	struct media_pad pad;
 	struct gpio_desc *reset_gpio;
 	struct regulator_bulk_data supplies[ARRAY_SIZE(imx335_supply_name)];
+	struct regmap *cci;
 
 	struct clk *inclk;
 	struct v4l2_ctrl_handler ctrl_handler;
@@ -171,6 +212,7 @@ struct imx335 {
 		struct v4l2_ctrl *again_ctrl;
 	};
 	u32 vblank;
+	u32 lane_mode;
 	const struct imx335_mode *cur_mode;
 	struct mutex mutex;
 	unsigned long link_freq_bitmap;
@@ -210,140 +252,135 @@ static const int imx335_tpg_val[] = {
 };
 
 /* Sensor mode registers */
-static const struct imx335_reg mode_2592x1940_regs[] = {
-	{0x3000, 0x01},
-	{0x3002, 0x00},
-	{0x3018, 0x04},
-	{0x302c, 0x3c},
-	{0x302e, 0x20},
-	{0x3056, 0x94},
-	{0x3074, 0xc8},
-	{0x3076, 0x28},
-	{0x304c, 0x00},
-	{0x31a1, 0x00},
-	{0x3288, 0x21},
-	{0x328a, 0x02},
-	{0x3414, 0x05},
-	{0x3416, 0x18},
-	{0x3648, 0x01},
-	{0x364a, 0x04},
-	{0x364c, 0x04},
-	{0x3678, 0x01},
-	{0x367c, 0x31},
-	{0x367e, 0x31},
-	{0x3706, 0x10},
-	{0x3708, 0x03},
-	{0x3714, 0x02},
-	{0x3715, 0x02},
-	{0x3716, 0x01},
-	{0x3717, 0x03},
-	{0x371c, 0x3d},
-	{0x371d, 0x3f},
-	{0x372c, 0x00},
-	{0x372d, 0x00},
-	{0x372e, 0x46},
-	{0x372f, 0x00},
-	{0x3730, 0x89},
-	{0x3731, 0x00},
-	{0x3732, 0x08},
-	{0x3733, 0x01},
-	{0x3734, 0xfe},
-	{0x3735, 0x05},
-	{0x3740, 0x02},
-	{0x375d, 0x00},
-	{0x375e, 0x00},
-	{0x375f, 0x11},
-	{0x3760, 0x01},
-	{0x3768, 0x1b},
-	{0x3769, 0x1b},
-	{0x376a, 0x1b},
-	{0x376b, 0x1b},
-	{0x376c, 0x1a},
-	{0x376d, 0x17},
-	{0x376e, 0x0f},
-	{0x3776, 0x00},
-	{0x3777, 0x00},
-	{0x3778, 0x46},
-	{0x3779, 0x00},
-	{0x377a, 0x89},
-	{0x377b, 0x00},
-	{0x377c, 0x08},
-	{0x377d, 0x01},
-	{0x377e, 0x23},
-	{0x377f, 0x02},
-	{0x3780, 0xd9},
-	{0x3781, 0x03},
-	{0x3782, 0xf5},
-	{0x3783, 0x06},
-	{0x3784, 0xa5},
-	{0x3788, 0x0f},
-	{0x378a, 0xd9},
-	{0x378b, 0x03},
-	{0x378c, 0xeb},
-	{0x378d, 0x05},
-	{0x378e, 0x87},
-	{0x378f, 0x06},
-	{0x3790, 0xf5},
-	{0x3792, 0x43},
-	{0x3794, 0x7a},
-	{0x3796, 0xa1},
-	{0x37b0, 0x36},
-	{0x3a00, 0x00},
+static const struct cci_reg_sequence mode_2592x1940_regs[] = {
+	{ IMX335_REG_MODE_SELECT, IMX335_MODE_STANDBY },
+	{ IMX335_REG_MASTER_MODE, 0x00 },
+	{ IMX335_REG_WINMODE, 0x04 },
+	{ IMX335_REG_HTRIMMING_START, 48 },
+	{ IMX335_REG_HNUM, 2592 },
+	{ IMX335_REG_Y_OUT_SIZE, 1944 },
+	{ IMX335_REG_AREA3_ST_ADR_1, 176 },
+	{ IMX335_REG_AREA3_WIDTH_1, 3928 },
+	{ IMX335_REG_OPB_SIZE_V, 0 },
+	{ IMX335_REG_XVS_XHS_DRV, 0x00 },
+	{ CCI_REG8(0x3288), 0x21 },
+	{ CCI_REG8(0x328a), 0x02 },
+	{ CCI_REG8(0x3414), 0x05 },
+	{ CCI_REG8(0x3416), 0x18 },
+	{ CCI_REG8(0x3648), 0x01 },
+	{ CCI_REG8(0x364a), 0x04 },
+	{ CCI_REG8(0x364c), 0x04 },
+	{ CCI_REG8(0x3678), 0x01 },
+	{ CCI_REG8(0x367c), 0x31 },
+	{ CCI_REG8(0x367e), 0x31 },
+	{ CCI_REG8(0x3706), 0x10 },
+	{ CCI_REG8(0x3708), 0x03 },
+	{ CCI_REG8(0x3714), 0x02 },
+	{ CCI_REG8(0x3715), 0x02 },
+	{ CCI_REG8(0x3716), 0x01 },
+	{ CCI_REG8(0x3717), 0x03 },
+	{ CCI_REG8(0x371c), 0x3d },
+	{ CCI_REG8(0x371d), 0x3f },
+	{ CCI_REG8(0x372c), 0x00 },
+	{ CCI_REG8(0x372d), 0x00 },
+	{ CCI_REG8(0x372e), 0x46 },
+	{ CCI_REG8(0x372f), 0x00 },
+	{ CCI_REG8(0x3730), 0x89 },
+	{ CCI_REG8(0x3731), 0x00 },
+	{ CCI_REG8(0x3732), 0x08 },
+	{ CCI_REG8(0x3733), 0x01 },
+	{ CCI_REG8(0x3734), 0xfe },
+	{ CCI_REG8(0x3735), 0x05 },
+	{ CCI_REG8(0x3740), 0x02 },
+	{ CCI_REG8(0x375d), 0x00 },
+	{ CCI_REG8(0x375e), 0x00 },
+	{ CCI_REG8(0x375f), 0x11 },
+	{ CCI_REG8(0x3760), 0x01 },
+	{ CCI_REG8(0x3768), 0x1b },
+	{ CCI_REG8(0x3769), 0x1b },
+	{ CCI_REG8(0x376a), 0x1b },
+	{ CCI_REG8(0x376b), 0x1b },
+	{ CCI_REG8(0x376c), 0x1a },
+	{ CCI_REG8(0x376d), 0x17 },
+	{ CCI_REG8(0x376e), 0x0f },
+	{ CCI_REG8(0x3776), 0x00 },
+	{ CCI_REG8(0x3777), 0x00 },
+	{ CCI_REG8(0x3778), 0x46 },
+	{ CCI_REG8(0x3779), 0x00 },
+	{ CCI_REG8(0x377a), 0x89 },
+	{ CCI_REG8(0x377b), 0x00 },
+	{ CCI_REG8(0x377c), 0x08 },
+	{ CCI_REG8(0x377d), 0x01 },
+	{ CCI_REG8(0x377e), 0x23 },
+	{ CCI_REG8(0x377f), 0x02 },
+	{ CCI_REG8(0x3780), 0xd9 },
+	{ CCI_REG8(0x3781), 0x03 },
+	{ CCI_REG8(0x3782), 0xf5 },
+	{ CCI_REG8(0x3783), 0x06 },
+	{ CCI_REG8(0x3784), 0xa5 },
+	{ CCI_REG8(0x3788), 0x0f },
+	{ CCI_REG8(0x378a), 0xd9 },
+	{ CCI_REG8(0x378b), 0x03 },
+	{ CCI_REG8(0x378c), 0xeb },
+	{ CCI_REG8(0x378d), 0x05 },
+	{ CCI_REG8(0x378e), 0x87 },
+	{ CCI_REG8(0x378f), 0x06 },
+	{ CCI_REG8(0x3790), 0xf5 },
+	{ CCI_REG8(0x3792), 0x43 },
+	{ CCI_REG8(0x3794), 0x7a },
+	{ CCI_REG8(0x3796), 0xa1 },
+	{ CCI_REG8(0x37b0), 0x36 },
+	{ CCI_REG8(0x3a00), 0x00 },
 };
 
-static const struct imx335_reg raw10_framefmt_regs[] = {
-	{0x3050, 0x00},
-	{0x319d, 0x00},
-	{0x341c, 0xff},
-	{0x341d, 0x01},
+static const struct cci_reg_sequence raw10_framefmt_regs[] = {
+	{ IMX335_REG_ADBIT, 0x00 },
+	{ IMX335_REG_MDBIT, 0x00 },
+	{ IMX335_REG_ADBIT1, 0x1ff },
 };
 
-static const struct imx335_reg raw12_framefmt_regs[] = {
-	{0x3050, 0x01},
-	{0x319d, 0x01},
-	{0x341c, 0x47},
-	{0x341d, 0x00},
+static const struct cci_reg_sequence raw12_framefmt_regs[] = {
+	{ IMX335_REG_ADBIT, 0x01 },
+	{ IMX335_REG_MDBIT, 0x01 },
+	{ IMX335_REG_ADBIT1, 0x47 },
 };
 
-static const struct imx335_reg mipi_data_rate_1188Mbps[] = {
-	{0x300c, 0x3b},
-	{0x300d, 0x2a},
-	{0x314c, 0xc6},
-	{0x314d, 0x00},
-	{0x315a, 0x02},
-	{0x3168, 0xa0},
-	{0x316a, 0x7e},
-	{0x319e, 0x01},
-	{0x3a18, 0x8f},
-	{0x3a1a, 0x4f},
-	{0x3a1c, 0x47},
-	{0x3a1e, 0x37},
-	{0x3a1f, 0x01},
-	{0x3a20, 0x4f},
-	{0x3a22, 0x87},
-	{0x3a24, 0x4f},
-	{0x3a26, 0x7f},
-	{0x3a28, 0x3f},
+static const struct cci_reg_sequence mipi_data_rate_1188Mbps[] = {
+	{ IMX335_REG_BCWAIT_TIME, 0x3b },
+	{ IMX335_REG_CPWAIT_TIME, 0x2a },
+	{ IMX335_REG_INCLKSEL1, 0x00c6 },
+	{ IMX335_REG_INCLKSEL2, 0x02 },
+	{ IMX335_REG_INCLKSEL3, 0xa0 },
+	{ IMX335_REG_INCLKSEL4, 0x7e },
+	{ IMX335_REG_SYSMODE, 0x01 },
+	{ IMX335_REG_TCLKPOST, 0x8f },
+	{ IMX335_REG_TCLKPREPARE, 0x4f },
+	{ IMX335_REG_TCLK_TRAIL, 0x47 },
+	{ IMX335_REG_TCLK_ZERO, 0x0137 },
+	{ IMX335_REG_THS_PREPARE, 0x4f },
+	{ IMX335_REG_THS_ZERO,  0x87 },
+	{ IMX335_REG_THS_TRAIL, 0x4f },
+	{ IMX335_REG_THS_EXIT, 0x7f },
+	{ IMX335_REG_TPLX, 0x3f },
 };
 
-static const struct imx335_reg mipi_data_rate_891Mbps[] = {
-	{0x300c, 0x3b},
-	{0x300d, 0x2a},
-	{0x314c, 0x29},
-	{0x314d, 0x01},
-	{0x315a, 0x06},
-	{0x3168, 0xa0},
-	{0x316a, 0x7e},
-	{0x319e, 0x02},
-	{0x3a18, 0x7f},
-	{0x3a1a, 0x37},
-	{0x3a1c, 0x37},
-	{0x3a1e, 0xf7},
-	{0x3a20, 0x3f},
-	{0x3a22, 0x6f},
-	{0x3a24, 0x3f},
-	{0x3a26, 0x5f},
-	{0x3a28, 0x2f},
+static const struct cci_reg_sequence mipi_data_rate_891Mbps[] = {
+	{ IMX335_REG_BCWAIT_TIME, 0x3b },
+	{ IMX335_REG_CPWAIT_TIME, 0x2a },
+	{ IMX335_REG_INCLKSEL1, 0x0129 },
+	{ IMX335_REG_INCLKSEL2, 0x06 },
+	{ IMX335_REG_INCLKSEL3, 0xa0 },
+	{ IMX335_REG_INCLKSEL4, 0x7e },
+	{ IMX335_REG_SYSMODE, 0x02 },
+	{ IMX335_REG_TCLKPOST, 0x7f },
+	{ IMX335_REG_TCLKPREPARE, 0x37 },
+	{ IMX335_REG_TCLK_TRAIL, 0x37 },
+	{ IMX335_REG_TCLK_ZERO, 0xf7 },
+	{ IMX335_REG_THS_PREPARE, 0x3f },
+	{ IMX335_REG_THS_ZERO, 0x6f },
+	{ IMX335_REG_THS_TRAIL, 0x3f },
+	{ IMX335_REG_THS_EXIT, 0x5f },
+	{ IMX335_REG_TPLX, 0x2f },
 };
 
 static const s64 link_freq[] = {
@@ -372,10 +409,10 @@ static const u32 imx335_mbus_codes[] = {
 /* Supported sensor mode configurations */
 static const struct imx335_mode supported_mode = {
 	.width = 2592,
-	.height = 1940,
+	.height = 1944,
 	.hblank = 342,
-	.vblank = 2560,
-	.vblank_min = 2560,
+	.vblank = 2556,
+	.vblank_min = 2556,
 	.vblank_max = 133060,
 	.pclk = 396000000,
 	.reg_list = {
@@ -396,101 +433,6 @@ static inline struct imx335 *to_imx335(struct v4l2_subdev *subdev)
 }
 
 /**
- * imx335_read_reg() - Read registers.
- * @imx335: pointer to imx335 device
- * @reg: register address
- * @len: length of bytes to read. Max supported bytes is 4
- * @val: pointer to register value to be filled.
- *
- * Big endian register addresses with little endian values.
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx335_read_reg(struct imx335 *imx335, u16 reg, u32 len, u32 *val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx335->sd);
-	struct i2c_msg msgs[2] = {0};
-	u8 addr_buf[2] = {0};
-	u8 data_buf[4] = {0};
-	int ret;
-
-	if (WARN_ON(len > 4))
-		return -EINVAL;
-
-	put_unaligned_be16(reg, addr_buf);
-
-	/* Write register address */
-	msgs[0].addr = client->addr;
-	msgs[0].flags = 0;
-	msgs[0].len = ARRAY_SIZE(addr_buf);
-	msgs[0].buf = addr_buf;
-
-	/* Read data from register */
-	msgs[1].addr = client->addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].len = len;
-	msgs[1].buf = data_buf;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
-
-	*val = get_unaligned_le32(data_buf);
-
-	return 0;
-}
-
-/**
- * imx335_write_reg() - Write register
- * @imx335: pointer to imx335 device
- * @reg: register address
- * @len: length of bytes. Max supported bytes is 4
- * @val: register value
- *
- * Big endian register addresses with little endian values.
- *
- * Return: 0 if successful, error code otherwise.
- */
-static int imx335_write_reg(struct imx335 *imx335, u16 reg, u32 len, u32 val)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&imx335->sd);
-	u8 buf[6] = {0};
-
-	if (WARN_ON(len > 4))
-		return -EINVAL;
-
-	put_unaligned_be16(reg, buf);
-	put_unaligned_le32(val, buf + 2);
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
-
-	return 0;
-}
-
-/**
- * imx335_write_regs() - Write a list of registers
- * @imx335: pointer to imx335 device
- * @regs: list of registers to be written
- * @len: length of registers array
- *
- * Return: 0 if successful. error code otherwise.
- */
-static int imx335_write_regs(struct imx335 *imx335,
-			     const struct imx335_reg *regs, u32 len)
-{
-	unsigned int i;
-	int ret;
-
-	for (i = 0; i < len; i++) {
-		ret = imx335_write_reg(imx335, regs[i].address, 1, regs[i].val);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
-/**
  * imx335_update_controls() - Update control ranges based on streaming mode
  * @imx335: pointer to imx335 device
  * @mode: pointer to imx335_mode sensor mode
@@ -526,7 +468,8 @@ static int imx335_update_controls(struct imx335 *imx335,
 static int imx335_update_exp_gain(struct imx335 *imx335, u32 exposure, u32 gain)
 {
 	u32 lpfr, shutter;
-	int ret;
+	int ret_hold;
+	int ret = 0;
 
 	lpfr = imx335->vblank + imx335->cur_mode->height;
 	shutter = lpfr - exposure;
@@ -534,64 +477,55 @@ static int imx335_update_exp_gain(struct imx335 *imx335, u32 exposure, u32 gain)
 	dev_dbg(imx335->dev, "Set exp %u, analog gain %u, shutter %u, lpfr %u\n",
 		exposure, gain, shutter, lpfr);
 
-	ret = imx335_write_reg(imx335, IMX335_REG_HOLD, 1, 1);
-	if (ret)
-		return ret;
-
-	ret = imx335_write_reg(imx335, IMX335_REG_LPFR, 3, lpfr);
-	if (ret)
-		goto error_release_group_hold;
-
-	ret = imx335_write_reg(imx335, IMX335_REG_SHUTTER, 3, shutter);
-	if (ret)
-		goto error_release_group_hold;
-
-	ret = imx335_write_reg(imx335, IMX335_REG_AGAIN, 2, gain);
-
-error_release_group_hold:
-	imx335_write_reg(imx335, IMX335_REG_HOLD, 1, 0);
+	cci_write(imx335->cci, IMX335_REG_HOLD, 1, &ret);
+	cci_write(imx335->cci, IMX335_REG_VMAX, lpfr, &ret);
+	cci_write(imx335->cci, IMX335_REG_SHUTTER, shutter, &ret);
+	cci_write(imx335->cci, IMX335_REG_GAIN, gain, &ret);
+	/*
+	 * Unconditionally attempt to release the hold, but track the
+	 * error if the unhold itself fails.
+	 */
+	ret_hold = cci_write(imx335->cci, IMX335_REG_HOLD, 0, NULL);
+	if (ret_hold)
+		ret = ret_hold;
 
 	return ret;
 }
 
 static int imx335_update_test_pattern(struct imx335 *imx335, u32 pattern_index)
 {
-	int ret;
+	int ret = 0;
 
 	if (pattern_index >= ARRAY_SIZE(imx335_tpg_val))
 		return -EINVAL;
 
 	if (pattern_index) {
-		const struct imx335_reg tpg_enable_regs[] = {
-			{ 0x3148, 0x10 },
-			{ 0x3280, 0x00 },
-			{ 0x329c, 0x01 },
-			{ 0x32a0, 0x11 },
-			{ 0x3302, 0x00 },
-			{ 0x3303, 0x00 },
-			{ 0x336c, 0x00 },
+		const struct cci_reg_sequence tpg_enable_regs[] = {
+			{ IMX335_REG_TPG_TESTCLKEN, 0x10 },
+			{ IMX335_REG_TPG_DIG_CLP_MODE, 0x00 },
+			{ IMX335_REG_TPG_EN_DUOUT, 0x01 },
+			{ IMX335_REG_TPG_COLORWIDTH, 0x11 },
+			{ IMX335_REG_BLKLEVEL, 0x00 },
+			{ IMX335_REG_WRJ_OPEN, 0x00 },
 		};
 
-		ret = imx335_write_reg(imx335, IMX335_REG_TPG, 1,
-				       imx335_tpg_val[pattern_index]);
-		if (ret)
-			return ret;
+		cci_write(imx335->cci, IMX335_REG_TPG,
+			  imx335_tpg_val[pattern_index], &ret);
 
-		ret = imx335_write_regs(imx335, tpg_enable_regs,
-					ARRAY_SIZE(tpg_enable_regs));
+		cci_multi_reg_write(imx335->cci, tpg_enable_regs,
+				    ARRAY_SIZE(tpg_enable_regs), &ret);
 	} else {
-		const struct imx335_reg tpg_disable_regs[] = {
-			{ 0x3148, 0x00 },
-			{ 0x3280, 0x01 },
-			{ 0x329c, 0x00 },
-			{ 0x32a0, 0x10 },
-			{ 0x3302, 0x32 },
-			{ 0x3303, 0x00 },
-			{ 0x336c, 0x01 },
+		const struct cci_reg_sequence tpg_disable_regs[] = {
+			{ IMX335_REG_TPG_TESTCLKEN, 0x00 },
+			{ IMX335_REG_TPG_DIG_CLP_MODE, 0x01 },
+			{ IMX335_REG_TPG_EN_DUOUT, 0x00 },
+			{ IMX335_REG_TPG_COLORWIDTH, 0x10 },
+			{ IMX335_REG_BLKLEVEL, 0x32 },
+			{ IMX335_REG_WRJ_OPEN, 0x01 },
 		};
 
-		ret = imx335_write_regs(imx335, tpg_disable_regs,
-					ARRAY_SIZE(tpg_disable_regs));
+		cci_multi_reg_write(imx335->cci, tpg_disable_regs,
+				    ARRAY_SIZE(tpg_disable_regs), &ret);
 	}
 
 	return ret;
@@ -625,12 +559,14 @@ static int imx335_set_ctrl(struct v4l2_ctrl *ctrl)
 			imx335->vblank,
 			imx335->vblank + imx335->cur_mode->height);
 
-		return __v4l2_ctrl_modify_range(imx335->exp_ctrl,
-						IMX335_EXPOSURE_MIN,
-						imx335->vblank +
-						imx335->cur_mode->height -
-						IMX335_EXPOSURE_OFFSET,
-						1, IMX335_EXPOSURE_DEFAULT);
+		ret = __v4l2_ctrl_modify_range(imx335->exp_ctrl,
+					       IMX335_EXPOSURE_MIN,
+					       imx335->vblank +
+					       imx335->cur_mode->height -
+					       IMX335_EXPOSURE_OFFSET,
+					       1, IMX335_EXPOSURE_DEFAULT);
+		if (ret)
+			return ret;
 	}
 
 	/*
@@ -641,6 +577,13 @@ static int imx335_set_ctrl(struct v4l2_ctrl *ctrl)
 		return 0;
 
 	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		exposure = imx335->exp_ctrl->val;
+		analog_gain = imx335->again_ctrl->val;
+
+		ret = imx335_update_exp_gain(imx335, exposure, analog_gain);
+
+		break;
 	case V4L2_CID_EXPOSURE:
 		exposure = ctrl->val;
 		analog_gain = imx335->again_ctrl->val;
@@ -717,7 +660,8 @@ static int imx335_enum_frame_size(struct v4l2_subdev *sd,
 	struct imx335 *imx335 = to_imx335(sd);
 	u32 code;
 
-	if (fsize->index > ARRAY_SIZE(imx335_mbus_codes))
+	/* Only a single supported_mode available. */
+	if (fsize->index > 0)
 		return -EINVAL;
 
 	code = imx335_get_format_code(imx335, fsize->code);
@@ -890,12 +834,14 @@ static int imx335_set_framefmt(struct imx335 *imx335)
 {
 	switch (imx335->cur_mbus_code) {
 	case MEDIA_BUS_FMT_SRGGB10_1X10:
-		return imx335_write_regs(imx335, raw10_framefmt_regs,
-					 ARRAY_SIZE(raw10_framefmt_regs));
+		return cci_multi_reg_write(imx335->cci, raw10_framefmt_regs,
+					   ARRAY_SIZE(raw10_framefmt_regs),
+					   NULL);
 
 	case MEDIA_BUS_FMT_SRGGB12_1X12:
-		return imx335_write_regs(imx335, raw12_framefmt_regs,
-					 ARRAY_SIZE(raw12_framefmt_regs));
+		return cci_multi_reg_write(imx335->cci, raw12_framefmt_regs,
+					   ARRAY_SIZE(raw12_framefmt_regs),
+					   NULL);
 	}
 
 	return -EINVAL;
@@ -914,7 +860,8 @@ static int imx335_start_streaming(struct imx335 *imx335)
 
 	/* Setup PLL */
 	reg_list = &link_freq_reglist[__ffs(imx335->link_freq_bitmap)];
-	ret = imx335_write_regs(imx335, reg_list->regs, reg_list->num_of_regs);
+	ret = cci_multi_reg_write(imx335->cci, reg_list->regs,
+				  reg_list->num_of_regs, NULL);
 	if (ret) {
 		dev_err(imx335->dev, "%s failed to set plls\n", __func__);
 		return ret;
@@ -922,8 +869,8 @@ static int imx335_start_streaming(struct imx335 *imx335)
 
 	/* Write sensor mode registers */
 	reg_list = &imx335->cur_mode->reg_list;
-	ret = imx335_write_regs(imx335, reg_list->regs,
-				reg_list->num_of_regs);
+	ret = cci_multi_reg_write(imx335->cci, reg_list->regs,
+				  reg_list->num_of_regs, NULL);
 	if (ret) {
 		dev_err(imx335->dev, "fail to write initial registers\n");
 		return ret;
@@ -936,6 +883,12 @@ static int imx335_start_streaming(struct imx335 *imx335)
 		return ret;
 	}
 
+	/* Configure lanes */
+	ret = cci_write(imx335->cci, IMX335_REG_LANEMODE,
+			imx335->lane_mode, NULL);
+	if (ret)
+		return ret;
+
 	/* Setup handler will write actual exposure and gain */
 	ret =  __v4l2_ctrl_handler_setup(imx335->sd.ctrl_handler);
 	if (ret) {
@@ -944,8 +897,8 @@ static int imx335_start_streaming(struct imx335 *imx335)
 	}
 
 	/* Start streaming */
-	ret = imx335_write_reg(imx335, IMX335_REG_MODE_SELECT,
-			       1, IMX335_MODE_STREAMING);
+	ret = cci_write(imx335->cci, IMX335_REG_MODE_SELECT,
+			IMX335_MODE_STREAMING, NULL);
 	if (ret) {
 		dev_err(imx335->dev, "fail to start streaming\n");
 		return ret;
@@ -965,8 +918,8 @@ static int imx335_start_streaming(struct imx335 *imx335)
  */
 static int imx335_stop_streaming(struct imx335 *imx335)
 {
-	return imx335_write_reg(imx335, IMX335_REG_MODE_SELECT,
-				1, IMX335_MODE_STANDBY);
+	return cci_write(imx335->cci, IMX335_REG_MODE_SELECT,
+			 IMX335_MODE_STANDBY, NULL);
 }
 
 /**
@@ -1017,14 +970,14 @@ error_unlock:
 static int imx335_detect(struct imx335 *imx335)
 {
 	int ret;
-	u32 val;
+	u64 val;
 
-	ret = imx335_read_reg(imx335, IMX335_REG_ID, 2, &val);
+	ret = cci_read(imx335->cci, IMX335_REG_ID, &val, NULL);
 	if (ret)
 		return ret;
 
 	if (val != IMX335_ID) {
-		dev_err(imx335->dev, "chip id mismatch: %x!=%x\n",
+		dev_err(imx335->dev, "chip id mismatch: %x!=%llx\n",
 			IMX335_ID, val);
 		return -ENXIO;
 	}
@@ -1054,7 +1007,7 @@ static int imx335_parse_hw_config(struct imx335 *imx335)
 
 	/* Request optional reset pin */
 	imx335->reset_gpio = devm_gpiod_get_optional(imx335->dev, "reset",
-						     GPIOD_OUT_LOW);
+						     GPIOD_OUT_HIGH);
 	if (IS_ERR(imx335->reset_gpio)) {
 		dev_err(imx335->dev, "failed to get reset gpio %ld\n",
 			PTR_ERR(imx335->reset_gpio));
@@ -1096,7 +1049,14 @@ static int imx335_parse_hw_config(struct imx335 *imx335)
 	if (ret)
 		return ret;
 
-	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX335_NUM_DATA_LANES) {
+	switch (bus_cfg.bus.mipi_csi2.num_data_lanes) {
+	case 2:
+		imx335->lane_mode = IMX335_2LANE;
+		break;
+	case 4:
+		imx335->lane_mode = IMX335_4LANE;
+		break;
+	default:
 		dev_err(imx335->dev,
 			"number of CSI2 data lanes %d is not supported\n",
 			bus_cfg.bus.mipi_csi2.num_data_lanes);
@@ -1160,8 +1120,7 @@ static int imx335_power_on(struct device *dev)
 
 	usleep_range(500, 550); /* Tlow */
 
-	/* Set XCLR */
-	gpiod_set_value_cansleep(imx335->reset_gpio, 1);
+	gpiod_set_value_cansleep(imx335->reset_gpio, 0);
 
 	ret = clk_prepare_enable(imx335->inclk);
 	if (ret) {
@@ -1174,7 +1133,7 @@ static int imx335_power_on(struct device *dev)
 	return 0;
 
 error_reset:
-	gpiod_set_value_cansleep(imx335->reset_gpio, 0);
+	gpiod_set_value_cansleep(imx335->reset_gpio, 1);
 	regulator_bulk_disable(ARRAY_SIZE(imx335_supply_name), imx335->supplies);
 
 	return ret;
@@ -1191,7 +1150,7 @@ static int imx335_power_off(struct device *dev)
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct imx335 *imx335 = to_imx335(sd);
 
-	gpiod_set_value_cansleep(imx335->reset_gpio, 0);
+	gpiod_set_value_cansleep(imx335->reset_gpio, 1);
 	clk_disable_unprepare(imx335->inclk);
 	regulator_bulk_disable(ARRAY_SIZE(imx335_supply_name), imx335->supplies);
 
@@ -1208,10 +1167,16 @@ static int imx335_init_controls(struct imx335 *imx335)
 {
 	struct v4l2_ctrl_handler *ctrl_hdlr = &imx335->ctrl_handler;
 	const struct imx335_mode *mode = imx335->cur_mode;
+	struct v4l2_fwnode_device_properties props;
 	u32 lpfr;
 	int ret;
 
-	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 7);
+	ret = v4l2_fwnode_device_parse(imx335->dev, &props);
+	if (ret)
+		return ret;
+
+	/* v4l2_fwnode_device_properties can add two more controls */
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 9);
 	if (ret)
 		return ret;
 
@@ -1228,6 +1193,14 @@ static int imx335_init_controls(struct imx335 *imx335)
 					     IMX335_EXPOSURE_STEP,
 					     IMX335_EXPOSURE_DEFAULT);
 
+	/*
+	 * The sensor has an analog gain and a digital gain, both controlled
+	 * through a single gain value, expressed in 0.3dB increments. Values
+	 * from 0.0dB (0) to 30.0dB (100) apply analog gain only, higher values
+	 * up to 72.0dB (240) add further digital gain. Limit the range to
+	 * analog gain only, support for digital gain can be added separately
+	 * if needed.
+	 */
 	imx335->again_ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
 					       &imx335_ctrl_ops,
 					       V4L2_CID_ANALOGUE_GAIN,
@@ -1276,6 +1249,8 @@ static int imx335_init_controls(struct imx335 *imx335)
 	if (imx335->hblank_ctrl)
 		imx335->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
+	v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx335_ctrl_ops, &props);
+
 	if (ctrl_hdlr->error) {
 		dev_err(imx335->dev, "control init failed: %d\n",
 			ctrl_hdlr->error);
@@ -1304,6 +1279,11 @@ static int imx335_probe(struct i2c_client *client)
 		return -ENOMEM;
 
 	imx335->dev = &client->dev;
+	imx335->cci = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(imx335->cci)) {
+		dev_err(imx335->dev, "Unable to initialize I2C\n");
+		return -ENODEV;
+	}
 
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&imx335->sd, client, &imx335_subdev_ops);
diff --git a/drivers/media/i2c/imx355.c b/drivers/media/i2c/imx355.c
index 7e9c2f65fa08..b2dce67c0b6b 100644
--- a/drivers/media/i2c/imx355.c
+++ b/drivers/media/i2c/imx355.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (C) 2018 Intel Corporation
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
@@ -1520,6 +1520,7 @@ static const struct v4l2_subdev_internal_ops imx355_internal_ops = {
 static int imx355_init_controls(struct imx355 *imx355)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&imx355->sd);
+	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl_handler *ctrl_hdlr;
 	s64 exposure_max;
 	s64 vblank_def;
@@ -1531,7 +1532,7 @@ static int imx355_init_controls(struct imx355 *imx355)
 	int ret;
 
 	ctrl_hdlr = &imx355->ctrl_handler;
-	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
 	if (ret)
 		return ret;
 
@@ -1603,6 +1604,15 @@ static int imx355_init_controls(struct imx355 *imx355)
 		goto error;
 	}
 
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		goto error;
+
+	ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx355_ctrl_ops,
+					      &props);
+	if (ret)
+		goto error;
+
 	imx355->sd.ctrl_handler = ctrl_hdlr;
 
 	return 0;
diff --git a/drivers/media/i2c/imx412.c b/drivers/media/i2c/imx412.c
index 0efce329525e..c74097a59c42 100644
--- a/drivers/media/i2c/imx412.c
+++ b/drivers/media/i2c/imx412.c
@@ -4,7 +4,7 @@
  *
  * Copyright (C) 2021 Intel Corporation
  */
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -542,14 +542,13 @@ static int imx412_update_controls(struct imx412 *imx412,
  */
 static int imx412_update_exp_gain(struct imx412 *imx412, u32 exposure, u32 gain)
 {
-	u32 lpfr, shutter;
+	u32 lpfr;
 	int ret;
 
 	lpfr = imx412->vblank + imx412->cur_mode->height;
-	shutter = lpfr - exposure;
 
-	dev_dbg(imx412->dev, "Set exp %u, analog gain %u, shutter %u, lpfr %u",
-		exposure, gain, shutter, lpfr);
+	dev_dbg(imx412->dev, "Set exp %u, analog gain %u, lpfr %u\n",
+		exposure, gain, lpfr);
 
 	ret = imx412_write_reg(imx412, IMX412_REG_HOLD, 1, 1);
 	if (ret)
@@ -559,7 +558,7 @@ static int imx412_update_exp_gain(struct imx412 *imx412, u32 exposure, u32 gain)
 	if (ret)
 		goto error_release_group_hold;
 
-	ret = imx412_write_reg(imx412, IMX412_REG_EXPOSURE_CIT, 2, shutter);
+	ret = imx412_write_reg(imx412, IMX412_REG_EXPOSURE_CIT, 2, exposure);
 	if (ret)
 		goto error_release_group_hold;
 
@@ -595,7 +594,7 @@ static int imx412_set_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_VBLANK:
 		imx412->vblank = imx412->vblank_ctrl->val;
 
-		dev_dbg(imx412->dev, "Received vblank %u, new lpfr %u",
+		dev_dbg(imx412->dev, "Received vblank %u, new lpfr %u\n",
 			imx412->vblank,
 			imx412->vblank + imx412->cur_mode->height);
 
@@ -614,7 +613,7 @@ static int imx412_set_ctrl(struct v4l2_ctrl *ctrl)
 		exposure = ctrl->val;
 		analog_gain = imx412->again_ctrl->val;
 
-		dev_dbg(imx412->dev, "Received exp %u, analog gain %u",
+		dev_dbg(imx412->dev, "Received exp %u, analog gain %u\n",
 			exposure, analog_gain);
 
 		ret = imx412_update_exp_gain(imx412, exposure, analog_gain);
@@ -623,7 +622,7 @@ static int imx412_set_ctrl(struct v4l2_ctrl *ctrl)
 
 		break;
 	default:
-		dev_err(imx412->dev, "Invalid control %d", ctrl->id);
+		dev_err(imx412->dev, "Invalid control %d\n", ctrl->id);
 		ret = -EINVAL;
 	}
 
@@ -804,14 +803,14 @@ static int imx412_start_streaming(struct imx412 *imx412)
 	ret = imx412_write_regs(imx412, reg_list->regs,
 				reg_list->num_of_regs);
 	if (ret) {
-		dev_err(imx412->dev, "fail to write initial registers");
+		dev_err(imx412->dev, "fail to write initial registers\n");
 		return ret;
 	}
 
 	/* Setup handler will write actual exposure and gain */
 	ret =  __v4l2_ctrl_handler_setup(imx412->sd.ctrl_handler);
 	if (ret) {
-		dev_err(imx412->dev, "fail to setup handler");
+		dev_err(imx412->dev, "fail to setup handler\n");
 		return ret;
 	}
 
@@ -822,7 +821,7 @@ static int imx412_start_streaming(struct imx412 *imx412)
 	ret = imx412_write_reg(imx412, IMX412_REG_MODE_SELECT,
 			       1, IMX412_MODE_STREAMING);
 	if (ret) {
-		dev_err(imx412->dev, "fail to start streaming");
+		dev_err(imx412->dev, "fail to start streaming\n");
 		return ret;
 	}
 
@@ -896,7 +895,7 @@ static int imx412_detect(struct imx412 *imx412)
 		return ret;
 
 	if (val != IMX412_ID) {
-		dev_err(imx412->dev, "chip id mismatch: %x!=%x",
+		dev_err(imx412->dev, "chip id mismatch: %x!=%x\n",
 			IMX412_ID, val);
 		return -ENXIO;
 	}
@@ -928,7 +927,7 @@ static int imx412_parse_hw_config(struct imx412 *imx412)
 	imx412->reset_gpio = devm_gpiod_get_optional(imx412->dev, "reset",
 						     GPIOD_OUT_LOW);
 	if (IS_ERR(imx412->reset_gpio)) {
-		dev_err(imx412->dev, "failed to get reset gpio %ld",
+		dev_err(imx412->dev, "failed to get reset gpio %ld\n",
 			PTR_ERR(imx412->reset_gpio));
 		return PTR_ERR(imx412->reset_gpio);
 	}
@@ -936,13 +935,13 @@ static int imx412_parse_hw_config(struct imx412 *imx412)
 	/* Get sensor input clock */
 	imx412->inclk = devm_clk_get(imx412->dev, NULL);
 	if (IS_ERR(imx412->inclk)) {
-		dev_err(imx412->dev, "could not get inclk");
+		dev_err(imx412->dev, "could not get inclk\n");
 		return PTR_ERR(imx412->inclk);
 	}
 
 	rate = clk_get_rate(imx412->inclk);
 	if (rate != IMX412_INCLK_RATE) {
-		dev_err(imx412->dev, "inclk frequency mismatch");
+		dev_err(imx412->dev, "inclk frequency mismatch\n");
 		return -EINVAL;
 	}
 
@@ -967,14 +966,14 @@ static int imx412_parse_hw_config(struct imx412 *imx412)
 
 	if (bus_cfg.bus.mipi_csi2.num_data_lanes != IMX412_NUM_DATA_LANES) {
 		dev_err(imx412->dev,
-			"number of CSI2 data lanes %d is not supported",
+			"number of CSI2 data lanes %d is not supported\n",
 			bus_cfg.bus.mipi_csi2.num_data_lanes);
 		ret = -EINVAL;
 		goto done_endpoint_free;
 	}
 
 	if (!bus_cfg.nr_of_link_frequencies) {
-		dev_err(imx412->dev, "no link frequencies defined");
+		dev_err(imx412->dev, "no link frequencies defined\n");
 		ret = -EINVAL;
 		goto done_endpoint_free;
 	}
@@ -1035,7 +1034,7 @@ static int imx412_power_on(struct device *dev)
 
 	ret = clk_prepare_enable(imx412->inclk);
 	if (ret) {
-		dev_err(imx412->dev, "fail to enable inclk");
+		dev_err(imx412->dev, "fail to enable inclk\n");
 		goto error_reset;
 	}
 
@@ -1146,7 +1145,7 @@ static int imx412_init_controls(struct imx412 *imx412)
 		imx412->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
 	if (ctrl_hdlr->error) {
-		dev_err(imx412->dev, "control init failed: %d",
+		dev_err(imx412->dev, "control init failed: %d\n",
 			ctrl_hdlr->error);
 		v4l2_ctrl_handler_free(ctrl_hdlr);
 		return ctrl_hdlr->error;
@@ -1184,7 +1183,7 @@ static int imx412_probe(struct i2c_client *client)
 
 	ret = imx412_parse_hw_config(imx412);
 	if (ret) {
-		dev_err(imx412->dev, "HW configuration is not supported");
+		dev_err(imx412->dev, "HW configuration is not supported\n");
 		return ret;
 	}
 
@@ -1192,14 +1191,14 @@ static int imx412_probe(struct i2c_client *client)
 
 	ret = imx412_power_on(imx412->dev);
 	if (ret) {
-		dev_err(imx412->dev, "failed to power-on the sensor");
+		dev_err(imx412->dev, "failed to power-on the sensor\n");
 		goto error_mutex_destroy;
 	}
 
 	/* Check module identity */
 	ret = imx412_detect(imx412);
 	if (ret) {
-		dev_err(imx412->dev, "failed to find sensor: %d", ret);
+		dev_err(imx412->dev, "failed to find sensor: %d\n", ret);
 		goto error_power_off;
 	}
 
@@ -1209,7 +1208,7 @@ static int imx412_probe(struct i2c_client *client)
 
 	ret = imx412_init_controls(imx412);
 	if (ret) {
-		dev_err(imx412->dev, "failed to init controls: %d", ret);
+		dev_err(imx412->dev, "failed to init controls: %d\n", ret);
 		goto error_power_off;
 	}
 
@@ -1223,14 +1222,14 @@ static int imx412_probe(struct i2c_client *client)
 	imx412->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&imx412->sd.entity, 1, &imx412->pad);
 	if (ret) {
-		dev_err(imx412->dev, "failed to init entity pads: %d", ret);
+		dev_err(imx412->dev, "failed to init entity pads: %d\n", ret);
 		goto error_handler_free;
 	}
 
 	ret = v4l2_async_register_subdev_sensor(&imx412->sd);
 	if (ret < 0) {
 		dev_err(imx412->dev,
-			"failed to register async subdev: %d", ret);
+			"failed to register async subdev: %d\n", ret);
 		goto error_media_entity;
 	}
 
diff --git a/drivers/media/i2c/imx415.c b/drivers/media/i2c/imx415.c
index a20b0db330d3..9f37779bd611 100644
--- a/drivers/media/i2c/imx415.c
+++ b/drivers/media/i2c/imx415.c
@@ -26,6 +26,10 @@
 #define IMX415_PIXEL_ARRAY_WIDTH  3864
 #define IMX415_PIXEL_ARRAY_HEIGHT 2192
 #define IMX415_PIXEL_ARRAY_VBLANK 58
+#define IMX415_EXPOSURE_OFFSET	  8
+
+#define IMX415_PIXEL_RATE_74_25MHZ	891000000
+#define IMX415_PIXEL_RATE_72MHZ		864000000
 
 #define IMX415_NUM_CLK_PARAM_REGS 11
 
@@ -51,7 +55,10 @@
 #define IMX415_OUTSEL		  CCI_REG8(0x30c0)
 #define IMX415_DRV		  CCI_REG8(0x30c1)
 #define IMX415_VMAX		  CCI_REG24_LE(0x3024)
+#define IMX415_VMAX_MAX		  0xfffff
 #define IMX415_HMAX		  CCI_REG16_LE(0x3028)
+#define IMX415_HMAX_MAX		  0xffff
+#define IMX415_HMAX_MULTIPLIER	  12
 #define IMX415_SHR0		  CCI_REG24_LE(0x3050)
 #define IMX415_GAIN_PCG_0	  CCI_REG16_LE(0x3090)
 #define IMX415_AGAIN_MIN	  0
@@ -445,11 +452,8 @@ static const struct imx415_clk_params imx415_clk_params[] = {
 	},
 };
 
-/* all-pixel 2-lane 720 Mbps 15.74 Hz mode */
-static const struct cci_reg_sequence imx415_mode_2_720[] = {
-	{ IMX415_VMAX, 0x08CA },
-	{ IMX415_HMAX, 0x07F0 },
-	{ IMX415_LANEMODE, IMX415_LANEMODE_2 },
+/* 720 Mbps CSI configuration */
+static const struct cci_reg_sequence imx415_linkrate_720mbps[] = {
 	{ IMX415_TCLKPOST, 0x006F },
 	{ IMX415_TCLKPREPARE, 0x002F },
 	{ IMX415_TCLKTRAIL, 0x002F },
@@ -461,11 +465,8 @@ static const struct cci_reg_sequence imx415_mode_2_720[] = {
 	{ IMX415_TLPX, 0x0027 },
 };
 
-/* all-pixel 2-lane 1440 Mbps 30.01 Hz mode */
-static const struct cci_reg_sequence imx415_mode_2_1440[] = {
-	{ IMX415_VMAX, 0x08CA },
-	{ IMX415_HMAX, 0x042A },
-	{ IMX415_LANEMODE, IMX415_LANEMODE_2 },
+/* 1440 Mbps CSI configuration */
+static const struct cci_reg_sequence imx415_linkrate_1440mbps[] = {
 	{ IMX415_TCLKPOST, 0x009F },
 	{ IMX415_TCLKPREPARE, 0x0057 },
 	{ IMX415_TCLKTRAIL, 0x0057 },
@@ -477,11 +478,8 @@ static const struct cci_reg_sequence imx415_mode_2_1440[] = {
 	{ IMX415_TLPX, 0x004F },
 };
 
-/* all-pixel 4-lane 891 Mbps 30 Hz mode */
-static const struct cci_reg_sequence imx415_mode_4_891[] = {
-	{ IMX415_VMAX, 0x08CA },
-	{ IMX415_HMAX, 0x044C },
-	{ IMX415_LANEMODE, IMX415_LANEMODE_4 },
+/* 891 Mbps CSI configuration */
+static const struct cci_reg_sequence imx415_linkrate_891mbps[] = {
 	{ IMX415_TCLKPOST, 0x007F },
 	{ IMX415_TCLKPREPARE, 0x0037 },
 	{ IMX415_TCLKTRAIL, 0x0037 },
@@ -498,39 +496,9 @@ struct imx415_mode_reg_list {
 	const struct cci_reg_sequence *regs;
 };
 
-/*
- * Mode : number of lanes, lane rate and frame rate dependent settings
- *
- * pixel_rate and hmax_pix are needed to calculate hblank for the v4l2 ctrl
- * interface. These values can not be found in the data sheet and should be
- * treated as virtual values. Use following table when adding new modes.
- *
- * lane_rate  lanes    fps     hmax_pix   pixel_rate
- *
- *     594      2     10.000     4400       99000000
- *     891      2     15.000     4400      148500000
- *     720      2     15.748     4064      144000000
- *    1782      2     30.000     4400      297000000
- *    2079      2     30.000     4400      297000000
- *    1440      2     30.019     4510      304615385
- *
- *     594      4     20.000     5500      247500000
- *     594      4     25.000     4400      247500000
- *     720      4     25.000     4400      247500000
- *     720      4     30.019     4510      304615385
- *     891      4     30.000     4400      297000000
- *    1440      4     30.019     4510      304615385
- *    1440      4     60.038     4510      609230769
- *    1485      4     60.000     4400      594000000
- *    1782      4     60.000     4400      594000000
- *    2079      4     60.000     4400      594000000
- *    2376      4     90.164     4392      891000000
- */
 struct imx415_mode {
 	u64 lane_rate;
-	u32 lanes;
-	u32 hmax_pix;
-	u64 pixel_rate;
+	u32 hmax_min[2];
 	struct imx415_mode_reg_list reg_list;
 };
 
@@ -538,32 +506,26 @@ struct imx415_mode {
 static const struct imx415_mode supported_modes[] = {
 	{
 		.lane_rate = 720000000,
-		.lanes = 2,
-		.hmax_pix = 4064,
-		.pixel_rate = 144000000,
+		.hmax_min = { 2032, 1066 },
 		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(imx415_mode_2_720),
-			.regs = imx415_mode_2_720,
+			.num_of_regs = ARRAY_SIZE(imx415_linkrate_720mbps),
+			.regs = imx415_linkrate_720mbps,
 		},
 	},
 	{
 		.lane_rate = 1440000000,
-		.lanes = 2,
-		.hmax_pix = 4510,
-		.pixel_rate = 304615385,
+		.hmax_min = { 1066, 533 },
 		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(imx415_mode_2_1440),
-			.regs = imx415_mode_2_1440,
+			.num_of_regs = ARRAY_SIZE(imx415_linkrate_1440mbps),
+			.regs = imx415_linkrate_1440mbps,
 		},
 	},
 	{
 		.lane_rate = 891000000,
-		.lanes = 4,
-		.hmax_pix = 4400,
-		.pixel_rate = 297000000,
+		.hmax_min = { 2200, 1100 },
 		.reg_list = {
-			.num_of_regs = ARRAY_SIZE(imx415_mode_4_891),
-			.regs = imx415_mode_4_891,
+			.num_of_regs = ARRAY_SIZE(imx415_linkrate_891mbps),
+			.regs = imx415_linkrate_891mbps,
 		},
 	},
 };
@@ -587,6 +549,7 @@ static const char *const imx415_test_pattern_menu[] = {
 struct imx415 {
 	struct device *dev;
 	struct clk *clk;
+	unsigned long pixel_rate;
 	struct regulator_bulk_data supplies[ARRAY_SIZE(imx415_supply_names)];
 	struct gpio_desc *reset;
 	struct regmap *regmap;
@@ -598,8 +561,10 @@ struct imx415 {
 
 	struct v4l2_ctrl_handler ctrls;
 	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *hflip;
 	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *exposure;
 
 	unsigned int cur_mode;
 	unsigned int num_data_lanes;
@@ -730,17 +695,38 @@ static int imx415_s_ctrl(struct v4l2_ctrl *ctrl)
 					     ctrls);
 	const struct v4l2_mbus_framefmt *format;
 	struct v4l2_subdev_state *state;
+	u32 exposure_max;
 	unsigned int vmax;
 	unsigned int flip;
 	int ret;
 
-	if (!pm_runtime_get_if_in_use(sensor->dev))
-		return 0;
-
 	state = v4l2_subdev_get_locked_active_state(&sensor->subdev);
 	format = v4l2_subdev_state_get_format(state, 0);
 
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		exposure_max = format->height + ctrl->val -
+			       IMX415_EXPOSURE_OFFSET;
+		__v4l2_ctrl_modify_range(sensor->exposure,
+					 sensor->exposure->minimum,
+					 exposure_max, sensor->exposure->step,
+					 sensor->exposure->default_value);
+	}
+
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
 	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		ret = cci_write(sensor->regmap, IMX415_VMAX,
+				format->height + ctrl->val, NULL);
+		if (ret)
+			return ret;
+		/*
+		 * Exposure is set based on VMAX which has just changed, so
+		 * program exposure register as well
+		 */
+		ctrl = sensor->exposure;
+		fallthrough;
 	case V4L2_CID_EXPOSURE:
 		/* clamp the exposure value to VMAX. */
 		vmax = format->height + sensor->vblank->cur.val;
@@ -766,6 +752,13 @@ static int imx415_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = imx415_set_testpattern(sensor, ctrl->val);
 		break;
 
+	case V4L2_CID_HBLANK:
+		ret = cci_write(sensor->regmap, IMX415_HMAX,
+				(format->width + ctrl->val) /
+						IMX415_HMAX_MULTIPLIER,
+				NULL);
+		break;
+
 	default:
 		ret = -EINVAL;
 		break;
@@ -784,11 +777,12 @@ static int imx415_ctrls_init(struct imx415 *sensor)
 {
 	struct v4l2_fwnode_device_properties props;
 	struct v4l2_ctrl *ctrl;
-	u64 pixel_rate = supported_modes[sensor->cur_mode].pixel_rate;
-	u64 lane_rate = supported_modes[sensor->cur_mode].lane_rate;
+	const struct imx415_mode *cur_mode = &supported_modes[sensor->cur_mode];
+	u64 lane_rate = cur_mode->lane_rate;
 	u32 exposure_max = IMX415_PIXEL_ARRAY_HEIGHT +
-			   IMX415_PIXEL_ARRAY_VBLANK - 8;
-	u32 hblank;
+			   IMX415_PIXEL_ARRAY_VBLANK -
+			   IMX415_EXPOSURE_OFFSET;
+	u32 hblank_min, hblank_max;
 	unsigned int i;
 	int ret;
 
@@ -816,36 +810,33 @@ static int imx415_ctrls_init(struct imx415 *sensor)
 	if (ctrl)
 		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-	v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops, V4L2_CID_EXPOSURE,
-			  4, exposure_max, 1, exposure_max);
+	sensor->exposure = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
+					     V4L2_CID_EXPOSURE, 4,
+					     exposure_max, 1, exposure_max);
 
 	v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
 			  V4L2_CID_ANALOGUE_GAIN, IMX415_AGAIN_MIN,
 			  IMX415_AGAIN_MAX, IMX415_AGAIN_STEP,
 			  IMX415_AGAIN_MIN);
 
-	hblank = supported_modes[sensor->cur_mode].hmax_pix -
-		 IMX415_PIXEL_ARRAY_WIDTH;
+	hblank_min = (cur_mode->hmax_min[sensor->num_data_lanes == 2 ? 0 : 1] *
+		      IMX415_HMAX_MULTIPLIER) - IMX415_PIXEL_ARRAY_WIDTH;
+	hblank_max = (IMX415_HMAX_MAX * IMX415_HMAX_MULTIPLIER) -
+		     IMX415_PIXEL_ARRAY_WIDTH;
 	ctrl = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
-				 V4L2_CID_HBLANK, hblank, hblank, 1, hblank);
-	if (ctrl)
-		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+				 V4L2_CID_HBLANK, hblank_min,
+				 hblank_max, IMX415_HMAX_MULTIPLIER,
+				 hblank_min);
 
 	sensor->vblank = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
 					   V4L2_CID_VBLANK,
 					   IMX415_PIXEL_ARRAY_VBLANK,
-					   IMX415_PIXEL_ARRAY_VBLANK, 1,
-					   IMX415_PIXEL_ARRAY_VBLANK);
-	if (sensor->vblank)
-		sensor->vblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+					   IMX415_VMAX_MAX - IMX415_PIXEL_ARRAY_HEIGHT,
+					   1, IMX415_PIXEL_ARRAY_VBLANK);
 
-	/*
-	 * The pixel rate used here is a virtual value and can be used for
-	 * calculating the frame rate together with hblank. It may not
-	 * necessarily be the physically correct pixel clock.
-	 */
-	v4l2_ctrl_new_std(&sensor->ctrls, NULL, V4L2_CID_PIXEL_RATE, pixel_rate,
-			  pixel_rate, 1, pixel_rate);
+	v4l2_ctrl_new_std(&sensor->ctrls, NULL, V4L2_CID_PIXEL_RATE,
+			  sensor->pixel_rate, sensor->pixel_rate, 1,
+			  sensor->pixel_rate);
 
 	sensor->hflip = v4l2_ctrl_new_std(&sensor->ctrls, &imx415_ctrl_ops,
 					  V4L2_CID_HFLIP, 0, 1, 1, 0);
@@ -890,7 +881,12 @@ static int imx415_set_mode(struct imx415 *sensor, int mode)
 			    IMX415_NUM_CLK_PARAM_REGS,
 			    &ret);
 
-	return 0;
+	ret = cci_write(sensor->regmap, IMX415_LANEMODE,
+			sensor->num_data_lanes == 2 ? IMX415_LANEMODE_2 :
+						      IMX415_LANEMODE_4,
+			NULL);
+
+	return ret;
 }
 
 static int imx415_setup(struct imx415 *sensor, struct v4l2_subdev_state *state)
@@ -1113,8 +1109,7 @@ static int imx415_subdev_init(struct imx415 *sensor)
 	if (ret)
 		return ret;
 
-	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-				V4L2_SUBDEV_FL_HAS_EVENTS;
+	sensor->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
 	sensor->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 	ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
@@ -1302,8 +1297,6 @@ static int imx415_parse_hw_config(struct imx415 *sensor)
 		}
 
 		for (j = 0; j < ARRAY_SIZE(supported_modes); ++j) {
-			if (sensor->num_data_lanes != supported_modes[j].lanes)
-				continue;
 			if (bus_cfg.link_frequencies[i] * 2 !=
 			    supported_modes[j].lane_rate)
 				continue;
@@ -1318,6 +1311,17 @@ static int imx415_parse_hw_config(struct imx415 *sensor)
 				    "no valid sensor mode defined\n");
 		goto done_endpoint_free;
 	}
+	switch (inck) {
+	case 27000000:
+	case 37125000:
+	case 74250000:
+		sensor->pixel_rate = IMX415_PIXEL_RATE_74_25MHZ;
+		break;
+	case 24000000:
+	case 72000000:
+		sensor->pixel_rate = IMX415_PIXEL_RATE_72MHZ;
+		break;
+	}
 
 	lane_rate = supported_modes[sensor->cur_mode].lane_rate;
 	for (i = 0; i < ARRAY_SIZE(imx415_clk_params); ++i) {
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
index b37a2aaf8ac0..c84e1e0e6109 100644
--- a/drivers/media/i2c/ir-kbd-i2c.c
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -35,7 +35,7 @@
  *	Copyright (C) 2011 Andy Walls <awalls@md.metrocast.net>
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
diff --git a/drivers/media/i2c/isl7998x.c b/drivers/media/i2c/isl7998x.c
index c7089035bbc1..5ffd53e005ee 100644
--- a/drivers/media/i2c/isl7998x.c
+++ b/drivers/media/i2c/isl7998x.c
@@ -1561,8 +1561,8 @@ static const struct of_device_id isl7998x_of_match[] = {
 MODULE_DEVICE_TABLE(of, isl7998x_of_match);
 
 static const struct i2c_device_id isl7998x_id[] = {
-	{ "isl79987", 0 },
-	{ /* sentinel */ },
+	{ "isl79987" },
+	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, isl7998x_id);
 
diff --git a/drivers/media/i2c/ks0127.c b/drivers/media/i2c/ks0127.c
index 5c583f57e3f3..f3fba9179684 100644
--- a/drivers/media/i2c/ks0127.c
+++ b/drivers/media/i2c/ks0127.c
@@ -175,14 +175,6 @@ MODULE_LICENSE("GPL");
 * mga_dev : represents one ks0127 chip.
 ****************************************************************************/
 
-struct adjust {
-	int	contrast;
-	int	bright;
-	int	hue;
-	int	ugain;
-	int	vgain;
-};
-
 struct ks0127 {
 	struct v4l2_subdev sd;
 	v4l2_std_id	norm;
@@ -685,9 +677,9 @@ static void ks0127_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ks0127_id[] = {
-	{ "ks0127", 0 },
-	{ "ks0127b", 0 },
-	{ "ks0122s", 0 },
+	{ "ks0127" },
+	{ "ks0127b" },
+	{ "ks0122s" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ks0127_id);
diff --git a/drivers/media/i2c/lm3560.c b/drivers/media/i2c/lm3560.c
index 05283ac68f2d..f4cc844f4e3c 100644
--- a/drivers/media/i2c/lm3560.c
+++ b/drivers/media/i2c/lm3560.c
@@ -455,8 +455,8 @@ static void lm3560_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lm3560_id_table[] = {
-	{LM3559_NAME, 0},
-	{LM3560_NAME, 0},
+	{ LM3559_NAME },
+	{ LM3560_NAME },
 	{}
 };
 
diff --git a/drivers/media/i2c/lm3646.c b/drivers/media/i2c/lm3646.c
index fab3a7e05f92..2d16e42ec224 100644
--- a/drivers/media/i2c/lm3646.c
+++ b/drivers/media/i2c/lm3646.c
@@ -386,7 +386,7 @@ static void lm3646_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id lm3646_id_table[] = {
-	{LM3646_NAME, 0},
+	{ LM3646_NAME },
 	{}
 };
 
diff --git a/drivers/media/i2c/lt6911uxe.c b/drivers/media/i2c/lt6911uxe.c
new file mode 100644
index 000000000000..24857d683fcf
--- /dev/null
+++ b/drivers/media/i2c/lt6911uxe.c
@@ -0,0 +1,707 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2023 - 2025 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/v4l2-dv-timings.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+
+#define LT6911UXE_CHIP_ID		0x2102
+#define REG_CHIP_ID			CCI_REG16(0xe100)
+
+#define REG_ENABLE_I2C			CCI_REG8(0xe0ee)
+#define REG_HALF_PIX_CLK		CCI_REG24(0xe085)
+#define REG_BYTE_CLK			CCI_REG24(0xe092)
+#define REG_HALF_H_TOTAL		CCI_REG16(0xe088)
+#define REG_V_TOTAL			CCI_REG16(0xe08a)
+#define REG_HALF_H_ACTIVE		CCI_REG16(0xe08c)
+#define REG_V_ACTIVE			CCI_REG16(0xe08e)
+#define REG_MIPI_FORMAT			CCI_REG8(0xe096)
+#define REG_MIPI_TX_CTRL		CCI_REG8(0xe0b0)
+
+/* Interrupts */
+#define REG_INT_HDMI			CCI_REG8(0xe084)
+#define INT_VIDEO_DISAPPEAR		0x0
+#define INT_VIDEO_READY			0x1
+
+#define LT6911UXE_DEFAULT_LANES		4
+#define LT6911_PAGE_CONTROL		0xff
+#define YUV422_8_BIT			0x7
+
+static const struct v4l2_dv_timings_cap lt6911uxe_timings_cap_4kp30 = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with CLANG */
+	.reserved = { 0 },
+	/* Pixel clock from REF_01 p. 20. Min/max height/width are unknown */
+	V4L2_INIT_BT_TIMINGS(160, 3840,			/* min/max width */
+			     120, 2160,			/* min/max height */
+			     50000000, 594000000,	/* min/max pixelclock */
+			     V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			     V4L2_DV_BT_STD_CVT,
+			     V4L2_DV_BT_CAP_PROGRESSIVE |
+			     V4L2_DV_BT_CAP_CUSTOM |
+			     V4L2_DV_BT_CAP_REDUCED_BLANKING)
+};
+
+static const struct regmap_range_cfg lt6911uxe_ranges[] = {
+	{
+		.name = "register_range",
+		.range_min =  0,
+		.range_max = 0xffff,
+		.selector_reg = LT6911_PAGE_CONTROL,
+		.selector_mask = 0xff,
+		.selector_shift = 0,
+		.window_start = 0,
+		.window_len = 0x100,
+	},
+};
+
+static const struct regmap_config lt6911uxe_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = 0xffff,
+	.ranges = lt6911uxe_ranges,
+	.num_ranges = ARRAY_SIZE(lt6911uxe_ranges),
+};
+
+struct lt6911uxe_mode {
+	u32 width;
+	u32 height;
+	u32 htotal;
+	u32 vtotal;
+	u32 code;
+	u32 fps;
+	u32 lanes;
+	s64 link_freq;
+	u64 pixel_clk;
+};
+
+struct lt6911uxe {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_dv_timings timings;
+	struct lt6911uxe_mode cur_mode;
+	struct regmap *regmap;
+	struct gpio_desc *reset_gpio;
+	struct gpio_desc *irq_gpio;
+};
+
+static const struct v4l2_event lt6911uxe_ev_source_change = {
+	.type = V4L2_EVENT_SOURCE_CHANGE,
+	.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+};
+
+static inline struct lt6911uxe *to_lt6911uxe(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct lt6911uxe, sd);
+}
+
+static s64 get_pixel_rate(struct lt6911uxe *lt6911uxe)
+{
+	s64 pixel_rate;
+
+	pixel_rate = (s64)lt6911uxe->cur_mode.width *
+		     lt6911uxe->cur_mode.height *
+		     lt6911uxe->cur_mode.fps * 16;
+	do_div(pixel_rate, lt6911uxe->cur_mode.lanes);
+
+	return pixel_rate;
+}
+
+static int lt6911uxe_get_detected_timings(struct v4l2_subdev *sd,
+					  struct v4l2_dv_timings *timings)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct v4l2_bt_timings *bt = &timings->bt;
+
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
+	timings->type = V4L2_DV_BT_656_1120;
+
+	bt->width = lt6911uxe->cur_mode.width;
+	bt->height = lt6911uxe->cur_mode.height;
+	bt->vsync = lt6911uxe->cur_mode.vtotal - lt6911uxe->cur_mode.height;
+	bt->hsync = lt6911uxe->cur_mode.htotal - lt6911uxe->cur_mode.width;
+	bt->pixelclock = lt6911uxe->cur_mode.pixel_clk;
+
+	return 0;
+}
+
+static int lt6911uxe_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				  struct v4l2_dv_timings *timings)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct v4l2_subdev_state *state;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (v4l2_match_dv_timings(&lt6911uxe->timings, timings, 0, false)) {
+		v4l2_subdev_unlock_state(state);
+		return 0;
+	}
+
+	if (!v4l2_valid_dv_timings(timings, &lt6911uxe_timings_cap_4kp30,
+				   NULL, NULL)) {
+		v4l2_subdev_unlock_state(state);
+		return -ERANGE;
+	}
+	lt6911uxe->timings = *timings;
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+}
+
+static int lt6911uxe_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				  struct v4l2_dv_timings *timings)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct v4l2_subdev_state *state;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	*timings = lt6911uxe->timings;
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+}
+
+static int lt6911uxe_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				      struct v4l2_dv_timings *timings)
+{
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	ret = lt6911uxe_get_detected_timings(sd, timings);
+	if (ret) {
+		v4l2_subdev_unlock_state(state);
+		return ret;
+	}
+
+	if (!v4l2_valid_dv_timings(timings, &lt6911uxe_timings_cap_4kp30,
+				   NULL, NULL)) {
+		v4l2_subdev_unlock_state(state);
+		return -ERANGE;
+	}
+
+	v4l2_subdev_unlock_state(state);
+	return 0;
+}
+
+static int lt6911uxe_enum_dv_timings(struct v4l2_subdev *sd,
+				     struct v4l2_enum_dv_timings *timings)
+{
+	return v4l2_enum_dv_timings_cap(timings,
+			&lt6911uxe_timings_cap_4kp30, NULL, NULL);
+}
+
+static int lt6911uxe_dv_timings_cap(struct v4l2_subdev *sd,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	*cap = lt6911uxe_timings_cap_4kp30;
+	return 0;
+}
+
+static int lt6911uxe_status_update(struct lt6911uxe *lt6911uxe)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&lt6911uxe->sd);
+	u64 int_event;
+	u64 byte_clk, half_pix_clk, fps, format;
+	u64 half_htotal, vtotal, half_width, height;
+	int ret = 0;
+
+	/* Read interrupt event */
+	cci_read(lt6911uxe->regmap, REG_INT_HDMI, &int_event, &ret);
+	if (ret) {
+		dev_err(&client->dev, "failed to read interrupt event: %d\n",
+			ret);
+		return ret;
+	}
+
+	switch (int_event) {
+	case INT_VIDEO_READY:
+		cci_read(lt6911uxe->regmap, REG_BYTE_CLK, &byte_clk, &ret);
+		byte_clk *= 1000;
+		cci_read(lt6911uxe->regmap, REG_HALF_PIX_CLK,
+			 &half_pix_clk, &ret);
+		half_pix_clk *= 1000;
+
+		if (ret || byte_clk == 0 || half_pix_clk == 0) {
+			dev_dbg(&client->dev,
+				"invalid ByteClock or PixelClock\n");
+			return -EINVAL;
+		}
+
+		cci_read(lt6911uxe->regmap, REG_HALF_H_TOTAL,
+			 &half_htotal, &ret);
+		cci_read(lt6911uxe->regmap, REG_V_TOTAL, &vtotal, &ret);
+		if (ret || half_htotal == 0 || vtotal == 0) {
+			dev_dbg(&client->dev, "invalid htotal or vtotal\n");
+			return -EINVAL;
+		}
+
+		fps = div_u64(half_pix_clk, half_htotal * vtotal);
+		if (fps > 60) {
+			dev_dbg(&client->dev,
+				"max fps is 60, current fps: %llu\n", fps);
+			return -EINVAL;
+		}
+
+		cci_read(lt6911uxe->regmap, REG_HALF_H_ACTIVE,
+			 &half_width, &ret);
+		cci_read(lt6911uxe->regmap, REG_V_ACTIVE, &height, &ret);
+		if (ret || half_width == 0 || half_width * 2 > 3840 ||
+		    height == 0 || height > 2160) {
+			dev_dbg(&client->dev, "invalid width or height\n");
+			return -EINVAL;
+		}
+
+		/*
+		 * Get MIPI format, YUV422_8_BIT is expected in lt6911uxe
+		 */
+		cci_read(lt6911uxe->regmap, REG_MIPI_FORMAT, &format, &ret);
+		if (format != YUV422_8_BIT) {
+			dev_dbg(&client->dev, "invalid MIPI format\n");
+			return -EINVAL;
+		}
+
+		lt6911uxe->cur_mode.height = height;
+		lt6911uxe->cur_mode.width = half_width * 2;
+		lt6911uxe->cur_mode.fps = fps;
+		/* MIPI Clock Rate = ByteClock × 4, defined in lt6911uxe spec */
+		lt6911uxe->cur_mode.link_freq = byte_clk * 4;
+		lt6911uxe->cur_mode.pixel_clk = half_pix_clk * 2;
+		lt6911uxe->cur_mode.vtotal = vtotal;
+		lt6911uxe->cur_mode.htotal = half_htotal * 2;
+		break;
+
+	case INT_VIDEO_DISAPPEAR:
+		cci_write(lt6911uxe->regmap, REG_MIPI_TX_CTRL, 0x0, &ret);
+		lt6911uxe->cur_mode.height = 0;
+		lt6911uxe->cur_mode.width = 0;
+		lt6911uxe->cur_mode.fps = 0;
+		lt6911uxe->cur_mode.link_freq = 0;
+		break;
+
+	default:
+		ret = -ENOLINK;
+	}
+	v4l2_subdev_notify_event(&lt6911uxe->sd, &lt6911uxe_ev_source_change);
+	return ret;
+}
+
+static int lt6911uxe_init_controls(struct lt6911uxe *lt6911uxe)
+{
+	struct v4l2_ctrl_handler *ctrl_hdlr;
+	s64 pixel_rate;
+	int ret;
+
+	ctrl_hdlr = &lt6911uxe->ctrl_handler;
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	if (ret)
+		return ret;
+
+	pixel_rate = get_pixel_rate(lt6911uxe);
+	lt6911uxe->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, NULL,
+						  V4L2_CID_PIXEL_RATE,
+						  pixel_rate, pixel_rate, 1,
+						  pixel_rate);
+
+	if (ctrl_hdlr->error) {
+		ret = ctrl_hdlr->error;
+		goto hdlr_free;
+	}
+	lt6911uxe->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+
+hdlr_free:
+	v4l2_ctrl_handler_free(ctrl_hdlr);
+	return ret;
+}
+
+static void lt6911uxe_update_pad_format(const struct lt6911uxe_mode *mode,
+					struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = mode->code;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int lt6911uxe_enable_streams(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    u32 pad, u64 streams_mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret < 0)
+		return ret;
+
+	cci_write(lt6911uxe->regmap, REG_MIPI_TX_CTRL, 0x1, &ret);
+	if (ret) {
+		dev_err(&client->dev, "failed to start stream: %d\n", ret);
+		goto err_rpm_put;
+	}
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(&client->dev);
+	return ret;
+}
+
+static int lt6911uxe_disable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     u32 pad, u64 streams_mask)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct i2c_client *client = v4l2_get_subdevdata(&lt6911uxe->sd);
+	int ret;
+
+	ret = cci_write(lt6911uxe->regmap, REG_MIPI_TX_CTRL, 0x0, NULL);
+	if (ret)
+		dev_err(&client->dev, "failed to stop stream: %d\n", ret);
+
+	pm_runtime_put(&client->dev);
+	return 0;
+}
+
+static int lt6911uxe_set_format(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_format *fmt)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	u64 pixel_rate;
+
+	lt6911uxe_update_pad_format(&lt6911uxe->cur_mode, &fmt->format);
+	*v4l2_subdev_state_get_format(sd_state, fmt->pad) = fmt->format;
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	pixel_rate = get_pixel_rate(lt6911uxe);
+	__v4l2_ctrl_modify_range(lt6911uxe->pixel_rate, pixel_rate,
+				 pixel_rate, 1, pixel_rate);
+
+	return 0;
+}
+
+static int lt6911uxe_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *sd_state,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+
+	if (code->index)
+		return -EINVAL;
+
+	code->code = lt6911uxe->cur_mode.code;
+
+	return 0;
+}
+
+static int lt6911uxe_get_mbus_config(struct v4l2_subdev *sd,
+				     unsigned int pad,
+				     struct v4l2_mbus_config *cfg)
+{
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct v4l2_subdev_state *state;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	cfg->type = V4L2_MBUS_CSI2_DPHY;
+	cfg->link_freq = lt6911uxe->cur_mode.link_freq;
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+}
+
+static int lt6911uxe_init_state(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY
+		: V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+
+	return lt6911uxe_set_format(sd, sd_state, &fmt);
+}
+
+static const struct v4l2_subdev_video_ops lt6911uxe_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+/*
+ * lt6911uxe provides editable EDID for customers, but only can be edited like
+ * updating flash. Due to this limitation, it is not possible to implement
+ * EDID support.
+ */
+static const struct v4l2_subdev_pad_ops lt6911uxe_pad_ops = {
+	.set_fmt = lt6911uxe_set_format,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.enable_streams = lt6911uxe_enable_streams,
+	.disable_streams = lt6911uxe_disable_streams,
+	.enum_mbus_code = lt6911uxe_enum_mbus_code,
+	.get_frame_interval = v4l2_subdev_get_frame_interval,
+	.s_dv_timings = lt6911uxe_s_dv_timings,
+	.g_dv_timings = lt6911uxe_g_dv_timings,
+	.query_dv_timings = lt6911uxe_query_dv_timings,
+	.enum_dv_timings = lt6911uxe_enum_dv_timings,
+	.dv_timings_cap = lt6911uxe_dv_timings_cap,
+	.get_mbus_config = lt6911uxe_get_mbus_config,
+};
+
+static const struct v4l2_subdev_core_ops lt6911uxe_subdev_core_ops = {
+	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops lt6911uxe_subdev_ops = {
+	.core = &lt6911uxe_subdev_core_ops,
+	.video = &lt6911uxe_video_ops,
+	.pad = &lt6911uxe_pad_ops,
+};
+
+static const struct media_entity_operations lt6911uxe_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops lt6911uxe_internal_ops = {
+	.init_state = lt6911uxe_init_state,
+};
+
+static int lt6911uxe_fwnode_parse(struct lt6911uxe *lt6911uxe,
+				  struct device *dev)
+{
+	struct fwnode_handle *endpoint;
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	int ret;
+
+	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
+						   FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!endpoint)
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "endpoint node not found\n");
+
+	ret = v4l2_fwnode_endpoint_parse(endpoint, &bus_cfg);
+	fwnode_handle_put(endpoint);
+	if (ret) {
+		dev_err(dev, "failed to parse endpoint node: %d\n", ret);
+		goto out_err;
+	}
+
+	/*
+	 * Check the number of MIPI CSI2 data lanes,
+	 * lt6911uxe only support 4 lanes.
+	 */
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != LT6911UXE_DEFAULT_LANES) {
+		dev_err(dev, "only 4 data lanes are currently supported\n");
+		ret = -EINVAL;
+		goto out_err;
+	}
+	lt6911uxe->cur_mode.lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+	lt6911uxe->cur_mode.code = MEDIA_BUS_FMT_UYVY8_1X16;
+
+	return 0;
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	return ret;
+}
+
+static int lt6911uxe_identify_module(struct lt6911uxe *lt6911uxe,
+				     struct device *dev)
+{
+	u64 val;
+	int ret = 0;
+
+	/* Chip ID should be confirmed when the I2C slave is active */
+	cci_write(lt6911uxe->regmap, REG_ENABLE_I2C, 0x1, &ret);
+	cci_read(lt6911uxe->regmap, REG_CHIP_ID, &val, &ret);
+	cci_write(lt6911uxe->regmap, REG_ENABLE_I2C, 0x0, &ret);
+	if (ret)
+		return dev_err_probe(dev, ret, "fail to read chip id\n");
+
+	if (val != LT6911UXE_CHIP_ID) {
+		return dev_err_probe(dev, -ENXIO, "chip id mismatch: %x!=%x\n",
+				     LT6911UXE_CHIP_ID, (u16)val);
+	}
+
+	return 0;
+}
+
+static irqreturn_t lt6911uxe_threaded_irq_fn(int irq, void *dev_id)
+{
+	struct v4l2_subdev *sd = dev_id;
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE
+	};
+
+	lt6911uxe_status_update(lt6911uxe);
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	/*
+	 * As a HDMI to CSI2 bridge, it needs to update the format in time
+	 * when the HDMI source changes.
+	 */
+	lt6911uxe_set_format(sd, state, &fmt);
+	v4l2_subdev_unlock_state(state);
+
+	return IRQ_HANDLED;
+}
+
+static void lt6911uxe_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct lt6911uxe *lt6911uxe = to_lt6911uxe(sd);
+
+	free_irq(gpiod_to_irq(lt6911uxe->irq_gpio), lt6911uxe);
+	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(&lt6911uxe->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+}
+
+static int lt6911uxe_probe(struct i2c_client *client)
+{
+	struct lt6911uxe *lt6911uxe;
+	struct device *dev = &client->dev;
+	int ret;
+
+	lt6911uxe = devm_kzalloc(dev, sizeof(*lt6911uxe), GFP_KERNEL);
+	if (!lt6911uxe)
+		return -ENOMEM;
+
+	lt6911uxe->regmap = devm_regmap_init_i2c(client,
+						 &lt6911uxe_regmap_config);
+	if (IS_ERR(lt6911uxe->regmap))
+		return dev_err_probe(dev, PTR_ERR(lt6911uxe->regmap),
+				     "failed to init CCI\n");
+
+	v4l2_i2c_subdev_init(&lt6911uxe->sd, client, &lt6911uxe_subdev_ops);
+
+	lt6911uxe->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_IN);
+	if (IS_ERR(lt6911uxe->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(lt6911uxe->reset_gpio),
+				     "failed to get reset gpio\n");
+
+	lt6911uxe->irq_gpio = devm_gpiod_get(dev, "hpd", GPIOD_IN);
+	if (IS_ERR(lt6911uxe->irq_gpio))
+		return dev_err_probe(dev, PTR_ERR(lt6911uxe->irq_gpio),
+				     "failed to get hpd gpio\n");
+
+	ret = lt6911uxe_fwnode_parse(lt6911uxe, dev);
+	if (ret)
+		return ret;
+
+	usleep_range(10000, 10500);
+
+	ret = lt6911uxe_identify_module(lt6911uxe, dev);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to find chip\n");
+
+	ret = lt6911uxe_init_controls(lt6911uxe);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to init control\n");
+
+	lt6911uxe->sd.dev = dev;
+	lt6911uxe->sd.internal_ops = &lt6911uxe_internal_ops;
+	lt6911uxe->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	lt6911uxe->sd.entity.ops = &lt6911uxe_subdev_entity_ops;
+	lt6911uxe->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	lt6911uxe->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&lt6911uxe->sd.entity, 1, &lt6911uxe->pad);
+	if (ret) {
+		dev_err(dev, "failed to init entity pads: %d\n", ret);
+		goto v4l2_ctrl_handler_free;
+	}
+
+	/*
+	 * Device is already turned on by i2c-core with ACPI domain PM.
+	 * Enable runtime PM and turn off the device.
+	 */
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_idle(dev);
+
+	ret = v4l2_subdev_init_finalize(&lt6911uxe->sd);
+	if (ret) {
+		dev_err(dev, "failed to init v4l2 subdev: %d\n", ret);
+		goto media_entity_cleanup;
+	}
+
+	/* Setting irq */
+	ret = request_threaded_irq(gpiod_to_irq(lt6911uxe->irq_gpio), NULL,
+				   lt6911uxe_threaded_irq_fn,
+				   IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING |
+				   IRQF_ONESHOT, NULL, lt6911uxe);
+	if (ret) {
+		dev_err(dev, "failed to request IRQ: %d\n", ret);
+		goto subdev_cleanup;
+	}
+
+	ret = v4l2_async_register_subdev_sensor(&lt6911uxe->sd);
+	if (ret) {
+		dev_err(dev, "failed to register V4L2 subdev: %d\n", ret);
+		goto free_irq;
+	}
+
+	return 0;
+
+free_irq:
+	free_irq(gpiod_to_irq(lt6911uxe->irq_gpio), lt6911uxe);
+
+subdev_cleanup:
+	v4l2_subdev_cleanup(&lt6911uxe->sd);
+
+media_entity_cleanup:
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	media_entity_cleanup(&lt6911uxe->sd.entity);
+
+v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(lt6911uxe->sd.ctrl_handler);
+
+	return ret;
+}
+
+static const struct acpi_device_id lt6911uxe_acpi_ids[] = {
+	{ "INTC10C5" },
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, lt6911uxe_acpi_ids);
+
+static struct i2c_driver lt6911uxe_i2c_driver = {
+	.driver = {
+		.name = "lt6911uxe",
+		.acpi_match_table = ACPI_PTR(lt6911uxe_acpi_ids),
+	},
+	.probe = lt6911uxe_probe,
+	.remove = lt6911uxe_remove,
+};
+
+module_i2c_driver(lt6911uxe_i2c_driver);
+
+MODULE_AUTHOR("Yan Dongcheng <dongcheng.yan@intel.com>");
+MODULE_DESCRIPTION("Lontium lt6911uxe HDMI to MIPI Bridge Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/m52790.c b/drivers/media/i2c/m52790.c
index f8a69142aae9..9e1ecfd01e2a 100644
--- a/drivers/media/i2c/m52790.c
+++ b/drivers/media/i2c/m52790.c
@@ -163,7 +163,7 @@ static void m52790_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id m52790_id[] = {
-	{ "m52790", 0 },
+	{ "m52790" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, m52790_id);
diff --git a/drivers/media/i2c/max2175.c b/drivers/media/i2c/max2175.c
index cd73d2096ae4..bf02ca23a284 100644
--- a/drivers/media/i2c/max2175.c
+++ b/drivers/media/i2c/max2175.c
@@ -1413,8 +1413,8 @@ static void max2175_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id max2175_id[] = {
-	{ DRIVER_NAME, 0},
-	{},
+	{ DRIVER_NAME },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, max2175_id);
 
diff --git a/drivers/media/i2c/max9271.h b/drivers/media/i2c/max9271.h
index dc5e4e70ba6f..0bf1d40811eb 100644
--- a/drivers/media/i2c/max9271.h
+++ b/drivers/media/i2c/max9271.h
@@ -8,6 +8,9 @@
  * Copyright (C) 2015 Cogent Embedded, Inc.
  */
 
+#ifndef __MEDIA_I2C_MAX9271_H__
+#define __MEDIA_I2C_MAX9271_H__
+
 #include <linux/i2c.h>
 
 #define MAX9271_DEFAULT_ADDR	0x40
@@ -231,3 +234,5 @@ int max9271_set_deserializer_address(struct max9271_device *dev, u8 addr);
  * Return 0 on success or a negative error code on failure
  */
 int max9271_set_translation(struct max9271_device *dev, u8 source, u8 dest);
+
+#endif /* __MEDIA_I2C_MAX9271_H__ */
diff --git a/drivers/media/i2c/max9286.c b/drivers/media/i2c/max9286.c
index d685d445cf23..9fc4e130a273 100644
--- a/drivers/media/i2c/max9286.c
+++ b/drivers/media/i2c/max9286.c
@@ -19,7 +19,6 @@
 #include <linux/i2c.h>
 #include <linux/i2c-mux.h>
 #include <linux/module.h>
-#include <linux/mutex.h>
 #include <linux/of_graph.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
@@ -198,12 +197,6 @@ struct max9286_priv {
 	struct v4l2_ctrl *pixelrate_ctrl;
 	unsigned int pixelrate;
 
-	struct v4l2_mbus_framefmt fmt[MAX9286_N_SINKS];
-	struct v4l2_fract interval;
-
-	/* Protects controls and fmt structures */
-	struct mutex mutex;
-
 	unsigned int nsources;
 	unsigned int source_mask;
 	unsigned int route_mask;
@@ -383,7 +376,7 @@ static int max9286_i2c_mux_init(struct max9286_priv *priv)
 	for_each_source(priv, source) {
 		unsigned int index = to_index(priv, source);
 
-		ret = i2c_mux_add_adapter(priv->mux, 0, index, 0);
+		ret = i2c_mux_add_adapter(priv->mux, 0, index);
 		if (ret < 0)
 			goto error;
 	}
@@ -576,11 +569,14 @@ static void max9286_set_video_format(struct max9286_priv *priv,
 		      MAX9286_INVVS | MAX9286_HVSRC_D14);
 }
 
-static void max9286_set_fsync_period(struct max9286_priv *priv)
+static void max9286_set_fsync_period(struct max9286_priv *priv,
+				     struct v4l2_subdev_state *state)
 {
+	const struct v4l2_fract *interval;
 	u32 fsync;
 
-	if (!priv->interval.numerator || !priv->interval.denominator) {
+	interval = v4l2_subdev_state_get_interval(state, MAX9286_SRC_PAD);
+	if (!interval->numerator || !interval->denominator) {
 		/*
 		 * Special case, a null interval enables automatic FRAMESYNC
 		 * mode. FRAMESYNC is taken from the slowest link.
@@ -596,8 +592,8 @@ static void max9286_set_fsync_period(struct max9286_priv *priv)
 	 * The FRAMESYNC generator is configured with a period expressed as a
 	 * number of PCLK periods.
 	 */
-	fsync = div_u64((u64)priv->pixelrate * priv->interval.numerator,
-			priv->interval.denominator);
+	fsync = div_u64((u64)priv->pixelrate * interval->numerator,
+			interval->denominator);
 
 	dev_dbg(&priv->client->dev, "fsync period %u (pclk %u)\n", fsync,
 		priv->pixelrate);
@@ -788,22 +784,25 @@ static void max9286_v4l2_notifier_unregister(struct max9286_priv *priv)
 static int max9286_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct max9286_priv *priv = sd_to_max9286(sd);
+	struct v4l2_subdev_state *state;
 	struct max9286_source *source;
 	unsigned int i;
 	bool sync = false;
-	int ret;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
 
 	if (enable) {
 		const struct v4l2_mbus_framefmt *format;
 
 		/*
-		 * Get the format from the first used sink pad, as all sink
-		 * formats must be identical.
+		 * Get the format from the source pad, as all formats must be
+		 * identical.
 		 */
-		format = &priv->fmt[__ffs(priv->bound_sources)];
+		format = v4l2_subdev_state_get_format(state, MAX9286_SRC_PAD);
 
 		max9286_set_video_format(priv, format);
-		max9286_set_fsync_period(priv);
+		max9286_set_fsync_period(priv, state);
 
 		/*
 		 * The frame sync between cameras is transmitted across the
@@ -816,12 +815,12 @@ static int max9286_s_stream(struct v4l2_subdev *sd, int enable)
 		for_each_source(priv, source) {
 			ret = v4l2_subdev_call(source->sd, video, s_stream, 1);
 			if (ret)
-				return ret;
+				goto unlock;
 		}
 
 		ret = max9286_check_video_links(priv);
 		if (ret)
-			return ret;
+			goto unlock;
 
 		/*
 		 * Wait until frame synchronization is locked.
@@ -842,7 +841,8 @@ static int max9286_s_stream(struct v4l2_subdev *sd, int enable)
 		if (!sync) {
 			dev_err(&priv->client->dev,
 				"Failed to get frame synchronization\n");
-			return -EXDEV; /* Invalid cross-device link */
+			ret = -EXDEV; /* Invalid cross-device link */
+			goto unlock;
 		}
 
 		/*
@@ -865,26 +865,21 @@ static int max9286_s_stream(struct v4l2_subdev *sd, int enable)
 		max9286_i2c_mux_close(priv);
 	}
 
-	return 0;
+unlock:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
 }
 
 static int max9286_get_frame_interval(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_state *sd_state,
 				      struct v4l2_subdev_frame_interval *interval)
 {
-	struct max9286_priv *priv = sd_to_max9286(sd);
-
-	/*
-	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
-	 * subdev active state API.
-	 */
-	if (interval->which != V4L2_SUBDEV_FORMAT_ACTIVE)
-		return -EINVAL;
-
 	if (interval->pad != MAX9286_SRC_PAD)
 		return -EINVAL;
 
-	interval->interval = priv->interval;
+	interval->interval = *v4l2_subdev_state_get_interval(sd_state,
+							     interval->pad);
 
 	return 0;
 }
@@ -893,19 +888,11 @@ static int max9286_set_frame_interval(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_state *sd_state,
 				      struct v4l2_subdev_frame_interval *interval)
 {
-	struct max9286_priv *priv = sd_to_max9286(sd);
-
-	/*
-	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
-	 * subdev active state API.
-	 */
-	if (interval->which != V4L2_SUBDEV_FORMAT_ACTIVE)
-		return -EINVAL;
-
 	if (interval->pad != MAX9286_SRC_PAD)
 		return -EINVAL;
 
-	priv->interval = interval->interval;
+	*v4l2_subdev_state_get_interval(sd_state,
+					interval->pad) = interval->interval;
 
 	return 0;
 }
@@ -914,39 +901,28 @@ static int max9286_enum_mbus_code(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_state *sd_state,
 				  struct v4l2_subdev_mbus_code_enum *code)
 {
-	if (code->pad || code->index > 0)
+	if (code->pad || code->index >= ARRAY_SIZE(max9286_formats))
 		return -EINVAL;
 
-	code->code = MEDIA_BUS_FMT_UYVY8_1X16;
+	code->code = max9286_formats[code->index].code;
 
 	return 0;
 }
 
-static struct v4l2_mbus_framefmt *
-max9286_get_pad_format(struct max9286_priv *priv,
-		       struct v4l2_subdev_state *sd_state,
-		       unsigned int pad, u32 which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_state_get_format(sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &priv->fmt[pad];
-	default:
-		return NULL;
-	}
-}
-
 static int max9286_set_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_subdev_state *sd_state,
+			   struct v4l2_subdev_state *state,
 			   struct v4l2_subdev_format *format)
 {
 	struct max9286_priv *priv = sd_to_max9286(sd);
-	struct v4l2_mbus_framefmt *cfg_fmt;
+	struct max9286_source *source;
 	unsigned int i;
 
+	/*
+	 * Disable setting format on the source pad: format is propagated
+	 * from the sinks.
+	 */
 	if (format->pad == MAX9286_SRC_PAD)
-		return -EINVAL;
+		return v4l2_subdev_get_fmt(sd, state, format);
 
 	/* Validate the format. */
 	for (i = 0; i < ARRAY_SIZE(max9286_formats); ++i) {
@@ -957,42 +933,17 @@ static int max9286_set_fmt(struct v4l2_subdev *sd,
 	if (i == ARRAY_SIZE(max9286_formats))
 		format->format.code = max9286_formats[0].code;
 
-	cfg_fmt = max9286_get_pad_format(priv, sd_state, format->pad,
-					 format->which);
-	if (!cfg_fmt)
-		return -EINVAL;
-
-	mutex_lock(&priv->mutex);
-	*cfg_fmt = format->format;
-	mutex_unlock(&priv->mutex);
-
-	return 0;
-}
-
-static int max9286_get_fmt(struct v4l2_subdev *sd,
-			   struct v4l2_subdev_state *sd_state,
-			   struct v4l2_subdev_format *format)
-{
-	struct max9286_priv *priv = sd_to_max9286(sd);
-	struct v4l2_mbus_framefmt *cfg_fmt;
-	unsigned int pad = format->pad;
-
 	/*
-	 * Multiplexed Stream Support: Support link validation by returning the
-	 * format of the first bound link. All links must have the same format,
-	 * as we do not support mixing and matching of cameras connected to the
-	 * max9286.
+	 * Apply the same format on all the other pad as all links must have the
+	 * same format.
 	 */
-	if (pad == MAX9286_SRC_PAD)
-		pad = __ffs(priv->bound_sources);
+	for_each_source(priv, source) {
+		unsigned int index = to_index(priv, source);
 
-	cfg_fmt = max9286_get_pad_format(priv, sd_state, pad, format->which);
-	if (!cfg_fmt)
-		return -EINVAL;
+		*v4l2_subdev_state_get_format(state, index) = format->format;
+	}
 
-	mutex_lock(&priv->mutex);
-	format->format = *cfg_fmt;
-	mutex_unlock(&priv->mutex);
+	*v4l2_subdev_state_get_format(state, MAX9286_SRC_PAD) = format->format;
 
 	return 0;
 }
@@ -1003,7 +954,7 @@ static const struct v4l2_subdev_video_ops max9286_video_ops = {
 
 static const struct v4l2_subdev_pad_ops max9286_pad_ops = {
 	.enum_mbus_code = max9286_enum_mbus_code,
-	.get_fmt	= max9286_get_fmt,
+	.get_fmt	= v4l2_subdev_get_fmt,
 	.set_fmt	= max9286_set_fmt,
 	.get_frame_interval = max9286_get_frame_interval,
 	.set_frame_interval = max9286_set_frame_interval,
@@ -1025,26 +976,29 @@ static const struct v4l2_mbus_framefmt max9286_default_format = {
 	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
 };
 
-static void max9286_init_format(struct v4l2_mbus_framefmt *fmt)
+static int max9286_init_state(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state)
 {
-	*fmt = max9286_default_format;
-}
+	struct v4l2_fract *interval;
 
-static int max9286_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
-{
-	struct v4l2_mbus_framefmt *format;
-	unsigned int i;
+	for (unsigned int i = 0; i < MAX9286_N_PADS; i++)
+		*v4l2_subdev_state_get_format(state, i) = max9286_default_format;
 
-	for (i = 0; i < MAX9286_N_SINKS; i++) {
-		format = v4l2_subdev_state_get_format(fh->state, i);
-		max9286_init_format(format);
-	}
+	/*
+	 * Special case: a null interval enables automatic FRAMESYNC mode.
+	 *
+	 * FRAMESYNC is taken from the slowest link. See register 0x01
+	 * configuration.
+	 */
+	interval = v4l2_subdev_state_get_interval(state, MAX9286_SRC_PAD);
+	interval->numerator = 0;
+	interval->denominator = 0;
 
 	return 0;
 }
 
 static const struct v4l2_subdev_internal_ops max9286_subdev_internal_ops = {
-	.open = max9286_open,
+	.init_state = max9286_init_state,
 };
 
 static const struct media_entity_operations max9286_media_ops = {
@@ -1079,10 +1033,6 @@ static int max9286_v4l2_register(struct max9286_priv *priv)
 	}
 
 	/* Configure V4L2 for the MAX9286 itself */
-
-	for (i = 0; i < MAX9286_N_SINKS; i++)
-		max9286_init_format(&priv->fmt[i]);
-
 	v4l2_i2c_subdev_init(&priv->sd, priv->client, &max9286_subdev_ops);
 	priv->sd.internal_ops = &max9286_subdev_internal_ops;
 	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
@@ -1109,14 +1059,21 @@ static int max9286_v4l2_register(struct max9286_priv *priv)
 	if (ret)
 		goto err_async;
 
+	priv->sd.state_lock = priv->ctrls.lock;
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret)
+		goto err_async;
+
 	ret = v4l2_async_register_subdev(&priv->sd);
 	if (ret < 0) {
 		dev_err(dev, "Unable to register subdevice\n");
-		goto err_async;
+		goto err_subdev;
 	}
 
 	return 0;
 
+err_subdev:
+	v4l2_subdev_cleanup(&priv->sd);
 err_async:
 	v4l2_ctrl_handler_free(&priv->ctrls);
 	max9286_v4l2_notifier_unregister(priv);
@@ -1126,6 +1083,7 @@ err_async:
 
 static void max9286_v4l2_unregister(struct max9286_priv *priv)
 {
+	v4l2_subdev_cleanup(&priv->sd);
 	v4l2_ctrl_handler_free(&priv->ctrls);
 	v4l2_async_unregister_subdev(&priv->sd);
 	max9286_v4l2_notifier_unregister(priv);
@@ -1182,7 +1140,6 @@ static int max9286_setup(struct max9286_priv *priv)
 	max9286_write(priv, 0x69, (0xf & ~priv->route_mask));
 
 	max9286_set_video_format(priv, &max9286_default_format);
-	max9286_set_fsync_period(priv);
 
 	cfg = max9286_read(priv, 0x1c);
 	if (cfg < 0)
@@ -1629,8 +1586,6 @@ static int max9286_probe(struct i2c_client *client)
 	if (!priv)
 		return -ENOMEM;
 
-	mutex_init(&priv->mutex);
-
 	priv->client = client;
 
 	/* GPIO values default to high */
diff --git a/drivers/media/i2c/max96714.c b/drivers/media/i2c/max96714.c
new file mode 100644
index 000000000000..3cc1b1ae47d1
--- /dev/null
+++ b/drivers/media/i2c/max96714.c
@@ -0,0 +1,1024 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Maxim GMSL2 Deserializer Driver
+ *
+ * Copyright (C) 2024 Collabora Ltd.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bitops.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/i2c-mux.h>
+#include <linux/module.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define MAX96714_DEVICE_ID  0xc9
+#define MAX96714F_DEVICE_ID 0xca
+#define MAX96714_NPORTS     2
+#define MAX96714_PAD_SINK   0
+#define MAX96714_PAD_SOURCE 1
+#define MAX96714_CSI_NLANES 4
+
+/* DEV */
+#define MAX96714_REG13                 CCI_REG8(0x0d)
+#define MAX96714_DEV_REV               CCI_REG8(0x0e)
+#define MAX96714_DEV_REV_MASK          GENMASK(3, 0)
+#define MAX96714_LINK_LOCK             CCI_REG8(0x13)
+#define MAX96714_LINK_LOCK_BIT         BIT(3)
+#define MAX96714_IO_CHK0               CCI_REG8(0x38)
+#define MAX96714_PATTERN_CLK_FREQ      GENMASK(1, 0)
+/* VID_RX */
+#define MAX96714_VIDEO_RX8             CCI_REG8(0x11a)
+#define MAX96714_VID_LOCK              BIT(6)
+
+/* VRX_PATGEN_0 */
+#define MAX96714_PATGEN_0              CCI_REG8(0x240)
+#define MAX96714_PATGEN_1              CCI_REG8(0x241)
+#define MAX96714_PATGEN_MODE           GENMASK(5, 4)
+#define MAX96714_PATGEN_VS_DLY         CCI_REG24(0x242)
+#define MAX96714_PATGEN_VS_HIGH        CCI_REG24(0x245)
+#define MAX96714_PATGEN_VS_LOW         CCI_REG24(0x248)
+#define MAX96714_PATGEN_V2H            CCI_REG24(0x24b)
+#define MAX96714_PATGEN_HS_HIGH        CCI_REG16(0x24e)
+#define MAX96714_PATGEN_HS_LOW         CCI_REG16(0x250)
+#define MAX96714_PATGEN_HS_CNT         CCI_REG16(0x252)
+#define MAX96714_PATGEN_V2D            CCI_REG24(0x254)
+#define MAX96714_PATGEN_DE_HIGH        CCI_REG16(0x257)
+#define MAX96714_PATGEN_DE_LOW         CCI_REG16(0x259)
+#define MAX96714_PATGEN_DE_CNT         CCI_REG16(0x25b)
+#define MAX96714_PATGEN_GRAD_INC       CCI_REG8(0x25d)
+#define MAX96714_PATGEN_CHKB_COLOR_A   CCI_REG24(0x25e)
+#define MAX96714_PATGEN_CHKB_COLOR_B   CCI_REG24(0x261)
+#define MAX96714_PATGEN_CHKB_RPT_CNT_A CCI_REG8(0x264)
+#define MAX96714_PATGEN_CHKB_RPT_CNT_B CCI_REG8(0x265)
+#define MAX96714_PATGEN_CHKB_ALT       CCI_REG8(0x266)
+/* BACKTOP */
+#define MAX96714_BACKTOP25             CCI_REG8(0x320)
+#define CSI_DPLL_FREQ_MASK             GENMASK(4, 0)
+
+/* MIPI_PHY */
+#define MAX96714_MIPI_PHY0             CCI_REG8(0x330)
+#define MAX96714_FORCE_CSI_OUT         BIT(7)
+#define MAX96714_MIPI_STDBY_N          CCI_REG8(0x332)
+#define MAX96714_MIPI_STDBY_MASK       GENMASK(5, 4)
+#define MAX96714_MIPI_LANE_MAP         CCI_REG8(0x333)
+#define MAX96714_MIPI_POLARITY         CCI_REG8(0x335)
+#define MAX96714_MIPI_POLARITY_MASK    GENMASK(5, 0)
+
+/* MIPI_TX */
+#define MAX96714_MIPI_LANE_CNT         CCI_REG8(0x44a)
+#define MAX96714_CSI2_LANE_CNT_MASK    GENMASK(7, 6)
+#define MAX96714_MIPI_TX52             CCI_REG8(0x474)
+#define MAX96714_TUN_EN                BIT(0)
+
+#define MHZ(v) ((u32)((v)  * 1000000U))
+
+enum max96714_vpg_mode {
+	MAX96714_VPG_DISABLED = 0,
+	MAX96714_VPG_CHECKERBOARD = 1,
+	MAX96714_VPG_GRADIENT = 2,
+};
+
+struct max96714_rxport {
+	struct {
+		struct v4l2_subdev   *sd;
+		u16                  pad;
+		struct fwnode_handle *ep_fwnode;
+	} source;
+	struct regulator	     *poc;
+};
+
+struct max96714_txport {
+	struct v4l2_fwnode_endpoint vep;
+};
+
+struct max96714_priv {
+	struct i2c_client                 *client;
+	struct regmap                     *regmap;
+	struct gpio_desc                  *pd_gpio;
+	struct max96714_rxport            rxport;
+	struct i2c_mux_core               *mux;
+	u64                               enabled_source_streams;
+	struct v4l2_subdev		  sd;
+	struct media_pad		  pads[MAX96714_NPORTS];
+	struct v4l2_mbus_config_mipi_csi2 mipi_csi2;
+	struct v4l2_ctrl_handler          ctrl_handler;
+	struct v4l2_async_notifier        notifier;
+	s64                               tx_link_freq;
+	enum max96714_vpg_mode            pattern;
+};
+
+static inline struct max96714_priv *sd_to_max96714(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max96714_priv, sd);
+}
+
+static int max96714_enable_tx_port(struct max96714_priv *priv)
+{
+	return cci_update_bits(priv->regmap, MAX96714_MIPI_STDBY_N,
+			       MAX96714_MIPI_STDBY_MASK,
+			       MAX96714_MIPI_STDBY_MASK, NULL);
+}
+
+static int max96714_disable_tx_port(struct max96714_priv *priv)
+{
+	return cci_update_bits(priv->regmap, MAX96714_MIPI_STDBY_N,
+			       MAX96714_MIPI_STDBY_MASK, 0, NULL);
+}
+
+static bool max96714_tx_port_enabled(struct max96714_priv *priv)
+{
+	u64 val;
+
+	cci_read(priv->regmap, MAX96714_MIPI_STDBY_N, &val, NULL);
+
+	return val & MAX96714_MIPI_STDBY_MASK;
+}
+
+static int max96714_apply_patgen_timing(struct max96714_priv *priv,
+					struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt =
+		v4l2_subdev_state_get_format(state, MAX96714_PAD_SOURCE);
+	const u32 h_active = fmt->width;
+	const u32 h_fp = 88;
+	const u32 h_sw = 44;
+	const u32 h_bp = 148;
+	u32 h_tot;
+	const u32 v_active = fmt->height;
+	const u32 v_fp = 4;
+	const u32 v_sw = 5;
+	const u32 v_bp = 36;
+	u32 v_tot;
+	int ret = 0;
+
+	h_tot = h_active + h_fp + h_sw + h_bp;
+	v_tot = v_active + v_fp + v_sw + v_bp;
+
+	/* 75 Mhz pixel clock */
+	cci_update_bits(priv->regmap, MAX96714_IO_CHK0,
+			MAX96714_PATTERN_CLK_FREQ, 1, &ret);
+
+	dev_info(&priv->client->dev, "height: %d width: %d\n", fmt->height,
+		 fmt->width);
+
+	cci_write(priv->regmap, MAX96714_PATGEN_VS_DLY, 0, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_VS_HIGH, v_sw * h_tot, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_VS_LOW,
+		  (v_active + v_fp + v_bp) * h_tot, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_HS_HIGH, h_sw, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_HS_LOW, h_active + h_fp + h_bp,
+		  &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_V2D,
+		  h_tot * (v_sw + v_bp) + (h_sw + h_bp), &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_HS_CNT, v_tot, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_DE_HIGH, h_active, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_DE_LOW, h_fp + h_sw + h_bp,
+		  &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_DE_CNT, v_active, &ret);
+	/* B G R */
+	cci_write(priv->regmap, MAX96714_PATGEN_CHKB_COLOR_A, 0xfecc00, &ret);
+	/* B G R */
+	cci_write(priv->regmap, MAX96714_PATGEN_CHKB_COLOR_B, 0x006aa7, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_CHKB_RPT_CNT_A, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_CHKB_RPT_CNT_B, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_CHKB_ALT, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96714_PATGEN_GRAD_INC, 0x10, &ret);
+
+	return ret;
+}
+
+static int max96714_apply_patgen(struct max96714_priv *priv,
+				 struct v4l2_subdev_state *state)
+{
+	unsigned int val;
+	int ret = 0;
+
+	if (priv->pattern)
+		ret = max96714_apply_patgen_timing(priv, state);
+
+	cci_write(priv->regmap, MAX96714_PATGEN_0, priv->pattern ? 0xfb : 0,
+		  &ret);
+
+	val = FIELD_PREP(MAX96714_PATGEN_MODE, priv->pattern);
+	cci_update_bits(priv->regmap, MAX96714_PATGEN_1, MAX96714_PATGEN_MODE,
+			val, &ret);
+	return ret;
+}
+
+static int max96714_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max96714_priv *priv =
+		container_of(ctrl->handler, struct max96714_priv, ctrl_handler);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		if (priv->enabled_source_streams)
+			return -EBUSY;
+		priv->pattern = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = cci_update_bits(priv->regmap, MAX96714_MIPI_PHY0,
+			      MAX96714_FORCE_CSI_OUT,
+			      priv->pattern ? MAX96714_FORCE_CSI_OUT : 0, NULL);
+
+	/* Pattern generator doesn't work with tunnel mode */
+	return cci_update_bits(priv->regmap, MAX96714_MIPI_TX52,
+			       MAX96714_TUN_EN,
+			       priv->pattern ? 0 : MAX96714_TUN_EN, &ret);
+}
+
+static const char * const max96714_test_pattern[] = {
+	"Disabled",
+	"Checkerboard",
+	"Gradient"
+};
+
+static const struct v4l2_ctrl_ops max96714_ctrl_ops = {
+	.s_ctrl = max96714_s_ctrl,
+};
+
+static int max96714_enable_streams(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   u32 source_pad, u64 streams_mask)
+{
+	struct max96714_priv *priv = sd_to_max96714(sd);
+	u64 sink_streams;
+	int ret;
+
+	if (!priv->enabled_source_streams)
+		max96714_enable_tx_port(priv);
+
+	ret = max96714_apply_patgen(priv, state);
+	if (ret)
+		goto err;
+
+	if (!priv->pattern) {
+		if (!priv->rxport.source.sd) {
+			ret = -ENODEV;
+			goto err;
+		}
+
+		sink_streams =
+			v4l2_subdev_state_xlate_streams(state,
+							MAX96714_PAD_SOURCE,
+							MAX96714_PAD_SINK,
+							&streams_mask);
+
+		ret = v4l2_subdev_enable_streams(priv->rxport.source.sd,
+						 priv->rxport.source.pad,
+						 sink_streams);
+		if (ret)
+			goto err;
+	}
+
+	priv->enabled_source_streams |= streams_mask;
+
+	return 0;
+
+err:
+	if (!priv->enabled_source_streams)
+		max96714_disable_tx_port(priv);
+
+	return ret;
+}
+
+static int max96714_disable_streams(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    u32 source_pad, u64 streams_mask)
+{
+	struct max96714_priv *priv = sd_to_max96714(sd);
+	u64 sink_streams;
+
+	if (!priv->pattern) {
+		int ret;
+
+		sink_streams =
+			v4l2_subdev_state_xlate_streams(state,
+							MAX96714_PAD_SOURCE,
+							MAX96714_PAD_SINK,
+							&streams_mask);
+
+		ret = v4l2_subdev_disable_streams(priv->rxport.source.sd,
+						  priv->rxport.source.pad,
+						  sink_streams);
+		if (ret)
+			return ret;
+	}
+
+	priv->enabled_source_streams &= ~streams_mask;
+
+	if (!priv->enabled_source_streams)
+		max96714_disable_tx_port(priv);
+
+	return 0;
+}
+
+static int max96714_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state,
+			    struct v4l2_subdev_format *format)
+{
+	struct max96714_priv *priv = sd_to_max96714(sd);
+	struct v4l2_mbus_framefmt *fmt;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    priv->enabled_source_streams)
+		return -EBUSY;
+
+	/* No transcoding, source and sink formats must match. */
+	if (format->pad == MAX96714_PAD_SOURCE)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int _max96714_set_routing(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 enum v4l2_subdev_format_whence which,
+				 struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 1280,
+		.height = 1080,
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.field = V4L2_FIELD_NONE,
+	};
+	int ret;
+
+	/*
+	 * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
+	 * frame desc is made dynamically allocated.
+	 */
+	if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
+		return -EINVAL;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	return v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+}
+
+static int max96714_set_routing(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				enum v4l2_subdev_format_whence which,
+				struct v4l2_subdev_krouting *routing)
+{
+	struct max96714_priv *priv = sd_to_max96714(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
+		return -EBUSY;
+
+	return _max96714_set_routing(sd, state, which, routing);
+}
+
+static int max96714_init_state(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = MAX96714_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = MAX96714_PAD_SOURCE,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		}
+	};
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return _max96714_set_routing(sd, state, V4L2_SUBDEV_FORMAT_ACTIVE,
+				     &routing);
+}
+
+static const struct v4l2_subdev_pad_ops max96714_pad_ops = {
+	.enable_streams = max96714_enable_streams,
+	.disable_streams = max96714_disable_streams,
+
+	.set_routing = max96714_set_routing,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = max96714_set_fmt,
+};
+
+static bool max96714_link_locked(struct max96714_priv *priv)
+{
+	u64 val = 0;
+
+	cci_read(priv->regmap, MAX96714_LINK_LOCK, &val, NULL);
+
+	return val & MAX96714_LINK_LOCK_BIT;
+}
+
+static void max96714_link_status(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_info(dev, "Link locked:%d\n", max96714_link_locked(priv));
+}
+
+static bool max96714_pipe_locked(struct max96714_priv *priv)
+{
+	u64 val;
+
+	cci_read(priv->regmap, MAX96714_VIDEO_RX8, &val, NULL);
+
+	return val & MAX96714_VID_LOCK;
+}
+
+static void max96714_pipe_status(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+
+	dev_info(dev, "Pipe vidlock:%d\n", max96714_pipe_locked(priv));
+}
+
+static void max96714_csi_status(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	u64 freq = 0;
+
+	cci_read(priv->regmap, MAX96714_BACKTOP25, &freq, NULL);
+	freq = FIELD_GET(CSI_DPLL_FREQ_MASK, freq);
+
+	dev_info(dev, "CSI controller DPLL freq:%u00MHz CSIPHY enabled:%d\n",
+		 (u8)freq, max96714_tx_port_enabled(priv));
+}
+
+static int max96714_log_status(struct v4l2_subdev *sd)
+{
+	struct max96714_priv *priv = sd_to_max96714(sd);
+	struct device *dev = &priv->client->dev;
+
+	dev_info(dev, "Deserializer: max96714\n");
+
+	max96714_link_status(priv);
+	max96714_pipe_status(priv);
+	max96714_csi_status(priv);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops max96714_subdev_core_ops = {
+	.log_status = max96714_log_status,
+};
+
+static const struct v4l2_subdev_video_ops max96714_video_ops = {
+	.s_stream	= v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_internal_ops max96714_internal_ops = {
+	.init_state = max96714_init_state,
+};
+
+static const struct v4l2_subdev_ops max96714_subdev_ops = {
+	.video = &max96714_video_ops,
+	.core = &max96714_subdev_core_ops,
+	.pad = &max96714_pad_ops,
+};
+
+static const struct media_entity_operations max96714_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int max96714_notify_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *subdev,
+				 struct v4l2_async_connection *asd)
+{
+	struct max96714_priv *priv = sd_to_max96714(notifier->sd);
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&subdev->entity,
+					  priv->rxport.source.ep_fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to find pad for %s\n", subdev->name);
+		return ret;
+	}
+
+	priv->rxport.source.sd = subdev;
+	priv->rxport.source.pad = ret;
+
+	ret = media_create_pad_link(&priv->rxport.source.sd->entity,
+				    priv->rxport.source.pad, &priv->sd.entity,
+				    MAX96714_PAD_SINK,
+				    MEDIA_LNK_FL_ENABLED |
+				    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(dev, "Unable to link %s:%u -> %s:%u\n",
+			priv->rxport.source.sd->name, priv->rxport.source.pad,
+			priv->sd.name, MAX96714_PAD_SINK);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations max96714_notify_ops = {
+	.bound = max96714_notify_bound,
+};
+
+static int max96714_v4l2_notifier_register(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct max96714_rxport *rxport = &priv->rxport;
+	struct v4l2_async_connection *asd;
+	int ret;
+
+	if (!rxport->source.ep_fwnode)
+		return 0;
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	asd = v4l2_async_nf_add_fwnode(&priv->notifier,
+				       rxport->source.ep_fwnode,
+				       struct v4l2_async_connection);
+	if (IS_ERR(asd)) {
+		dev_err(dev, "Failed to add subdev: %pe", asd);
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return PTR_ERR(asd);
+	}
+
+	priv->notifier.ops = &max96714_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max96714_create_subdev(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &max96714_subdev_ops);
+	priv->sd.internal_ops = &max96714_internal_ops;
+
+	v4l2_ctrl_handler_init(&priv->ctrl_handler, 1);
+	priv->sd.ctrl_handler = &priv->ctrl_handler;
+
+	v4l2_ctrl_new_int_menu(&priv->ctrl_handler, NULL, V4L2_CID_LINK_FREQ,
+			       0, 0, &priv->tx_link_freq);
+	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler,
+				     &max96714_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(max96714_test_pattern) - 1,
+				     0, 0, max96714_test_pattern);
+	if (priv->ctrl_handler.error) {
+		ret = priv->ctrl_handler.error;
+		goto err_free_ctrl;
+	}
+
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &max96714_entity_ops;
+
+	priv->pads[MAX96714_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	priv->pads[MAX96714_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&priv->sd.entity,
+				     MAX96714_NPORTS,
+				     priv->pads);
+	if (ret)
+		goto err_free_ctrl;
+
+	priv->sd.state_lock = priv->sd.ctrl_handler->lock;
+
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = max96714_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err(dev, "v4l2 subdev notifier register failed: %d\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret) {
+		dev_err(dev, "v4l2_async_register_subdev error: %d\n", ret);
+		goto err_unreg_notif;
+	}
+
+	return 0;
+
+err_unreg_notif:
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&priv->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&priv->sd.entity);
+err_free_ctrl:
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+
+	return ret;
+};
+
+static void max96714_destroy_subdev(struct max96714_priv *priv)
+{
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+	v4l2_async_unregister_subdev(&priv->sd);
+
+	v4l2_subdev_cleanup(&priv->sd);
+
+	media_entity_cleanup(&priv->sd.entity);
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+}
+
+static int max96714_i2c_mux_select(struct i2c_mux_core *mux, u32 chan)
+{
+	return 0;
+}
+
+static int max96714_i2c_mux_init(struct max96714_priv *priv)
+{
+	priv->mux = i2c_mux_alloc(priv->client->adapter, &priv->client->dev,
+				  1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
+				  max96714_i2c_mux_select, NULL);
+	if (!priv->mux)
+		return -ENOMEM;
+
+	return i2c_mux_add_adapter(priv->mux, 0, 0);
+}
+
+static int max96714_init_tx_port(struct max96714_priv *priv)
+{
+	struct v4l2_mbus_config_mipi_csi2 *mipi;
+	unsigned long lanes_used = 0;
+	unsigned int val, lane;
+	int ret;
+
+	ret = max96714_disable_tx_port(priv);
+
+	mipi = &priv->mipi_csi2;
+	val = div_u64(priv->tx_link_freq * 2, MHZ(100));
+
+	cci_update_bits(priv->regmap, MAX96714_BACKTOP25,
+			CSI_DPLL_FREQ_MASK, val, &ret);
+
+	val = FIELD_PREP(MAX96714_CSI2_LANE_CNT_MASK, mipi->num_data_lanes - 1);
+	cci_update_bits(priv->regmap, MAX96714_MIPI_LANE_CNT,
+			MAX96714_CSI2_LANE_CNT_MASK, val, &ret);
+
+	/* lanes polarity */
+	val = 0;
+	for (lane = 0; lane < mipi->num_data_lanes + 1; lane++) {
+		if (!mipi->lane_polarities[lane])
+			continue;
+		if (lane == 0)
+			/* clock lane */
+			val |= BIT(5);
+		else if (lane < 3)
+			/* Lane D0 and D1 */
+			val |= BIT(lane - 1);
+		else
+			/* D2 and D3 */
+			val |= BIT(lane);
+	}
+
+	cci_update_bits(priv->regmap, MAX96714_MIPI_POLARITY,
+			MAX96714_MIPI_POLARITY_MASK, val, &ret);
+
+	/* lanes mapping */
+	val = 0;
+	for (lane = 0; lane < mipi->num_data_lanes; lane++) {
+		val |= (mipi->data_lanes[lane] - 1) << (lane * 2);
+		lanes_used |= BIT(mipi->data_lanes[lane] - 1);
+	}
+
+	/*
+	 * Unused lanes need to be mapped as well to not have
+	 * the same lanes mapped twice.
+	 */
+	for (; lane < MAX96714_CSI_NLANES; lane++) {
+		unsigned int idx = find_first_zero_bit(&lanes_used,
+						       MAX96714_CSI_NLANES);
+
+		val |= idx << (lane * 2);
+		lanes_used |= BIT(idx);
+	}
+
+	return cci_write(priv->regmap, MAX96714_MIPI_LANE_MAP, val, &ret);
+}
+
+static int max96714_rxport_enable_poc(struct max96714_priv *priv)
+{
+	struct max96714_rxport *rxport = &priv->rxport;
+
+	if (!rxport->poc)
+		return 0;
+
+	return regulator_enable(rxport->poc);
+}
+
+static int max96714_rxport_disable_poc(struct max96714_priv *priv)
+{
+	struct max96714_rxport *rxport = &priv->rxport;
+
+	if (!rxport->poc)
+		return 0;
+
+	return regulator_disable(rxport->poc);
+}
+
+static int max96714_parse_dt_txport(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct fwnode_handle *ep_fwnode;
+	u32 num_data_lanes;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    MAX96714_PAD_SOURCE, 0, 0);
+	if (!ep_fwnode)
+		return -EINVAL;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep_fwnode, &vep);
+	fwnode_handle_put(ep_fwnode);
+	if (ret) {
+		dev_err(dev, "tx: failed to parse endpoint data\n");
+		return -EINVAL;
+	}
+
+	if (vep.nr_of_link_frequencies != 1) {
+		ret = -EINVAL;
+		goto err_free_vep;
+	}
+
+	priv->tx_link_freq = vep.link_frequencies[0];
+	/* Min 50MHz, Max 1250MHz, 50MHz step */
+	if (priv->tx_link_freq < MHZ(50) || priv->tx_link_freq > MHZ(1250) ||
+	    (u32)priv->tx_link_freq % MHZ(50)) {
+		dev_err(dev, "tx: invalid link frequency\n");
+		ret = -EINVAL;
+		goto err_free_vep;
+	}
+
+	num_data_lanes = vep.bus.mipi_csi2.num_data_lanes;
+	if (num_data_lanes < 1 || num_data_lanes > MAX96714_CSI_NLANES) {
+		dev_err(dev,
+			"tx: invalid number of data lanes must be 1 to 4\n");
+		ret = -EINVAL;
+		goto err_free_vep;
+	}
+
+	priv->mipi_csi2 = vep.bus.mipi_csi2;
+
+err_free_vep:
+	v4l2_fwnode_endpoint_free(&vep);
+
+	return ret;
+}
+
+static int max96714_parse_dt_rxport(struct max96714_priv *priv)
+{
+	static const char *poc_name = "port0-poc";
+	struct max96714_rxport *rxport = &priv->rxport;
+	struct device *dev = &priv->client->dev;
+	struct fwnode_handle *ep_fwnode;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    MAX96714_PAD_SINK, 0, 0);
+	if (!ep_fwnode)
+		return -ENOENT;
+
+	rxport->source.ep_fwnode = fwnode_graph_get_remote_endpoint(ep_fwnode);
+	fwnode_handle_put(ep_fwnode);
+
+	if (!rxport->source.ep_fwnode) {
+		dev_err(dev, "rx: no remote endpoint\n");
+		return -EINVAL;
+	}
+
+	rxport->poc = devm_regulator_get_optional(dev, poc_name);
+	if (IS_ERR(rxport->poc)) {
+		ret = PTR_ERR(rxport->poc);
+		if (ret == -ENODEV) {
+			rxport->poc = NULL;
+		} else {
+			dev_err(dev, "rx: failed to get POC supply: %d\n", ret);
+			goto err_put_source_ep_fwnode;
+		}
+	}
+
+	return 0;
+
+err_put_source_ep_fwnode:
+	fwnode_handle_put(rxport->source.ep_fwnode);
+	return ret;
+}
+
+static int max96714_parse_dt(struct max96714_priv *priv)
+{
+	int ret;
+
+	ret = max96714_parse_dt_txport(priv);
+	if (ret)
+		return ret;
+
+	ret = max96714_parse_dt_rxport(priv);
+	/*
+	 * The deserializer can create a test pattern even if the
+	 * rx port is not connected to a serializer.
+	 */
+	if (ret && ret == -ENOENT)
+		ret = 0;
+
+	return ret;
+}
+
+static int max96714_enable_core_hw(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	u64 val;
+	int ret;
+
+	if (priv->pd_gpio) {
+		/* wait min 2 ms for reset to complete */
+		gpiod_set_value_cansleep(priv->pd_gpio, 1);
+		fsleep(2000);
+		gpiod_set_value_cansleep(priv->pd_gpio, 0);
+		/* wait min 2 ms for power up to finish */
+		fsleep(2000);
+	}
+
+	ret = cci_read(priv->regmap, MAX96714_REG13, &val, NULL);
+	if (ret) {
+		dev_err_probe(dev, ret, "Cannot read first register, abort\n");
+		goto err_pd_gpio;
+	}
+
+	if (val != MAX96714_DEVICE_ID && val != MAX96714F_DEVICE_ID) {
+		dev_err(dev, "Unsupported device id expected %x got %x\n",
+			MAX96714F_DEVICE_ID, (u8)val);
+		ret = -EOPNOTSUPP;
+		goto err_pd_gpio;
+	}
+
+	ret = cci_read(priv->regmap, MAX96714_DEV_REV, &val, NULL);
+	if (ret)
+		goto err_pd_gpio;
+
+	dev_dbg(dev, "Found %x (rev %lx)\n", MAX96714F_DEVICE_ID,
+		(u8)val & MAX96714_DEV_REV_MASK);
+
+	ret = cci_read(priv->regmap, MAX96714_MIPI_TX52, &val, NULL);
+	if (ret)
+		goto err_pd_gpio;
+
+	if (!(val & MAX96714_TUN_EN)) {
+		dev_err(dev, "Only supporting tunnel mode");
+		ret = -EOPNOTSUPP;
+		goto err_pd_gpio;
+	}
+
+	return 0;
+
+err_pd_gpio:
+	gpiod_set_value_cansleep(priv->pd_gpio, 1);
+	return ret;
+}
+
+static void max96714_disable_core_hw(struct max96714_priv *priv)
+{
+	gpiod_set_value_cansleep(priv->pd_gpio, 1);
+}
+
+static int max96714_get_hw_resources(struct max96714_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+
+	priv->regmap = devm_cci_regmap_init_i2c(priv->client, 16);
+	if (IS_ERR(priv->regmap))
+		return PTR_ERR(priv->regmap);
+
+	priv->pd_gpio =
+		devm_gpiod_get_optional(dev, "powerdown", GPIOD_OUT_HIGH);
+	if (IS_ERR(priv->pd_gpio))
+		return dev_err_probe(dev, PTR_ERR(priv->pd_gpio),
+				     "Cannot get powerdown GPIO\n");
+	return 0;
+}
+
+static int max96714_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct max96714_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+
+	ret = max96714_get_hw_resources(priv);
+	if (ret)
+		return ret;
+
+	ret = max96714_enable_core_hw(priv);
+	if (ret)
+		return ret;
+
+	ret = max96714_parse_dt(priv);
+	if (ret)
+		goto err_disable_core_hw;
+
+	max96714_init_tx_port(priv);
+
+	ret = max96714_rxport_enable_poc(priv);
+	if (ret)
+		goto err_free_ports;
+
+	ret = max96714_i2c_mux_init(priv);
+	if (ret)
+		goto err_disable_poc;
+
+	ret = max96714_create_subdev(priv);
+	if (ret)
+		goto err_del_mux;
+
+	return 0;
+
+err_del_mux:
+	i2c_mux_del_adapters(priv->mux);
+err_disable_poc:
+	max96714_rxport_disable_poc(priv);
+err_free_ports:
+	fwnode_handle_put(priv->rxport.source.ep_fwnode);
+err_disable_core_hw:
+	max96714_disable_core_hw(priv);
+
+	return ret;
+}
+
+static void max96714_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct max96714_priv *priv = sd_to_max96714(sd);
+
+	max96714_destroy_subdev(priv);
+	i2c_mux_del_adapters(priv->mux);
+	max96714_rxport_disable_poc(priv);
+	fwnode_handle_put(priv->rxport.source.ep_fwnode);
+	max96714_disable_core_hw(priv);
+	gpiod_set_value_cansleep(priv->pd_gpio, 1);
+}
+
+static const struct of_device_id max96714_of_ids[] = {
+	{ .compatible = "maxim,max96714f" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max96714_of_ids);
+
+static struct i2c_driver max96714_i2c_driver = {
+	.driver	= {
+		.name		= "max96714",
+		.of_match_table	= max96714_of_ids,
+	},
+	.probe		= max96714_probe,
+	.remove		= max96714_remove,
+};
+
+module_i2c_driver(max96714_i2c_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Maxim Integrated GMSL2 Deserializers Driver");
+MODULE_AUTHOR("Julien Massot <julien.massot@collabora.com>");
diff --git a/drivers/media/i2c/max96717.c b/drivers/media/i2c/max96717.c
new file mode 100644
index 000000000000..3746729366ac
--- /dev/null
+++ b/drivers/media/i2c/max96717.c
@@ -0,0 +1,1103 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Maxim GMSL2 Serializer Driver
+ *
+ * Copyright (C) 2024 Collabora Ltd.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/gpio/driver.h>
+#include <linux/i2c-mux.h>
+#include <linux/i2c.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define MAX96717_DEVICE_ID  0xbf
+#define MAX96717F_DEVICE_ID 0xc8
+#define MAX96717_PORTS      2
+#define MAX96717_PAD_SINK   0
+#define MAX96717_PAD_SOURCE 1
+#define MAX96717_CSI_NLANES 4
+
+#define MAX96717_DEFAULT_CLKOUT_RATE	24000000UL
+
+/* DEV */
+#define MAX96717_REG3    CCI_REG8(0x3)
+#define MAX96717_RCLKSEL GENMASK(1, 0)
+#define RCLKSEL_REF_PLL  CCI_REG8(0x3)
+#define MAX96717_REG6    CCI_REG8(0x6)
+#define RCLKEN           BIT(5)
+#define MAX96717_DEV_ID  CCI_REG8(0xd)
+#define MAX96717_DEV_REV CCI_REG8(0xe)
+#define MAX96717_DEV_REV_MASK GENMASK(3, 0)
+
+/* VID_TX Z */
+#define MAX96717_VIDEO_TX0 CCI_REG8(0x110)
+#define MAX96717_VIDEO_AUTO_BPP BIT(3)
+#define MAX96717_VIDEO_TX2 CCI_REG8(0x112)
+#define MAX96717_VIDEO_PCLKDET BIT(7)
+
+/* VTX_Z */
+#define MAX96717_VTX0                  CCI_REG8(0x24e)
+#define MAX96717_VTX1                  CCI_REG8(0x24f)
+#define MAX96717_PATTERN_CLK_FREQ      GENMASK(3, 1)
+#define MAX96717_VTX_VS_DLY            CCI_REG24(0x250)
+#define MAX96717_VTX_VS_HIGH           CCI_REG24(0x253)
+#define MAX96717_VTX_VS_LOW            CCI_REG24(0x256)
+#define MAX96717_VTX_V2H               CCI_REG24(0x259)
+#define MAX96717_VTX_HS_HIGH           CCI_REG16(0x25c)
+#define MAX96717_VTX_HS_LOW            CCI_REG16(0x25e)
+#define MAX96717_VTX_HS_CNT            CCI_REG16(0x260)
+#define MAX96717_VTX_V2D               CCI_REG24(0x262)
+#define MAX96717_VTX_DE_HIGH           CCI_REG16(0x265)
+#define MAX96717_VTX_DE_LOW            CCI_REG16(0x267)
+#define MAX96717_VTX_DE_CNT            CCI_REG16(0x269)
+#define MAX96717_VTX29                 CCI_REG8(0x26b)
+#define MAX96717_VTX_MODE              GENMASK(1, 0)
+#define MAX96717_VTX_GRAD_INC          CCI_REG8(0x26c)
+#define MAX96717_VTX_CHKB_COLOR_A      CCI_REG24(0x26d)
+#define MAX96717_VTX_CHKB_COLOR_B      CCI_REG24(0x270)
+#define MAX96717_VTX_CHKB_RPT_CNT_A    CCI_REG8(0x273)
+#define MAX96717_VTX_CHKB_RPT_CNT_B    CCI_REG8(0x274)
+#define MAX96717_VTX_CHKB_ALT          CCI_REG8(0x275)
+
+/* GPIO */
+#define MAX96717_NUM_GPIO         11
+#define MAX96717_GPIO_REG_A(gpio) CCI_REG8(0x2be + (gpio) * 3)
+#define MAX96717_GPIO_OUT         BIT(4)
+#define MAX96717_GPIO_IN          BIT(3)
+#define MAX96717_GPIO_RX_EN       BIT(2)
+#define MAX96717_GPIO_TX_EN       BIT(1)
+#define MAX96717_GPIO_OUT_DIS     BIT(0)
+
+/* FRONTTOP */
+/* MAX96717 only have CSI port 'B' */
+#define MAX96717_FRONTOP0     CCI_REG8(0x308)
+#define MAX96717_START_PORT_B BIT(5)
+
+/* MIPI_RX */
+#define MAX96717_MIPI_RX1       CCI_REG8(0x331)
+#define MAX96717_MIPI_LANES_CNT GENMASK(5, 4)
+#define MAX96717_MIPI_RX2       CCI_REG8(0x332) /* phy1 Lanes map */
+#define MAX96717_PHY2_LANES_MAP GENMASK(7, 4)
+#define MAX96717_MIPI_RX3       CCI_REG8(0x333) /* phy2 Lanes map */
+#define MAX96717_PHY1_LANES_MAP GENMASK(3, 0)
+#define MAX96717_MIPI_RX4       CCI_REG8(0x334) /* phy1 lane polarities */
+#define MAX96717_PHY1_LANES_POL GENMASK(6, 4)
+#define MAX96717_MIPI_RX5       CCI_REG8(0x335) /* phy2 lane polarities */
+#define MAX96717_PHY2_LANES_POL GENMASK(2, 0)
+
+/* MIPI_RX_EXT */
+#define MAX96717_MIPI_RX_EXT11 CCI_REG8(0x383)
+#define MAX96717_TUN_MODE      BIT(7)
+
+/* REF_VTG */
+#define REF_VTG0                CCI_REG8(0x3f0)
+#define REFGEN_PREDEF_EN        BIT(6)
+#define REFGEN_PREDEF_FREQ_MASK GENMASK(5, 4)
+#define REFGEN_PREDEF_FREQ_ALT  BIT(3)
+#define REFGEN_RST              BIT(1)
+#define REFGEN_EN               BIT(0)
+
+/* MISC */
+#define PIO_SLEW_1 CCI_REG8(0x570)
+
+enum max96717_vpg_mode {
+	MAX96717_VPG_DISABLED = 0,
+	MAX96717_VPG_CHECKERBOARD = 1,
+	MAX96717_VPG_GRADIENT = 2,
+};
+
+struct max96717_priv {
+	struct i2c_client		  *client;
+	struct regmap			  *regmap;
+	struct i2c_mux_core		  *mux;
+	struct v4l2_mbus_config_mipi_csi2 mipi_csi2;
+	struct v4l2_subdev                sd;
+	struct media_pad                  pads[MAX96717_PORTS];
+	struct v4l2_ctrl_handler          ctrl_handler;
+	struct v4l2_async_notifier        notifier;
+	struct v4l2_subdev                *source_sd;
+	u16                               source_sd_pad;
+	u64			          enabled_source_streams;
+	u8                                pll_predef_index;
+	struct clk_hw                     clk_hw;
+	struct gpio_chip                  gpio_chip;
+	enum max96717_vpg_mode            pattern;
+};
+
+static inline struct max96717_priv *sd_to_max96717(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct max96717_priv, sd);
+}
+
+static inline struct max96717_priv *clk_hw_to_max96717(struct clk_hw *hw)
+{
+	return container_of(hw, struct max96717_priv, clk_hw);
+}
+
+static int max96717_i2c_mux_select(struct i2c_mux_core *mux, u32 chan)
+{
+	return 0;
+}
+
+static int max96717_i2c_mux_init(struct max96717_priv *priv)
+{
+	priv->mux = i2c_mux_alloc(priv->client->adapter, &priv->client->dev,
+				  1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
+				  max96717_i2c_mux_select, NULL);
+	if (!priv->mux)
+		return -ENOMEM;
+
+	return i2c_mux_add_adapter(priv->mux, 0, 0);
+}
+
+static inline int max96717_start_csi(struct max96717_priv *priv, bool start)
+{
+	return cci_update_bits(priv->regmap, MAX96717_FRONTOP0,
+			       MAX96717_START_PORT_B,
+			       start ? MAX96717_START_PORT_B : 0, NULL);
+}
+
+static int max96717_apply_patgen_timing(struct max96717_priv *priv,
+					struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt =
+		v4l2_subdev_state_get_format(state, MAX96717_PAD_SOURCE);
+	const u32 h_active = fmt->width;
+	const u32 h_fp = 88;
+	const u32 h_sw = 44;
+	const u32 h_bp = 148;
+	u32 h_tot;
+	const u32 v_active = fmt->height;
+	const u32 v_fp = 4;
+	const u32 v_sw = 5;
+	const u32 v_bp = 36;
+	u32 v_tot;
+	int ret = 0;
+
+	h_tot = h_active + h_fp + h_sw + h_bp;
+	v_tot = v_active + v_fp + v_sw + v_bp;
+
+	/* 75 Mhz pixel clock */
+	cci_update_bits(priv->regmap, MAX96717_VTX1,
+			MAX96717_PATTERN_CLK_FREQ, 0xa, &ret);
+
+	dev_info(&priv->client->dev, "height: %d width: %d\n", fmt->height,
+		 fmt->width);
+
+	cci_write(priv->regmap, MAX96717_VTX_VS_DLY, 0, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_VS_HIGH, v_sw * h_tot, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_VS_LOW,
+		  (v_active + v_fp + v_bp) * h_tot, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_HS_HIGH, h_sw, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_HS_LOW, h_active + h_fp + h_bp,
+		  &ret);
+	cci_write(priv->regmap, MAX96717_VTX_V2D,
+		  h_tot * (v_sw + v_bp) + (h_sw + h_bp), &ret);
+	cci_write(priv->regmap, MAX96717_VTX_HS_CNT, v_tot, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_DE_HIGH, h_active, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_DE_LOW, h_fp + h_sw + h_bp,
+		  &ret);
+	cci_write(priv->regmap, MAX96717_VTX_DE_CNT, v_active, &ret);
+	/* B G R */
+	cci_write(priv->regmap, MAX96717_VTX_CHKB_COLOR_A, 0xfecc00, &ret);
+	/* B G R */
+	cci_write(priv->regmap, MAX96717_VTX_CHKB_COLOR_B, 0x006aa7, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_CHKB_RPT_CNT_A, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_CHKB_RPT_CNT_B, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_CHKB_ALT, 0x3c, &ret);
+	cci_write(priv->regmap, MAX96717_VTX_GRAD_INC, 0x10, &ret);
+
+	return ret;
+}
+
+static int max96717_apply_patgen(struct max96717_priv *priv,
+				 struct v4l2_subdev_state *state)
+{
+	unsigned int val;
+	int ret = 0;
+
+	if (priv->pattern)
+		ret = max96717_apply_patgen_timing(priv, state);
+
+	cci_write(priv->regmap, MAX96717_VTX0, priv->pattern ? 0xfb : 0,
+		  &ret);
+
+	val = FIELD_PREP(MAX96717_VTX_MODE, priv->pattern);
+	cci_update_bits(priv->regmap, MAX96717_VTX29, MAX96717_VTX_MODE,
+			val, &ret);
+	return ret;
+}
+
+static int max96717_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct max96717_priv *priv =
+		container_of(ctrl->handler, struct max96717_priv, ctrl_handler);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_TEST_PATTERN:
+		if (priv->enabled_source_streams)
+			return -EBUSY;
+		priv->pattern = ctrl->val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Use bpp from bpp register */
+	ret = cci_update_bits(priv->regmap, MAX96717_VIDEO_TX0,
+			      MAX96717_VIDEO_AUTO_BPP,
+			      priv->pattern ? 0 : MAX96717_VIDEO_AUTO_BPP,
+			      NULL);
+
+	/*
+	 * Pattern generator doesn't work with tunnel mode.
+	 * Needs RGB color format and deserializer tunnel mode must be disabled.
+	 */
+	return cci_update_bits(priv->regmap, MAX96717_MIPI_RX_EXT11,
+			       MAX96717_TUN_MODE,
+			       priv->pattern ? 0 : MAX96717_TUN_MODE, &ret);
+}
+
+static const char * const max96717_test_pattern[] = {
+	"Disabled",
+	"Checkerboard",
+	"Gradient"
+};
+
+static const struct v4l2_ctrl_ops max96717_ctrl_ops = {
+	.s_ctrl = max96717_s_ctrl,
+};
+
+static int max96717_gpiochip_get(struct gpio_chip *gpiochip,
+				 unsigned int offset)
+{
+	struct max96717_priv *priv = gpiochip_get_data(gpiochip);
+	u64 val;
+	int ret;
+
+	ret = cci_read(priv->regmap, MAX96717_GPIO_REG_A(offset),
+		       &val, NULL);
+	if (ret)
+		return ret;
+
+	if (val & MAX96717_GPIO_OUT_DIS)
+		return !!(val & MAX96717_GPIO_IN);
+	else
+		return !!(val & MAX96717_GPIO_OUT);
+}
+
+static void max96717_gpiochip_set(struct gpio_chip *gpiochip,
+				  unsigned int offset, int value)
+{
+	struct max96717_priv *priv = gpiochip_get_data(gpiochip);
+
+	cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
+			MAX96717_GPIO_OUT, MAX96717_GPIO_OUT, NULL);
+}
+
+static int max96717_gpio_get_direction(struct gpio_chip *gpiochip,
+				       unsigned int offset)
+{
+	struct max96717_priv *priv = gpiochip_get_data(gpiochip);
+	u64 val;
+	int ret;
+
+	ret = cci_read(priv->regmap, MAX96717_GPIO_REG_A(offset), &val, NULL);
+	if (ret < 0)
+		return ret;
+
+	return !!(val & MAX96717_GPIO_OUT_DIS);
+}
+
+static int max96717_gpio_direction_out(struct gpio_chip *gpiochip,
+				       unsigned int offset, int value)
+{
+	struct max96717_priv *priv = gpiochip_get_data(gpiochip);
+
+	return cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
+			       MAX96717_GPIO_OUT_DIS | MAX96717_GPIO_OUT,
+			       value ? MAX96717_GPIO_OUT : 0, NULL);
+}
+
+static int max96717_gpio_direction_in(struct gpio_chip *gpiochip,
+				      unsigned int offset)
+{
+	struct max96717_priv *priv = gpiochip_get_data(gpiochip);
+
+	return cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
+			       MAX96717_GPIO_OUT_DIS, MAX96717_GPIO_OUT_DIS,
+			       NULL);
+}
+
+static int max96717_gpiochip_probe(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct gpio_chip *gc = &priv->gpio_chip;
+	int i, ret = 0;
+
+	gc->label = dev_name(dev);
+	gc->parent = dev;
+	gc->owner = THIS_MODULE;
+	gc->ngpio = MAX96717_NUM_GPIO;
+	gc->base = -1;
+	gc->can_sleep = true;
+	gc->get_direction = max96717_gpio_get_direction;
+	gc->direction_input = max96717_gpio_direction_in;
+	gc->direction_output = max96717_gpio_direction_out;
+	gc->set = max96717_gpiochip_set;
+	gc->get = max96717_gpiochip_get;
+	gc->of_gpio_n_cells = 2;
+
+	/* Disable GPIO forwarding */
+	for (i = 0; i < gc->ngpio; i++)
+		cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(i),
+				MAX96717_GPIO_RX_EN | MAX96717_GPIO_TX_EN,
+				0, &ret);
+
+	if (ret)
+		return ret;
+
+	ret = devm_gpiochip_add_data(dev, gc, priv);
+	if (ret) {
+		dev_err(dev, "Unable to create gpio_chip\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int _max96717_set_routing(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 1280,
+		.height = 1080,
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.field = V4L2_FIELD_NONE,
+	};
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int max96717_set_routing(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				enum v4l2_subdev_format_whence which,
+				struct v4l2_subdev_krouting *routing)
+{
+	struct max96717_priv *priv = sd_to_max96717(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
+		return -EBUSY;
+
+	return _max96717_set_routing(sd, state, routing);
+}
+
+static int max96717_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state,
+			    struct v4l2_subdev_format *format)
+{
+	struct max96717_priv *priv = sd_to_max96717(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	u64 stream_source_mask;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    priv->enabled_source_streams)
+		return -EBUSY;
+
+	/* No transcoding, source and sink formats must match. */
+	if (format->pad == MAX96717_PAD_SOURCE)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	/* Set sink format */
+	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	/* Propagate to source format */
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+	*fmt = format->format;
+
+	stream_source_mask = BIT(format->stream);
+
+	return v4l2_subdev_state_xlate_streams(state, MAX96717_PAD_SOURCE,
+					       MAX96717_PAD_SINK,
+					       &stream_source_mask);
+}
+
+static int max96717_init_state(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = MAX96717_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = MAX96717_PAD_SOURCE,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	return _max96717_set_routing(sd, state, &routing);
+}
+
+static bool max96717_pipe_pclkdet(struct max96717_priv *priv)
+{
+	u64 val = 0;
+
+	cci_read(priv->regmap, MAX96717_VIDEO_TX2, &val, NULL);
+
+	return val & MAX96717_VIDEO_PCLKDET;
+}
+
+static int max96717_log_status(struct v4l2_subdev *sd)
+{
+	struct max96717_priv *priv = sd_to_max96717(sd);
+	struct device *dev = &priv->client->dev;
+
+	dev_info(dev, "Serializer: max96717\n");
+	dev_info(dev, "Pipe: pclkdet:%d\n", max96717_pipe_pclkdet(priv));
+
+	return 0;
+}
+
+static int max96717_enable_streams(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state, u32 pad,
+				   u64 streams_mask)
+{
+	struct max96717_priv *priv = sd_to_max96717(sd);
+	u64 sink_streams;
+	int ret;
+
+	if (!priv->enabled_source_streams)
+		max96717_start_csi(priv, true);
+
+	ret = max96717_apply_patgen(priv, state);
+	if (ret)
+		goto stop_csi;
+
+	if (!priv->pattern) {
+		sink_streams =
+			v4l2_subdev_state_xlate_streams(state,
+							MAX96717_PAD_SOURCE,
+							MAX96717_PAD_SINK,
+							&streams_mask);
+
+		ret = v4l2_subdev_enable_streams(priv->source_sd,
+						 priv->source_sd_pad,
+						 sink_streams);
+		if (ret)
+			goto stop_csi;
+	}
+
+	priv->enabled_source_streams |= streams_mask;
+
+	return 0;
+
+stop_csi:
+	if (!priv->enabled_source_streams)
+		max96717_start_csi(priv, false);
+
+	return ret;
+}
+
+static int max96717_disable_streams(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state, u32 pad,
+				    u64 streams_mask)
+{
+	struct max96717_priv *priv = sd_to_max96717(sd);
+	u64 sink_streams;
+
+	/*
+	 * Stop the CSI receiver first then the source,
+	 * otherwise the device may become unresponsive
+	 * while holding the I2C bus low.
+	 */
+	priv->enabled_source_streams &= ~streams_mask;
+	if (!priv->enabled_source_streams)
+		max96717_start_csi(priv, false);
+
+	if (!priv->pattern) {
+		int ret;
+
+		sink_streams =
+			v4l2_subdev_state_xlate_streams(state,
+							MAX96717_PAD_SOURCE,
+							MAX96717_PAD_SINK,
+							&streams_mask);
+
+		ret = v4l2_subdev_disable_streams(priv->source_sd,
+						  priv->source_sd_pad,
+						  sink_streams);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops max96717_pad_ops = {
+	.enable_streams = max96717_enable_streams,
+	.disable_streams = max96717_disable_streams,
+	.set_routing = max96717_set_routing,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = max96717_set_fmt,
+};
+
+static const struct v4l2_subdev_core_ops max96717_subdev_core_ops = {
+	.log_status = max96717_log_status,
+};
+
+static const struct v4l2_subdev_internal_ops max96717_internal_ops = {
+	.init_state = max96717_init_state,
+};
+
+static const struct v4l2_subdev_ops max96717_subdev_ops = {
+	.core = &max96717_subdev_core_ops,
+	.pad = &max96717_pad_ops,
+};
+
+static const struct media_entity_operations max96717_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int max96717_notify_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *source_subdev,
+				 struct v4l2_async_connection *asd)
+{
+	struct max96717_priv *priv = sd_to_max96717(notifier->sd);
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	ret = media_entity_get_fwnode_pad(&source_subdev->entity,
+					  source_subdev->fwnode,
+					  MEDIA_PAD_FL_SOURCE);
+	if (ret < 0) {
+		dev_err(dev, "Failed to find pad for %s\n",
+			source_subdev->name);
+		return ret;
+	}
+
+	priv->source_sd = source_subdev;
+	priv->source_sd_pad = ret;
+
+	ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
+				    &priv->sd.entity, 0,
+				    MEDIA_LNK_FL_ENABLED |
+				    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_err(dev, "Unable to link %s:%u -> %s:0\n",
+			source_subdev->name, priv->source_sd_pad,
+			priv->sd.name);
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_async_notifier_operations max96717_notify_ops = {
+	.bound = max96717_notify_bound,
+};
+
+static int max96717_v4l2_notifier_register(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep_fwnode;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    MAX96717_PAD_SINK, 0, 0);
+	if (!ep_fwnode) {
+		dev_err(dev, "No graph endpoint\n");
+		return -ENODEV;
+	}
+
+	v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
+					      struct v4l2_async_connection);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (IS_ERR(asd)) {
+		dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return PTR_ERR(asd);
+	}
+
+	priv->notifier.ops = &max96717_notify_ops;
+
+	ret = v4l2_async_nf_register(&priv->notifier);
+	if (ret) {
+		dev_err(dev, "Failed to register subdev_notifier");
+		v4l2_async_nf_cleanup(&priv->notifier);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int max96717_subdev_init(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	int ret;
+
+	v4l2_i2c_subdev_init(&priv->sd, priv->client, &max96717_subdev_ops);
+	priv->sd.internal_ops = &max96717_internal_ops;
+
+	v4l2_ctrl_handler_init(&priv->ctrl_handler, 1);
+	priv->sd.ctrl_handler = &priv->ctrl_handler;
+
+	v4l2_ctrl_new_std_menu_items(&priv->ctrl_handler,
+				     &max96717_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(max96717_test_pattern) - 1,
+				     0, 0, max96717_test_pattern);
+	if (priv->ctrl_handler.error) {
+		ret = priv->ctrl_handler.error;
+		goto err_free_ctrl;
+	}
+
+	priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
+	priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	priv->sd.entity.ops = &max96717_entity_ops;
+
+	priv->pads[MAX96717_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	priv->pads[MAX96717_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
+	if (ret) {
+		dev_err_probe(dev, ret, "Failed to init pads\n");
+		goto err_free_ctrl;
+	}
+
+	ret = v4l2_subdev_init_finalize(&priv->sd);
+	if (ret) {
+		dev_err_probe(dev, ret,
+			      "v4l2 subdev init finalized failed\n");
+		goto err_entity_cleanup;
+	}
+	ret = max96717_v4l2_notifier_register(priv);
+	if (ret) {
+		dev_err_probe(dev, ret,
+			      "v4l2 subdev notifier register failed\n");
+		goto err_free_state;
+	}
+
+	ret = v4l2_async_register_subdev(&priv->sd);
+	if (ret) {
+		dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
+		goto err_unreg_notif;
+	}
+
+	return 0;
+
+err_unreg_notif:
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+err_free_state:
+	v4l2_subdev_cleanup(&priv->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&priv->sd.entity);
+err_free_ctrl:
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+
+	return ret;
+}
+
+static void max96717_subdev_uninit(struct max96717_priv *priv)
+{
+	v4l2_async_unregister_subdev(&priv->sd);
+	v4l2_async_nf_unregister(&priv->notifier);
+	v4l2_async_nf_cleanup(&priv->notifier);
+	v4l2_subdev_cleanup(&priv->sd);
+	media_entity_cleanup(&priv->sd.entity);
+	v4l2_ctrl_handler_free(&priv->ctrl_handler);
+}
+
+struct max96717_pll_predef_freq {
+	unsigned long freq;
+	bool is_alt;
+	u8 val;
+};
+
+static const struct max96717_pll_predef_freq max96717_predef_freqs[] = {
+	{ 13500000, true,  0 }, { 19200000, false, 0 },
+	{ 24000000, true,  1 }, { 27000000, false, 1 },
+	{ 37125000, false, 2 }, { 74250000, false, 3 },
+};
+
+static unsigned long
+max96717_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct max96717_priv *priv = clk_hw_to_max96717(hw);
+
+	return max96717_predef_freqs[priv->pll_predef_index].freq;
+}
+
+static unsigned int max96717_clk_find_best_index(struct max96717_priv *priv,
+						 unsigned long rate)
+{
+	unsigned int i, idx = 0;
+	unsigned long diff_new, diff_old = U32_MAX;
+
+	for (i = 0; i < ARRAY_SIZE(max96717_predef_freqs); i++) {
+		diff_new = abs(rate - max96717_predef_freqs[i].freq);
+		if (diff_new < diff_old) {
+			diff_old = diff_new;
+			idx = i;
+		}
+	}
+
+	return idx;
+}
+
+static long max96717_clk_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *parent_rate)
+{
+	struct max96717_priv *priv = clk_hw_to_max96717(hw);
+	struct device *dev = &priv->client->dev;
+	unsigned int idx;
+
+	idx = max96717_clk_find_best_index(priv, rate);
+
+	if (rate != max96717_predef_freqs[idx].freq) {
+		dev_warn(dev, "Request CLK freq:%lu, found CLK freq:%lu\n",
+			 rate, max96717_predef_freqs[idx].freq);
+	}
+
+	return max96717_predef_freqs[idx].freq;
+}
+
+static int max96717_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long parent_rate)
+{
+	struct max96717_priv *priv = clk_hw_to_max96717(hw);
+	unsigned int val, idx;
+	int ret = 0;
+
+	idx = max96717_clk_find_best_index(priv, rate);
+
+	val = FIELD_PREP(REFGEN_PREDEF_FREQ_MASK,
+			 max96717_predef_freqs[idx].val);
+
+	if (max96717_predef_freqs[idx].is_alt)
+		val |= REFGEN_PREDEF_FREQ_ALT;
+
+	val |= REFGEN_RST | REFGEN_PREDEF_EN;
+
+	cci_write(priv->regmap, REF_VTG0, val, &ret);
+	cci_update_bits(priv->regmap, REF_VTG0, REFGEN_RST | REFGEN_EN,
+			REFGEN_EN, &ret);
+	if (ret)
+		return ret;
+
+	priv->pll_predef_index = idx;
+
+	return 0;
+}
+
+static int max96717_clk_prepare(struct clk_hw *hw)
+{
+	struct max96717_priv *priv = clk_hw_to_max96717(hw);
+
+	return cci_update_bits(priv->regmap, MAX96717_REG6, RCLKEN,
+			       RCLKEN, NULL);
+}
+
+static void max96717_clk_unprepare(struct clk_hw *hw)
+{
+	struct max96717_priv *priv = clk_hw_to_max96717(hw);
+
+	cci_update_bits(priv->regmap, MAX96717_REG6, RCLKEN, 0, NULL);
+}
+
+static const struct clk_ops max96717_clk_ops = {
+	.prepare     = max96717_clk_prepare,
+	.unprepare   = max96717_clk_unprepare,
+	.set_rate    = max96717_clk_set_rate,
+	.recalc_rate = max96717_clk_recalc_rate,
+	.round_rate  = max96717_clk_round_rate,
+};
+
+static int max96717_register_clkout(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct clk_init_data init = { .ops = &max96717_clk_ops };
+	int ret;
+
+	init.name = kasprintf(GFP_KERNEL, "max96717.%s.clk_out", dev_name(dev));
+	if (!init.name)
+		return -ENOMEM;
+
+	/* RCLKSEL Reference PLL output */
+	ret = cci_update_bits(priv->regmap, MAX96717_REG3, MAX96717_RCLKSEL,
+			      MAX96717_RCLKSEL, NULL);
+	/* MFP4 fastest slew rate */
+	cci_update_bits(priv->regmap, PIO_SLEW_1, BIT(5) | BIT(4), 0, &ret);
+	if (ret)
+		goto free_init_name;
+
+	priv->clk_hw.init = &init;
+
+	/* Initialize to 24 MHz */
+	ret = max96717_clk_set_rate(&priv->clk_hw,
+				    MAX96717_DEFAULT_CLKOUT_RATE, 0);
+	if (ret < 0)
+		goto free_init_name;
+
+	ret = devm_clk_hw_register(dev, &priv->clk_hw);
+	kfree(init.name);
+	if (ret)
+		return dev_err_probe(dev, ret, "Cannot register clock HW\n");
+
+	ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
+					  &priv->clk_hw);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Cannot add OF clock provider\n");
+
+	return 0;
+
+free_init_name:
+	kfree(init.name);
+	return ret;
+}
+
+static int max96717_init_csi_lanes(struct max96717_priv *priv)
+{
+	struct v4l2_mbus_config_mipi_csi2 *mipi = &priv->mipi_csi2;
+	unsigned long lanes_used = 0;
+	unsigned int nlanes, lane, val = 0;
+	int ret;
+
+	nlanes = mipi->num_data_lanes;
+
+	ret = cci_update_bits(priv->regmap, MAX96717_MIPI_RX1,
+			      MAX96717_MIPI_LANES_CNT,
+			      FIELD_PREP(MAX96717_MIPI_LANES_CNT,
+					 nlanes - 1), NULL);
+
+	/* lanes polarity */
+	for (lane = 0; lane < nlanes + 1; lane++) {
+		if (!mipi->lane_polarities[lane])
+			continue;
+		/* Clock lane */
+		if (lane == 0)
+			val |= BIT(2);
+		else if (lane < 3)
+			val |= BIT(lane - 1);
+		else
+			val |= BIT(lane);
+	}
+
+	cci_update_bits(priv->regmap, MAX96717_MIPI_RX5,
+			MAX96717_PHY2_LANES_POL,
+			FIELD_PREP(MAX96717_PHY2_LANES_POL, val), &ret);
+
+	cci_update_bits(priv->regmap, MAX96717_MIPI_RX4,
+			MAX96717_PHY1_LANES_POL,
+			FIELD_PREP(MAX96717_PHY1_LANES_POL,
+				   val >> 3), &ret);
+	/* lanes mapping */
+	for (lane = 0, val = 0; lane < nlanes; lane++) {
+		val |= (mipi->data_lanes[lane] - 1) << (lane * 2);
+		lanes_used |= BIT(mipi->data_lanes[lane] - 1);
+	}
+
+	/*
+	 * Unused lanes need to be mapped as well to not have
+	 * the same lanes mapped twice.
+	 */
+	for (; lane < MAX96717_CSI_NLANES; lane++) {
+		unsigned int idx = find_first_zero_bit(&lanes_used,
+						       MAX96717_CSI_NLANES);
+
+		val |= idx << (lane * 2);
+		lanes_used |= BIT(idx);
+	}
+
+	cci_update_bits(priv->regmap, MAX96717_MIPI_RX3,
+			MAX96717_PHY1_LANES_MAP,
+			FIELD_PREP(MAX96717_PHY1_LANES_MAP, val), &ret);
+
+	return cci_update_bits(priv->regmap, MAX96717_MIPI_RX2,
+			       MAX96717_PHY2_LANES_MAP,
+			       FIELD_PREP(MAX96717_PHY2_LANES_MAP, val >> 4),
+			       &ret);
+}
+
+static int max96717_hw_init(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	u64 dev_id, val;
+	int ret;
+
+	ret = cci_read(priv->regmap, MAX96717_DEV_ID, &dev_id, NULL);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Fail to read the device id\n");
+
+	if (dev_id != MAX96717_DEVICE_ID && dev_id != MAX96717F_DEVICE_ID)
+		return dev_err_probe(dev, -EOPNOTSUPP,
+				     "Unsupported device id got %x\n", (u8)dev_id);
+
+	ret = cci_read(priv->regmap, MAX96717_DEV_REV, &val, NULL);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Fail to read device revision");
+
+	dev_dbg(dev, "Found %x (rev %lx)\n", (u8)dev_id,
+		(u8)val & MAX96717_DEV_REV_MASK);
+
+	ret = cci_read(priv->regmap, MAX96717_MIPI_RX_EXT11, &val, NULL);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Fail to read mipi rx extension");
+
+	if (!(val & MAX96717_TUN_MODE))
+		return dev_err_probe(dev, -EOPNOTSUPP,
+				     "Only supporting tunnel mode");
+
+	return max96717_init_csi_lanes(priv);
+}
+
+static int max96717_parse_dt(struct max96717_priv *priv)
+{
+	struct device *dev = &priv->client->dev;
+	struct v4l2_fwnode_endpoint vep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct fwnode_handle *ep_fwnode;
+	unsigned char num_data_lanes;
+	int ret;
+
+	ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
+						    MAX96717_PAD_SINK, 0, 0);
+	if (!ep_fwnode)
+		return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
+
+	ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
+
+	fwnode_handle_put(ep_fwnode);
+
+	if (ret < 0)
+		return dev_err_probe(dev, ret, "Failed to parse sink endpoint");
+
+	num_data_lanes = vep.bus.mipi_csi2.num_data_lanes;
+	if (num_data_lanes < 1 || num_data_lanes > MAX96717_CSI_NLANES)
+		return dev_err_probe(dev, -EINVAL,
+				     "Invalid data lanes must be 1 to 4\n");
+
+	priv->mipi_csi2 = vep.bus.mipi_csi2;
+
+	return 0;
+}
+
+static int max96717_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct max96717_priv *priv;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+	priv->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(priv->regmap)) {
+		ret = PTR_ERR(priv->regmap);
+		return dev_err_probe(dev, ret, "Failed to init regmap\n");
+	}
+
+	ret = max96717_parse_dt(priv);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to parse the dt\n");
+
+	ret = max96717_hw_init(priv);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Failed to initialize the hardware\n");
+
+	ret = max96717_gpiochip_probe(priv);
+	if (ret)
+		return dev_err_probe(&client->dev, ret,
+				     "Failed to init gpiochip\n");
+
+	ret = max96717_register_clkout(priv);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to register clkout\n");
+
+	ret = max96717_subdev_init(priv);
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "Failed to initialize v4l2 subdev\n");
+
+	ret = max96717_i2c_mux_init(priv);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
+		max96717_subdev_uninit(priv);
+	}
+
+	return ret;
+}
+
+static void max96717_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct max96717_priv *priv = sd_to_max96717(sd);
+
+	max96717_subdev_uninit(priv);
+	i2c_mux_del_adapters(priv->mux);
+}
+
+static const struct of_device_id max96717_of_ids[] = {
+	{ .compatible = "maxim,max96717f" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max96717_of_ids);
+
+static struct i2c_driver max96717_i2c_driver = {
+	.driver	= {
+		.name		= "max96717",
+		.of_match_table	= max96717_of_ids,
+	},
+	.probe		= max96717_probe,
+	.remove		= max96717_remove,
+};
+
+module_i2c_driver(max96717_i2c_driver);
+
+MODULE_DESCRIPTION("Maxim GMSL2 MAX96717 Serializer Driver");
+MODULE_AUTHOR("Julien Massot <julien.massot@collabora.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ml86v7667.c b/drivers/media/i2c/ml86v7667.c
index 5b72d4434224..57ba3693649a 100644
--- a/drivers/media/i2c/ml86v7667.c
+++ b/drivers/media/i2c/ml86v7667.c
@@ -424,8 +424,8 @@ static void ml86v7667_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ml86v7667_id[] = {
-	{DRV_NAME, 0},
-	{},
+	{ DRV_NAME },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, ml86v7667_id);
 
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
index 599a5bc7cbb3..4c0b0ad68c08 100644
--- a/drivers/media/i2c/msp3400-driver.c
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -874,7 +874,7 @@ static const struct dev_pm_ops msp3400_pm_ops = {
 };
 
 static const struct i2c_device_id msp_id[] = {
-	{ "msp3400", 0 },
+	{ "msp3400" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, msp_id);
diff --git a/drivers/media/i2c/mt9m001.c b/drivers/media/i2c/mt9m001.c
index ad1a3ab77411..12d3e86bdc0f 100644
--- a/drivers/media/i2c/mt9m001.c
+++ b/drivers/media/i2c/mt9m001.c
@@ -854,7 +854,7 @@ static void mt9m001_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mt9m001_id[] = {
-	{ "mt9m001", 0 },
+	{ "mt9m001" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9m001_id);
diff --git a/drivers/media/i2c/mt9m111.c b/drivers/media/i2c/mt9m111.c
index ceeeb94c38d5..9aa5dcda3805 100644
--- a/drivers/media/i2c/mt9m111.c
+++ b/drivers/media/i2c/mt9m111.c
@@ -1383,7 +1383,7 @@ static const struct of_device_id mt9m111_of_match[] = {
 MODULE_DEVICE_TABLE(of, mt9m111_of_match);
 
 static const struct i2c_device_id mt9m111_id[] = {
-	{ "mt9m111", 0 },
+	{ "mt9m111" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index f4b481212356..4ef5fb06131d 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -15,15 +15,14 @@
 #include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/log2.h>
+#include <linux/mod_devicetable.h>
 #include <linux/module.h>
-#include <linux/of.h>
-#include <linux/of_graph.h>
 #include <linux/pm.h>
+#include <linux/property.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
-#include <media/i2c/mt9p031.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -112,9 +111,8 @@
 #define MT9P031_TEST_PATTERN_RED			0xa2
 #define MT9P031_TEST_PATTERN_BLUE			0xa3
 
-enum mt9p031_model {
-	MT9P031_MODEL_COLOR,
-	MT9P031_MODEL_MONOCHROME,
+struct mt9p031_model_info {
+	u32 code;
 };
 
 struct mt9p031 {
@@ -122,14 +120,17 @@ struct mt9p031 {
 	struct media_pad pad;
 	struct v4l2_rect crop;  /* Sensor window */
 	struct v4l2_mbus_framefmt format;
-	struct mt9p031_platform_data *pdata;
 	struct mutex power_lock; /* lock to protect power_count */
 	int power_count;
 
 	struct clk *clk;
 	struct regulator_bulk_data regulators[3];
 
-	enum mt9p031_model model;
+	unsigned int pixclk_pol:1;
+	int ext_freq;
+	int target_freq;
+
+	u32 code;
 	struct aptina_pll pll;
 	unsigned int clk_div;
 	bool use_pll;
@@ -229,7 +230,6 @@ static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 	};
 
 	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
-	struct mt9p031_platform_data *pdata = mt9p031->pdata;
 	unsigned long ext_freq;
 	int ret;
 
@@ -237,7 +237,7 @@ static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 	if (IS_ERR(mt9p031->clk))
 		return PTR_ERR(mt9p031->clk);
 
-	ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
+	ret = clk_set_rate(mt9p031->clk, mt9p031->ext_freq);
 	if (ret < 0)
 		return ret;
 
@@ -249,7 +249,7 @@ static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 	if (ext_freq > limits.ext_clock_max) {
 		unsigned int div;
 
-		div = DIV_ROUND_UP(ext_freq, pdata->target_freq);
+		div = DIV_ROUND_UP(ext_freq, mt9p031->target_freq);
 		div = roundup_pow_of_two(div) / 2;
 
 		mt9p031->clk_div = min_t(unsigned int, div, 64);
@@ -259,7 +259,7 @@ static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
 	}
 
 	mt9p031->pll.ext_clock = ext_freq;
-	mt9p031->pll.pix_clock = pdata->target_freq;
+	mt9p031->pll.pix_clock = mt9p031->target_freq;
 	mt9p031->use_pll = true;
 
 	return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
@@ -380,7 +380,7 @@ static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
 	}
 
 	/* Configure the pixel clock polarity */
-	if (mt9p031->pdata && mt9p031->pdata->pixclk_pol) {
+	if (mt9p031->pixclk_pol) {
 		ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
 				MT9P031_PIXEL_CLOCK_INVERT);
 		if (ret < 0)
@@ -712,12 +712,7 @@ static int mt9p031_init_state(struct v4l2_subdev *subdev,
 	crop->height = MT9P031_WINDOW_HEIGHT_DEF;
 
 	format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
-
-	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
-		format->code = MEDIA_BUS_FMT_Y12_1X12;
-	else
-		format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
-
+	format->code = mt9p031->code;
 	format->width = MT9P031_WINDOW_WIDTH_DEF;
 	format->height = MT9P031_WINDOW_HEIGHT_DEF;
 	format->field = V4L2_FIELD_NONE;
@@ -1066,54 +1061,41 @@ static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
  * Driver initialization and probing
  */
 
-static struct mt9p031_platform_data *
-mt9p031_get_pdata(struct i2c_client *client)
+static int mt9p031_parse_properties(struct mt9p031 *mt9p031, struct device *dev)
 {
-	struct mt9p031_platform_data *pdata = NULL;
-	struct device_node *np;
 	struct v4l2_fwnode_endpoint endpoint = {
 		.bus_type = V4L2_MBUS_PARALLEL
 	};
+	struct fwnode_handle *np;
+	int ret;
 
-	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
-		return client->dev.platform_data;
-
-	np = of_graph_get_endpoint_by_regs(client->dev.of_node, 0, -1);
+	np = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
 	if (!np)
-		return NULL;
-
-	if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
-		goto done;
+		return dev_err_probe(dev, -EINVAL, "endpoint node not found\n");
 
-	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata)
-		goto done;
+	ret = v4l2_fwnode_endpoint_parse(np, &endpoint);
+	fwnode_handle_put(np);
+	if (ret)
+		return dev_err_probe(dev, -EINVAL, "could not parse endpoint\n");
 
-	of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
-	of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
+	fwnode_property_read_u32(np, "input-clock-frequency",
+				 &mt9p031->ext_freq);
+	fwnode_property_read_u32(np, "pixel-clock-frequency",
+				 &mt9p031->target_freq);
 
-	pdata->pixclk_pol = !!(endpoint.bus.parallel.flags &
-			       V4L2_MBUS_PCLK_SAMPLE_RISING);
+	mt9p031->pixclk_pol = !!(endpoint.bus.parallel.flags &
+				 V4L2_MBUS_PCLK_SAMPLE_RISING);
 
-done:
-	of_node_put(np);
-	return pdata;
+	return 0;
 }
 
 static int mt9p031_probe(struct i2c_client *client)
 {
-	const struct i2c_device_id *did = i2c_client_get_device_id(client);
-	struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
 	struct i2c_adapter *adapter = client->adapter;
 	struct mt9p031 *mt9p031;
 	unsigned int i;
 	int ret;
 
-	if (pdata == NULL) {
-		dev_err(&client->dev, "No platform data\n");
-		return -EINVAL;
-	}
-
 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
 		dev_warn(&client->dev,
 			"I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
@@ -1124,10 +1106,13 @@ static int mt9p031_probe(struct i2c_client *client)
 	if (mt9p031 == NULL)
 		return -ENOMEM;
 
-	mt9p031->pdata = pdata;
+	ret = mt9p031_parse_properties(mt9p031, &client->dev);
+	if (ret)
+		return ret;
+
 	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
 	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
-	mt9p031->model = did->driver_data;
+	mt9p031->code = (uintptr_t)device_get_match_data(&client->dev);
 
 	mt9p031->regulators[0].supply = "vdd";
 	mt9p031->regulators[1].supply = "vdd_io";
@@ -1155,8 +1140,8 @@ static int mt9p031_probe(struct i2c_client *client)
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_VFLIP, 0, 1, 1, 0);
 	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
-			  V4L2_CID_PIXEL_RATE, pdata->target_freq,
-			  pdata->target_freq, 1, pdata->target_freq);
+			  V4L2_CID_PIXEL_RATE, mt9p031->target_freq,
+			  mt9p031->target_freq, 1, mt9p031->target_freq);
 	v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
 			  V4L2_CID_TEST_PATTERN,
 			  ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
@@ -1223,32 +1208,29 @@ static void mt9p031_remove(struct i2c_client *client)
 	mutex_destroy(&mt9p031->power_lock);
 }
 
-static const struct i2c_device_id mt9p031_id[] = {
-	{ "mt9p006", MT9P031_MODEL_COLOR },
-	{ "mt9p031", MT9P031_MODEL_COLOR },
-	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
-	{ }
+static const struct mt9p031_model_info mt9p031_models_bayer = {
+	.code = MEDIA_BUS_FMT_SGRBG12_1X12
+};
+
+static const struct mt9p031_model_info mt9p031_models_mono = {
+	.code = MEDIA_BUS_FMT_Y12_1X12
 };
-MODULE_DEVICE_TABLE(i2c, mt9p031_id);
 
-#if IS_ENABLED(CONFIG_OF)
 static const struct of_device_id mt9p031_of_match[] = {
-	{ .compatible = "aptina,mt9p006", },
-	{ .compatible = "aptina,mt9p031", },
-	{ .compatible = "aptina,mt9p031m", },
-	{ /* sentinel */ },
+	{ .compatible = "aptina,mt9p006", .data = &mt9p031_models_bayer },
+	{ .compatible = "aptina,mt9p031", .data = &mt9p031_models_bayer },
+	{ .compatible = "aptina,mt9p031m", .data = &mt9p031_models_mono },
+	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
-#endif
 
 static struct i2c_driver mt9p031_i2c_driver = {
 	.driver = {
-		.of_match_table = of_match_ptr(mt9p031_of_match),
+		.of_match_table = mt9p031_of_match,
 		.name = "mt9p031",
 	},
 	.probe          = mt9p031_probe,
 	.remove         = mt9p031_remove,
-	.id_table       = mt9p031_id,
 };
 
 module_i2c_driver(mt9p031_i2c_driver);
diff --git a/drivers/media/i2c/mt9t112.c b/drivers/media/i2c/mt9t112.c
index fb1588c57cc8..878dff9b7577 100644
--- a/drivers/media/i2c/mt9t112.c
+++ b/drivers/media/i2c/mt9t112.c
@@ -1109,7 +1109,7 @@ static void mt9t112_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mt9t112_id[] = {
-	{ "mt9t112", 0 },
+	{ "mt9t112" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9t112_id);
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
index 8834ff8786e5..055b7915260a 100644
--- a/drivers/media/i2c/mt9v011.c
+++ b/drivers/media/i2c/mt9v011.c
@@ -582,7 +582,7 @@ static void mt9v011_remove(struct i2c_client *c)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id mt9v011_id[] = {
-	{ "mt9v011", 0 },
+	{ "mt9v011" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, mt9v011_id);
diff --git a/drivers/media/i2c/mt9v111.c b/drivers/media/i2c/mt9v111.c
index b0b98ed3c150..723fe138e7bc 100644
--- a/drivers/media/i2c/mt9v111.c
+++ b/drivers/media/i2c/mt9v111.c
@@ -1263,8 +1263,9 @@ static void mt9v111_remove(struct i2c_client *client)
 
 static const struct of_device_id mt9v111_of_match[] = {
 	{ .compatible = "aptina,mt9v111", },
-	{ /* sentinel */ },
+	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(of, mt9v111_of_match);
 
 static struct i2c_driver mt9v111_driver = {
 	.driver = {
diff --git a/drivers/media/i2c/og01a1b.c b/drivers/media/i2c/og01a1b.c
index bac9597faf68..78d5d406e4b7 100644
--- a/drivers/media/i2c/og01a1b.c
+++ b/drivers/media/i2c/og01a1b.c
@@ -1,12 +1,15 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2022 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -418,6 +421,12 @@ static const struct og01a1b_mode supported_modes[] = {
 };
 
 struct og01a1b {
+	struct clk *xvclk;
+	struct gpio_desc *reset_gpio;
+	struct regulator *avdd;
+	struct regulator *dovdd;
+	struct regulator *dvdd;
+
 	struct v4l2_subdev sd;
 	struct media_pad pad;
 	struct v4l2_ctrl_handler ctrl_handler;
@@ -898,8 +907,10 @@ static int og01a1b_identify_module(struct og01a1b *og01a1b)
 	return 0;
 }
 
-static int og01a1b_check_hwcfg(struct device *dev)
+static int og01a1b_check_hwcfg(struct og01a1b *og01a1b)
 {
+	struct i2c_client *client = v4l2_get_subdevdata(&og01a1b->sd);
+	struct device *dev = &client->dev;
 	struct fwnode_handle *ep;
 	struct fwnode_handle *fwnode = dev_fwnode(dev);
 	struct v4l2_fwnode_endpoint bus_cfg = {
@@ -913,10 +924,13 @@ static int og01a1b_check_hwcfg(struct device *dev)
 		return -ENXIO;
 
 	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
-
 	if (ret) {
-		dev_err(dev, "can't get clock frequency");
-		return ret;
+		if (!og01a1b->xvclk) {
+			dev_err(dev, "can't get clock frequency");
+			return ret;
+		}
+
+		mclk = clk_get_rate(og01a1b->xvclk);
 	}
 
 	if (mclk != OG01A1B_MCLK) {
@@ -967,6 +981,83 @@ check_hwcfg_error:
 	return ret;
 }
 
+/* Power/clock management functions */
+static int og01a1b_power_on(struct device *dev)
+{
+	unsigned long delay = DIV_ROUND_UP(8192UL * USEC_PER_SEC, OG01A1B_MCLK);
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+	int ret;
+
+	if (og01a1b->avdd) {
+		ret = regulator_enable(og01a1b->avdd);
+		if (ret)
+			return ret;
+	}
+
+	if (og01a1b->dovdd) {
+		ret = regulator_enable(og01a1b->dovdd);
+		if (ret)
+			goto avdd_disable;
+	}
+
+	if (og01a1b->dvdd) {
+		ret = regulator_enable(og01a1b->dvdd);
+		if (ret)
+			goto dovdd_disable;
+	}
+
+	ret = clk_prepare_enable(og01a1b->xvclk);
+	if (ret)
+		goto dvdd_disable;
+
+	gpiod_set_value_cansleep(og01a1b->reset_gpio, 0);
+
+	if (og01a1b->reset_gpio)
+		usleep_range(5 * USEC_PER_MSEC, 6 * USEC_PER_MSEC);
+	else if (og01a1b->xvclk)
+		usleep_range(delay, 2 * delay);
+
+	return 0;
+
+dvdd_disable:
+	if (og01a1b->dvdd)
+		regulator_disable(og01a1b->dvdd);
+dovdd_disable:
+	if (og01a1b->dovdd)
+		regulator_disable(og01a1b->dovdd);
+avdd_disable:
+	if (og01a1b->avdd)
+		regulator_disable(og01a1b->avdd);
+
+	return ret;
+}
+
+static int og01a1b_power_off(struct device *dev)
+{
+	unsigned long delay = DIV_ROUND_UP(512 * USEC_PER_SEC, OG01A1B_MCLK);
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct og01a1b *og01a1b = to_og01a1b(sd);
+
+	if (og01a1b->xvclk)
+		usleep_range(delay, 2 * delay);
+
+	clk_disable_unprepare(og01a1b->xvclk);
+
+	gpiod_set_value_cansleep(og01a1b->reset_gpio, 1);
+
+	if (og01a1b->dvdd)
+		regulator_disable(og01a1b->dvdd);
+
+	if (og01a1b->dovdd)
+		regulator_disable(og01a1b->dovdd);
+
+	if (og01a1b->avdd)
+		regulator_disable(og01a1b->avdd);
+
+	return 0;
+}
+
 static void og01a1b_remove(struct i2c_client *client)
 {
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
@@ -984,22 +1075,78 @@ static int og01a1b_probe(struct i2c_client *client)
 	struct og01a1b *og01a1b;
 	int ret;
 
-	ret = og01a1b_check_hwcfg(&client->dev);
+	og01a1b = devm_kzalloc(&client->dev, sizeof(*og01a1b), GFP_KERNEL);
+	if (!og01a1b)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&og01a1b->sd, client, &og01a1b_subdev_ops);
+
+	og01a1b->xvclk = devm_clk_get_optional(&client->dev, NULL);
+	if (IS_ERR(og01a1b->xvclk)) {
+		ret = PTR_ERR(og01a1b->xvclk);
+		dev_err(&client->dev, "failed to get xvclk clock: %d\n", ret);
+		return ret;
+	}
+
+	ret = og01a1b_check_hwcfg(og01a1b);
 	if (ret) {
 		dev_err(&client->dev, "failed to check HW configuration: %d",
 			ret);
 		return ret;
 	}
 
-	og01a1b = devm_kzalloc(&client->dev, sizeof(*og01a1b), GFP_KERNEL);
-	if (!og01a1b)
-		return -ENOMEM;
+	og01a1b->reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+						      GPIOD_OUT_LOW);
+	if (IS_ERR(og01a1b->reset_gpio)) {
+		dev_err(&client->dev, "cannot get reset GPIO\n");
+		return PTR_ERR(og01a1b->reset_gpio);
+	}
+
+	og01a1b->avdd = devm_regulator_get_optional(&client->dev, "avdd");
+	if (IS_ERR(og01a1b->avdd)) {
+		ret = PTR_ERR(og01a1b->avdd);
+		if (ret != -ENODEV) {
+			dev_err_probe(&client->dev, ret,
+				      "Failed to get 'avdd' regulator\n");
+			return ret;
+		}
+
+		og01a1b->avdd = NULL;
+	}
+
+	og01a1b->dovdd = devm_regulator_get_optional(&client->dev, "dovdd");
+	if (IS_ERR(og01a1b->dovdd)) {
+		ret = PTR_ERR(og01a1b->dovdd);
+		if (ret != -ENODEV) {
+			dev_err_probe(&client->dev, ret,
+				      "Failed to get 'dovdd' regulator\n");
+			return ret;
+		}
+
+		og01a1b->dovdd = NULL;
+	}
+
+	og01a1b->dvdd = devm_regulator_get_optional(&client->dev, "dvdd");
+	if (IS_ERR(og01a1b->dvdd)) {
+		ret = PTR_ERR(og01a1b->dvdd);
+		if (ret != -ENODEV) {
+			dev_err_probe(&client->dev, ret,
+				      "Failed to get 'dvdd' regulator\n");
+			return ret;
+		}
+
+		og01a1b->dvdd = NULL;
+	}
+
+	/* The sensor must be powered on to read the CHIP_ID register */
+	ret = og01a1b_power_on(&client->dev);
+	if (ret)
+		return ret;
 
-	v4l2_i2c_subdev_init(&og01a1b->sd, client, &og01a1b_subdev_ops);
 	ret = og01a1b_identify_module(og01a1b);
 	if (ret) {
 		dev_err(&client->dev, "failed to find sensor: %d", ret);
-		return ret;
+		goto power_off;
 	}
 
 	mutex_init(&og01a1b->mutex);
@@ -1028,10 +1175,7 @@ static int og01a1b_probe(struct i2c_client *client)
 		goto probe_error_media_entity_cleanup;
 	}
 
-	/*
-	 * Device is already turned on by i2c-core with ACPI domain PM.
-	 * Enable runtime PM and turn off the device.
-	 */
+	/* Enable runtime PM and turn off the device */
 	pm_runtime_set_active(&client->dev);
 	pm_runtime_enable(&client->dev);
 	pm_runtime_idle(&client->dev);
@@ -1045,9 +1189,16 @@ probe_error_v4l2_ctrl_handler_free:
 	v4l2_ctrl_handler_free(og01a1b->sd.ctrl_handler);
 	mutex_destroy(&og01a1b->mutex);
 
+power_off:
+	og01a1b_power_off(&client->dev);
+
 	return ret;
 }
 
+static const struct dev_pm_ops og01a1b_pm_ops = {
+	SET_RUNTIME_PM_OPS(og01a1b_power_off, og01a1b_power_on, NULL)
+};
+
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id og01a1b_acpi_ids[] = {
 	{"OVTI01AC"},
@@ -1057,10 +1208,18 @@ static const struct acpi_device_id og01a1b_acpi_ids[] = {
 MODULE_DEVICE_TABLE(acpi, og01a1b_acpi_ids);
 #endif
 
+static const struct of_device_id og01a1b_of_match[] = {
+	{ .compatible = "ovti,og01a1b" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, og01a1b_of_match);
+
 static struct i2c_driver og01a1b_i2c_driver = {
 	.driver = {
 		.name = "og01a1b",
+		.pm = &og01a1b_pm_ops,
 		.acpi_match_table = ACPI_PTR(og01a1b_acpi_ids),
+		.of_match_table = og01a1b_of_match,
 	},
 	.probe = og01a1b_probe,
 	.remove = og01a1b_remove,
diff --git a/drivers/media/i2c/ov01a10.c b/drivers/media/i2c/ov01a10.c
index 5606437f37d0..141cb6f75b55 100644
--- a/drivers/media/i2c/ov01a10.c
+++ b/drivers/media/i2c/ov01a10.c
@@ -3,7 +3,7 @@
  * Copyright (c) 2023 Intel Corporation.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/acpi.h>
 #include <linux/bitfield.h>
@@ -13,7 +13,6 @@
 
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 
 #define OV01A10_LINK_FREQ_400MHZ	400000000ULL
@@ -804,8 +803,6 @@ static int ov01a10_get_selection(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_core_ops ov01a10_core_ops = {
 	.log_status = v4l2_ctrl_subdev_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_video_ops ov01a10_video_ops = {
@@ -892,8 +889,7 @@ static int ov01a10_probe(struct i2c_client *client)
 	}
 
 	ov01a10->sd.state_lock = ov01a10->ctrl_handler.lock;
-	ov01a10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
-		V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov01a10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	ov01a10->sd.entity.ops = &ov01a10_subdev_entity_ops;
 	ov01a10->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 	ov01a10->pad.flags = MEDIA_PAD_FL_SOURCE;
diff --git a/drivers/media/i2c/ov02c10.c b/drivers/media/i2c/ov02c10.c
new file mode 100644
index 000000000000..089a4fd9627c
--- /dev/null
+++ b/drivers/media/i2c/ov02c10.c
@@ -0,0 +1,1013 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2022 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/version.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV02C10_LINK_FREQ_400MHZ	400000000ULL
+#define OV02C10_MCLK			19200000
+#define OV02C10_RGB_DEPTH		10
+
+#define OV02C10_REG_CHIP_ID		CCI_REG16(0x300a)
+#define OV02C10_CHIP_ID			0x5602
+
+#define OV02C10_REG_STREAM_CONTROL	CCI_REG8(0x0100)
+
+#define OV02C10_REG_HTS			CCI_REG16(0x380c)
+
+/* vertical-timings from sensor */
+#define OV02C10_REG_VTS			CCI_REG16(0x380e)
+#define OV02C10_VTS_MAX			0xffff
+
+/* Exposure controls from sensor */
+#define OV02C10_REG_EXPOSURE		CCI_REG16(0x3501)
+#define OV02C10_EXPOSURE_MIN		4
+#define OV02C10_EXPOSURE_MAX_MARGIN	8
+#define OV02C10_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV02C10_REG_ANALOG_GAIN		CCI_REG16(0x3508)
+#define OV02C10_ANAL_GAIN_MIN		0x10
+#define OV02C10_ANAL_GAIN_MAX		0xf8
+#define OV02C10_ANAL_GAIN_STEP		1
+#define OV02C10_ANAL_GAIN_DEFAULT	0x10
+
+/* Digital gain controls from sensor */
+#define OV02C10_REG_DIGITAL_GAIN	CCI_REG24(0x350a)
+#define OV02C10_DGTL_GAIN_MIN		0x0400
+#define OV02C10_DGTL_GAIN_MAX		0x3fff
+#define OV02C10_DGTL_GAIN_STEP		1
+#define OV02C10_DGTL_GAIN_DEFAULT	0x0400
+
+/* Rotate */
+#define OV02C10_ROTATE_CONTROL		CCI_REG8(0x3820)
+#define OV02C10_ISP_X_WIN_CONTROL	CCI_REG16(0x3810)
+#define OV02C10_ISP_Y_WIN_CONTROL	CCI_REG16(0x3812)
+#define OV02C10_CONFIG_ROTATE		0x18
+
+/* Test Pattern Control */
+#define OV02C10_REG_TEST_PATTERN		CCI_REG8(0x4503)
+#define OV02C10_TEST_PATTERN_ENABLE		BIT(7)
+
+struct ov02c10_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Sensor register settings for this resolution */
+	const struct reg_sequence *reg_sequence;
+	const int sequence_length;
+	/* Sensor register settings for 1 or 2 lane config */
+	const struct reg_sequence *lane_settings[2];
+	const int lane_settings_length[2];
+};
+
+static const struct reg_sequence sensor_1928x1092_30fps_setting[] = {
+	{0x0301, 0x08},
+	{0x0303, 0x06},
+	{0x0304, 0x01},
+	{0x0305, 0xe0},
+	{0x0313, 0x40},
+	{0x031c, 0x4f},
+	{0x3020, 0x97},
+	{0x3022, 0x01},
+	{0x3026, 0xb4},
+	{0x303b, 0x00},
+	{0x303c, 0x4f},
+	{0x303d, 0xe6},
+	{0x303e, 0x00},
+	{0x303f, 0x03},
+	{0x3021, 0x23},
+	{0x3501, 0x04},
+	{0x3502, 0x6c},
+	{0x3504, 0x0c},
+	{0x3507, 0x00},
+	{0x3508, 0x08},
+	{0x3509, 0x00},
+	{0x350a, 0x01},
+	{0x350b, 0x00},
+	{0x350c, 0x41},
+	{0x3600, 0x84},
+	{0x3603, 0x08},
+	{0x3610, 0x57},
+	{0x3611, 0x1b},
+	{0x3613, 0x78},
+	{0x3623, 0x00},
+	{0x3632, 0xa0},
+	{0x3642, 0xe8},
+	{0x364c, 0x70},
+	{0x365f, 0x0f},
+	{0x3708, 0x30},
+	{0x3714, 0x24},
+	{0x3725, 0x02},
+	{0x3737, 0x08},
+	{0x3739, 0x28},
+	{0x3749, 0x32},
+	{0x374a, 0x32},
+	{0x374b, 0x32},
+	{0x374c, 0x32},
+	{0x374d, 0x81},
+	{0x374e, 0x81},
+	{0x374f, 0x81},
+	{0x3752, 0x36},
+	{0x3753, 0x36},
+	{0x3754, 0x36},
+	{0x3761, 0x00},
+	{0x376c, 0x81},
+	{0x3774, 0x18},
+	{0x3776, 0x08},
+	{0x377c, 0x81},
+	{0x377d, 0x81},
+	{0x377e, 0x81},
+	{0x37a0, 0x44},
+	{0x37a6, 0x44},
+	{0x37aa, 0x0d},
+	{0x37ae, 0x00},
+	{0x37cb, 0x03},
+	{0x37cc, 0x01},
+	{0x37d8, 0x02},
+	{0x37d9, 0x10},
+	{0x37e1, 0x10},
+	{0x37e2, 0x18},
+	{0x37e3, 0x08},
+	{0x37e4, 0x08},
+	{0x37e5, 0x02},
+	{0x37e6, 0x08},
+
+	/* 1928x1092 */
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x00},
+	{0x3804, 0x07},
+	{0x3805, 0x8f},
+	{0x3806, 0x04},
+	{0x3807, 0x47},
+	{0x3808, 0x07},
+	{0x3809, 0x88},
+	{0x380a, 0x04},
+	{0x380b, 0x44},
+	{0x3810, 0x00},
+	{0x3811, 0x02},
+	{0x3812, 0x00},
+	{0x3813, 0x02},
+	{0x3814, 0x01},
+	{0x3815, 0x01},
+	{0x3816, 0x01},
+	{0x3817, 0x01},
+
+	{0x3820, 0xb0},
+	{0x3821, 0x00},
+	{0x3822, 0x80},
+	{0x3823, 0x08},
+	{0x3824, 0x00},
+	{0x3825, 0x20},
+	{0x3826, 0x00},
+	{0x3827, 0x08},
+	{0x382a, 0x00},
+	{0x382b, 0x08},
+	{0x382d, 0x00},
+	{0x382e, 0x00},
+	{0x382f, 0x23},
+	{0x3834, 0x00},
+	{0x3839, 0x00},
+	{0x383a, 0xd1},
+	{0x383e, 0x03},
+	{0x393d, 0x29},
+	{0x393f, 0x6e},
+	{0x394b, 0x06},
+	{0x394c, 0x06},
+	{0x394d, 0x08},
+	{0x394f, 0x01},
+	{0x3950, 0x01},
+	{0x3951, 0x01},
+	{0x3952, 0x01},
+	{0x3953, 0x01},
+	{0x3954, 0x01},
+	{0x3955, 0x01},
+	{0x3956, 0x01},
+	{0x3957, 0x0e},
+	{0x3958, 0x08},
+	{0x3959, 0x08},
+	{0x395a, 0x08},
+	{0x395b, 0x13},
+	{0x395c, 0x09},
+	{0x395d, 0x05},
+	{0x395e, 0x02},
+	{0x395f, 0x00},
+	{0x395f, 0x00},
+	{0x3960, 0x00},
+	{0x3961, 0x00},
+	{0x3962, 0x00},
+	{0x3963, 0x00},
+	{0x3964, 0x00},
+	{0x3965, 0x00},
+	{0x3966, 0x00},
+	{0x3967, 0x00},
+	{0x3968, 0x01},
+	{0x3969, 0x01},
+	{0x396a, 0x01},
+	{0x396b, 0x01},
+	{0x396c, 0x10},
+	{0x396d, 0xf0},
+	{0x396e, 0x11},
+	{0x396f, 0x00},
+	{0x3970, 0x37},
+	{0x3971, 0x37},
+	{0x3972, 0x37},
+	{0x3973, 0x37},
+	{0x3974, 0x00},
+	{0x3975, 0x3c},
+	{0x3976, 0x3c},
+	{0x3977, 0x3c},
+	{0x3978, 0x3c},
+	{0x3c00, 0x0f},
+	{0x3c20, 0x01},
+	{0x3c21, 0x08},
+	{0x3f00, 0x8b},
+	{0x3f02, 0x0f},
+	{0x4000, 0xc3},
+	{0x4001, 0xe0},
+	{0x4002, 0x00},
+	{0x4003, 0x40},
+	{0x4008, 0x04},
+	{0x4009, 0x23},
+	{0x400a, 0x04},
+	{0x400b, 0x01},
+	{0x4077, 0x06},
+	{0x4078, 0x00},
+	{0x4079, 0x1a},
+	{0x407a, 0x7f},
+	{0x407b, 0x01},
+	{0x4080, 0x03},
+	{0x4081, 0x84},
+	{0x4308, 0x03},
+	{0x4309, 0xff},
+	{0x430d, 0x00},
+	{0x4806, 0x00},
+	{0x4813, 0x00},
+	{0x4837, 0x10},
+	{0x4857, 0x05},
+	{0x4500, 0x07},
+	{0x4501, 0x00},
+	{0x4503, 0x00},
+	{0x450a, 0x04},
+	{0x450e, 0x00},
+	{0x450f, 0x00},
+	{0x4900, 0x00},
+	{0x4901, 0x00},
+	{0x4902, 0x01},
+	{0x5001, 0x50},
+	{0x5006, 0x00},
+	{0x5080, 0x40},
+	{0x5181, 0x2b},
+	{0x5202, 0xa3},
+	{0x5206, 0x01},
+	{0x5207, 0x00},
+	{0x520a, 0x01},
+	{0x520b, 0x00},
+	{0x365d, 0x00},
+	{0x4815, 0x40},
+	{0x4816, 0x12},
+	{0x4f00, 0x01},
+};
+
+static const struct reg_sequence sensor_1928x1092_30fps_1lane_setting[] = {
+	{0x301b, 0xd2},
+	{0x3027, 0xe1},
+	{0x380c, 0x08},
+	{0x380d, 0xe8},
+	{0x380e, 0x04},
+	{0x380f, 0x8c},
+	{0x394e, 0x0b},
+	{0x4800, 0x24},
+	{0x5000, 0xf5},
+	/* plls */
+	{0x0303, 0x05},
+	{0x0305, 0x90},
+	{0x0316, 0x90},
+	{0x3016, 0x12},
+};
+
+static const struct reg_sequence sensor_1928x1092_30fps_2lane_setting[] = {
+	{0x301b, 0xf0},
+	{0x3027, 0xf1},
+	{0x380c, 0x04},
+	{0x380d, 0x74},
+	{0x380e, 0x09},
+	{0x380f, 0x18},
+	{0x394e, 0x0a},
+	{0x4041, 0x20},
+	{0x4884, 0x04},
+	{0x4800, 0x64},
+	{0x4d00, 0x03},
+	{0x4d01, 0xd8},
+	{0x4d02, 0xba},
+	{0x4d03, 0xa0},
+	{0x4d04, 0xb7},
+	{0x4d05, 0x34},
+	{0x4d0d, 0x00},
+	{0x5000, 0xfd},
+	{0x481f, 0x30},
+	/* plls */
+	{0x0303, 0x05},
+	{0x0305, 0x90},
+	{0x0316, 0x90},
+	{0x3016, 0x32},
+};
+
+static const char * const ov02c10_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bar",
+	"Top-Bottom Darker Color Bar",
+	"Right-Left Darker Color Bar",
+	"Color Bar type 4",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV02C10_LINK_FREQ_400MHZ,
+};
+
+static const struct ov02c10_mode supported_modes[] = {
+	{
+		.width = 1928,
+		.height = 1092,
+		.hts = 2280,
+		.vts_min = 1164,
+		.reg_sequence = sensor_1928x1092_30fps_setting,
+		.sequence_length = ARRAY_SIZE(sensor_1928x1092_30fps_setting),
+		.lane_settings = {
+			sensor_1928x1092_30fps_1lane_setting,
+			sensor_1928x1092_30fps_2lane_setting
+		},
+		.lane_settings_length = {
+			ARRAY_SIZE(sensor_1928x1092_30fps_1lane_setting),
+			ARRAY_SIZE(sensor_1928x1092_30fps_2lane_setting),
+		},
+	},
+};
+
+static const char * const ov02c10_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
+};
+
+struct ov02c10 {
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct regmap *regmap;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+
+	struct clk *img_clk;
+	struct gpio_desc *reset;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov02c10_supply_names)];
+
+	/* MIPI lane info */
+	u32 link_freq_index;
+	u8 mipi_lanes;
+};
+
+static inline struct ov02c10 *to_ov02c10(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov02c10, sd);
+}
+
+static int ov02c10_test_pattern(struct ov02c10 *ov02c10, int pattern)
+{
+	int ret = 0;
+
+	if (!pattern)
+		return cci_update_bits(ov02c10->regmap, OV02C10_REG_TEST_PATTERN,
+				       BIT(7), 0, NULL);
+
+	cci_update_bits(ov02c10->regmap, OV02C10_REG_TEST_PATTERN,
+			0x03, pattern - 1, &ret);
+	cci_update_bits(ov02c10->regmap, OV02C10_REG_TEST_PATTERN,
+			BIT(7), OV02C10_TEST_PATTERN_ENABLE, &ret);
+	return ret;
+}
+
+static int ov02c10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov02c10 *ov02c10 = container_of(ctrl->handler,
+					     struct ov02c10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02c10->sd);
+	const u32 height = supported_modes[0].height;
+	s64 exposure_max;
+	int ret = 0;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = height + ctrl->val - OV02C10_EXPOSURE_MAX_MARGIN;
+		__v4l2_ctrl_modify_range(ov02c10->exposure,
+					 ov02c10->exposure->minimum,
+					 exposure_max, ov02c10->exposure->step,
+					 exposure_max);
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		cci_write(ov02c10->regmap, OV02C10_REG_ANALOG_GAIN,
+			  ctrl->val << 4, &ret);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		cci_write(ov02c10->regmap, OV02C10_REG_DIGITAL_GAIN,
+			  ctrl->val << 6, &ret);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		cci_write(ov02c10->regmap, OV02C10_REG_EXPOSURE,
+			  ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_VBLANK:
+		cci_write(ov02c10->regmap, OV02C10_REG_VTS, height + ctrl->val,
+			  &ret);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		ret = ov02c10_test_pattern(ov02c10, ctrl->val);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov02c10_ctrl_ops = {
+	.s_ctrl = ov02c10_set_ctrl,
+};
+
+static int ov02c10_init_controls(struct ov02c10 *ov02c10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02c10->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr = &ov02c10->ctrl_handler;
+	const struct ov02c10_mode *mode = &supported_modes[0];
+	u32 vblank_min, vblank_max, vblank_default, vts_def;
+	struct v4l2_fwnode_device_properties props;
+	s64 exposure_max, h_blank, pixel_rate;
+	int ret;
+
+	v4l2_ctrl_handler_init(ctrl_hdlr, 10);
+
+	ov02c10->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						    &ov02c10_ctrl_ops,
+						    V4L2_CID_LINK_FREQ,
+						    ov02c10->link_freq_index, 0,
+						    link_freq_menu_items);
+	if (ov02c10->link_freq)
+		ov02c10->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	/* MIPI lanes are DDR -> use link-freq * 2 */
+	pixel_rate = div_u64(link_freq_menu_items[ov02c10->link_freq_index] *
+			     2 * ov02c10->mipi_lanes, OV02C10_RGB_DEPTH);
+
+	ov02c10->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops,
+						V4L2_CID_PIXEL_RATE, 0,
+						pixel_rate, 1, pixel_rate);
+
+	/*
+	 * For default multiple min by number of lanes to keep the default
+	 * FPS the same indepenedent of the lane count.
+	 */
+	vts_def = mode->vts_min * ov02c10->mipi_lanes;
+
+	vblank_min = mode->vts_min - mode->height;
+	vblank_max = OV02C10_VTS_MAX - mode->height;
+	vblank_default = vts_def - mode->height;
+	ov02c10->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops,
+					    V4L2_CID_VBLANK, vblank_min,
+					    vblank_max, 1, vblank_default);
+
+	h_blank = mode->hts - mode->width;
+	ov02c10->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops,
+					    V4L2_CID_HBLANK, h_blank, h_blank,
+					    1, h_blank);
+	if (ov02c10->hblank)
+		ov02c10->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV02C10_ANAL_GAIN_MIN, OV02C10_ANAL_GAIN_MAX,
+			  OV02C10_ANAL_GAIN_STEP, OV02C10_ANAL_GAIN_DEFAULT);
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV02C10_DGTL_GAIN_MIN, OV02C10_DGTL_GAIN_MAX,
+			  OV02C10_DGTL_GAIN_STEP, OV02C10_DGTL_GAIN_DEFAULT);
+	exposure_max = vts_def - OV02C10_EXPOSURE_MAX_MARGIN;
+	ov02c10->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov02c10_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OV02C10_EXPOSURE_MIN,
+					      exposure_max,
+					      OV02C10_EXPOSURE_STEP,
+					      exposure_max);
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov02c10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov02c10_test_pattern_menu) - 1,
+				     0, 0, ov02c10_test_pattern_menu);
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov02c10_ctrl_ops, &props);
+
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	ov02c10->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov02c10_update_pad_format(const struct ov02c10_mode *mode,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov02c10_enable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  u32 pad, u64 streams_mask)
+{
+	const struct ov02c10_mode *mode = &supported_modes[0];
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+	const struct reg_sequence *reg_sequence;
+	int ret, sequence_length;
+
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret)
+		return ret;
+
+	reg_sequence = mode->reg_sequence;
+	sequence_length = mode->sequence_length;
+	ret = regmap_multi_reg_write(ov02c10->regmap,
+				     reg_sequence, sequence_length);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode\n");
+		goto out;
+	}
+
+	reg_sequence = mode->lane_settings[ov02c10->mipi_lanes - 1];
+	sequence_length = mode->lane_settings_length[ov02c10->mipi_lanes - 1];
+	ret = regmap_multi_reg_write(ov02c10->regmap,
+				     reg_sequence, sequence_length);
+	if (ret) {
+		dev_err(&client->dev, "failed to write lane settings\n");
+		goto out;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov02c10->sd.ctrl_handler);
+	if (ret)
+		goto out;
+
+	ret = cci_write(ov02c10->regmap, OV02C10_REG_STREAM_CONTROL, 1, NULL);
+out:
+	if (ret)
+		pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static int ov02c10_disable_streams(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   u32 pad, u64 streams_mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+
+	cci_write(ov02c10->regmap, OV02C10_REG_STREAM_CONTROL, 0, NULL);
+	pm_runtime_put(&client->dev);
+
+	return 0;
+}
+
+/* This function tries to get power control resources */
+static int ov02c10_get_pm_resources(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+	int i;
+
+	ov02c10->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov02c10->reset))
+		return dev_err_probe(dev, PTR_ERR(ov02c10->reset),
+				     "failed to get reset gpio\n");
+
+	for (i = 0; i < ARRAY_SIZE(ov02c10_supply_names); i++)
+		ov02c10->supplies[i].supply = ov02c10_supply_names[i];
+
+	return devm_regulator_bulk_get(dev, ARRAY_SIZE(ov02c10_supply_names),
+				       ov02c10->supplies);
+}
+
+static int ov02c10_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+
+	gpiod_set_value_cansleep(ov02c10->reset, 1);
+
+	regulator_bulk_disable(ARRAY_SIZE(ov02c10_supply_names),
+			       ov02c10->supplies);
+
+	clk_disable_unprepare(ov02c10->img_clk);
+
+	return 0;
+}
+
+static int ov02c10_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+	int ret;
+
+	ret = clk_prepare_enable(ov02c10->img_clk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable imaging clock: %d", ret);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov02c10_supply_names),
+				    ov02c10->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators: %d", ret);
+		clk_disable_unprepare(ov02c10->img_clk);
+		return ret;
+	}
+
+	if (ov02c10->reset) {
+		/* Assert reset for at least 2ms on back to back off-on */
+		usleep_range(2000, 2200);
+		gpiod_set_value_cansleep(ov02c10->reset, 0);
+		usleep_range(5000, 5100);
+	}
+
+	return 0;
+}
+
+static int ov02c10_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	const struct ov02c10_mode *mode = &supported_modes[0];
+	struct ov02c10 *ov02c10 = to_ov02c10(sd);
+	s32 vblank_def, h_blank;
+
+	ov02c10_update_pad_format(mode, &fmt->format);
+	*v4l2_subdev_state_get_format(sd_state, fmt->pad) = fmt->format;
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		return 0;
+
+	/* Update limits and set FPS to default */
+	vblank_def = mode->vts_min * ov02c10->mipi_lanes - mode->height;
+	__v4l2_ctrl_modify_range(ov02c10->vblank, mode->vts_min - mode->height,
+				 OV02C10_VTS_MAX - mode->height, 1, vblank_def);
+	__v4l2_ctrl_s_ctrl(ov02c10->vblank, vblank_def);
+	h_blank = mode->hts - mode->width;
+	__v4l2_ctrl_modify_range(ov02c10->hblank, h_blank, h_blank, 1, h_blank);
+
+	return 0;
+}
+
+static int ov02c10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov02c10_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov02c10_init_state(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state)
+{
+	ov02c10_update_pad_format(&supported_modes[0],
+				  v4l2_subdev_state_get_format(sd_state, 0));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov02c10_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_pad_ops ov02c10_pad_ops = {
+	.set_fmt = ov02c10_set_format,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.enum_mbus_code = ov02c10_enum_mbus_code,
+	.enum_frame_size = ov02c10_enum_frame_size,
+	.enable_streams = ov02c10_enable_streams,
+	.disable_streams = ov02c10_disable_streams,
+};
+
+static const struct v4l2_subdev_ops ov02c10_subdev_ops = {
+	.video = &ov02c10_video_ops,
+	.pad = &ov02c10_pad_ops,
+};
+
+static const struct media_entity_operations ov02c10_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov02c10_internal_ops = {
+	.init_state = ov02c10_init_state,
+};
+
+static int ov02c10_identify_module(struct ov02c10 *ov02c10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02c10->sd);
+	u64 chip_id;
+	int ret;
+
+	ret = cci_read(ov02c10->regmap, OV02C10_REG_CHIP_ID, &chip_id, NULL);
+	if (ret)
+		return ret;
+
+	if (chip_id != OV02C10_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%llx",
+			OV02C10_CHIP_ID, chip_id);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int ov02c10_check_hwcfg(struct device *dev, struct ov02c10 *ov02c10)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep, *fwnode = dev_fwnode(dev);
+	unsigned long link_freq_bitmap;
+	u32 mclk;
+	int ret;
+
+	/*
+	 * Sometimes the fwnode graph is initialized by the bridge driver,
+	 * wait for this.
+	 */
+	ep = fwnode_graph_get_endpoint_by_id(fwnode, 0, 0, 0);
+	if (!ep)
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
+
+	ov02c10->img_clk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov02c10->img_clk)) {
+		fwnode_handle_put(ep);
+		return dev_err_probe(dev, PTR_ERR(ov02c10->img_clk),
+				     "failed to get imaging clock\n");
+	}
+
+	if (ov02c10->img_clk) {
+		mclk = clk_get_rate(ov02c10->img_clk);
+	} else {
+		ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
+		if (ret) {
+			fwnode_handle_put(ep);
+			return dev_err_probe(dev, ret,
+					     "reading clock-frequency property\n");
+		}
+	}
+
+	if (mclk != OV02C10_MCLK) {
+		fwnode_handle_put(ep);
+		return dev_err_probe(dev, -EINVAL,
+				     "external clock %u is not supported\n",
+				     mclk);
+	}
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return dev_err_probe(dev, ret, "parsing endpoint failed\n");
+
+	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       link_freq_menu_items,
+				       ARRAY_SIZE(link_freq_menu_items),
+				       &link_freq_bitmap);
+	if (ret)
+		goto check_hwcfg_error;
+
+	/* v4l2_link_freq_to_bitmap() guarantees at least 1 bit is set */
+	ov02c10->link_freq_index = ffs(link_freq_bitmap) - 1;
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 1 &&
+	    bus_cfg.bus.mipi_csi2.num_data_lanes != 2) {
+		ret = dev_err_probe(dev, -EINVAL,
+				    "number of CSI2 data lanes %u is not supported\n",
+				    bus_cfg.bus.mipi_csi2.num_data_lanes);
+		goto check_hwcfg_error;
+	}
+
+	ov02c10->mipi_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+
+check_hwcfg_error:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+	return ret;
+}
+
+static void ov02c10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev)) {
+		ov02c10_power_off(&client->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
+}
+
+static int ov02c10_probe(struct i2c_client *client)
+{
+	struct ov02c10 *ov02c10;
+	int ret;
+
+	ov02c10 = devm_kzalloc(&client->dev, sizeof(*ov02c10), GFP_KERNEL);
+	if (!ov02c10)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ov02c10->sd, client, &ov02c10_subdev_ops);
+
+	/* Check HW config */
+	ret = ov02c10_check_hwcfg(&client->dev, ov02c10);
+	if (ret)
+		return ret;
+
+	ret = ov02c10_get_pm_resources(&client->dev);
+	if (ret)
+		return ret;
+
+	ov02c10->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(ov02c10->regmap))
+		return PTR_ERR(ov02c10->regmap);
+
+	ret = ov02c10_power_on(&client->dev);
+	if (ret) {
+		dev_err_probe(&client->dev, ret, "failed to power on\n");
+		return ret;
+	}
+
+	ret = ov02c10_identify_module(ov02c10);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d", ret);
+		goto probe_error_power_off;
+	}
+
+	ret = ov02c10_init_controls(ov02c10);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov02c10->sd.internal_ops = &ov02c10_internal_ops;
+	ov02c10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov02c10->sd.entity.ops = &ov02c10_subdev_entity_ops;
+	ov02c10->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov02c10->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov02c10->sd.entity, 1, &ov02c10->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov02c10->sd.state_lock = ov02c10->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&ov02c10->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to init subdev: %d", ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov02c10->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_v4l2_subdev_cleanup;
+	}
+
+	pm_runtime_idle(&client->dev);
+	return 0;
+
+probe_error_v4l2_subdev_cleanup:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	v4l2_subdev_cleanup(&ov02c10->sd);
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov02c10->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov02c10->sd.ctrl_handler);
+
+probe_error_power_off:
+	ov02c10_power_off(&client->dev);
+
+	return ret;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(ov02c10_pm_ops, ov02c10_power_off,
+				 ov02c10_power_on, NULL);
+
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id ov02c10_acpi_ids[] = {
+	{ "OVTI02C1" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov02c10_acpi_ids);
+#endif
+
+static const struct of_device_id ov02c10_of_match[] = {
+	{ .compatible = "ovti,ov02c10" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov02c10_of_match);
+
+static struct i2c_driver ov02c10_i2c_driver = {
+	.driver = {
+		.name = "ov02c10",
+		.pm = pm_sleep_ptr(&ov02c10_pm_ops),
+		.acpi_match_table = ACPI_PTR(ov02c10_acpi_ids),
+		.of_match_table = ov02c10_of_match,
+	},
+	.probe = ov02c10_probe,
+	.remove = ov02c10_remove,
+};
+
+module_i2c_driver(ov02c10_i2c_driver);
+
+MODULE_AUTHOR("Hao Yao <hao.yao@intel.com>");
+MODULE_AUTHOR("Heimir Thor Sverrisson <heimir.sverrisson@gmail.com>");
+MODULE_AUTHOR("Hans de Goede <hansg@kernel.org>");
+MODULE_DESCRIPTION("OmniVision OV02C10 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov02e10.c b/drivers/media/i2c/ov02e10.c
new file mode 100644
index 000000000000..d74dc62e189d
--- /dev/null
+++ b/drivers/media/i2c/ov02e10.c
@@ -0,0 +1,969 @@
+// SPDX-License-Identifier: GPL-2.0
+// Copyright (c) 2023 Intel Corporation.
+
+#include <linux/acpi.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#define OV02E10_LINK_FREQ_360MHZ	360000000ULL
+#define OV02E10_SCLK			36000000LL
+#define OV02E10_MCLK			19200000
+#define OV02E10_DATA_LANES		2
+#define OV02E10_RGB_DEPTH		10
+
+#define OV02E10_REG_PAGE_FLAG		CCI_REG8(0xfd)
+#define OV02E10_PAGE_0			0x0
+#define OV02E10_PAGE_1			0x1
+#define OV02E10_PAGE_2			0x2
+#define OV02E10_PAGE_3			0x3
+#define OV02E10_PAGE_5			0x4
+#define OV02E10_PAGE_7			0x5
+#define OV02E10_PAGE_8			0x6
+#define OV02E10_PAGE_9			0xF
+#define OV02E10_PAGE_D			0x8
+#define OV02E10_PAGE_E			0x9
+#define OV02E10_PAGE_F			0xA
+
+#define OV02E10_REG_CHIP_ID		CCI_REG32(0x00)
+#define OV02E10_CHIP_ID			0x45025610
+
+/* Horizontal and vertical flip */
+#define OV02E10_REG_ORIENTATION		CCI_REG8(0x32)
+
+/* vertical-timings from sensor */
+#define OV02E10_REG_VTS			CCI_REG16(0x35)
+#define OV02E10_VTS_DEF			2244
+#define OV02E10_VTS_MIN			2244
+#define OV02E10_VTS_MAX			0x7fff
+
+/* horizontal-timings from sensor */
+#define OV02E10_REG_HTS			CCI_REG16(0x37)
+
+/* Exposure controls from sensor */
+#define OV02E10_REG_EXPOSURE		CCI_REG16(0x03)
+#define OV02E10_EXPOSURE_MIN		1
+#define OV02E10_EXPOSURE_MAX_MARGIN	2
+#define OV02E10_EXPOSURE_STEP		1
+
+/* Analog gain controls from sensor */
+#define OV02E10_REG_ANALOG_GAIN		CCI_REG8(0x24)
+#define OV02E10_ANAL_GAIN_MIN		0x10
+#define OV02E10_ANAL_GAIN_MAX		0xf8
+#define OV02E10_ANAL_GAIN_STEP		1
+
+/* Digital gain controls from sensor */
+#define OV02E10_REG_DIGITAL_GAIN	CCI_REG16(0x21)
+#define OV02E10_DGTL_GAIN_MIN		256
+#define OV02E10_DGTL_GAIN_MAX		1020
+#define OV02E10_DGTL_GAIN_STEP		1
+#define OV02E10_DGTL_GAIN_DEFAULT	256
+
+/* Register update control */
+#define OV02E10_REG_COMMAND_UPDATE	CCI_REG8(0xE7)
+#define OV02E10_COMMAND_UPDATE		0x00
+#define OV02E10_COMMAND_HOLD		0x01
+
+/* Test Pattern Control */
+#define OV02E10_REG_TEST_PATTERN	CCI_REG8(0x12)
+#define OV02E10_TEST_PATTERN_ENABLE	BIT(0)
+#define OV02E10_TEST_PATTERN_BAR_SHIFT	1
+
+struct reg_sequence_list {
+	u32 num_regs;
+	const struct reg_sequence *regs;
+};
+
+struct ov02e10_mode {
+	/* Frame width in pixels */
+	u32 width;
+
+	/* Frame height in pixels */
+	u32 height;
+
+	/* Horizontal timining size */
+	u32 hts;
+
+	/* Default vertical timing */
+	u32 vts_def;
+
+	/* Min vertical timining size */
+	u32 vts_min;
+
+	/* Sensor register settings for this resolution */
+	const struct reg_sequence_list reg_list;
+};
+
+static const struct reg_sequence mode_1928x1088_30fps_2lane[] = {
+	{ 0xfd, 0x00 },
+	{ 0x20, 0x00 },
+	{ 0x20, 0x0b },
+	{ 0x21, 0x02 },
+	{ 0x10, 0x23 },
+	{ 0xc5, 0x04 },
+	{ 0x21, 0x00 },
+	{ 0x14, 0x96 },
+	{ 0x17, 0x01 },
+	{ 0xfd, 0x01 },
+	{ 0x03, 0x00 },
+	{ 0x04, 0x04 },
+	{ 0x05, 0x04 },
+	{ 0x06, 0x62 },
+	{ 0x07, 0x01 },
+	{ 0x22, 0x80 },
+	{ 0x24, 0xff },
+	{ 0x40, 0xc6 },
+	{ 0x41, 0x18 },
+	{ 0x45, 0x3f },
+	{ 0x48, 0x0c },
+	{ 0x4c, 0x08 },
+	{ 0x51, 0x12 },
+	{ 0x52, 0x10 },
+	{ 0x57, 0x98 },
+	{ 0x59, 0x06 },
+	{ 0x5a, 0x04 },
+	{ 0x5c, 0x38 },
+	{ 0x5e, 0x10 },
+	{ 0x67, 0x11 },
+	{ 0x7b, 0x04 },
+	{ 0x81, 0x12 },
+	{ 0x90, 0x51 },
+	{ 0x91, 0x09 },
+	{ 0x92, 0x21 },
+	{ 0x93, 0x28 },
+	{ 0x95, 0x54 },
+	{ 0x9d, 0x20 },
+	{ 0x9e, 0x04 },
+	{ 0xb1, 0x9a },
+	{ 0xb2, 0x86 },
+	{ 0xb6, 0x3f },
+	{ 0xb9, 0x30 },
+	{ 0xc1, 0x01 },
+	{ 0xc5, 0xa0 },
+	{ 0xc6, 0x73 },
+	{ 0xc7, 0x04 },
+	{ 0xc8, 0x25 },
+	{ 0xc9, 0x05 },
+	{ 0xca, 0x28 },
+	{ 0xcb, 0x00 },
+	{ 0xcf, 0x16 },
+	{ 0xd2, 0xd0 },
+	{ 0xd7, 0x3f },
+	{ 0xd8, 0x40 },
+	{ 0xd9, 0x40 },
+	{ 0xda, 0x44 },
+	{ 0xdb, 0x3d },
+	{ 0xdc, 0x3d },
+	{ 0xdd, 0x3d },
+	{ 0xde, 0x3d },
+	{ 0xdf, 0xf0 },
+	{ 0xea, 0x0f },
+	{ 0xeb, 0x04 },
+	{ 0xec, 0x29 },
+	{ 0xee, 0x47 },
+	{ 0xfd, 0x01 },
+	{ 0x31, 0x01 },
+	{ 0x27, 0x00 },
+	{ 0x2f, 0x41 },
+	{ 0xfd, 0x02 },
+	{ 0xa1, 0x01 },
+	{ 0xfd, 0x02 },
+	{ 0x9a, 0x03 },
+	{ 0xfd, 0x03 },
+	{ 0x9d, 0x0f },
+	{ 0xfd, 0x07 },
+	{ 0x42, 0x00 },
+	{ 0x43, 0xad },
+	{ 0x44, 0x00 },
+	{ 0x45, 0xa8 },
+	{ 0x46, 0x00 },
+	{ 0x47, 0xa8 },
+	{ 0x48, 0x00 },
+	{ 0x49, 0xad },
+	{ 0xfd, 0x00 },
+	{ 0xc4, 0x01 },
+	{ 0xfd, 0x01 },
+	{ 0x33, 0x03 },
+	{ 0xfd, 0x00 },
+	{ 0x20, 0x1f },
+};
+
+static const char *const ov02e10_test_pattern_menu[] = {
+	"Disabled",
+	"Color Bar",
+};
+
+static const s64 link_freq_menu_items[] = {
+	OV02E10_LINK_FREQ_360MHZ,
+};
+
+static const struct ov02e10_mode supported_modes[] = {
+	{
+		.width = 1928,
+		.height = 1088,
+		.hts = 534,
+		.vts_def = 2244,
+		.vts_min = 2244,
+		.reg_list = {
+			.num_regs = ARRAY_SIZE(mode_1928x1088_30fps_2lane),
+			.regs = mode_1928x1088_30fps_2lane,
+		},
+	},
+};
+
+static const char * const ov02e10_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
+};
+
+struct ov02e10 {
+	struct regmap *regmap;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+
+	/* V4L2 Controls */
+	struct v4l2_ctrl *link_freq;
+	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
+	struct v4l2_ctrl *exposure;
+	struct v4l2_ctrl *vflip;
+	struct v4l2_ctrl *hflip;
+
+	struct clk *img_clk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov02e10_supply_names)];
+	struct gpio_desc *reset;
+
+	/* Current mode */
+	const struct ov02e10_mode *cur_mode;
+
+	/* MIPI lanes info */
+	u32 link_freq_index;
+	u8 mipi_lanes;
+};
+
+static inline struct ov02e10 *to_ov02e10(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct ov02e10, sd);
+}
+
+static u64 to_pixel_rate(u32 f_index)
+{
+	u64 pixel_rate = link_freq_menu_items[f_index] * 2 * OV02E10_DATA_LANES;
+
+	do_div(pixel_rate, OV02E10_RGB_DEPTH);
+
+	return pixel_rate;
+}
+
+static u64 to_pixels_per_line(u32 hts, u32 f_index)
+{
+	u64 ppl = hts * to_pixel_rate(f_index);
+
+	do_div(ppl, OV02E10_SCLK);
+
+	return ppl;
+}
+
+static void ov02e10_test_pattern(struct ov02e10 *ov02e10, u32 pattern, int *pret)
+{
+	if (pattern)
+		pattern = pattern << OV02E10_TEST_PATTERN_BAR_SHIFT |
+			  OV02E10_TEST_PATTERN_ENABLE;
+
+	cci_write(ov02e10->regmap, OV02E10_REG_TEST_PATTERN, pattern, pret);
+}
+
+static int ov02e10_set_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct ov02e10 *ov02e10 = container_of(ctrl->handler,
+					       struct ov02e10, ctrl_handler);
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02e10->sd);
+	s64 exposure_max;
+	int ret;
+
+	/* Propagate change of current control to all related controls */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		/* Update max exposure while meeting expected vblanking */
+		exposure_max = ov02e10->cur_mode->height + ctrl->val -
+			       OV02E10_EXPOSURE_MAX_MARGIN;
+		ret = __v4l2_ctrl_modify_range(ov02e10->exposure,
+					       ov02e10->exposure->minimum,
+					       exposure_max,
+					       ov02e10->exposure->step,
+					       exposure_max);
+		if (ret)
+			return ret;
+	}
+
+	/* V4L2 controls values will be applied only when power is already up */
+	if (!pm_runtime_get_if_in_use(&client->dev))
+		return 0;
+
+	ret = cci_write(ov02e10->regmap, OV02E10_REG_COMMAND_UPDATE,
+			OV02E10_COMMAND_HOLD, NULL);
+
+	switch (ctrl->id) {
+	case V4L2_CID_ANALOGUE_GAIN:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		cci_write(ov02e10->regmap, OV02E10_REG_ANALOG_GAIN,
+			  ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_DIGITAL_GAIN:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		cci_write(ov02e10->regmap, OV02E10_REG_DIGITAL_GAIN,
+			  ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_EXPOSURE:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		cci_write(ov02e10->regmap, OV02E10_REG_EXPOSURE,
+			  ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_HFLIP:
+	case V4L2_CID_VFLIP:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		cci_write(ov02e10->regmap, OV02E10_REG_ORIENTATION,
+			  ov02e10->hflip->val | ov02e10->vflip->val << 1, &ret);
+		break;
+	case V4L2_CID_VBLANK:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		cci_write(ov02e10->regmap, OV02E10_REG_VTS,
+			  ov02e10->cur_mode->height + ctrl->val, &ret);
+		break;
+
+	case V4L2_CID_TEST_PATTERN:
+		cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			  OV02E10_PAGE_1, &ret);
+		ov02e10_test_pattern(ov02e10, ctrl->val, &ret);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	cci_write(ov02e10->regmap, OV02E10_REG_COMMAND_UPDATE,
+		  OV02E10_COMMAND_UPDATE, &ret);
+
+	pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops ov02e10_ctrl_ops = {
+	.s_ctrl = ov02e10_set_ctrl,
+};
+
+static int ov02e10_init_controls(struct ov02e10 *ov02e10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02e10->sd);
+	struct v4l2_ctrl_handler *ctrl_hdlr = &ov02e10->ctrl_handler;
+	const struct ov02e10_mode *mode = ov02e10->cur_mode;
+	u32 vblank_min, vblank_max, vblank_def;
+	struct v4l2_fwnode_device_properties props;
+	s64 exposure_max, h_blank, pixel_rate;
+	int ret;
+
+	v4l2_ctrl_handler_init(ctrl_hdlr, 12);
+
+	ov02e10->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
+						    &ov02e10_ctrl_ops,
+						    V4L2_CID_LINK_FREQ,
+						    ov02e10->link_freq_index,
+						    0, link_freq_menu_items);
+	if (ov02e10->link_freq)
+		ov02e10->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	pixel_rate = to_pixel_rate(ov02e10->link_freq_index);
+	ov02e10->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+						V4L2_CID_PIXEL_RATE, 0,
+						pixel_rate, 1, pixel_rate);
+
+	vblank_min = mode->vts_min - mode->height;
+	vblank_max = OV02E10_VTS_MAX - mode->height;
+	vblank_def = mode->vts_def - mode->height;
+	ov02e10->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+					    V4L2_CID_VBLANK, vblank_min,
+					    vblank_max, 1, vblank_def);
+
+	h_blank = mode->hts - mode->width;
+	ov02e10->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+					    V4L2_CID_HBLANK, h_blank, h_blank,
+					    1, h_blank);
+	if (ov02e10->hblank)
+		ov02e10->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
+			  OV02E10_ANAL_GAIN_MIN, OV02E10_ANAL_GAIN_MAX,
+			  OV02E10_ANAL_GAIN_STEP, OV02E10_ANAL_GAIN_MIN);
+
+	v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV02E10_DGTL_GAIN_MIN, OV02E10_DGTL_GAIN_MAX,
+			  OV02E10_DGTL_GAIN_STEP, OV02E10_DGTL_GAIN_DEFAULT);
+
+	exposure_max = mode->vts_def - OV02E10_EXPOSURE_MAX_MARGIN;
+	ov02e10->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+					      V4L2_CID_EXPOSURE,
+					      OV02E10_EXPOSURE_MIN,
+					      exposure_max,
+					      OV02E10_EXPOSURE_STEP,
+					      exposure_max);
+
+	ov02e10->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+					   V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (ov02e10->hflip)
+		ov02e10->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	ov02e10->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &ov02e10_ctrl_ops,
+					   V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (ov02e10->vflip)
+		ov02e10->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &ov02e10_ctrl_ops,
+				     V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(ov02e10_test_pattern_menu) - 1,
+				     0, 0, ov02e10_test_pattern_menu);
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret)
+		return ret;
+
+	v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov02e10_ctrl_ops, &props);
+
+	if (ctrl_hdlr->error)
+		return ctrl_hdlr->error;
+
+	ov02e10->sd.ctrl_handler = ctrl_hdlr;
+
+	return 0;
+}
+
+static void ov02e10_update_pad_format(const struct ov02e10_mode *mode,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+	fmt->field = V4L2_FIELD_NONE;
+}
+
+static int ov02e10_set_stream_mode(struct ov02e10 *ov02e10, u8 val)
+{
+	int ret = 0;
+
+	cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG, OV02E10_PAGE_0, &ret);
+	cci_write(ov02e10->regmap, CCI_REG8(0xa0), val, &ret);
+	cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG, OV02E10_PAGE_1, &ret);
+	cci_write(ov02e10->regmap, CCI_REG8(0x01), 0x02, &ret);
+
+	return ret;
+}
+
+static int ov02e10_enable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  u32 pad, u64 streams_mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+	const struct reg_sequence_list *reg_list;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret)
+		return ret;
+
+	reg_list = &ov02e10->cur_mode->reg_list;
+	ret = regmap_multi_reg_write(ov02e10->regmap, reg_list->regs,
+				     reg_list->num_regs);
+	if (ret) {
+		dev_err(&client->dev, "failed to set mode\n");
+		goto out;
+	}
+
+	ret = __v4l2_ctrl_handler_setup(ov02e10->sd.ctrl_handler);
+	if (ret)
+		goto out;
+
+	ret = ov02e10_set_stream_mode(ov02e10, 1);
+
+out:
+	if (ret)
+		pm_runtime_put(&client->dev);
+
+	return ret;
+}
+
+static int ov02e10_disable_streams(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state,
+				   u32 pad, u64 streams_mask)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+
+	ov02e10_set_stream_mode(ov02e10, 0);
+	pm_runtime_put(&client->dev);
+
+	return 0;
+}
+
+static int ov02e10_get_pm_resources(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+	int i;
+
+	ov02e10->reset = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov02e10->reset))
+		return dev_err_probe(dev, PTR_ERR(ov02e10->reset),
+				     "failed to get reset gpio\n");
+
+	for (i = 0; i < ARRAY_SIZE(ov02e10_supply_names); i++)
+		ov02e10->supplies[i].supply = ov02e10_supply_names[i];
+
+	return devm_regulator_bulk_get(dev, ARRAY_SIZE(ov02e10_supply_names),
+				       ov02e10->supplies);
+}
+
+static int ov02e10_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+
+	if (ov02e10->reset)
+		gpiod_set_value_cansleep(ov02e10->reset, 1);
+
+	regulator_bulk_disable(ARRAY_SIZE(ov02e10_supply_names),
+			       ov02e10->supplies);
+
+	clk_disable_unprepare(ov02e10->img_clk);
+
+	return 0;
+}
+
+static int ov02e10_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+	int ret;
+
+	ret = clk_prepare_enable(ov02e10->img_clk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable imaging clock: %d\n", ret);
+		return ret;
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov02e10_supply_names),
+				    ov02e10->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	if (ov02e10->reset) {
+		usleep_range(5000, 5100);
+		gpiod_set_value_cansleep(ov02e10->reset, 0);
+		usleep_range(8000, 8100);
+	}
+
+	return 0;
+
+disable_clk:
+	clk_disable_unprepare(ov02e10->img_clk);
+
+	return ret;
+}
+
+static int ov02e10_set_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+	const struct ov02e10_mode *mode;
+	s32 vblank_def, h_blank;
+	int ret = 0;
+
+	mode = v4l2_find_nearest_size(supported_modes,
+				      ARRAY_SIZE(supported_modes),
+				      width, height, fmt->format.width,
+				      fmt->format.height);
+
+	ov02e10_update_pad_format(mode, &fmt->format);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*v4l2_subdev_state_get_format(sd_state, fmt->pad) = fmt->format;
+	} else {
+		ov02e10->cur_mode = mode;
+		ret = __v4l2_ctrl_s_ctrl(ov02e10->link_freq,
+					 ov02e10->link_freq_index);
+		if (ret)
+			return ret;
+
+		ret = __v4l2_ctrl_s_ctrl_int64(ov02e10->pixel_rate,
+					       to_pixel_rate(ov02e10->link_freq_index));
+		if (ret)
+			return ret;
+
+		/* Update limits and set FPS to default */
+		vblank_def = mode->vts_def - mode->height;
+		ret = __v4l2_ctrl_modify_range(ov02e10->vblank,
+					       mode->vts_min - mode->height,
+					       OV02E10_VTS_MAX - mode->height,
+					       1, vblank_def);
+		if (ret)
+			return ret;
+
+		ret = __v4l2_ctrl_s_ctrl(ov02e10->vblank, vblank_def);
+		if (ret)
+			return ret;
+
+		h_blank = to_pixels_per_line(mode->hts, ov02e10->link_freq_index);
+		h_blank -= mode->width;
+		ret = __v4l2_ctrl_modify_range(ov02e10->hblank, h_blank,
+					       h_blank, 1, h_blank);
+	}
+
+	return ret;
+}
+
+static int ov02e10_get_format(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct ov02e10 *ov02e10 = to_ov02e10(sd);
+
+	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
+		fmt->format = *v4l2_subdev_state_get_format(sd_state, fmt->pad);
+	else
+		ov02e10_update_pad_format(ov02e10->cur_mode, &fmt->format);
+
+	return 0;
+}
+
+static int ov02e10_enum_mbus_code(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index > 0)
+		return -EINVAL;
+
+	code->code = MEDIA_BUS_FMT_SGRBG10_1X10;
+
+	return 0;
+}
+
+static int ov02e10_enum_frame_size(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *sd_state,
+				   struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(supported_modes))
+		return -EINVAL;
+
+	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
+		return -EINVAL;
+
+	fse->min_width = supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static int ov02e10_init_state(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state)
+{
+	ov02e10_update_pad_format(&supported_modes[0],
+				  v4l2_subdev_state_get_format(sd_state, 0));
+
+	return 0;
+}
+
+static const struct v4l2_subdev_video_ops ov02e10_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_pad_ops ov02e10_pad_ops = {
+	.set_fmt = ov02e10_set_format,
+	.get_fmt = ov02e10_get_format,
+	.enum_mbus_code = ov02e10_enum_mbus_code,
+	.enum_frame_size = ov02e10_enum_frame_size,
+	.enable_streams = ov02e10_enable_streams,
+	.disable_streams = ov02e10_disable_streams,
+};
+
+static const struct v4l2_subdev_ops ov02e10_subdev_ops = {
+	.video = &ov02e10_video_ops,
+	.pad = &ov02e10_pad_ops,
+};
+
+static const struct media_entity_operations ov02e10_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops ov02e10_internal_ops = {
+	.init_state = ov02e10_init_state,
+};
+
+static int ov02e10_identify_module(struct ov02e10 *ov02e10)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov02e10->sd);
+	int ret;
+	u64 val;
+
+	ret = cci_write(ov02e10->regmap, OV02E10_REG_PAGE_FLAG,
+			OV02E10_PAGE_0, NULL);
+	cci_read(ov02e10->regmap, OV02E10_REG_CHIP_ID, &val, &ret);
+	if (ret)
+		return ret;
+
+	if (val != OV02E10_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV02E10_CHIP_ID, (u32)val);
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+static int ov02e10_check_hwcfg(struct device *dev, struct ov02e10 *ov02e10)
+{
+	struct v4l2_fwnode_endpoint bus_cfg = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY
+	};
+	struct fwnode_handle *ep;
+	struct fwnode_handle *fwnode = dev_fwnode(dev);
+	unsigned long link_freq_bitmap;
+	u32 ext_clk;
+	int ret;
+
+	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
+	if (!ep)
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
+	fwnode_handle_put(ep);
+	if (ret)
+		return dev_err_probe(dev, ret, "parsing endpoint failed\n");
+
+	ov02e10->img_clk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov02e10->img_clk)) {
+		ret = dev_err_probe(dev, PTR_ERR(ov02e10->img_clk),
+				    "failed to get imaging clock\n");
+		goto out_err;
+	}
+
+	if (ov02e10->img_clk) {
+		ext_clk = clk_get_rate(ov02e10->img_clk);
+	} else {
+		ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+					       &ext_clk);
+		if (ret) {
+			dev_err(dev, "can't get clock frequency\n");
+			goto out_err;
+		}
+	}
+
+	if (ext_clk != OV02E10_MCLK) {
+		dev_err(dev, "external clock %d is not supported\n",
+			ext_clk);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV02E10_DATA_LANES) {
+		dev_err(dev, "number of CSI2 data lanes %d is not supported\n",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined\n");
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       link_freq_menu_items,
+				       ARRAY_SIZE(link_freq_menu_items),
+				       &link_freq_bitmap);
+	if (ret)
+		goto out_err;
+
+	/* v4l2_link_freq_to_bitmap() guarantees at least 1 bit is set */
+	ov02e10->link_freq_index = ffs(link_freq_bitmap) - 1;
+	ov02e10->mipi_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+
+out_err:
+	v4l2_fwnode_endpoint_free(&bus_cfg);
+
+	return ret;
+}
+
+static void ov02e10_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_async_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+	v4l2_ctrl_handler_free(sd->ctrl_handler);
+	pm_runtime_disable(&client->dev);
+
+	if (!pm_runtime_status_suspended(&client->dev)) {
+		ov02e10_power_off(&client->dev);
+		pm_runtime_set_suspended(&client->dev);
+	}
+}
+
+static int ov02e10_probe(struct i2c_client *client)
+{
+	struct ov02e10 *ov02e10;
+	int ret;
+
+	ov02e10 = devm_kzalloc(&client->dev, sizeof(*ov02e10), GFP_KERNEL);
+	if (!ov02e10)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&ov02e10->sd, client, &ov02e10_subdev_ops);
+
+	/* Check HW config */
+	ret = ov02e10_check_hwcfg(&client->dev, ov02e10);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	ov02e10->regmap = devm_cci_regmap_init_i2c(client, 8);
+	if (IS_ERR(ov02e10->regmap))
+		return PTR_ERR(ov02e10->regmap);
+
+	ret = ov02e10_get_pm_resources(&client->dev);
+	if (ret)
+		return ret;
+
+	ret = ov02e10_power_on(&client->dev);
+	if (ret) {
+		dev_err_probe(&client->dev, ret, "failed to power on\n");
+		return ret;
+	}
+
+	/* Check module identity */
+	ret = ov02e10_identify_module(ov02e10);
+	if (ret) {
+		dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+		goto probe_error_power_off;
+	}
+
+	ov02e10->cur_mode = &supported_modes[0];
+	ret = ov02e10_init_controls(ov02e10);
+	if (ret) {
+		dev_err(&client->dev, "failed to init controls: %d\n", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	/* Initialize subdev */
+	ov02e10->sd.internal_ops = &ov02e10_internal_ops;
+	ov02e10->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	ov02e10->sd.entity.ops = &ov02e10_subdev_entity_ops;
+	ov02e10->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+
+	/* Initialize source pad */
+	ov02e10->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&ov02e10->sd.entity, 1, &ov02e10->pad);
+	if (ret) {
+		dev_err(&client->dev, "failed to init entity pads: %d", ret);
+		goto probe_error_v4l2_ctrl_handler_free;
+	}
+
+	ov02e10->sd.state_lock = ov02e10->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&ov02e10->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to init subdev: %d", ret);
+		goto probe_error_media_entity_cleanup;
+	}
+
+	pm_runtime_set_active(&client->dev);
+	pm_runtime_enable(&client->dev);
+
+	ret = v4l2_async_register_subdev_sensor(&ov02e10->sd);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to register V4L2 subdev: %d",
+			ret);
+		goto probe_error_v4l2_subdev_cleanup;
+	}
+
+	pm_runtime_idle(&client->dev);
+	return 0;
+
+probe_error_v4l2_subdev_cleanup:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	v4l2_subdev_cleanup(&ov02e10->sd);
+
+probe_error_media_entity_cleanup:
+	media_entity_cleanup(&ov02e10->sd.entity);
+
+probe_error_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(ov02e10->sd.ctrl_handler);
+
+probe_error_power_off:
+	ov02e10_power_off(&client->dev);
+
+	return ret;
+}
+
+static DEFINE_RUNTIME_DEV_PM_OPS(ov02e10_pm_ops, ov02e10_power_off,
+				 ov02e10_power_on, NULL);
+
+static const struct acpi_device_id ov02e10_acpi_ids[] = {
+	{ "OVTI02E1" },
+	{ /* sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(acpi, ov02e10_acpi_ids);
+
+static const struct of_device_id ov02e10_of_match[] = {
+	{ .compatible = "ovti,ov02e10" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov02e10_of_match);
+
+static struct i2c_driver ov02e10_i2c_driver = {
+	.driver = {
+		.name = "ov02e10",
+		.pm = pm_sleep_ptr(&ov02e10_pm_ops),
+		.acpi_match_table = ov02e10_acpi_ids,
+		.of_match_table = ov02e10_of_match,
+	},
+	.probe = ov02e10_probe,
+	.remove = ov02e10_remove,
+};
+
+module_i2c_driver(ov02e10_i2c_driver);
+
+MODULE_AUTHOR("Jingjing Xiong <jingjing.xiong@intel.com>");
+MODULE_AUTHOR("Hans de Goede <hdegoede@redhat.com>");
+MODULE_AUTHOR("Alan Stern <stern@rowland.harvard.edu>");
+MODULE_AUTHOR("Bryan O'Donoghue <bryan.odonoghue@linaro.org>");
+MODULE_DESCRIPTION("OmniVision OV02E10 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/ov08x40.c b/drivers/media/i2c/ov08x40.c
index 48df077522ad..e0094305ca2a 100644
--- a/drivers/media/i2c/ov08x40.c
+++ b/drivers/media/i2c/ov08x40.c
@@ -1,12 +1,16 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2022 Intel Corporation.
 
-#include <asm-generic/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
+#include <linux/clk.h>
 #include <linux/i2c.h>
+#include <linux/gpio/consumer.h>
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/pm_runtime.h>
+#include <linux/regulator/consumer.h>
+#include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -104,6 +108,7 @@
 
 enum {
 	OV08X40_LINK_FREQ_400MHZ_INDEX,
+	OV08X40_LINK_FREQ_749MHZ_INDEX,
 };
 
 struct ov08x40_reg {
@@ -147,67 +152,7 @@ struct ov08x40_mode {
 	u16 exposure_shift;
 };
 
-static const struct ov08x40_reg mipi_data_rate_800mbps[] = {
-	{0x0103, 0x01},
-	{0x1000, 0x00},
-	{0x1601, 0xd0},
-	{0x1001, 0x04},
-	{0x5004, 0x53},
-	{0x5110, 0x00},
-	{0x5111, 0x14},
-	{0x5112, 0x01},
-	{0x5113, 0x7b},
-	{0x5114, 0x00},
-	{0x5152, 0xa3},
-	{0x5a52, 0x1f},
-	{0x5a1a, 0x0e},
-	{0x5a1b, 0x10},
-	{0x5a1f, 0x0e},
-	{0x5a27, 0x0e},
-	{0x6002, 0x2e},
-};
-
-static const struct ov08x40_reg mode_3856x2416_regs[] = {
-	{0x5000, 0x5d},
-	{0x5001, 0x20},
-	{0x5008, 0xb0},
-	{0x50c1, 0x00},
-	{0x53c1, 0x00},
-	{0x5f40, 0x00},
-	{0x5f41, 0x40},
-	{0x0300, 0x3a},
-	{0x0301, 0xc8},
-	{0x0302, 0x31},
-	{0x0303, 0x03},
-	{0x0304, 0x01},
-	{0x0305, 0xa1},
-	{0x0306, 0x04},
-	{0x0307, 0x01},
-	{0x0308, 0x03},
-	{0x0309, 0x03},
-	{0x0310, 0x0a},
-	{0x0311, 0x02},
-	{0x0312, 0x01},
-	{0x0313, 0x08},
-	{0x0314, 0x66},
-	{0x0315, 0x00},
-	{0x0316, 0x34},
-	{0x0320, 0x02},
-	{0x0321, 0x03},
-	{0x0323, 0x05},
-	{0x0324, 0x01},
-	{0x0325, 0xb8},
-	{0x0326, 0x4a},
-	{0x0327, 0x04},
-	{0x0329, 0x00},
-	{0x032a, 0x05},
-	{0x032b, 0x00},
-	{0x032c, 0x00},
-	{0x032d, 0x00},
-	{0x032e, 0x02},
-	{0x032f, 0xa0},
-	{0x0350, 0x00},
-	{0x0360, 0x01},
+static const struct ov08x40_reg ov08x40_global_regs[] = {
 	{0x1216, 0x60},
 	{0x1217, 0x5b},
 	{0x1218, 0x00},
@@ -217,7 +162,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x198e, 0x00},
 	{0x198f, 0x01},
 	{0x3009, 0x04},
-	{0x3012, 0x41},
 	{0x3015, 0x00},
 	{0x3016, 0xb0},
 	{0x3017, 0xf0},
@@ -242,11 +186,10 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3058, 0x80},
 	{0x3059, 0x00},
 	{0x3107, 0x86},
-	{0x3400, 0x1c},
 	{0x3401, 0x80},
 	{0x3402, 0x8c},
-	{0x3419, 0x13},
-	{0x341a, 0x89},
+	{0x3404, 0x01},
+	{0x3407, 0x01},
 	{0x341b, 0x30},
 	{0x3420, 0x00},
 	{0x3421, 0x00},
@@ -254,7 +197,7 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3423, 0x00},
 	{0x3424, 0x00},
 	{0x3425, 0x00},
-	{0x3426, 0x00},
+	{0x3426, 0x10},
 	{0x3427, 0x00},
 	{0x3428, 0x0f},
 	{0x3429, 0x00},
@@ -283,27 +226,28 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3455, 0x80},
 	{0x3456, 0x08},
 	{0x3500, 0x00},
-	{0x3501, 0x02},
-	{0x3502, 0x00},
+	{0x3502, 0x10},
 	{0x3504, 0x4c},
 	{0x3506, 0x30},
 	{0x3507, 0x00},
-	{0x3508, 0x01},
-	{0x3509, 0x00},
 	{0x350a, 0x01},
 	{0x350b, 0x00},
 	{0x350c, 0x00},
 	{0x3540, 0x00},
-	{0x3541, 0x01},
-	{0x3542, 0x00},
 	{0x3544, 0x4c},
 	{0x3546, 0x30},
 	{0x3547, 0x00},
-	{0x3548, 0x01},
 	{0x3549, 0x00},
 	{0x354a, 0x01},
 	{0x354b, 0x00},
 	{0x354c, 0x00},
+	{0x3601, 0x40},
+	{0x3602, 0x90},
+	{0x3608, 0x0a},
+	{0x3609, 0x08},
+	{0x360f, 0x99},
+	{0x3680, 0xa4},
+	{0x3682, 0x80},
 	{0x3688, 0x02},
 	{0x368a, 0x2e},
 	{0x368e, 0x71},
@@ -313,9 +257,7 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x36a4, 0x00},
 	{0x36a6, 0x00},
 	{0x3711, 0x00},
-	{0x3712, 0x51},
 	{0x3713, 0x00},
-	{0x3714, 0x24},
 	{0x3716, 0x00},
 	{0x3718, 0x07},
 	{0x371a, 0x1c},
@@ -324,7 +266,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3725, 0x32},
 	{0x3727, 0x05},
 	{0x3760, 0x02},
-	{0x3761, 0x17},
 	{0x3762, 0x02},
 	{0x3763, 0x02},
 	{0x3764, 0x02},
@@ -334,41 +275,19 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3768, 0x02},
 	{0x3769, 0x00},
 	{0x376b, 0x20},
-	{0x376e, 0x03},
-	{0x37b0, 0x00},
-	{0x37b1, 0xab},
 	{0x37b2, 0x01},
-	{0x37b3, 0x82},
-	{0x37b4, 0x00},
-	{0x37b5, 0xe4},
-	{0x37b6, 0x01},
-	{0x37b7, 0xee},
 	{0x3800, 0x00},
 	{0x3801, 0x00},
 	{0x3802, 0x00},
-	{0x3803, 0x00},
 	{0x3804, 0x0f},
 	{0x3805, 0x1f},
 	{0x3806, 0x09},
-	{0x3807, 0x7f},
-	{0x3808, 0x0f},
-	{0x3809, 0x10},
-	{0x380a, 0x09},
-	{0x380b, 0x70},
 	{0x380c, 0x02},
-	{0x380d, 0x80},
-	{0x380e, 0x13},
-	{0x380f, 0x88},
 	{0x3810, 0x00},
-	{0x3811, 0x08},
 	{0x3812, 0x00},
-	{0x3813, 0x07},
 	{0x3814, 0x11},
 	{0x3815, 0x11},
-	{0x3820, 0x00},
-	{0x3821, 0x04},
 	{0x3822, 0x00},
-	{0x3823, 0x04},
 	{0x3828, 0x0f},
 	{0x382a, 0x80},
 	{0x382e, 0x41},
@@ -383,7 +302,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x3847, 0x00},
 	{0x384a, 0x00},
 	{0x384c, 0x02},
-	{0x384d, 0x80},
 	{0x3856, 0x50},
 	{0x3857, 0x30},
 	{0x3858, 0x80},
@@ -397,9 +315,45 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x388d, 0x00},
 	{0x388e, 0x00},
 	{0x388f, 0x00},
-	{0x3894, 0x00},
 	{0x3895, 0x00},
+	{0x3911, 0x90},
+	{0x3913, 0x90},
+	{0x3921, 0x0f},
+	{0x3928, 0x15},
+	{0x3929, 0x2a},
+	{0x392c, 0x02},
+	{0x392e, 0x04},
+	{0x392f, 0x03},
+	{0x3931, 0x07},
+	{0x3932, 0x10},
+	{0x3938, 0x09},
+	{0x3a1f, 0x8a},
+	{0x3a22, 0x91},
+	{0x3a23, 0x15},
+	{0x3a25, 0x96},
+	{0x3a28, 0xb4},
+	{0x3a29, 0x26},
+	{0x3a2b, 0xba},
+	{0x3a2e, 0xbf},
+	{0x3a2f, 0x18},
+	{0x3a31, 0xc1},
+	{0x3a74, 0x84},
+	{0x3a99, 0x84},
+	{0x3ab9, 0xa6},
+	{0x3aba, 0xba},
+	{0x3b0a, 0x01},
+	{0x3b0b, 0x00},
+	{0x3b0e, 0x01},
+	{0x3b0f, 0x00},
+	{0x3b12, 0x84},
+	{0x3b14, 0xbb},
+	{0x3b15, 0xbf},
+	{0x3b1b, 0xc9},
+	{0x3b21, 0xc9},
+	{0x3b3f, 0x9d},
+	{0x3b45, 0x9d},
 	{0x3c84, 0x00},
+	{0x3d84, 0x04},
 	{0x3d85, 0x8b},
 	{0x3daa, 0x80},
 	{0x3dab, 0x14},
@@ -421,44 +375,21 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x4008, 0x00},
 	{0x4009, 0x05},
 	{0x400a, 0x00},
-	{0x400b, 0x08},
 	{0x400c, 0x00},
-	{0x400d, 0x08},
 	{0x400e, 0x14},
 	{0x4010, 0xf4},
 	{0x4011, 0x03},
 	{0x4012, 0x55},
 	{0x4015, 0x00},
-	{0x4016, 0x2d},
 	{0x4017, 0x00},
 	{0x4018, 0x0f},
+	{0x4019, 0x00},
+	{0x401a, 0x40},
 	{0x401b, 0x08},
 	{0x401c, 0x00},
 	{0x401d, 0x10},
 	{0x401e, 0x02},
 	{0x401f, 0x00},
-	{0x4050, 0x06},
-	{0x4051, 0xff},
-	{0x4052, 0xff},
-	{0x4053, 0xff},
-	{0x4054, 0xff},
-	{0x4055, 0xff},
-	{0x4056, 0xff},
-	{0x4057, 0x7f},
-	{0x4058, 0x00},
-	{0x4059, 0x00},
-	{0x405a, 0x00},
-	{0x405b, 0x00},
-	{0x405c, 0x07},
-	{0x405d, 0xff},
-	{0x405e, 0x07},
-	{0x405f, 0xff},
-	{0x4080, 0x78},
-	{0x4081, 0x78},
-	{0x4082, 0x78},
-	{0x4083, 0x78},
-	{0x4019, 0x00},
-	{0x401a, 0x40},
 	{0x4020, 0x04},
 	{0x4021, 0x00},
 	{0x4022, 0x04},
@@ -483,6 +414,22 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x4045, 0x80},
 	{0x4046, 0x00},
 	{0x4047, 0x80},
+	{0x4050, 0x06},
+	{0x4051, 0xff},
+	{0x4052, 0xff},
+	{0x4053, 0xff},
+	{0x4054, 0xff},
+	{0x4055, 0xff},
+	{0x4056, 0xff},
+	{0x4057, 0x7f},
+	{0x4058, 0x00},
+	{0x4059, 0x00},
+	{0x405a, 0x00},
+	{0x405b, 0x00},
+	{0x405c, 0x07},
+	{0x405d, 0xff},
+	{0x405e, 0x07},
+	{0x405f, 0xff},
 	{0x4060, 0x00},
 	{0x4061, 0x00},
 	{0x4062, 0x00},
@@ -515,6 +462,10 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x407d, 0x00},
 	{0x407e, 0x00},
 	{0x407f, 0x00},
+	{0x4080, 0x78},
+	{0x4081, 0x78},
+	{0x4082, 0x78},
+	{0x4083, 0x78},
 	{0x40e0, 0x00},
 	{0x40e1, 0x00},
 	{0x40e2, 0x00},
@@ -557,7 +508,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x431b, 0x00},
 	{0x431c, 0x00},
 	{0x4500, 0x07},
-	{0x4501, 0x00},
 	{0x4502, 0x00},
 	{0x4503, 0x0f},
 	{0x4504, 0x80},
@@ -570,7 +520,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x4510, 0x00},
 	{0x4523, 0x00},
 	{0x4526, 0x00},
-	{0x4542, 0x00},
 	{0x4543, 0x00},
 	{0x4544, 0x00},
 	{0x4545, 0x00},
@@ -589,8 +538,6 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x480c, 0x80},
 	{0x480f, 0x32},
 	{0x4813, 0xe4},
-	{0x4837, 0x14},
-	{0x4850, 0x42},
 	{0x4884, 0x04},
 	{0x4c00, 0xf8},
 	{0x4c01, 0x44},
@@ -601,93 +548,37 @@ static const struct ov08x40_reg mode_3856x2416_regs[] = {
 	{0x4d05, 0x00},
 	{0x4d06, 0x0c},
 	{0x4d07, 0x00},
-	{0x3d84, 0x04},
-	{0x3680, 0xa4},
-	{0x3682, 0x80},
-	{0x3601, 0x40},
-	{0x3602, 0x90},
-	{0x3608, 0x0a},
-	{0x3938, 0x09},
-	{0x3a74, 0x84},
-	{0x3a99, 0x84},
-	{0x3ab9, 0xa6},
-	{0x3aba, 0xba},
-	{0x3b12, 0x84},
-	{0x3b14, 0xbb},
-	{0x3b15, 0xbf},
-	{0x3a29, 0x26},
-	{0x3a1f, 0x8a},
-	{0x3a22, 0x91},
-	{0x3a25, 0x96},
-	{0x3a28, 0xb4},
-	{0x3a2b, 0xba},
-	{0x3a2e, 0xbf},
-	{0x3a31, 0xc1},
-	{0x3a20, 0x00},
-	{0x3939, 0x9d},
-	{0x3902, 0x0e},
-	{0x3903, 0x0e},
-	{0x3904, 0x0e},
-	{0x3905, 0x0e},
-	{0x3906, 0x07},
-	{0x3907, 0x0d},
-	{0x3908, 0x11},
-	{0x3909, 0x12},
-	{0x360f, 0x99},
-	{0x390c, 0x33},
-	{0x390d, 0x66},
-	{0x390e, 0xaa},
-	{0x3911, 0x90},
-	{0x3913, 0x90},
-	{0x3915, 0x90},
-	{0x3917, 0x90},
-	{0x3b3f, 0x9d},
-	{0x3b45, 0x9d},
-	{0x3b1b, 0xc9},
-	{0x3b21, 0xc9},
-	{0x3440, 0xa4},
-	{0x3a23, 0x15},
-	{0x3a26, 0x1d},
-	{0x3a2c, 0x4a},
-	{0x3a2f, 0x18},
-	{0x3a32, 0x55},
-	{0x3b0a, 0x01},
-	{0x3b0b, 0x00},
-	{0x3b0e, 0x01},
-	{0x3b0f, 0x00},
-	{0x392c, 0x02},
-	{0x392d, 0x02},
-	{0x392e, 0x04},
-	{0x392f, 0x03},
-	{0x3930, 0x08},
-	{0x3931, 0x07},
-	{0x3932, 0x10},
-	{0x3933, 0x0c},
-	{0x3609, 0x08},
-	{0x3921, 0x0f},
-	{0x3928, 0x15},
-	{0x3929, 0x2a},
-	{0x392a, 0x54},
-	{0x392b, 0xa8},
-	{0x3426, 0x10},
-	{0x3407, 0x01},
-	{0x3404, 0x01},
-	{0x3500, 0x00},
-	{0x3501, 0x10},
-	{0x3502, 0x10},
-	{0x3508, 0x0f},
-	{0x3509, 0x80},
-};
-
-static const struct ov08x40_reg mode_1928x1208_regs[] = {
-	{0x5000, 0x55},
-	{0x5001, 0x00},
 	{0x5008, 0xb0},
 	{0x50c1, 0x00},
 	{0x53c1, 0x00},
 	{0x5f40, 0x00},
 	{0x5f41, 0x40},
-	{0x0300, 0x3a},
+};
+
+static const struct ov08x40_reg_list ov08x40_global_setting = {
+	.num_of_regs = ARRAY_SIZE(ov08x40_global_regs),
+	.regs = ov08x40_global_regs,
+};
+
+static const struct ov08x40_reg mipi_data_rate_800mbps[] = {
+	{0x0103, 0x01},
+	{0x1000, 0x00},
+	{0x1601, 0xd0},
+	{0x1001, 0x04},
+	{0x5004, 0x53},
+	{0x5110, 0x00},
+	{0x5111, 0x14},
+	{0x5112, 0x01},
+	{0x5113, 0x7b},
+	{0x5114, 0x00},
+	{0x5152, 0xa3},
+	{0x5a52, 0x1f},
+	{0x5a1a, 0x0e},
+	{0x5a1b, 0x10},
+	{0x5a1f, 0x0e},
+	{0x5a27, 0x0e},
+	{0x6002, 0x2e},
+	{0x0300, 0x3a}, /* PLL CTRL */
 	{0x0301, 0xc8},
 	{0x0302, 0x31},
 	{0x0303, 0x03},
@@ -720,421 +611,522 @@ static const struct ov08x40_reg mode_1928x1208_regs[] = {
 	{0x032f, 0xa0},
 	{0x0350, 0x00},
 	{0x0360, 0x01},
-	{0x1216, 0x60},
-	{0x1217, 0x5b},
-	{0x1218, 0x00},
-	{0x1220, 0x24},
-	{0x198a, 0x00},
-	{0x198b, 0x01},
-	{0x198e, 0x00},
-	{0x198f, 0x01},
-	{0x3009, 0x04},
+	{0x3012, 0x41}, /* MIPI SC Lanes */
+};
+
+static const struct ov08x40_reg mipi_data_rate_1500mbps[] = {
+	{0x0103, 0x01},
+	{0x1000, 0x00},
+	{0x1601, 0xd0},
+	{0x1001, 0x04},
+	{0x5004, 0x53},
+	{0x5110, 0x00},
+	{0x5111, 0x14},
+	{0x5112, 0x01},
+	{0x5113, 0x7b},
+	{0x5114, 0x00},
+	{0x5152, 0xa3},
+	{0x5a52, 0x1f},
+	{0x5a1a, 0x0e},
+	{0x5a1b, 0x10},
+	{0x5a1f, 0x0e},
+	{0x5a27, 0x0e},
+	{0x6002, 0x2e},
+	{0x0300, 0x3a}, /* PLL */
+	{0x0301, 0x88},
+	{0x0302, 0x31},
+	{0x0303, 0x05},
+	{0x0304, 0x01},
+	{0x0305, 0x38},
+	{0x0306, 0x04},
+	{0x0307, 0x00},
+	{0x0308, 0x03},
+	{0x0309, 0x02},
+	{0x0310, 0x0a},
+	{0x0311, 0x02},
+	{0x0312, 0x01},
+	{0x0313, 0x08},
+	{0x0314, 0x00},
+	{0x0315, 0x00},
+	{0x0316, 0x2c},
+	{0x0320, 0x02},
+	{0x0321, 0x03},
+	{0x0323, 0x05},
+	{0x0324, 0x01},
+	{0x0325, 0xb8},
+	{0x0326, 0x4a},
+	{0x0327, 0x04},
+	{0x0329, 0x00},
+	{0x032a, 0x05},
+	{0x032b, 0x00},
+	{0x032c, 0x00},
+	{0x032d, 0x00},
+	{0x032e, 0x02},
+	{0x032f, 0xa0},
+	{0x0350, 0x00},
+	{0x0360, 0x01},
+	{0x3012, 0x21}, /* MIPI SC Lanes */
+};
+
+static const struct ov08x40_reg mode_3856x2176_regs_800mbps[] = {
+	{0x5000, 0x5d},
+	{0x5001, 0x20},
+	{0x3012, 0x41},
+	{0x3400, 0x1c},
+	{0x3419, 0x13},
+	{0x341a, 0x89},
+	{0x3426, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3548, 0x01},
+	{0x3712, 0x51},
+	{0x3714, 0x24},
+	{0x3761, 0x17},
+	{0x376e, 0x03},
+	{0x37b0, 0x00},
+	{0x37b1, 0xab},
+	{0x37b3, 0x82},
+	{0x37b4, 0x00},
+	{0x37b5, 0xe4},
+	{0x37b6, 0x01},
+	{0x37b7, 0xee},
+	{0x3820, 0x00},
+	{0x3821, 0x04},
+	{0x3823, 0x04},
+	{0x384d, 0x80},
+	{0x3894, 0x00},
+	{0x400b, 0x08},
+	{0x400d, 0x08},
+	{0x4016, 0x2d},
+	{0x4501, 0x00},
+	{0x4542, 0x00},
+	{0x4837, 0x14},
+	{0x4850, 0x42},
+	{0x3a20, 0x00},
+	{0x3939, 0x9d},
+	{0x3902, 0x0e},
+	{0x3903, 0x0e},
+	{0x3904, 0x0e},
+	{0x3905, 0x0e},
+	{0x3906, 0x07},
+	{0x3907, 0x0d},
+	{0x3908, 0x11},
+	{0x3909, 0x12},
+	{0x390c, 0x33},
+	{0x390d, 0x66},
+	{0x390e, 0xaa},
+	{0x3915, 0x90},
+	{0x3917, 0x90},
+	{0x3440, 0xa4},
+	{0x3a26, 0x1d},
+	{0x3a2c, 0x4a},
+	{0x3a32, 0x55},
+	{0x392d, 0x02},
+	{0x3930, 0x08},
+	{0x3933, 0x0c},
+	{0x392a, 0x54},
+	{0x392b, 0xa8},
+	{0x380d, 0x80},
+	{0x380e, 0x13},
+	{0x380f, 0x88},
+	{0x3803, 0x70},
+	{0x3807, 0x0f},
+	{0x3808, 0x0f},
+	{0x3809, 0x10},
+	{0x380a, 0x08},
+	{0x380b, 0x80},
+	{0x3811, 0x08},
+	{0x3813, 0x10},
+	{0x3501, 0x10},
+	{0x3508, 0x0f},
+	{0x3509, 0x80},
+	{0x3813, 0x0f},
+};
+
+/* OV08X 1C 3856x2176_DPHY1500M-2L */
+static const struct ov08x40_reg mode_3856x2176_regs_1500mbps[] = {
+	{0x5000, 0x5d},
+	{0x5001, 0x20},
+	{0x3012, 0x21},
+	{0x3400, 0x1c},
+	{0x3419, 0x12},
+	{0x341a, 0x99},
+	{0x3426, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3548, 0x01},
+	{0x3712, 0x51},
+	{0x3714, 0x24},
+	{0x3761, 0x17},
+	{0x376e, 0x03},
+	{0x37b0, 0x00},
+	{0x37b1, 0xab},
+	{0x37b3, 0x82},
+	{0x37b4, 0x00},
+	{0x37b5, 0xe4},
+	{0x37b6, 0x01},
+	{0x37b7, 0xee},
+	{0x3803, 0x70},
+	{0x3807, 0x0f},
+	{0x3808, 0x0f},
+	{0x3809, 0x10},
+	{0x380a, 0x08},
+	{0x380b, 0x80},
+	{0x380d, 0xa0},
+	{0x380e, 0x12},
+	{0x380f, 0x98},
+	{0x3811, 0x08},
+	{0x3813, 0x10},
+	{0x3820, 0x00},
+	{0x3821, 0x04},
+	{0x3823, 0x04},
+	{0x384d, 0xa0},
+	{0x3894, 0x00},
+	{0x400b, 0x08},
+	{0x400d, 0x08},
+	{0x4016, 0x2d},
+	{0x4501, 0x00},
+	{0x4542, 0x00},
+	{0x4837, 0x0a},
+	{0x4850, 0x47},
+	{0x3a20, 0x00},
+	{0x3939, 0x9d},
+	{0x3902, 0x0e},
+	{0x3903, 0x0e},
+	{0x3904, 0x0e},
+	{0x3905, 0x0e},
+	{0x3906, 0x07},
+	{0x3907, 0x0d},
+	{0x3908, 0x11},
+	{0x3909, 0x12},
+	{0x390c, 0x33},
+	{0x390d, 0x66},
+	{0x390e, 0xaa},
+	{0x3915, 0x90},
+	{0x3917, 0x90},
+	{0x3440, 0xa4},
+	{0x3a26, 0x1d},
+	{0x3a2c, 0x4a},
+	{0x3a32, 0x55},
+	{0x392d, 0x02},
+	{0x3930, 0x08},
+	{0x3933, 0x0c},
+	{0x392a, 0x54},
+	{0x392b, 0xa8},
+	{0x3501, 0x10},
+	{0x3508, 0x0f},
+	{0x3509, 0x80},
+	{0x3813, 0x0f},
+};
+
+/* OV08X 4C1stg 1928x1088_DPHY1500M-2L 30fps */
+static const struct ov08x40_reg mode_1928x1088_regs_1500mbps[] = {
+	{0x5000, 0x55},
+	{0x5001, 0x00},
+	{0x3012, 0x21},
+	{0x3400, 0x30},
+	{0x3419, 0x08},
+	{0x341a, 0x4f},
+	{0x3426, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3548, 0x01},
+	{0x3712, 0x50},
+	{0x3714, 0x21},
+	{0x3761, 0x28},
+	{0x376e, 0x07},
+	{0x37b0, 0x01},
+	{0x37b1, 0x0f},
+	{0x37b3, 0xd6},
+	{0x37b4, 0x01},
+	{0x37b5, 0x48},
+	{0x37b6, 0x02},
+	{0x37b7, 0x40},
+	{0x3803, 0x78},
+	{0x3807, 0x07},
+	{0x3808, 0x07},
+	{0x3809, 0x88},
+	{0x380a, 0x04},
+	{0x380b, 0x40},
+	{0x380d, 0xf0},
+	{0x380e, 0x08},
+	{0x380f, 0x4e},
+	{0x3811, 0x04},
+	{0x3813, 0x03},
+	{0x3820, 0x02},
+	{0x3821, 0x14},
+	{0x3823, 0x84},
+	{0x384d, 0xf0},
+	{0x3894, 0x03},
+	{0x400b, 0x04},
+	{0x400d, 0x04},
+	{0x4016, 0x27},
+	{0x4501, 0x10},
+	{0x4542, 0x01},
+	{0x4837, 0x0a},
+	{0x4850, 0x47},
+	{0x4911, 0x00},
+	{0x4919, 0x00},
+	{0x491a, 0x40},
+	{0x4920, 0x04},
+	{0x4921, 0x00},
+	{0x4922, 0x04},
+	{0x4923, 0x00},
+	{0x4924, 0x04},
+	{0x4925, 0x00},
+	{0x4926, 0x04},
+	{0x4927, 0x00},
+	{0x4930, 0x00},
+	{0x4931, 0x00},
+	{0x4932, 0x00},
+	{0x4933, 0x00},
+	{0x4934, 0x00},
+	{0x4935, 0x00},
+	{0x4936, 0x00},
+	{0x4937, 0x00},
+	{0x4940, 0x00},
+	{0x4941, 0x80},
+	{0x4942, 0x00},
+	{0x4943, 0x80},
+	{0x4944, 0x00},
+	{0x4945, 0x80},
+	{0x4946, 0x00},
+	{0x4947, 0x80},
+	{0x4960, 0x00},
+	{0x4961, 0x00},
+	{0x4962, 0x00},
+	{0x4963, 0x00},
+	{0x4964, 0x00},
+	{0x4965, 0x00},
+	{0x4966, 0x00},
+	{0x4967, 0x00},
+	{0x4968, 0x00},
+	{0x4969, 0x00},
+	{0x496a, 0x00},
+	{0x496b, 0x00},
+	{0x496c, 0x00},
+	{0x496d, 0x00},
+	{0x496e, 0x00},
+	{0x496f, 0x00},
+	{0x4970, 0x00},
+	{0x4971, 0x00},
+	{0x4972, 0x00},
+	{0x4973, 0x00},
+	{0x4974, 0x00},
+	{0x4975, 0x00},
+	{0x4976, 0x00},
+	{0x4977, 0x00},
+	{0x4978, 0x00},
+	{0x4979, 0x00},
+	{0x497a, 0x00},
+	{0x497b, 0x00},
+	{0x497c, 0x00},
+	{0x497d, 0x00},
+	{0x497e, 0x00},
+	{0x497f, 0x00},
+	{0x49e0, 0x00},
+	{0x49e1, 0x00},
+	{0x49e2, 0x00},
+	{0x49e3, 0x00},
+	{0x49e4, 0x00},
+	{0x49e5, 0x00},
+	{0x49e6, 0x00},
+	{0x49e7, 0x00},
+	{0x49e8, 0x00},
+	{0x49e9, 0x80},
+	{0x49ea, 0x00},
+	{0x49eb, 0x80},
+	{0x49ec, 0x00},
+	{0x49ed, 0x80},
+	{0x49ee, 0x00},
+	{0x49ef, 0x80},
+	{0x49f0, 0x02},
+	{0x49f1, 0x04},
+	{0x3a20, 0x05},
+	{0x3939, 0x6b},
+	{0x3902, 0x10},
+	{0x3903, 0x10},
+	{0x3904, 0x10},
+	{0x3905, 0x10},
+	{0x3906, 0x01},
+	{0x3907, 0x0b},
+	{0x3908, 0x10},
+	{0x3909, 0x13},
+	{0x390b, 0x11},
+	{0x390c, 0x21},
+	{0x390d, 0x32},
+	{0x390e, 0x76},
+	{0x3a1a, 0x1c},
+	{0x3a26, 0x17},
+	{0x3a2c, 0x50},
+	{0x3a32, 0x4f},
+	{0x3ace, 0x01},
+	{0x3ad2, 0x01},
+	{0x3ad6, 0x01},
+	{0x3ada, 0x01},
+	{0x3ade, 0x01},
+	{0x3ae2, 0x01},
+	{0x3aee, 0x01},
+	{0x3af2, 0x01},
+	{0x3af6, 0x01},
+	{0x3afa, 0x01},
+	{0x3afe, 0x01},
+	{0x3b02, 0x01},
+	{0x3b06, 0x01},
+	{0x392d, 0x01},
+	{0x3930, 0x09},
+	{0x3933, 0x0d},
+	{0x392a, 0x52},
+	{0x392b, 0xa3},
+	{0x340b, 0x1b},
+	{0x3501, 0x01},
+	{0x3508, 0x0f},
+	{0x3509, 0x00},
+	{0x3541, 0x00},
+	{0x3542, 0x80},
+	{0x3548, 0x0f},
+	{0x3813, 0x03},
+};
+
+static const struct ov08x40_reg mode_3856x2416_regs[] = {
+	{0x5000, 0x5d},
+	{0x5001, 0x20},
+	{0x3012, 0x41},
+	{0x3400, 0x1c},
+	{0x3419, 0x13},
+	{0x341a, 0x89},
+	{0x3426, 0x00},
+	{0x3501, 0x02},
+	{0x3502, 0x00},
+	{0x3508, 0x01},
+	{0x3509, 0x00},
+	{0x3541, 0x01},
+	{0x3542, 0x00},
+	{0x3548, 0x01},
+	{0x3712, 0x51},
+	{0x3714, 0x24},
+	{0x3761, 0x17},
+	{0x376e, 0x03},
+	{0x37b0, 0x00},
+	{0x37b1, 0xab},
+	{0x37b3, 0x82},
+	{0x37b4, 0x00},
+	{0x37b5, 0xe4},
+	{0x37b6, 0x01},
+	{0x37b7, 0xee},
+	{0x3803, 0x00},
+	{0x3807, 0x7f},
+	{0x3808, 0x0f},
+	{0x3809, 0x10},
+	{0x380a, 0x09},
+	{0x380b, 0x70},
+	{0x380d, 0x80},
+	{0x380e, 0x13},
+	{0x380f, 0x88},
+	{0x3811, 0x08},
+	{0x3813, 0x07},
+	{0x3820, 0x00},
+	{0x3821, 0x04},
+	{0x3823, 0x04},
+	{0x384d, 0x80},
+	{0x3894, 0x00},
+	{0x400b, 0x08},
+	{0x400d, 0x08},
+	{0x4016, 0x2d},
+	{0x4501, 0x00},
+	{0x4542, 0x00},
+	{0x4837, 0x14},
+	{0x4850, 0x42},
+	{0x3a20, 0x00},
+	{0x3939, 0x9d},
+	{0x3902, 0x0e},
+	{0x3903, 0x0e},
+	{0x3904, 0x0e},
+	{0x3905, 0x0e},
+	{0x3906, 0x07},
+	{0x3907, 0x0d},
+	{0x3908, 0x11},
+	{0x3909, 0x12},
+	{0x390c, 0x33},
+	{0x390d, 0x66},
+	{0x390e, 0xaa},
+	{0x3915, 0x90},
+	{0x3917, 0x90},
+	{0x3440, 0xa4},
+	{0x3a26, 0x1d},
+	{0x3a2c, 0x4a},
+	{0x3a32, 0x55},
+	{0x392d, 0x02},
+	{0x3930, 0x08},
+	{0x3933, 0x0c},
+	{0x392a, 0x54},
+	{0x392b, 0xa8},
+	{0x3501, 0x10},
+	{0x3508, 0x0f},
+	{0x3509, 0x80},
+};
+
+static const struct ov08x40_reg mode_1928x1208_regs[] = {
+	{0x5000, 0x55},
+	{0x5001, 0x00},
 	{0x3012, 0x41},
-	{0x3015, 0x00},
-	{0x3016, 0xb0},
-	{0x3017, 0xf0},
-	{0x3018, 0xf0},
-	{0x3019, 0xd2},
-	{0x301a, 0xb0},
-	{0x301c, 0x81},
-	{0x301d, 0x02},
-	{0x301e, 0x80},
-	{0x3022, 0xf0},
-	{0x3025, 0x89},
-	{0x3030, 0x03},
-	{0x3044, 0xc2},
-	{0x3050, 0x35},
-	{0x3051, 0x60},
-	{0x3052, 0x25},
-	{0x3053, 0x00},
-	{0x3054, 0x00},
-	{0x3055, 0x02},
-	{0x3056, 0x80},
-	{0x3057, 0x80},
-	{0x3058, 0x80},
-	{0x3059, 0x00},
-	{0x3107, 0x86},
 	{0x3400, 0x1c},
-	{0x3401, 0x80},
-	{0x3402, 0x8c},
 	{0x3419, 0x08},
 	{0x341a, 0xaf},
-	{0x341b, 0x30},
-	{0x3420, 0x00},
-	{0x3421, 0x00},
-	{0x3422, 0x00},
-	{0x3423, 0x00},
-	{0x3424, 0x00},
-	{0x3425, 0x00},
 	{0x3426, 0x00},
-	{0x3427, 0x00},
-	{0x3428, 0x0f},
-	{0x3429, 0x00},
-	{0x342a, 0x00},
-	{0x342b, 0x00},
-	{0x342c, 0x00},
-	{0x342d, 0x00},
-	{0x342e, 0x00},
-	{0x342f, 0x11},
-	{0x3430, 0x11},
-	{0x3431, 0x10},
-	{0x3432, 0x00},
-	{0x3433, 0x00},
-	{0x3434, 0x00},
-	{0x3435, 0x00},
-	{0x3436, 0x00},
-	{0x3437, 0x00},
-	{0x3442, 0x02},
-	{0x3443, 0x02},
-	{0x3444, 0x07},
-	{0x3450, 0x00},
-	{0x3451, 0x00},
-	{0x3452, 0x18},
-	{0x3453, 0x18},
-	{0x3454, 0x00},
-	{0x3455, 0x80},
-	{0x3456, 0x08},
-	{0x3500, 0x00},
 	{0x3501, 0x02},
 	{0x3502, 0x00},
-	{0x3504, 0x4c},
-	{0x3506, 0x30},
-	{0x3507, 0x00},
 	{0x3508, 0x01},
 	{0x3509, 0x00},
-	{0x350a, 0x01},
-	{0x350b, 0x00},
-	{0x350c, 0x00},
-	{0x3540, 0x00},
 	{0x3541, 0x01},
 	{0x3542, 0x00},
-	{0x3544, 0x4c},
-	{0x3546, 0x30},
-	{0x3547, 0x00},
 	{0x3548, 0x01},
-	{0x3549, 0x00},
-	{0x354a, 0x01},
-	{0x354b, 0x00},
-	{0x354c, 0x00},
-	{0x3688, 0x02},
-	{0x368a, 0x2e},
-	{0x368e, 0x71},
-	{0x3696, 0xd1},
-	{0x3699, 0x00},
-	{0x369a, 0x00},
-	{0x36a4, 0x00},
-	{0x36a6, 0x00},
-	{0x3711, 0x00},
 	{0x3712, 0x50},
-	{0x3713, 0x00},
 	{0x3714, 0x21},
-	{0x3716, 0x00},
-	{0x3718, 0x07},
-	{0x371a, 0x1c},
-	{0x371b, 0x00},
-	{0x3720, 0x08},
-	{0x3725, 0x32},
-	{0x3727, 0x05},
-	{0x3760, 0x02},
 	{0x3761, 0x28},
-	{0x3762, 0x02},
-	{0x3763, 0x02},
-	{0x3764, 0x02},
-	{0x3765, 0x2c},
-	{0x3766, 0x04},
-	{0x3767, 0x2c},
-	{0x3768, 0x02},
-	{0x3769, 0x00},
-	{0x376b, 0x20},
 	{0x376e, 0x07},
 	{0x37b0, 0x01},
 	{0x37b1, 0x0f},
-	{0x37b2, 0x01},
 	{0x37b3, 0xd6},
 	{0x37b4, 0x01},
 	{0x37b5, 0x48},
 	{0x37b6, 0x02},
 	{0x37b7, 0x40},
-	{0x3800, 0x00},
-	{0x3801, 0x00},
-	{0x3802, 0x00},
 	{0x3803, 0x00},
-	{0x3804, 0x0f},
-	{0x3805, 0x1f},
-	{0x3806, 0x09},
 	{0x3807, 0x7f},
 	{0x3808, 0x07},
 	{0x3809, 0x88},
 	{0x380a, 0x04},
 	{0x380b, 0xb8},
-	{0x380c, 0x02},
 	{0x380d, 0xd0},
 	{0x380e, 0x11},
 	{0x380f, 0x5c},
-	{0x3810, 0x00},
 	{0x3811, 0x04},
-	{0x3812, 0x00},
 	{0x3813, 0x03},
-	{0x3814, 0x11},
-	{0x3815, 0x11},
 	{0x3820, 0x02},
 	{0x3821, 0x14},
-	{0x3822, 0x00},
 	{0x3823, 0x04},
-	{0x3828, 0x0f},
-	{0x382a, 0x80},
-	{0x382e, 0x41},
-	{0x3837, 0x08},
-	{0x383a, 0x81},
-	{0x383b, 0x81},
-	{0x383c, 0x11},
-	{0x383d, 0x11},
-	{0x383e, 0x00},
-	{0x383f, 0x38},
-	{0x3840, 0x00},
-	{0x3847, 0x00},
-	{0x384a, 0x00},
-	{0x384c, 0x02},
 	{0x384d, 0xd0},
-	{0x3856, 0x50},
-	{0x3857, 0x30},
-	{0x3858, 0x80},
-	{0x3859, 0x40},
-	{0x3860, 0x00},
-	{0x3888, 0x00},
-	{0x3889, 0x00},
-	{0x388a, 0x00},
-	{0x388b, 0x00},
-	{0x388c, 0x00},
-	{0x388d, 0x00},
-	{0x388e, 0x00},
-	{0x388f, 0x00},
 	{0x3894, 0x00},
-	{0x3895, 0x00},
-	{0x3c84, 0x00},
-	{0x3d85, 0x8b},
-	{0x3daa, 0x80},
-	{0x3dab, 0x14},
-	{0x3dac, 0x80},
-	{0x3dad, 0xc8},
-	{0x3dae, 0x81},
-	{0x3daf, 0x7b},
-	{0x3f00, 0x10},
-	{0x3f01, 0x11},
-	{0x3f06, 0x0d},
-	{0x3f07, 0x0b},
-	{0x3f08, 0x0d},
-	{0x3f09, 0x0b},
-	{0x3f0a, 0x01},
-	{0x3f0b, 0x11},
-	{0x3f0c, 0x33},
-	{0x4001, 0x07},
-	{0x4007, 0x20},
-	{0x4008, 0x00},
-	{0x4009, 0x05},
-	{0x400a, 0x00},
 	{0x400b, 0x04},
-	{0x400c, 0x00},
 	{0x400d, 0x04},
-	{0x400e, 0x14},
-	{0x4010, 0xf4},
-	{0x4011, 0x03},
-	{0x4012, 0x55},
-	{0x4015, 0x00},
 	{0x4016, 0x27},
-	{0x4017, 0x00},
-	{0x4018, 0x0f},
-	{0x401b, 0x08},
-	{0x401c, 0x00},
-	{0x401d, 0x10},
-	{0x401e, 0x02},
-	{0x401f, 0x00},
-	{0x4050, 0x06},
-	{0x4051, 0xff},
-	{0x4052, 0xff},
-	{0x4053, 0xff},
-	{0x4054, 0xff},
-	{0x4055, 0xff},
-	{0x4056, 0xff},
-	{0x4057, 0x7f},
-	{0x4058, 0x00},
-	{0x4059, 0x00},
-	{0x405a, 0x00},
-	{0x405b, 0x00},
-	{0x405c, 0x07},
-	{0x405d, 0xff},
-	{0x405e, 0x07},
-	{0x405f, 0xff},
-	{0x4080, 0x78},
-	{0x4081, 0x78},
-	{0x4082, 0x78},
-	{0x4083, 0x78},
-	{0x4019, 0x00},
-	{0x401a, 0x40},
-	{0x4020, 0x04},
-	{0x4021, 0x00},
-	{0x4022, 0x04},
-	{0x4023, 0x00},
-	{0x4024, 0x04},
-	{0x4025, 0x00},
-	{0x4026, 0x04},
-	{0x4027, 0x00},
-	{0x4030, 0x00},
-	{0x4031, 0x00},
-	{0x4032, 0x00},
-	{0x4033, 0x00},
-	{0x4034, 0x00},
-	{0x4035, 0x00},
-	{0x4036, 0x00},
-	{0x4037, 0x00},
-	{0x4040, 0x00},
-	{0x4041, 0x80},
-	{0x4042, 0x00},
-	{0x4043, 0x80},
-	{0x4044, 0x00},
-	{0x4045, 0x80},
-	{0x4046, 0x00},
-	{0x4047, 0x80},
-	{0x4060, 0x00},
-	{0x4061, 0x00},
-	{0x4062, 0x00},
-	{0x4063, 0x00},
-	{0x4064, 0x00},
-	{0x4065, 0x00},
-	{0x4066, 0x00},
-	{0x4067, 0x00},
-	{0x4068, 0x00},
-	{0x4069, 0x00},
-	{0x406a, 0x00},
-	{0x406b, 0x00},
-	{0x406c, 0x00},
-	{0x406d, 0x00},
-	{0x406e, 0x00},
-	{0x406f, 0x00},
-	{0x4070, 0x00},
-	{0x4071, 0x00},
-	{0x4072, 0x00},
-	{0x4073, 0x00},
-	{0x4074, 0x00},
-	{0x4075, 0x00},
-	{0x4076, 0x00},
-	{0x4077, 0x00},
-	{0x4078, 0x00},
-	{0x4079, 0x00},
-	{0x407a, 0x00},
-	{0x407b, 0x00},
-	{0x407c, 0x00},
-	{0x407d, 0x00},
-	{0x407e, 0x00},
-	{0x407f, 0x00},
-	{0x40e0, 0x00},
-	{0x40e1, 0x00},
-	{0x40e2, 0x00},
-	{0x40e3, 0x00},
-	{0x40e4, 0x00},
-	{0x40e5, 0x00},
-	{0x40e6, 0x00},
-	{0x40e7, 0x00},
-	{0x40e8, 0x00},
-	{0x40e9, 0x80},
-	{0x40ea, 0x00},
-	{0x40eb, 0x80},
-	{0x40ec, 0x00},
-	{0x40ed, 0x80},
-	{0x40ee, 0x00},
-	{0x40ef, 0x80},
-	{0x40f0, 0x02},
-	{0x40f1, 0x04},
-	{0x4300, 0x00},
-	{0x4301, 0x00},
-	{0x4302, 0x00},
-	{0x4303, 0x00},
-	{0x4304, 0x00},
-	{0x4305, 0x00},
-	{0x4306, 0x00},
-	{0x4307, 0x00},
-	{0x4308, 0x00},
-	{0x4309, 0x00},
-	{0x430a, 0x00},
-	{0x430b, 0xff},
-	{0x430c, 0xff},
-	{0x430d, 0x00},
-	{0x430e, 0x00},
-	{0x4315, 0x00},
-	{0x4316, 0x00},
-	{0x4317, 0x00},
-	{0x4318, 0x00},
-	{0x4319, 0x00},
-	{0x431a, 0x00},
-	{0x431b, 0x00},
-	{0x431c, 0x00},
-	{0x4500, 0x07},
 	{0x4501, 0x10},
-	{0x4502, 0x00},
-	{0x4503, 0x0f},
-	{0x4504, 0x80},
-	{0x4506, 0x01},
-	{0x4509, 0x05},
-	{0x450c, 0x00},
-	{0x450d, 0x20},
-	{0x450e, 0x00},
-	{0x450f, 0x00},
-	{0x4510, 0x00},
-	{0x4523, 0x00},
-	{0x4526, 0x00},
 	{0x4542, 0x00},
-	{0x4543, 0x00},
-	{0x4544, 0x00},
-	{0x4545, 0x00},
-	{0x4546, 0x00},
-	{0x4547, 0x10},
-	{0x4602, 0x00},
-	{0x4603, 0x15},
-	{0x460b, 0x07},
-	{0x4680, 0x11},
-	{0x4686, 0x00},
-	{0x4687, 0x00},
-	{0x4700, 0x00},
-	{0x4800, 0x64},
-	{0x4806, 0x40},
-	{0x480b, 0x10},
-	{0x480c, 0x80},
-	{0x480f, 0x32},
-	{0x4813, 0xe4},
 	{0x4837, 0x14},
 	{0x4850, 0x42},
-	{0x4884, 0x04},
-	{0x4c00, 0xf8},
-	{0x4c01, 0x44},
-	{0x4c03, 0x00},
-	{0x4d00, 0x00},
-	{0x4d01, 0x16},
-	{0x4d04, 0x10},
-	{0x4d05, 0x00},
-	{0x4d06, 0x0c},
-	{0x4d07, 0x00},
-	{0x3d84, 0x04},
-	{0x3680, 0xa4},
-	{0x3682, 0x80},
-	{0x3601, 0x40},
-	{0x3602, 0x90},
-	{0x3608, 0x0a},
-	{0x3938, 0x09},
-	{0x3a74, 0x84},
-	{0x3a99, 0x84},
-	{0x3ab9, 0xa6},
-	{0x3aba, 0xba},
-	{0x3b12, 0x84},
-	{0x3b14, 0xbb},
-	{0x3b15, 0xbf},
-	{0x3a29, 0x26},
-	{0x3a1f, 0x8a},
-	{0x3a22, 0x91},
-	{0x3a25, 0x96},
-	{0x3a28, 0xb4},
-	{0x3a2b, 0xba},
-	{0x3a2e, 0xbf},
-	{0x3a31, 0xc1},
 	{0x3a20, 0x05},
 	{0x3939, 0x6b},
 	{0x3902, 0x10},
@@ -1145,22 +1137,13 @@ static const struct ov08x40_reg mode_1928x1208_regs[] = {
 	{0x3907, 0x0b},
 	{0x3908, 0x10},
 	{0x3909, 0x13},
-	{0x360f, 0x99},
 	{0x390b, 0x11},
 	{0x390c, 0x21},
 	{0x390d, 0x32},
 	{0x390e, 0x76},
-	{0x3911, 0x90},
-	{0x3913, 0x90},
-	{0x3b3f, 0x9d},
-	{0x3b45, 0x9d},
-	{0x3b1b, 0xc9},
-	{0x3b21, 0xc9},
 	{0x3a1a, 0x1c},
-	{0x3a23, 0x15},
 	{0x3a26, 0x17},
 	{0x3a2c, 0x50},
-	{0x3a2f, 0x18},
 	{0x3a32, 0x4f},
 	{0x3ace, 0x01},
 	{0x3ad2, 0x01},
@@ -1175,31 +1158,13 @@ static const struct ov08x40_reg mode_1928x1208_regs[] = {
 	{0x3afe, 0x01},
 	{0x3b02, 0x01},
 	{0x3b06, 0x01},
-	{0x3b0a, 0x01},
-	{0x3b0b, 0x00},
-	{0x3b0e, 0x01},
-	{0x3b0f, 0x00},
-	{0x392c, 0x02},
 	{0x392d, 0x01},
-	{0x392e, 0x04},
-	{0x392f, 0x03},
 	{0x3930, 0x09},
-	{0x3931, 0x07},
-	{0x3932, 0x10},
 	{0x3933, 0x0d},
-	{0x3609, 0x08},
-	{0x3921, 0x0f},
-	{0x3928, 0x15},
-	{0x3929, 0x2a},
 	{0x392a, 0x52},
 	{0x392b, 0xa3},
 	{0x340b, 0x1b},
-	{0x3426, 0x10},
-	{0x3407, 0x01},
-	{0x3404, 0x01},
-	{0x3500, 0x00},
 	{0x3501, 0x08},
-	{0x3502, 0x10},
 	{0x3508, 0x04},
 	{0x3509, 0x00},
 };
@@ -1214,8 +1179,9 @@ static const char * const ov08x40_test_pattern_menu[] = {
 
 /* Configurations for supported link frequencies */
 #define OV08X40_LINK_FREQ_400MHZ	400000000ULL
+#define OV08X40_LINK_FREQ_749MHZ	749000000ULL
 #define OV08X40_SCLK_96MHZ		96000000ULL
-#define OV08X40_EXT_CLK			19200000
+#define OV08X40_XVCLK			19200000
 #define OV08X40_DATA_LANES		4
 
 /*
@@ -1233,6 +1199,7 @@ static u64 link_freq_to_pixel_rate(u64 f)
 /* Menu items for LINK_FREQ V4L2 control */
 static const s64 link_freq_menu_items[] = {
 	OV08X40_LINK_FREQ_400MHZ,
+	OV08X40_LINK_FREQ_749MHZ,
 };
 
 /* Link frequency configs */
@@ -1243,6 +1210,12 @@ static const struct ov08x40_link_freq_config link_freq_configs[] = {
 			.regs = mipi_data_rate_800mbps,
 		}
 	},
+	[OV08X40_LINK_FREQ_749MHZ_INDEX] = {
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1500mbps),
+			.regs = mipi_data_rate_1500mbps,
+		}
+	},
 };
 
 /* Mode configs */
@@ -1263,6 +1236,22 @@ static const struct ov08x40_mode supported_modes[] = {
 		.exposure_margin = OV08X40_EXPOSURE_MAX_MARGIN,
 	},
 	{
+		.width = 3856,
+		.height = 2176,
+		.vts_def = OV08X40_VTS_30FPS,
+		.vts_min = OV08X40_VTS_30FPS,
+		.llp = 0x10aa, /* in normal mode, tline time = 2 * HTS / SCLK */
+		.lanes = 4,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3856x2176_regs_800mbps),
+			.regs = mode_3856x2176_regs_800mbps,
+		},
+		.link_freq_index = OV08X40_LINK_FREQ_400MHZ_INDEX,
+		.exposure_shift = 1,
+		.exposure_margin = OV08X40_EXPOSURE_MAX_MARGIN,
+	},
+
+	{
 		.width = 1928,
 		.height = 1208,
 		.vts_def = OV08X40_VTS_BIN_30FPS,
@@ -1277,6 +1266,42 @@ static const struct ov08x40_mode supported_modes[] = {
 		.exposure_shift = 0,
 		.exposure_margin = OV08X40_EXPOSURE_BIN_MAX_MARGIN,
 	},
+	{
+		.width = 3856,
+		.height = 2176,
+		.vts_def = OV08X40_VTS_30FPS,
+		.vts_min = OV08X40_VTS_30FPS,
+		.llp = 0x10aa, /* in normal mode, tline time = 2 * HTS / SCLK */
+		.lanes = 2,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_3856x2176_regs_1500mbps),
+			.regs = mode_3856x2176_regs_1500mbps,
+		},
+		.link_freq_index = OV08X40_LINK_FREQ_749MHZ_INDEX,
+		.exposure_shift = 1,
+		.exposure_margin = OV08X40_EXPOSURE_MAX_MARGIN,
+	},
+	{
+		.width = 1928,
+		.height = 1088,
+		.vts_def = OV08X40_VTS_BIN_30FPS,
+		.vts_min = OV08X40_VTS_BIN_30FPS,
+		.llp = 0x960,
+		.lanes = 2,
+		.reg_list = {
+			.num_of_regs = ARRAY_SIZE(mode_1928x1088_regs_1500mbps),
+			.regs = mode_1928x1088_regs_1500mbps,
+		},
+		.link_freq_index = OV08X40_LINK_FREQ_749MHZ_INDEX,
+		.exposure_shift = 0,
+		.exposure_margin = OV08X40_EXPOSURE_MAX_MARGIN,
+	},
+};
+
+static const char * const ov08x40_supply_names[] = {
+	"dovdd",	/* Digital I/O power */
+	"avdd",		/* Analog power */
+	"dvdd",		/* Digital core power */
 };
 
 struct ov08x40 {
@@ -1291,18 +1316,76 @@ struct ov08x40 {
 	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
 
+	struct clk		*xvclk;
+	struct gpio_desc	*reset_gpio;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov08x40_supply_names)];
+
 	/* Current mode */
 	const struct ov08x40_mode *cur_mode;
 
 	/* Mutex for serialized access */
 	struct mutex mutex;
 
+	/* data lanes */
+	u8 mipi_lanes;
+
 	/* True if the device has been identified */
 	bool identified;
+
+	unsigned long link_freq_bitmap;
 };
 
 #define to_ov08x40(_sd)	container_of(_sd, struct ov08x40, sd)
 
+static int ov08x40_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	int ret;
+
+	ret = clk_prepare_enable(ov08x->xvclk);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable xvclk\n");
+		return ret;
+	}
+
+	if (ov08x->reset_gpio) {
+		gpiod_set_value_cansleep(ov08x->reset_gpio, 1);
+		usleep_range(1000, 2000);
+	}
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov08x40_supply_names),
+				    ov08x->supplies);
+	if (ret < 0) {
+		dev_err(dev, "failed to enable regulators\n");
+		goto disable_clk;
+	}
+
+	gpiod_set_value_cansleep(ov08x->reset_gpio, 0);
+	usleep_range(5000, 5500);
+
+	return 0;
+
+disable_clk:
+	gpiod_set_value_cansleep(ov08x->reset_gpio, 1);
+	clk_disable_unprepare(ov08x->xvclk);
+
+	return ret;
+}
+
+static int ov08x40_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+
+	gpiod_set_value_cansleep(ov08x->reset_gpio, 1);
+	regulator_bulk_disable(ARRAY_SIZE(ov08x40_supply_names),
+			       ov08x->supplies);
+	clk_disable_unprepare(ov08x->xvclk);
+
+	return 0;
+}
+
 /* Read registers up to 4 at a time */
 static int ov08x40_read_reg(struct ov08x40 *ov08x,
 			    u16 reg, u32 len, u32 *val)
@@ -1332,22 +1415,20 @@ static int ov08x40_read_reg(struct ov08x40 *ov08x,
 
 	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
+		return ret < 0 ? ret : -EIO;
 
 	*val = be32_to_cpu(data_be);
 
 	return 0;
 }
 
-static int ov08x40_burst_fill_regs(struct ov08x40 *ov08x, u16 first_reg,
-				   u16 last_reg,  u8 val)
+static int __ov08x40_burst_fill_regs(struct i2c_client *client, u16 first_reg,
+				     u16 last_reg, size_t num_regs, u8 val)
 {
-	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
 	struct i2c_msg msgs;
-	size_t i, num_regs;
+	size_t i;
 	int ret;
 
-	num_regs = last_reg - first_reg + 1;
 	msgs.addr = client->addr;
 	msgs.flags = 0;
 	msgs.len = 2 + num_regs;
@@ -1373,12 +1454,37 @@ static int ov08x40_burst_fill_regs(struct ov08x40 *ov08x, u16 first_reg,
 	return 0;
 }
 
+static int ov08x40_burst_fill_regs(struct ov08x40 *ov08x, u16 first_reg,
+				   u16 last_reg,  u8 val)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	size_t num_regs, num_write_regs;
+	int ret;
+
+	num_regs = last_reg - first_reg + 1;
+	num_write_regs = num_regs;
+
+	if (client->adapter->quirks && client->adapter->quirks->max_write_len)
+		num_write_regs = client->adapter->quirks->max_write_len - 2;
+
+	while (first_reg < last_reg) {
+		ret = __ov08x40_burst_fill_regs(client, first_reg, last_reg,
+						num_write_regs, val);
+		if (ret)
+			return ret;
+
+		first_reg += num_write_regs;
+	}
+
+	return 0;
+}
+
 /* Write registers up to 4 at a time */
 static int ov08x40_write_reg(struct ov08x40 *ov08x,
 			     u16 reg, u32 len, u32 __val)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
-	int buf_i, val_i;
+	int buf_i, val_i, ret;
 	u8 buf[6], *val_p;
 	__be32 val;
 
@@ -1396,8 +1502,9 @@ static int ov08x40_write_reg(struct ov08x40 *ov08x,
 	while (val_i < 4)
 		buf[buf_i++] = val_p[val_i++];
 
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
+	ret = i2c_master_send(client, buf, len + 2);
+	if (ret != len + 2)
+		return ret < 0 ? ret : -EIO;
 
 	return 0;
 }
@@ -1641,6 +1748,15 @@ static int ov08x40_set_ctrl(struct v4l2_ctrl *ctrl)
 	return ret;
 }
 
+static bool filter_by_mipi_lanes(const void *array, size_t index,
+				 const void *context)
+{
+	const struct ov08x40_mode *mode = array;
+	const struct ov08x40 *ov08x = context;
+
+	return mode->lanes == ov08x->mipi_lanes;
+}
+
 static const struct v4l2_ctrl_ops ov08x40_ctrl_ops = {
 	.s_ctrl = ov08x40_set_ctrl,
 };
@@ -1662,18 +1778,28 @@ static int ov08x40_enum_frame_size(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state,
 				   struct v4l2_subdev_frame_size_enum *fse)
 {
-	if (fse->index >= ARRAY_SIZE(supported_modes))
-		return -EINVAL;
+	struct ov08x40 *ov08x = to_ov08x40(sd);
+	size_t i, count = 0;
 
 	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
 		return -EINVAL;
 
-	fse->min_width = supported_modes[fse->index].width;
-	fse->max_width = fse->min_width;
-	fse->min_height = supported_modes[fse->index].height;
-	fse->max_height = fse->min_height;
+	for (i = 0; i < ARRAY_SIZE(supported_modes); i++) {
+		if (!filter_by_mipi_lanes(&supported_modes[i], i, ov08x))
+			continue;
 
-	return 0;
+		if (count == fse->index) {
+			fse->min_width = supported_modes[i].width;
+			fse->max_width = fse->min_width;
+			fse->min_height = supported_modes[i].height;
+			fse->max_height = fse->min_height;
+			return 0;
+		}
+
+		count++;
+	}
+
+	return -EINVAL;
 }
 
 static void ov08x40_update_pad_format(const struct ov08x40_mode *mode,
@@ -1736,10 +1862,13 @@ ov08x40_set_pad_format(struct v4l2_subdev *sd,
 	if (fmt->format.code != MEDIA_BUS_FMT_SGRBG10_1X10)
 		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
 
-	mode = v4l2_find_nearest_size(supported_modes,
-				      ARRAY_SIZE(supported_modes),
-				      width, height,
-				      fmt->format.width, fmt->format.height);
+	mode = v4l2_find_nearest_size_conditional(supported_modes,
+						  ARRAY_SIZE(supported_modes),
+						  width, height,
+						  fmt->format.width,
+						  fmt->format.height,
+						  filter_by_mipi_lanes,
+						  ov08x);
 	ov08x40_update_pad_format(mode, fmt);
 	if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 		framefmt = v4l2_subdev_state_get_format(sd_state, fmt->pad);
@@ -1805,6 +1934,14 @@ static int ov08x40_start_streaming(struct ov08x40 *ov08x)
 		return ret;
 	}
 
+	reg_list = &ov08x40_global_setting;
+	ret = ov08x40_write_reg_list(ov08x, reg_list);
+	if (ret) {
+		dev_err(&client->dev, "%s failed to set global setting\n",
+			__func__);
+		return ret;
+	}
+
 	/* Apply default values of current mode */
 	reg_list = &ov08x->cur_mode->reg_list;
 	ret = ov08x40_write_reg_list(ov08x, reg_list);
@@ -1846,6 +1983,35 @@ static int ov08x40_stop_streaming(struct ov08x40 *ov08x)
 				 OV08X40_REG_VALUE_08BIT, OV08X40_MODE_STANDBY);
 }
 
+/* Verify chip ID */
+static int ov08x40_identify_module(struct ov08x40 *ov08x)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
+	int ret;
+	u32 val;
+
+	if (ov08x->identified)
+		return 0;
+
+	ret = ov08x40_read_reg(ov08x, OV08X40_REG_CHIP_ID,
+			       OV08X40_REG_VALUE_24BIT, &val);
+	if (ret) {
+		dev_err(&client->dev, "error reading chip-id register: %d\n", ret);
+		return ret;
+	}
+
+	if (val != OV08X40_CHIP_ID) {
+		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
+			OV08X40_CHIP_ID, val);
+		return -ENXIO;
+	}
+
+	dev_dbg(&client->dev, "chip id 0x%x\n", val);
+	ov08x->identified = true;
+
+	return 0;
+}
+
 static int ov08x40_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct ov08x40 *ov08x = to_ov08x40(sd);
@@ -1859,6 +2025,10 @@ static int ov08x40_set_stream(struct v4l2_subdev *sd, int enable)
 		if (ret < 0)
 			goto err_unlock;
 
+		ret = ov08x40_identify_module(ov08x);
+		if (ret)
+			goto err_rpm_put;
+
 		/*
 		 * Apply default & customized values
 		 * and then start streaming.
@@ -1883,32 +2053,6 @@ err_unlock:
 	return ret;
 }
 
-/* Verify chip ID */
-static int ov08x40_identify_module(struct ov08x40 *ov08x)
-{
-	struct i2c_client *client = v4l2_get_subdevdata(&ov08x->sd);
-	int ret;
-	u32 val;
-
-	if (ov08x->identified)
-		return 0;
-
-	ret = ov08x40_read_reg(ov08x, OV08X40_REG_CHIP_ID,
-			       OV08X40_REG_VALUE_24BIT, &val);
-	if (ret)
-		return ret;
-
-	if (val != OV08X40_CHIP_ID) {
-		dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
-			OV08X40_CHIP_ID, val);
-		return -ENXIO;
-	}
-
-	ov08x->identified = true;
-
-	return 0;
-}
-
 static const struct v4l2_subdev_video_ops ov08x40_video_ops = {
 	.s_stream = ov08x40_set_stream,
 };
@@ -1945,7 +2089,6 @@ static int ov08x40_init_controls(struct ov08x40 *ov08x)
 	s64 pixel_rate_min;
 	s64 pixel_rate_max;
 	const struct ov08x40_mode *mode;
-	u32 max;
 	int ret;
 
 	ctrl_hdlr = &ov08x->ctrl_handler;
@@ -1955,12 +2098,11 @@ static int ov08x40_init_controls(struct ov08x40 *ov08x)
 
 	mutex_init(&ov08x->mutex);
 	ctrl_hdlr->lock = &ov08x->mutex;
-	max = ARRAY_SIZE(link_freq_menu_items) - 1;
 	ov08x->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr,
 						  &ov08x40_ctrl_ops,
 						  V4L2_CID_LINK_FREQ,
-						  max,
-						  0,
+						  __fls(ov08x->link_freq_bitmap),
+						  __ffs(ov08x->link_freq_bitmap),
 						  link_freq_menu_items);
 	if (ov08x->link_freq)
 		ov08x->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
@@ -2049,69 +2191,93 @@ static void ov08x40_free_controls(struct ov08x40 *ov08x)
 	mutex_destroy(&ov08x->mutex);
 }
 
-static int ov08x40_check_hwcfg(struct device *dev)
+static int ov08x40_check_hwcfg(struct ov08x40 *ov08x, struct device *dev)
 {
 	struct v4l2_fwnode_endpoint bus_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY
 	};
 	struct fwnode_handle *ep;
 	struct fwnode_handle *fwnode = dev_fwnode(dev);
-	unsigned int i, j;
+	unsigned int i;
 	int ret;
-	u32 ext_clk;
-
-	if (!fwnode)
-		return -ENXIO;
-
-	ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
-				       &ext_clk);
-	if (ret) {
-		dev_err(dev, "can't get clock frequency");
-		return ret;
-	}
-
-	if (ext_clk != OV08X40_EXT_CLK) {
-		dev_err(dev, "external clock %d is not supported",
-			ext_clk);
-		return -EINVAL;
-	}
+	u32 xvclk_rate;
 
+	/*
+	 * Sometimes the fwnode graph is initialized by the bridge driver.
+	 * Bridge drivers doing this also add sensor properties, wait for this.
+	 */
 	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
 	if (!ep)
-		return -ENXIO;
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
 
 	ret = v4l2_fwnode_endpoint_alloc_parse(ep, &bus_cfg);
 	fwnode_handle_put(ep);
 	if (ret)
-		return ret;
+		return dev_err_probe(dev, ret, "parsing endpoint failed\n");
 
-	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV08X40_DATA_LANES) {
-		dev_err(dev, "number of CSI2 data lanes %d is not supported",
-			bus_cfg.bus.mipi_csi2.num_data_lanes);
-		ret = -EINVAL;
+	ov08x->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(ov08x->reset_gpio)) {
+		ret = dev_err_probe(dev, PTR_ERR(ov08x->reset_gpio),
+				    "getting reset GPIO\n");
 		goto out_err;
 	}
 
-	if (!bus_cfg.nr_of_link_frequencies) {
-		dev_err(dev, "no link frequencies defined");
+	for (i = 0; i < ARRAY_SIZE(ov08x40_supply_names); i++)
+		ov08x->supplies[i].supply = ov08x40_supply_names[i];
+
+	ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(ov08x40_supply_names),
+				      ov08x->supplies);
+	if (ret)
+		goto out_err;
+
+	ov08x->xvclk = devm_clk_get_optional(dev, NULL);
+	if (IS_ERR(ov08x->xvclk)) {
+		ret = dev_err_probe(dev, PTR_ERR(ov08x->xvclk),
+				    "getting xvclk\n");
+		goto out_err;
+	}
+	if (ov08x->xvclk) {
+		xvclk_rate = clk_get_rate(ov08x->xvclk);
+	} else {
+		ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
+					       &xvclk_rate);
+		if (ret) {
+			dev_err(dev, "can't get clock frequency\n");
+			goto out_err;
+		}
+	}
+
+	if (xvclk_rate != OV08X40_XVCLK) {
+		dev_err(dev, "external clock %d is not supported\n",
+			xvclk_rate);
 		ret = -EINVAL;
 		goto out_err;
 	}
 
-	for (i = 0; i < ARRAY_SIZE(link_freq_menu_items); i++) {
-		for (j = 0; j < bus_cfg.nr_of_link_frequencies; j++) {
-			if (link_freq_menu_items[i] ==
-				bus_cfg.link_frequencies[j])
-				break;
-		}
+	switch (bus_cfg.bus.mipi_csi2.num_data_lanes) {
+	case 2:
+	case 4:
+		ov08x->mipi_lanes = bus_cfg.bus.mipi_csi2.num_data_lanes;
+		break;
+	default:
+		dev_err(dev, "number of CSI2 data lanes %d is not supported\n",
+			bus_cfg.bus.mipi_csi2.num_data_lanes);
+		ret = -EINVAL;
+		goto out_err;
+	}
 
-		if (j == bus_cfg.nr_of_link_frequencies) {
-			dev_err(dev, "no link frequency %lld supported",
-				link_freq_menu_items[i]);
-			ret = -EINVAL;
-			goto out_err;
-		}
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_err(dev, "no link frequencies defined\n");
+		ret = -EINVAL;
+		goto out_err;
 	}
+	ret = v4l2_link_freq_to_bitmap(dev, bus_cfg.link_frequencies,
+				       bus_cfg.nr_of_link_frequencies,
+				       link_freq_menu_items,
+				       ARRAY_SIZE(link_freq_menu_items),
+				       &ov08x->link_freq_bitmap);
 
 out_err:
 	v4l2_fwnode_endpoint_free(&bus_cfg);
@@ -2120,33 +2286,34 @@ out_err:
 }
 
 static int ov08x40_probe(struct i2c_client *client)
-{
-	struct ov08x40 *ov08x;
+{	struct ov08x40 *ov08x;
 	int ret;
 	bool full_power;
 
-	/* Check HW config */
-	ret = ov08x40_check_hwcfg(&client->dev);
-	if (ret) {
-		dev_err(&client->dev, "failed to check hwcfg: %d", ret);
-		return ret;
-	}
-
 	ov08x = devm_kzalloc(&client->dev, sizeof(*ov08x), GFP_KERNEL);
 	if (!ov08x)
 		return -ENOMEM;
 
+	/* Check HW config */
+	ret = ov08x40_check_hwcfg(ov08x, &client->dev);
+	if (ret)
+		return ret;
+
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&ov08x->sd, client, &ov08x40_subdev_ops);
 
 	full_power = acpi_dev_state_d0(&client->dev);
 	if (full_power) {
-		/* Check module identity */
-		ret = ov08x40_identify_module(ov08x);
+		ret = ov08x40_power_on(&client->dev);
 		if (ret) {
-			dev_err(&client->dev, "failed to find sensor: %d\n", ret);
+			dev_err(&client->dev, "failed to power on\n");
 			return ret;
 		}
+
+		/* Check module identity */
+		ret = ov08x40_identify_module(ov08x);
+		if (ret)
+			goto probe_power_off;
 	}
 
 	/* Set default mode to max resolution */
@@ -2154,7 +2321,7 @@ static int ov08x40_probe(struct i2c_client *client)
 
 	ret = ov08x40_init_controls(ov08x);
 	if (ret)
-		return ret;
+		goto probe_power_off;
 
 	/* Initialize subdev */
 	ov08x->sd.internal_ops = &ov08x40_internal_ops;
@@ -2187,6 +2354,9 @@ error_media_entity:
 error_handler_free:
 	ov08x40_free_controls(ov08x);
 
+probe_power_off:
+	ov08x40_power_off(&client->dev);
+
 	return ret;
 }
 
@@ -2200,9 +2370,14 @@ static void ov08x40_remove(struct i2c_client *client)
 	ov08x40_free_controls(ov08x);
 
 	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		ov08x40_power_off(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 }
 
+static DEFINE_RUNTIME_DEV_PM_OPS(ov08x40_pm_ops, ov08x40_power_off,
+				 ov08x40_power_on, NULL);
+
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id ov08x40_acpi_ids[] = {
 	{"OVTI08F4"},
@@ -2212,10 +2387,18 @@ static const struct acpi_device_id ov08x40_acpi_ids[] = {
 MODULE_DEVICE_TABLE(acpi, ov08x40_acpi_ids);
 #endif
 
+static const struct of_device_id ov08x40_of_match[] = {
+	{ .compatible = "ovti,ov08x40" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, ov08x40_of_match);
+
 static struct i2c_driver ov08x40_i2c_driver = {
 	.driver = {
 		.name = "ov08x40",
 		.acpi_match_table = ACPI_PTR(ov08x40_acpi_ids),
+		.of_match_table = ov08x40_of_match,
+		.pm = pm_sleep_ptr(&ov08x40_pm_ops),
 	},
 	.probe = ov08x40_probe,
 	.remove = ov08x40_remove,
diff --git a/drivers/media/i2c/ov13858.c b/drivers/media/i2c/ov13858.c
index 09387e335d80..7a3fc1d28514 100644
--- a/drivers/media/i2c/ov13858.c
+++ b/drivers/media/i2c/ov13858.c
@@ -1740,8 +1740,8 @@ static void ov13858_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov13858_id_table[] = {
-	{"ov13858", 0},
-	{},
+	{ "ov13858" },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, ov13858_id_table);
diff --git a/drivers/media/i2c/ov13b10.c b/drivers/media/i2c/ov13b10.c
index 73c844aa5697..e85c7d33a670 100644
--- a/drivers/media/i2c/ov13b10.c
+++ b/drivers/media/i2c/ov13b10.c
@@ -34,9 +34,6 @@
 #define OV13B10_VTS_120FPS		0x0320
 #define OV13B10_VTS_MAX			0x7fff
 
-/* HBLANK control - read only */
-#define OV13B10_PPL_560MHZ		4704
-
 /* Exposure control */
 #define OV13B10_REG_EXPOSURE		0x3500
 #define OV13B10_EXPOSURE_MIN		4
@@ -95,7 +92,7 @@ struct ov13b10_reg_list {
 
 /* Link frequency config */
 struct ov13b10_link_freq_config {
-	u32 pixels_per_line;
+	u64 link_freq;
 
 	/* registers for this link frequency */
 	struct ov13b10_reg_list reg_list;
@@ -114,6 +111,10 @@ struct ov13b10_mode {
 
 	/* Index of Link frequency config to be used */
 	u32 link_freq_index;
+
+	/* Pixels per line in current mode */
+	u32 ppl;
+
 	/* Default register values */
 	struct ov13b10_reg_list reg_list;
 };
@@ -513,6 +514,52 @@ static const struct ov13b10_reg mode_1364x768_120fps_regs[] = {
 	{0x5001, 0x0d},
 };
 
+static const struct ov13b10_reg mode_2lanes_2104x1560_60fps_regs[] = {
+	{0x3016, 0x32},
+	{0x3106, 0x29},
+	{0x0305, 0xaf},
+	{0x3501, 0x06},
+	{0x3662, 0x88},
+	{0x3714, 0x28},
+	{0x3739, 0x10},
+	{0x37c2, 0x14},
+	{0x37d9, 0x06},
+	{0x37e2, 0x0c},
+	{0x3800, 0x00},
+	{0x3801, 0x00},
+	{0x3802, 0x00},
+	{0x3803, 0x08},
+	{0x3804, 0x10},
+	{0x3805, 0x8f},
+	{0x3806, 0x0c},
+	{0x3807, 0x47},
+	{0x3808, 0x08},
+	{0x3809, 0x38},
+	{0x380a, 0x06},
+	{0x380b, 0x18},
+	{0x380c, 0x04},
+	{0x380d, 0x98},
+	{0x380e, 0x06},
+	{0x380f, 0x3e},
+	{0x3810, 0x00},
+	{0x3811, 0x07},
+	{0x3812, 0x00},
+	{0x3813, 0x05},
+	{0x3814, 0x03},
+	{0x3816, 0x03},
+	{0x3820, 0x8b},
+	{0x3c8c, 0x18},
+	{0x4008, 0x00},
+	{0x4009, 0x05},
+	{0x4050, 0x00},
+	{0x4051, 0x05},
+	{0x4501, 0x08},
+	{0x4505, 0x00},
+	{0x4837, 0x0e},
+	{0x5000, 0xfd},
+	{0x5001, 0x0d},
+};
+
 static const char * const ov13b10_test_pattern_menu[] = {
 	"Disabled",
 	"Vertical Color Bar Type 1",
@@ -526,15 +573,16 @@ static const char * const ov13b10_test_pattern_menu[] = {
 #define OV13B10_LINK_FREQ_INDEX_0	0
 
 #define OV13B10_EXT_CLK			19200000
-#define OV13B10_DATA_LANES		4
+#define OV13B10_4_DATA_LANES		4
+#define OV13B10_2_DATA_LANES		2
 
 /*
- * pixel_rate = link_freq * data-rate * nr_of_lanes / bits_per_sample
- * data rate => double data rate; number of lanes => 4; bits per pixel => 10
+ * pixel_rate = data_rate * nr_of_lanes / bits_per_pixel
+ * data_rate => link_freq * 2; number of lanes => 4 or 2; bits per pixel => 10
  */
-static u64 link_freq_to_pixel_rate(u64 f)
+static u64 link_freq_to_pixel_rate(u64 f, u8 lanes)
 {
-	f *= 2 * OV13B10_DATA_LANES;
+	f *= 2 * lanes;
 	do_div(f, 10);
 
 	return f;
@@ -549,7 +597,7 @@ static const s64 link_freq_menu_items[] = {
 static const struct ov13b10_link_freq_config
 			link_freq_configs[] = {
 	{
-		.pixels_per_line = OV13B10_PPL_560MHZ,
+		.link_freq = OV13B10_LINK_FREQ_560MHZ,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mipi_data_rate_1120mbps),
 			.regs = mipi_data_rate_1120mbps,
@@ -558,12 +606,14 @@ static const struct ov13b10_link_freq_config
 };
 
 /* Mode configs */
-static const struct ov13b10_mode supported_modes[] = {
+static const struct ov13b10_mode supported_4_lanes_modes[] = {
+	/* 4 data lanes */
 	{
 		.width = 4208,
 		.height = 3120,
 		.vts_def = OV13B10_VTS_30FPS,
 		.vts_min = OV13B10_VTS_30FPS,
+		.ppl = 4704,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_4208x3120_regs),
 			.regs = mode_4208x3120_regs,
@@ -575,6 +625,7 @@ static const struct ov13b10_mode supported_modes[] = {
 		.height = 3120,
 		.vts_def = OV13B10_VTS_30FPS,
 		.vts_min = OV13B10_VTS_30FPS,
+		.ppl = 4704,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_4160x3120_regs),
 			.regs = mode_4160x3120_regs,
@@ -586,6 +637,7 @@ static const struct ov13b10_mode supported_modes[] = {
 		.height = 2340,
 		.vts_def = OV13B10_VTS_30FPS,
 		.vts_min = OV13B10_VTS_30FPS,
+		.ppl = 4704,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_4160x2340_regs),
 			.regs = mode_4160x2340_regs,
@@ -597,6 +649,7 @@ static const struct ov13b10_mode supported_modes[] = {
 		.height = 1560,
 		.vts_def = OV13B10_VTS_60FPS,
 		.vts_min = OV13B10_VTS_60FPS,
+		.ppl = 4704,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_2104x1560_regs),
 			.regs = mode_2104x1560_regs,
@@ -608,6 +661,7 @@ static const struct ov13b10_mode supported_modes[] = {
 		.height = 1170,
 		.vts_def = OV13B10_VTS_60FPS,
 		.vts_min = OV13B10_VTS_60FPS,
+		.ppl = 4704,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_2080x1170_regs),
 			.regs = mode_2080x1170_regs,
@@ -620,6 +674,7 @@ static const struct ov13b10_mode supported_modes[] = {
 		.vts_def = OV13B10_VTS_120FPS,
 		.vts_min = OV13B10_VTS_120FPS,
 		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+		.ppl = 4664,
 		.reg_list = {
 			.num_of_regs = ARRAY_SIZE(mode_1364x768_120fps_regs),
 			.regs = mode_1364x768_120fps_regs,
@@ -627,6 +682,23 @@ static const struct ov13b10_mode supported_modes[] = {
 	},
 };
 
+static const struct ov13b10_mode supported_2_lanes_modes[] = {
+	/* 2 data lanes */
+	{
+		.width = 2104,
+		.height = 1560,
+		.vts_def = OV13B10_VTS_60FPS,
+		.vts_min = OV13B10_VTS_60FPS,
+		.link_freq_index = OV13B10_LINK_FREQ_INDEX_0,
+		.ppl = 2352,
+		.reg_list = {
+			.num_of_regs =
+				ARRAY_SIZE(mode_2lanes_2104x1560_60fps_regs),
+			.regs = mode_2lanes_2104x1560_60fps_regs,
+		},
+	},
+};
+
 struct ov13b10 {
 	struct v4l2_subdev sd;
 	struct media_pad pad;
@@ -644,12 +716,20 @@ struct ov13b10 {
 	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
 
+	/* Supported modes */
+	const struct ov13b10_mode *supported_modes;
+
 	/* Current mode */
 	const struct ov13b10_mode *cur_mode;
 
 	/* Mutex for serialized access */
 	struct mutex mutex;
 
+	u8 supported_modes_num;
+
+	/* Data lanes used */
+	u8 data_lanes;
+
 	/* True if the device has been identified */
 	bool identified;
 };
@@ -753,8 +833,8 @@ static int ov13b10_write_reg_list(struct ov13b10 *ov13b,
 /* Open sub-device */
 static int ov13b10_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 {
-	const struct ov13b10_mode *default_mode = &supported_modes[0];
 	struct ov13b10 *ov13b = to_ov13b10(sd);
+	const struct ov13b10_mode *default_mode = ov13b->supported_modes;
 	struct v4l2_mbus_framefmt *try_fmt = v4l2_subdev_state_get_format(fh->state,
 									  0);
 
@@ -973,7 +1053,10 @@ static int ov13b10_enum_frame_size(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state,
 				   struct v4l2_subdev_frame_size_enum *fse)
 {
-	if (fse->index >= ARRAY_SIZE(supported_modes))
+	struct ov13b10 *ov13b = to_ov13b10(sd);
+	const struct ov13b10_mode *supported_modes = ov13b->supported_modes;
+
+	if (fse->index >= ov13b->supported_modes_num)
 		return -EINVAL;
 
 	if (fse->code != MEDIA_BUS_FMT_SGRBG10_1X10)
@@ -1033,6 +1116,7 @@ ov13b10_set_pad_format(struct v4l2_subdev *sd,
 {
 	struct ov13b10 *ov13b = to_ov13b10(sd);
 	const struct ov13b10_mode *mode;
+	const struct ov13b10_mode *supported_modes = ov13b->supported_modes;
 	struct v4l2_mbus_framefmt *framefmt;
 	s32 vblank_def;
 	s32 vblank_min;
@@ -1047,7 +1131,7 @@ ov13b10_set_pad_format(struct v4l2_subdev *sd,
 		fmt->format.code = MEDIA_BUS_FMT_SGRBG10_1X10;
 
 	mode = v4l2_find_nearest_size(supported_modes,
-				      ARRAY_SIZE(supported_modes),
+				      ov13b->supported_modes_num,
 				      width, height,
 				      fmt->format.width, fmt->format.height);
 	ov13b10_update_pad_format(mode, fmt);
@@ -1058,23 +1142,18 @@ ov13b10_set_pad_format(struct v4l2_subdev *sd,
 		ov13b->cur_mode = mode;
 		__v4l2_ctrl_s_ctrl(ov13b->link_freq, mode->link_freq_index);
 		link_freq = link_freq_menu_items[mode->link_freq_index];
-		pixel_rate = link_freq_to_pixel_rate(link_freq);
+		pixel_rate = link_freq_to_pixel_rate(link_freq,
+						     ov13b->data_lanes);
 		__v4l2_ctrl_s_ctrl_int64(ov13b->pixel_rate, pixel_rate);
 
 		/* Update limits and set FPS to default */
-		vblank_def = ov13b->cur_mode->vts_def -
-			     ov13b->cur_mode->height;
-		vblank_min = ov13b->cur_mode->vts_min -
-			     ov13b->cur_mode->height;
+		vblank_def = mode->vts_def - mode->height;
+		vblank_min = mode->vts_min - mode->height;
 		__v4l2_ctrl_modify_range(ov13b->vblank, vblank_min,
-					 OV13B10_VTS_MAX
-					 - ov13b->cur_mode->height,
-					 1,
-					 vblank_def);
+					 OV13B10_VTS_MAX - mode->height,
+					 1, vblank_def);
 		__v4l2_ctrl_s_ctrl(ov13b->vblank, vblank_def);
-		h_blank =
-			link_freq_configs[mode->link_freq_index].pixels_per_line
-			 - ov13b->cur_mode->width;
+		h_blank = mode->ppl - mode->width;
 		__v4l2_ctrl_modify_range(ov13b->hblank, h_blank,
 					 h_blank, 1, h_blank);
 	}
@@ -1311,7 +1390,8 @@ static int ov13b10_init_controls(struct ov13b10 *ov13b)
 	if (ov13b->link_freq)
 		ov13b->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0]);
+	pixel_rate_max = link_freq_to_pixel_rate(link_freq_menu_items[0],
+						 ov13b->data_lanes);
 	pixel_rate_min = 0;
 	/* By default, PIXEL_RATE is read only */
 	ov13b->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
@@ -1328,8 +1408,7 @@ static int ov13b10_init_controls(struct ov13b10 *ov13b)
 					  OV13B10_VTS_MAX - mode->height, 1,
 					  vblank_def);
 
-	hblank = link_freq_configs[mode->link_freq_index].pixels_per_line -
-		 mode->width;
+	hblank = mode->ppl - mode->width;
 	ov13b->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &ov13b10_ctrl_ops,
 					  V4L2_CID_HBLANK,
 					  hblank, hblank, 1, hblank);
@@ -1423,7 +1502,7 @@ static int ov13b10_get_pm_resources(struct device *dev)
 	return 0;
 }
 
-static int ov13b10_check_hwcfg(struct device *dev)
+static int ov13b10_check_hwcfg(struct device *dev, struct ov13b10 *ov13b)
 {
 	struct v4l2_fwnode_endpoint bus_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY
@@ -1433,6 +1512,7 @@ static int ov13b10_check_hwcfg(struct device *dev)
 	unsigned int i, j;
 	int ret;
 	u32 ext_clk;
+	u8 dlane;
 
 	if (!fwnode)
 		return -ENXIO;
@@ -1459,13 +1539,32 @@ static int ov13b10_check_hwcfg(struct device *dev)
 	if (ret)
 		return ret;
 
-	if (bus_cfg.bus.mipi_csi2.num_data_lanes != OV13B10_DATA_LANES) {
+	dlane = bus_cfg.bus.mipi_csi2.num_data_lanes;
+	switch (dlane) {
+	case OV13B10_4_DATA_LANES:
+		ov13b->supported_modes = supported_4_lanes_modes;
+		ov13b->supported_modes_num =
+			ARRAY_SIZE(supported_4_lanes_modes);
+		break;
+
+	case OV13B10_2_DATA_LANES:
+		ov13b->supported_modes = supported_2_lanes_modes;
+		ov13b->supported_modes_num =
+			ARRAY_SIZE(supported_2_lanes_modes);
+		break;
+
+	default:
 		dev_err(dev, "number of CSI2 data lanes %d is not supported",
-			bus_cfg.bus.mipi_csi2.num_data_lanes);
+			dlane);
 		ret = -EINVAL;
 		goto out_err;
 	}
 
+	ov13b->data_lanes = dlane;
+	ov13b->cur_mode = ov13b->supported_modes;
+	dev_dbg(dev, "%u lanes with %u modes selected\n",
+		ov13b->data_lanes, ov13b->supported_modes_num);
+
 	if (!bus_cfg.nr_of_link_frequencies) {
 		dev_err(dev, "no link frequencies defined");
 		ret = -EINVAL;
@@ -1499,17 +1598,17 @@ static int ov13b10_probe(struct i2c_client *client)
 	bool full_power;
 	int ret;
 
+	ov13b = devm_kzalloc(&client->dev, sizeof(*ov13b), GFP_KERNEL);
+	if (!ov13b)
+		return -ENOMEM;
+
 	/* Check HW config */
-	ret = ov13b10_check_hwcfg(&client->dev);
+	ret = ov13b10_check_hwcfg(&client->dev, ov13b);
 	if (ret) {
 		dev_err(&client->dev, "failed to check hwcfg: %d", ret);
 		return ret;
 	}
 
-	ov13b = devm_kzalloc(&client->dev, sizeof(*ov13b), GFP_KERNEL);
-	if (!ov13b)
-		return -ENOMEM;
-
 	/* Initialize subdev */
 	v4l2_i2c_subdev_init(&ov13b->sd, client, &ov13b10_subdev_ops);
 
@@ -1533,9 +1632,6 @@ static int ov13b10_probe(struct i2c_client *client)
 		}
 	}
 
-	/* Set default mode to max resolution */
-	ov13b->cur_mode = &supported_modes[0];
-
 	ret = ov13b10_init_controls(ov13b);
 	if (ret)
 		goto error_power_off;
diff --git a/drivers/media/i2c/ov2640.c b/drivers/media/i2c/ov2640.c
index 67c4bd2916e8..d27fc2df64e6 100644
--- a/drivers/media/i2c/ov2640.c
+++ b/drivers/media/i2c/ov2640.c
@@ -1271,7 +1271,7 @@ static void ov2640_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov2640_id[] = {
-	{ "ov2640", 0 },
+	{ "ov2640" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov2640_id);
diff --git a/drivers/media/i2c/ov2659.c b/drivers/media/i2c/ov2659.c
index d1653d7431d0..06b7896c3eaf 100644
--- a/drivers/media/i2c/ov2659.c
+++ b/drivers/media/i2c/ov2659.c
@@ -1551,8 +1551,8 @@ static const struct dev_pm_ops ov2659_pm_ops = {
 };
 
 static const struct i2c_device_id ov2659_id[] = {
-	{ "ov2659", 0 },
-	{ /* sentinel */ },
+	{ "ov2659" },
+	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, ov2659_id);
 
diff --git a/drivers/media/i2c/ov2680.c b/drivers/media/i2c/ov2680.c
index 39d321e2b7f9..7237fb27ecd0 100644
--- a/drivers/media/i2c/ov2680.c
+++ b/drivers/media/i2c/ov2680.c
@@ -75,6 +75,8 @@
 #define OV2680_ACTIVE_START_TOP			8
 #define OV2680_MIN_CROP_WIDTH			2
 #define OV2680_MIN_CROP_HEIGHT			2
+#define OV2680_MIN_VBLANK			4
+#define OV2680_MAX_VBLANK			0xffff
 
 /* Fixed pre-div of 1/2 */
 #define OV2680_PLL_PREDIV0			2
@@ -84,10 +86,7 @@
 
 /* 66MHz pixel clock: 66MHz / 1704 * 1294 = 30fps */
 #define OV2680_PIXELS_PER_LINE			1704
-#define OV2680_LINES_PER_FRAME			1294
-
-/* If possible send 16 extra rows / lines to the ISP as padding */
-#define OV2680_END_MARGIN			16
+#define OV2680_LINES_PER_FRAME_30FPS		1294
 
 /* Max exposure time is VTS - 8 */
 #define OV2680_INTEGRATION_TIME_MARGIN		8
@@ -130,6 +129,8 @@ struct ov2680_ctrls {
 	struct v4l2_ctrl *test_pattern;
 	struct v4l2_ctrl *link_freq;
 	struct v4l2_ctrl *pixel_rate;
+	struct v4l2_ctrl *vblank;
+	struct v4l2_ctrl *hblank;
 };
 
 struct ov2680_mode {
@@ -143,8 +144,6 @@ struct ov2680_mode {
 	u16				v_end;
 	u16				h_output_size;
 	u16				v_output_size;
-	u16				hts;
-	u16				vts;
 };
 
 struct ov2680_dev {
@@ -359,15 +358,11 @@ static void ov2680_calc_mode(struct ov2680_dev *sensor)
 	sensor->mode.v_start = (sensor->mode.crop.top +
 				(sensor->mode.crop.height - height) / 2) & ~1;
 	sensor->mode.h_end =
-		min(sensor->mode.h_start + width + OV2680_END_MARGIN - 1,
-		    OV2680_NATIVE_WIDTH - 1);
+		min(sensor->mode.h_start + width - 1, OV2680_NATIVE_WIDTH - 1);
 	sensor->mode.v_end =
-		min(sensor->mode.v_start + height + OV2680_END_MARGIN - 1,
-		    OV2680_NATIVE_HEIGHT - 1);
+		min(sensor->mode.v_start + height - 1, OV2680_NATIVE_HEIGHT - 1);
 	sensor->mode.h_output_size = orig_width;
 	sensor->mode.v_output_size = orig_height;
-	sensor->mode.hts = OV2680_PIXELS_PER_LINE;
-	sensor->mode.vts = OV2680_LINES_PER_FRAME;
 }
 
 static int ov2680_set_mode(struct ov2680_dev *sensor)
@@ -402,9 +397,8 @@ static int ov2680_set_mode(struct ov2680_dev *sensor)
 	cci_write(sensor->regmap, OV2680_REG_VERTICAL_OUTPUT_SIZE,
 		  sensor->mode.v_output_size, &ret);
 	cci_write(sensor->regmap, OV2680_REG_TIMING_HTS,
-		  sensor->mode.hts, &ret);
-	cci_write(sensor->regmap, OV2680_REG_TIMING_VTS,
-		  sensor->mode.vts, &ret);
+		  OV2680_PIXELS_PER_LINE, &ret);
+	/* VTS gets set by the vblank ctrl */
 	cci_write(sensor->regmap, OV2680_REG_ISP_X_WIN, 0, &ret);
 	cci_write(sensor->regmap, OV2680_REG_ISP_Y_WIN, 0, &ret);
 	cci_write(sensor->regmap, OV2680_REG_X_INC, inc, &ret);
@@ -478,6 +472,15 @@ static int ov2680_exposure_set(struct ov2680_dev *sensor, u32 exp)
 			 NULL);
 }
 
+static int ov2680_exposure_update_range(struct ov2680_dev *sensor)
+{
+	int exp_max = sensor->mode.fmt.height + sensor->ctrls.vblank->val -
+		      OV2680_INTEGRATION_TIME_MARGIN;
+
+	return __v4l2_ctrl_modify_range(sensor->ctrls.exposure, 0, exp_max,
+					1, exp_max);
+}
+
 static int ov2680_stream_enable(struct ov2680_dev *sensor)
 {
 	int ret;
@@ -644,7 +647,7 @@ static int ov2680_set_fmt(struct v4l2_subdev *sd,
 	struct v4l2_mbus_framefmt *try_fmt;
 	const struct v4l2_rect *crop;
 	unsigned int width, height;
-	int ret = 0;
+	int def, max, ret = 0;
 
 	crop = __ov2680_get_pad_crop(sensor, sd_state, format->pad,
 				     format->which);
@@ -673,6 +676,27 @@ static int ov2680_set_fmt(struct v4l2_subdev *sd,
 	sensor->mode.fmt = format->format;
 	ov2680_calc_mode(sensor);
 
+	/* vblank range is height dependent adjust and reset to default */
+	max = OV2680_MAX_VBLANK - height;
+	def = OV2680_LINES_PER_FRAME_30FPS - height;
+	ret = __v4l2_ctrl_modify_range(sensor->ctrls.vblank, OV2680_MIN_VBLANK,
+				       max, 1, def);
+	if (ret)
+		goto unlock;
+
+	ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.vblank, def);
+	if (ret)
+		goto unlock;
+
+	/* exposure range depends on vts which may have changed */
+	ret = ov2680_exposure_update_range(sensor);
+	if (ret)
+		goto unlock;
+
+	/* adjust hblank value for new width */
+	def = OV2680_PIXELS_PER_LINE - width;
+	ret = __v4l2_ctrl_modify_range(sensor->ctrls.hblank, def, def, 1, def);
+
 unlock:
 	mutex_unlock(&sensor->lock);
 
@@ -842,6 +866,13 @@ static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct ov2680_dev *sensor = to_ov2680_dev(sd);
 	int ret;
 
+	/* Update exposure range on vblank changes */
+	if (ctrl->id == V4L2_CID_VBLANK) {
+		ret = ov2680_exposure_update_range(sensor);
+		if (ret)
+			return ret;
+	}
+
 	/* Only apply changes to the controls if the device is powered up */
 	if (!pm_runtime_get_if_in_use(sensor->sd.dev)) {
 		ov2680_set_bayer_order(sensor, &sensor->mode.fmt);
@@ -864,6 +895,10 @@ static int ov2680_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_TEST_PATTERN:
 		ret = ov2680_test_pattern_set(sensor, ctrl->val);
 		break;
+	case V4L2_CID_VBLANK:
+		ret = cci_write(sensor->regmap, OV2680_REG_TIMING_VTS,
+				sensor->mode.fmt.height + ctrl->val, NULL);
+		break;
 	default:
 		ret = -EINVAL;
 		break;
@@ -922,8 +957,8 @@ static int ov2680_v4l2_register(struct ov2680_dev *sensor)
 	const struct v4l2_ctrl_ops *ops = &ov2680_ctrl_ops;
 	struct ov2680_ctrls *ctrls = &sensor->ctrls;
 	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
-	int exp_max = OV2680_LINES_PER_FRAME - OV2680_INTEGRATION_TIME_MARGIN;
-	int ret = 0;
+	struct v4l2_fwnode_device_properties props;
+	int def, max, ret = 0;
 
 	v4l2_i2c_subdev_init(&sensor->sd, client, &ov2680_subdev_ops);
 	sensor->sd.internal_ops = &ov2680_internal_ops;
@@ -936,7 +971,7 @@ static int ov2680_v4l2_register(struct ov2680_dev *sensor)
 	if (ret < 0)
 		return ret;
 
-	v4l2_ctrl_handler_init(hdl, 5);
+	v4l2_ctrl_handler_init(hdl, 11);
 
 	hdl->lock = &sensor->lock;
 
@@ -948,8 +983,9 @@ static int ov2680_v4l2_register(struct ov2680_dev *sensor)
 					ARRAY_SIZE(test_pattern_menu) - 1,
 					0, 0, test_pattern_menu);
 
+	max = OV2680_LINES_PER_FRAME_30FPS - OV2680_INTEGRATION_TIME_MARGIN;
 	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
-					    0, exp_max, 1, exp_max);
+					    0, max, 1, max);
 
 	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
 					0, 1023, 1, 250);
@@ -960,6 +996,21 @@ static int ov2680_v4l2_register(struct ov2680_dev *sensor)
 					      0, sensor->pixel_rate,
 					      1, sensor->pixel_rate);
 
+	max = OV2680_MAX_VBLANK - OV2680_DEFAULT_HEIGHT;
+	def = OV2680_LINES_PER_FRAME_30FPS - OV2680_DEFAULT_HEIGHT;
+	ctrls->vblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
+					  OV2680_MIN_VBLANK, max, 1, def);
+
+	def = OV2680_PIXELS_PER_LINE - OV2680_DEFAULT_WIDTH;
+	ctrls->hblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK,
+					  def, def, 1, def);
+
+	ret = v4l2_fwnode_device_parse(sensor->dev, &props);
+	if (ret)
+		goto cleanup_entity;
+
+	v4l2_ctrl_new_fwnode_properties(hdl, ops, &props);
+
 	if (hdl->error) {
 		ret = hdl->error;
 		goto cleanup_entity;
@@ -968,6 +1019,7 @@ static int ov2680_v4l2_register(struct ov2680_dev *sensor)
 	ctrls->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 	ctrls->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
 	ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
 	sensor->sd.ctrl_handler = hdl;
 
@@ -1116,25 +1168,24 @@ static int ov2680_parse_dt(struct ov2680_dev *sensor)
 	sensor->pixel_rate = sensor->link_freq[0] * 2;
 	do_div(sensor->pixel_rate, 10);
 
-	/* Verify bus cfg */
-	if (bus_cfg.bus.mipi_csi2.num_data_lanes != 1) {
-		ret = dev_err_probe(dev, -EINVAL,
-				    "only a 1-lane CSI2 config is supported");
-		goto out_free_bus_cfg;
+	if (!bus_cfg.nr_of_link_frequencies) {
+		dev_warn(dev, "Consider passing 'link-frequencies' in DT\n");
+		goto skip_link_freq_validation;
 	}
 
 	for (i = 0; i < bus_cfg.nr_of_link_frequencies; i++)
 		if (bus_cfg.link_frequencies[i] == sensor->link_freq[0])
 			break;
 
-	if (bus_cfg.nr_of_link_frequencies == 0 ||
-	    bus_cfg.nr_of_link_frequencies == i) {
+	if (bus_cfg.nr_of_link_frequencies == i) {
 		ret = dev_err_probe(dev, -EINVAL,
 				    "supported link freq %lld not found\n",
 				    sensor->link_freq[0]);
 		goto out_free_bus_cfg;
 	}
 
+skip_link_freq_validation:
+	ret = 0;
 out_free_bus_cfg:
 	v4l2_fwnode_endpoint_free(&bus_cfg);
 	return ret;
diff --git a/drivers/media/i2c/ov2740.c b/drivers/media/i2c/ov2740.c
index 552935ccb4a9..6cf461e3373c 100644
--- a/drivers/media/i2c/ov2740.c
+++ b/drivers/media/i2c/ov2740.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2020 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -11,6 +11,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/nvmem-provider.h>
 #include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-fwnode.h>
@@ -76,6 +77,12 @@
 /* OTP registers from sensor */
 #define OV2740_REG_OTP_CUSTOMER		0x7010
 
+static const char * const ov2740_supply_name[] = {
+	"AVDD",
+	"DOVDD",
+	"DVDD",
+};
+
 struct nvm_data {
 	struct nvmem_device *nvmem;
 	struct regmap *regmap;
@@ -523,14 +530,16 @@ struct ov2740 {
 	struct v4l2_ctrl *hblank;
 	struct v4l2_ctrl *exposure;
 
-	/* GPIOs, clocks */
+	/* GPIOs, clocks, regulators */
 	struct gpio_desc *reset_gpio;
+	struct gpio_desc *powerdown_gpio;
 	struct clk *clk;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(ov2740_supply_name)];
 
 	/* Current mode */
 	const struct ov2740_mode *cur_mode;
 
-	/* NVM data inforamtion */
+	/* NVM data information */
 	struct nvm_data *nvm;
 
 	/* Supported modes */
@@ -644,6 +653,8 @@ static int ov2740_identify_module(struct ov2740 *ov2740)
 		return -ENXIO;
 	}
 
+	dev_dbg(&client->dev, "chip id: 0x%x\n", val);
+
 	ov2740->identified = true;
 
 	return 0;
@@ -753,29 +764,32 @@ static const struct v4l2_ctrl_ops ov2740_ctrl_ops = {
 
 static int ov2740_init_controls(struct ov2740 *ov2740)
 {
+	struct i2c_client *client = v4l2_get_subdevdata(&ov2740->sd);
 	struct v4l2_ctrl_handler *ctrl_hdlr;
 	const struct ov2740_mode *cur_mode;
 	s64 exposure_max, h_blank, pixel_rate;
 	u32 vblank_min, vblank_max, vblank_default;
+	struct v4l2_fwnode_device_properties props;
 	int size;
 	int ret;
 
 	ctrl_hdlr = &ov2740->ctrl_handler;
-	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
+	ret = v4l2_ctrl_handler_init(ctrl_hdlr, 10);
 	if (ret)
 		return ret;
 
 	cur_mode = ov2740->cur_mode;
 	size = ARRAY_SIZE(link_freq_menu_items);
 
-	ov2740->link_freq = v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov2740_ctrl_ops,
-						   V4L2_CID_LINK_FREQ,
-						   size - 1, 0,
-						   link_freq_menu_items);
+	ov2740->link_freq =
+		v4l2_ctrl_new_int_menu(ctrl_hdlr, &ov2740_ctrl_ops,
+				       V4L2_CID_LINK_FREQ, size - 1,
+				       ov2740->supported_modes->link_freq_index,
+				       link_freq_menu_items);
 	if (ov2740->link_freq)
 		ov2740->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 
-	pixel_rate = to_pixel_rate(OV2740_LINK_FREQ_360MHZ_INDEX);
+	pixel_rate = to_pixel_rate(ov2740->supported_modes->link_freq_index);
 	ov2740->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &ov2740_ctrl_ops,
 					       V4L2_CID_PIXEL_RATE, 0,
 					       pixel_rate, 1, pixel_rate);
@@ -810,6 +824,15 @@ static int ov2740_init_controls(struct ov2740 *ov2740)
 				     V4L2_CID_TEST_PATTERN,
 				     ARRAY_SIZE(ov2740_test_pattern_menu) - 1,
 				     0, 0, ov2740_test_pattern_menu);
+
+	ret = v4l2_fwnode_device_parse(&client->dev, &props);
+	if (ret) {
+		v4l2_ctrl_handler_free(ctrl_hdlr);
+		return ret;
+	}
+
+	v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &ov2740_ctrl_ops, &props);
+
 	if (ctrl_hdlr->error) {
 		v4l2_ctrl_handler_free(ctrl_hdlr);
 		return ctrl_hdlr->error;
@@ -1131,7 +1154,8 @@ static int ov2740_check_hwcfg(struct device *dev)
 	 */
 	ep = fwnode_graph_get_next_endpoint(fwnode, NULL);
 	if (!ep)
-		return -EPROBE_DEFER;
+		return dev_err_probe(dev, -EPROBE_DEFER,
+				     "waiting for fwnode graph endpoint\n");
 
 	ret = fwnode_property_read_u32(fwnode, "clock-frequency", &mclk);
 	if (ret) {
@@ -1293,7 +1317,10 @@ static int ov2740_suspend(struct device *dev)
 	struct ov2740 *ov2740 = to_ov2740(sd);
 
 	gpiod_set_value_cansleep(ov2740->reset_gpio, 1);
+	gpiod_set_value_cansleep(ov2740->powerdown_gpio, 1);
 	clk_disable_unprepare(ov2740->clk);
+	regulator_bulk_disable(ARRAY_SIZE(ov2740_supply_name),
+			       ov2740->supplies);
 	return 0;
 }
 
@@ -1303,10 +1330,19 @@ static int ov2740_resume(struct device *dev)
 	struct ov2740 *ov2740 = to_ov2740(sd);
 	int ret;
 
-	ret = clk_prepare_enable(ov2740->clk);
+	ret = regulator_bulk_enable(ARRAY_SIZE(ov2740_supply_name),
+				    ov2740->supplies);
 	if (ret)
 		return ret;
 
+	ret = clk_prepare_enable(ov2740->clk);
+	if (ret) {
+		regulator_bulk_disable(ARRAY_SIZE(ov2740_supply_name),
+				       ov2740->supplies);
+		return ret;
+	}
+
+	gpiod_set_value_cansleep(ov2740->powerdown_gpio, 0);
 	gpiod_set_value_cansleep(ov2740->reset_gpio, 0);
 	msleep(20);
 
@@ -1318,6 +1354,7 @@ static int ov2740_probe(struct i2c_client *client)
 	struct device *dev = &client->dev;
 	struct ov2740 *ov2740;
 	bool full_power;
+	unsigned int i;
 	int ret;
 
 	ov2740 = devm_kzalloc(&client->dev, sizeof(*ov2740), GFP_KERNEL);
@@ -1329,18 +1366,41 @@ static int ov2740_probe(struct i2c_client *client)
 
 	ret = ov2740_check_hwcfg(dev);
 	if (ret)
-		return dev_err_probe(dev, ret, "failed to check HW configuration\n");
+		return ret;
 
 	ov2740->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
-	if (IS_ERR(ov2740->reset_gpio))
+	if (IS_ERR(ov2740->reset_gpio)) {
 		return dev_err_probe(dev, PTR_ERR(ov2740->reset_gpio),
 				     "failed to get reset GPIO\n");
+	}
+
+	ov2740->powerdown_gpio = devm_gpiod_get_optional(dev, "powerdown", GPIOD_OUT_HIGH);
+	if (IS_ERR(ov2740->powerdown_gpio)) {
+		return dev_err_probe(dev, PTR_ERR(ov2740->powerdown_gpio),
+				     "failed to get powerdown GPIO\n");
+	}
+
+	if (ov2740->reset_gpio || ov2740->powerdown_gpio) {
+		/*
+		 * Ensure reset/powerdown is asserted for at least 20 ms before
+		 * ov2740_resume() deasserts it.
+		 */
+		msleep(20);
+	}
 
 	ov2740->clk = devm_clk_get_optional(dev, "clk");
 	if (IS_ERR(ov2740->clk))
 		return dev_err_probe(dev, PTR_ERR(ov2740->clk),
 				     "failed to get clock\n");
 
+	for (i = 0; i < ARRAY_SIZE(ov2740_supply_name); i++)
+		ov2740->supplies[i].supply = ov2740_supply_name[i];
+
+	ret = devm_regulator_bulk_get(dev, ARRAY_SIZE(ov2740_supply_name),
+				      ov2740->supplies);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to get regulators\n");
+
 	full_power = acpi_dev_state_d0(&client->dev);
 	if (full_power) {
 		/* ACPI does not always clear the reset GPIO / enable the clock */
@@ -1396,12 +1456,12 @@ static int ov2740_probe(struct i2c_client *client)
 	return 0;
 
 probe_error_v4l2_subdev_cleanup:
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
 	v4l2_subdev_cleanup(&ov2740->sd);
 
 probe_error_media_entity_cleanup:
 	media_entity_cleanup(&ov2740->sd.entity);
-	pm_runtime_disable(&client->dev);
-	pm_runtime_set_suspended(&client->dev);
 
 probe_error_v4l2_ctrl_handler_free:
 	v4l2_ctrl_handler_free(ov2740->sd.ctrl_handler);
diff --git a/drivers/media/i2c/ov4689.c b/drivers/media/i2c/ov4689.c
index 403091651885..1c3a449f9354 100644
--- a/drivers/media/i2c/ov4689.c
+++ b/drivers/media/i2c/ov4689.c
@@ -3,7 +3,7 @@
  * ov4689 driver
  *
  * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
- * Copyright (C) 2022 Mikhail Rudenko
+ * Copyright (C) 2022, 2024 Mikhail Rudenko
  */
 
 #include <linux/clk.h>
@@ -15,45 +15,86 @@
 #include <linux/regulator/consumer.h>
 #include <media/media-entity.h>
 #include <media/v4l2-async.h>
+#include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-fwnode.h>
 
-#define CHIP_ID				0x004688
-#define OV4689_REG_CHIP_ID		0x300a
-
-#define OV4689_XVCLK_FREQ		24000000
-
-#define OV4689_REG_CTRL_MODE		0x0100
+#define OV4689_REG_CTRL_MODE		CCI_REG8(0x0100)
 #define OV4689_MODE_SW_STANDBY		0x0
 #define OV4689_MODE_STREAMING		BIT(0)
 
-#define OV4689_REG_EXPOSURE		0x3500
+#define OV4689_REG_CHIP_ID		CCI_REG16(0x300a)
+#define CHIP_ID				0x004688
+
+#define OV4689_REG_EXPOSURE		CCI_REG24(0x3500)
 #define OV4689_EXPOSURE_MIN		4
 #define OV4689_EXPOSURE_STEP		1
-#define OV4689_VTS_MAX			0x7fff
 
-#define OV4689_REG_GAIN_H		0x3508
-#define OV4689_REG_GAIN_L		0x3509
-#define OV4689_GAIN_H_MASK		0x07
-#define OV4689_GAIN_H_SHIFT		8
-#define OV4689_GAIN_L_MASK		0xff
+#define OV4689_REG_GAIN			CCI_REG16(0x3508)
 #define OV4689_GAIN_STEP		1
 #define OV4689_GAIN_DEFAULT		0x80
 
-#define OV4689_REG_TEST_PATTERN		0x5040
-#define OV4689_TEST_PATTERN_ENABLE	0x80
-#define OV4689_TEST_PATTERN_DISABLE	0x0
+#define OV4689_REG_DIG_GAIN		CCI_REG16(0x352a)
+#define OV4689_DIG_GAIN_MIN		1
+#define OV4689_DIG_GAIN_MAX		0x7fff
+#define OV4689_DIG_GAIN_STEP		1
+#define OV4689_DIG_GAIN_DEFAULT		0x800
 
-#define OV4689_REG_VTS			0x380e
+#define OV4689_REG_H_CROP_START		CCI_REG16(0x3800)
+#define OV4689_REG_V_CROP_START		CCI_REG16(0x3802)
+#define OV4689_REG_H_CROP_END		CCI_REG16(0x3804)
+#define OV4689_REG_V_CROP_END		CCI_REG16(0x3806)
+#define OV4689_REG_H_OUTPUT_SIZE	CCI_REG16(0x3808)
+#define OV4689_REG_V_OUTPUT_SIZE	CCI_REG16(0x380a)
 
-#define REG_NULL			0xFFFF
+#define OV4689_REG_HTS			CCI_REG16(0x380c)
+#define OV4689_HTS_DIVIDER		4
+#define OV4689_HTS_MAX			0x7fff
 
-#define OV4689_REG_VALUE_08BIT		1
-#define OV4689_REG_VALUE_16BIT		2
-#define OV4689_REG_VALUE_24BIT		3
+#define OV4689_REG_VTS			CCI_REG16(0x380e)
+#define OV4689_VTS_MAX			0x7fff
+
+#define OV4689_REG_H_WIN_OFF		CCI_REG16(0x3810)
+#define OV4689_REG_V_WIN_OFF		CCI_REG16(0x3812)
+
+#define OV4689_REG_TIMING_FORMAT1	CCI_REG8(0x3820) /* Vertical */
+#define OV4689_REG_TIMING_FORMAT2	CCI_REG8(0x3821) /* Horizontal */
+#define OV4689_TIMING_FLIP_MASK		GENMASK(2, 1)
+#define OV4689_TIMING_FLIP_ARRAY	BIT(1)
+#define OV4689_TIMING_FLIP_DIGITAL	BIT(2)
+#define OV4689_TIMING_FLIP_BOTH		(OV4689_TIMING_FLIP_ARRAY |\
+					 OV4689_TIMING_FLIP_DIGITAL)
+
+#define OV4689_REG_ANCHOR_LEFT_START	CCI_REG16(0x4020)
+#define OV4689_ANCHOR_LEFT_START_DEF	576
+#define OV4689_REG_ANCHOR_LEFT_END	CCI_REG16(0x4022)
+#define OV4689_ANCHOR_LEFT_END_DEF	831
+#define OV4689_REG_ANCHOR_RIGHT_START	CCI_REG16(0x4024)
+#define OV4689_ANCHOR_RIGHT_START_DEF	1984
+#define OV4689_REG_ANCHOR_RIGHT_END	CCI_REG16(0x4026)
+#define OV4689_ANCHOR_RIGHT_END_DEF	2239
+
+#define OV4689_REG_VFIFO_CTRL_01	CCI_REG8(0x4601)
+
+#define OV4689_REG_WB_GAIN_RED		CCI_REG16(0x500c)
+#define OV4689_REG_WB_GAIN_BLUE		CCI_REG16(0x5010)
+#define OV4689_WB_GAIN_MIN		1
+#define OV4689_WB_GAIN_MAX		0xfff
+#define OV4689_WB_GAIN_STEP		1
+#define OV4689_WB_GAIN_DEFAULT		0x400
+
+#define OV4689_REG_TEST_PATTERN		CCI_REG8(0x5040)
+#define OV4689_TEST_PATTERN_ENABLE	0x80
+#define OV4689_TEST_PATTERN_DISABLE	0x0
 
 #define OV4689_LANES			4
+#define OV4689_XVCLK_FREQ		24000000
+
+#define OV4689_PIXEL_ARRAY_WIDTH	2720
+#define OV4689_PIXEL_ARRAY_HEIGHT	1536
+#define OV4689_DUMMY_ROWS		8	/* 8 dummy rows on each side */
+#define OV4689_DUMMY_COLUMNS		16	/* 16 dummy columns on each side */
 
 static const char *const ov4689_supply_names[] = {
 	"avdd", /* Analog power */
@@ -61,11 +102,6 @@ static const char *const ov4689_supply_names[] = {
 	"dvdd", /* Digital core power */
 };
 
-struct regval {
-	u16 addr;
-	u8 val;
-};
-
 enum ov4689_mode_id {
 	OV4689_MODE_2688_1520 = 0,
 	OV4689_NUM_MODES,
@@ -75,20 +111,18 @@ struct ov4689_mode {
 	enum ov4689_mode_id id;
 	u32 width;
 	u32 height;
-	u32 max_fps;
 	u32 hts_def;
+	u32 hts_min;
 	u32 vts_def;
 	u32 exp_def;
 	u32 pixel_rate;
-	u32 sensor_width;
-	u32 sensor_height;
-	u32 crop_top;
-	u32 crop_left;
-	const struct regval *reg_list;
+	const struct cci_reg_sequence *reg_list;
+	unsigned int num_regs;
 };
 
 struct ov4689 {
-	struct i2c_client *client;
+	struct device *dev;
+	struct regmap *regmap;
 	struct clk *xvclk;
 	struct gpio_desc *reset_gpio;
 	struct gpio_desc *pwdn_gpio;
@@ -99,7 +133,6 @@ struct ov4689 {
 
 	u32 clock_rate;
 
-	struct mutex mutex; /* lock to protect ctrls and cur_mode */
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_ctrl *exposure;
 
@@ -119,95 +152,108 @@ struct ov4689_gain_range {
 
 /*
  * Xclk 24Mhz
- * max_framerate 30fps
+ * max_framerate 90fps
  * mipi_datarate per lane 1008Mbps
  */
-static const struct regval ov4689_2688x1520_regs[] = {
-	{0x0103, 0x01}, {0x3638, 0x00}, {0x0300, 0x00},
-	{0x0302, 0x2a}, {0x0303, 0x00}, {0x0304, 0x03},
-	{0x030b, 0x00}, {0x030d, 0x1e}, {0x030e, 0x04},
-	{0x030f, 0x01}, {0x0312, 0x01}, {0x031e, 0x00},
-	{0x3000, 0x20}, {0x3002, 0x00}, {0x3018, 0x72},
-	{0x3020, 0x93}, {0x3021, 0x03}, {0x3022, 0x01},
-	{0x3031, 0x0a}, {0x303f, 0x0c}, {0x3305, 0xf1},
-	{0x3307, 0x04}, {0x3309, 0x29}, {0x3500, 0x00},
-	{0x3501, 0x60}, {0x3502, 0x00}, {0x3503, 0x04},
-	{0x3504, 0x00}, {0x3505, 0x00}, {0x3506, 0x00},
-	{0x3507, 0x00}, {0x3508, 0x00}, {0x3509, 0x80},
-	{0x350a, 0x00}, {0x350b, 0x00}, {0x350c, 0x00},
-	{0x350d, 0x00}, {0x350e, 0x00}, {0x350f, 0x80},
-	{0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x00},
-	{0x3513, 0x00}, {0x3514, 0x00}, {0x3515, 0x80},
-	{0x3516, 0x00}, {0x3517, 0x00}, {0x3518, 0x00},
-	{0x3519, 0x00}, {0x351a, 0x00}, {0x351b, 0x80},
-	{0x351c, 0x00}, {0x351d, 0x00}, {0x351e, 0x00},
-	{0x351f, 0x00}, {0x3520, 0x00}, {0x3521, 0x80},
-	{0x3522, 0x08}, {0x3524, 0x08}, {0x3526, 0x08},
-	{0x3528, 0x08}, {0x352a, 0x08}, {0x3602, 0x00},
-	{0x3603, 0x40}, {0x3604, 0x02}, {0x3605, 0x00},
-	{0x3606, 0x00}, {0x3607, 0x00}, {0x3609, 0x12},
-	{0x360a, 0x40}, {0x360c, 0x08}, {0x360f, 0xe5},
-	{0x3608, 0x8f}, {0x3611, 0x00}, {0x3613, 0xf7},
-	{0x3616, 0x58}, {0x3619, 0x99}, {0x361b, 0x60},
-	{0x361c, 0x7a}, {0x361e, 0x79}, {0x361f, 0x02},
-	{0x3632, 0x00}, {0x3633, 0x10}, {0x3634, 0x10},
-	{0x3635, 0x10}, {0x3636, 0x15}, {0x3646, 0x86},
-	{0x364a, 0x0b}, {0x3700, 0x17}, {0x3701, 0x22},
-	{0x3703, 0x10}, {0x370a, 0x37}, {0x3705, 0x00},
-	{0x3706, 0x63}, {0x3709, 0x3c}, {0x370b, 0x01},
-	{0x370c, 0x30}, {0x3710, 0x24}, {0x3711, 0x0c},
-	{0x3716, 0x00}, {0x3720, 0x28}, {0x3729, 0x7b},
-	{0x372a, 0x84}, {0x372b, 0xbd}, {0x372c, 0xbc},
-	{0x372e, 0x52}, {0x373c, 0x0e}, {0x373e, 0x33},
-	{0x3743, 0x10}, {0x3744, 0x88}, {0x3745, 0xc0},
-	{0x374a, 0x43}, {0x374c, 0x00}, {0x374e, 0x23},
-	{0x3751, 0x7b}, {0x3752, 0x84}, {0x3753, 0xbd},
-	{0x3754, 0xbc}, {0x3756, 0x52}, {0x375c, 0x00},
-	{0x3760, 0x00}, {0x3761, 0x00}, {0x3762, 0x00},
-	{0x3763, 0x00}, {0x3764, 0x00}, {0x3767, 0x04},
-	{0x3768, 0x04}, {0x3769, 0x08}, {0x376a, 0x08},
-	{0x376b, 0x20}, {0x376c, 0x00}, {0x376d, 0x00},
-	{0x376e, 0x00}, {0x3773, 0x00}, {0x3774, 0x51},
-	{0x3776, 0xbd}, {0x3777, 0xbd}, {0x3781, 0x18},
-	{0x3783, 0x25}, {0x3798, 0x1b}, {0x3800, 0x00},
-	{0x3801, 0x08}, {0x3802, 0x00}, {0x3803, 0x04},
-	{0x3804, 0x0a}, {0x3805, 0x97}, {0x3806, 0x05},
-	{0x3807, 0xfb}, {0x3808, 0x0a}, {0x3809, 0x80},
-	{0x380a, 0x05}, {0x380b, 0xf0}, {0x380c, 0x0a},
-	{0x380d, 0x0e}, {0x380e, 0x06}, {0x380f, 0x12},
-	{0x3810, 0x00}, {0x3811, 0x08}, {0x3812, 0x00},
-	{0x3813, 0x04}, {0x3814, 0x01}, {0x3815, 0x01},
-	{0x3819, 0x01}, {0x3820, 0x00}, {0x3821, 0x06},
-	{0x3829, 0x00}, {0x382a, 0x01}, {0x382b, 0x01},
-	{0x382d, 0x7f}, {0x3830, 0x04}, {0x3836, 0x01},
-	{0x3837, 0x00}, {0x3841, 0x02}, {0x3846, 0x08},
-	{0x3847, 0x07}, {0x3d85, 0x36}, {0x3d8c, 0x71},
-	{0x3d8d, 0xcb}, {0x3f0a, 0x00}, {0x4000, 0xf1},
-	{0x4001, 0x40}, {0x4002, 0x04}, {0x4003, 0x14},
-	{0x400e, 0x00}, {0x4011, 0x00}, {0x401a, 0x00},
-	{0x401b, 0x00}, {0x401c, 0x00}, {0x401d, 0x00},
-	{0x401f, 0x00}, {0x4020, 0x00}, {0x4021, 0x10},
-	{0x4022, 0x07}, {0x4023, 0xcf}, {0x4024, 0x09},
-	{0x4025, 0x60}, {0x4026, 0x09}, {0x4027, 0x6f},
-	{0x4028, 0x00}, {0x4029, 0x02}, {0x402a, 0x06},
-	{0x402b, 0x04}, {0x402c, 0x02}, {0x402d, 0x02},
-	{0x402e, 0x0e}, {0x402f, 0x04}, {0x4302, 0xff},
-	{0x4303, 0xff}, {0x4304, 0x00}, {0x4305, 0x00},
-	{0x4306, 0x00}, {0x4308, 0x02}, {0x4500, 0x6c},
-	{0x4501, 0xc4}, {0x4502, 0x40}, {0x4503, 0x01},
-	{0x4601, 0xa7}, {0x4800, 0x04}, {0x4813, 0x08},
-	{0x481f, 0x40}, {0x4829, 0x78}, {0x4837, 0x10},
-	{0x4b00, 0x2a}, {0x4b0d, 0x00}, {0x4d00, 0x04},
-	{0x4d01, 0x42}, {0x4d02, 0xd1}, {0x4d03, 0x93},
-	{0x4d04, 0xf5}, {0x4d05, 0xc1}, {0x5000, 0xf3},
-	{0x5001, 0x11}, {0x5004, 0x00}, {0x500a, 0x00},
-	{0x500b, 0x00}, {0x5032, 0x00}, {0x5040, 0x00},
-	{0x5050, 0x0c}, {0x5500, 0x00}, {0x5501, 0x10},
-	{0x5502, 0x01}, {0x5503, 0x0f}, {0x8000, 0x00},
-	{0x8001, 0x00}, {0x8002, 0x00}, {0x8003, 0x00},
-	{0x8004, 0x00}, {0x8005, 0x00}, {0x8006, 0x00},
-	{0x8007, 0x00}, {0x8008, 0x00}, {0x3638, 0x00},
-	{REG_NULL, 0x00},
+static const struct cci_reg_sequence ov4689_2688x1520_regs[] = {
+	/* System control*/
+	{ CCI_REG8(0x0103), 0x01 }, /* SC_CTRL0103 software_reset = 1 */
+	{ CCI_REG8(0x3000), 0x20 }, /* SC_CMMN_PAD_OEN0 FSIN_output_enable = 1 */
+	{ CCI_REG8(0x3021), 0x03 }, /*
+				     * SC_CMMN_MISC_CTRL fst_stby_ctr = 0,
+				     * sleep_no_latch_enable = 0
+				     */
+
+	/* AEC PK */
+	{ CCI_REG8(0x3503), 0x04 }, /* AEC_MANUAL gain_input_as_sensor_gain_format = 1 */
+
+	/* ADC and analog control*/
+	{ CCI_REG8(0x3603), 0x40 },
+	{ CCI_REG8(0x3604), 0x02 },
+	{ CCI_REG8(0x3609), 0x12 },
+	{ CCI_REG8(0x360c), 0x08 },
+	{ CCI_REG8(0x360f), 0xe5 },
+	{ CCI_REG8(0x3608), 0x8f },
+	{ CCI_REG8(0x3611), 0x00 },
+	{ CCI_REG8(0x3613), 0xf7 },
+	{ CCI_REG8(0x3616), 0x58 },
+	{ CCI_REG8(0x3619), 0x99 },
+	{ CCI_REG8(0x361b), 0x60 },
+	{ CCI_REG8(0x361e), 0x79 },
+	{ CCI_REG8(0x3634), 0x10 },
+	{ CCI_REG8(0x3635), 0x10 },
+	{ CCI_REG8(0x3636), 0x15 },
+	{ CCI_REG8(0x3646), 0x86 },
+	{ CCI_REG8(0x364a), 0x0b },
+
+	/* Sensor control */
+	{ CCI_REG8(0x3700), 0x17 },
+	{ CCI_REG8(0x3701), 0x22 },
+	{ CCI_REG8(0x3703), 0x10 },
+	{ CCI_REG8(0x370a), 0x37 },
+	{ CCI_REG8(0x3706), 0x63 },
+	{ CCI_REG8(0x3709), 0x3c },
+	{ CCI_REG8(0x370c), 0x30 },
+	{ CCI_REG8(0x3710), 0x24 },
+	{ CCI_REG8(0x3720), 0x28 },
+	{ CCI_REG8(0x3729), 0x7b },
+	{ CCI_REG8(0x372b), 0xbd },
+	{ CCI_REG8(0x372c), 0xbc },
+	{ CCI_REG8(0x372e), 0x52 },
+	{ CCI_REG8(0x373c), 0x0e },
+	{ CCI_REG8(0x373e), 0x33 },
+	{ CCI_REG8(0x3743), 0x10 },
+	{ CCI_REG8(0x3744), 0x88 },
+	{ CCI_REG8(0x3745), 0xc0 },
+	{ CCI_REG8(0x374c), 0x00 },
+	{ CCI_REG8(0x374e), 0x23 },
+	{ CCI_REG8(0x3751), 0x7b },
+	{ CCI_REG8(0x3753), 0xbd },
+	{ CCI_REG8(0x3754), 0xbc },
+	{ CCI_REG8(0x3756), 0x52 },
+	{ CCI_REG8(0x376b), 0x20 },
+	{ CCI_REG8(0x3774), 0x51 },
+	{ CCI_REG8(0x3776), 0xbd },
+	{ CCI_REG8(0x3777), 0xbd },
+	{ CCI_REG8(0x3781), 0x18 },
+	{ CCI_REG8(0x3783), 0x25 },
+	{ CCI_REG8(0x3798), 0x1b },
+
+	/* Timing control */
+	{ CCI_REG8(0x3819), 0x01 }, /* VSYNC_END_L vsync_end_point[7:0] = 0x01 */
+
+	/* OTP control */
+	{ CCI_REG8(0x3d85), 0x36 }, /* OTP_REG85 OTP_power_up_load_setting_enable = 1,
+				     * OTP_power_up_load_data_enable = 1,
+				     * OTP_bist_select = 1 (compare with zero)
+				     */
+	{ CCI_REG8(0x3d8c), 0x71 }, /* OTP_SETTING_STT_ADDRESS_H */
+	{ CCI_REG8(0x3d8d), 0xcb }, /* OTP_SETTING_STT_ADDRESS_L */
+
+	/* BLC registers*/
+	{ CCI_REG8(0x4001), 0x40 }, /* DEBUG_MODE */
+	{ CCI_REG8(0x401b), 0x00 }, /* DEBUG_MODE */
+	{ CCI_REG8(0x401d), 0x00 }, /* DEBUG_MODE */
+	{ CCI_REG8(0x401f), 0x00 }, /* DEBUG_MODE */
+
+	/* ADC sync control */
+	{ CCI_REG8(0x4500), 0x6c }, /* ADC_SYNC_CTRL */
+	{ CCI_REG8(0x4503), 0x01 }, /* ADC_SYNC_CTRL */
+
+	/* Temperature monitor */
+	{ CCI_REG8(0x4d00), 0x04 }, /* TPM_CTRL_00 tmp_slope[15:8] = 0x04 */
+	{ CCI_REG8(0x4d01), 0x42 }, /* TPM_CTRL_01 tmp_slope[7:0] = 0x42 */
+	{ CCI_REG8(0x4d02), 0xd1 }, /* TPM_CTRL_02 tpm_offset[31:24] = 0xd1 */
+	{ CCI_REG8(0x4d03), 0x93 }, /* TPM_CTRL_03 tpm_offset[23:16] = 0x93 */
+	{ CCI_REG8(0x4d04), 0xf5 }, /* TPM_CTRL_04 tpm_offset[15:8]  = 0xf5 */
+	{ CCI_REG8(0x4d05), 0xc1 }, /* TPM_CTRL_05 tpm_offset[7:0]   = 0xc1 */
+
+	/* pre-ISP control */
+	{ CCI_REG8(0x5050), 0x0c }, /* DEBUG_MODE */
+
+	/* OTP-DPC control */
+	{ CCI_REG8(0x5501), 0x10 }, /* OTP_DPC_START_L otp_start_address[7:0] = 0x10 */
+	{ CCI_REG8(0x5503), 0x0f }, /* OTP_DPC_END_L otp_end_address[7:0] = 0x0f */
 };
 
 static const struct ov4689_mode supported_modes[] = {
@@ -215,16 +261,13 @@ static const struct ov4689_mode supported_modes[] = {
 		.id = OV4689_MODE_2688_1520,
 		.width = 2688,
 		.height = 1520,
-		.sensor_width = 2720,
-		.sensor_height = 1536,
-		.crop_top = 8,
-		.crop_left = 16,
-		.max_fps = 30,
 		.exp_def = 1536,
-		.hts_def = 4 * 2574,
+		.hts_def = 10296,
+		.hts_min = 3432,
 		.vts_def = 1554,
 		.pixel_rate = 480000000,
 		.reg_list = ov4689_2688x1520_regs,
+		.num_regs = ARRAY_SIZE(ov4689_2688x1520_regs),
 	},
 };
 
@@ -277,83 +320,6 @@ static const struct ov4689_gain_range ov4689_gain_ranges[] = {
 	},
 };
 
-/* Write registers up to 4 at a time */
-static int ov4689_write_reg(struct i2c_client *client, u16 reg, u32 len,
-			    u32 val)
-{
-	u32 buf_i, val_i;
-	__be32 val_be;
-	u8 *val_p;
-	u8 buf[6];
-
-	if (len > 4)
-		return -EINVAL;
-
-	buf[0] = reg >> 8;
-	buf[1] = reg & 0xff;
-
-	val_be = cpu_to_be32(val);
-	val_p = (u8 *)&val_be;
-	buf_i = 2;
-	val_i = 4 - len;
-
-	while (val_i < 4)
-		buf[buf_i++] = val_p[val_i++];
-
-	if (i2c_master_send(client, buf, len + 2) != len + 2)
-		return -EIO;
-
-	return 0;
-}
-
-static int ov4689_write_array(struct i2c_client *client,
-			      const struct regval *regs)
-{
-	int ret = 0;
-	u32 i;
-
-	for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
-		ret = ov4689_write_reg(client, regs[i].addr,
-				       OV4689_REG_VALUE_08BIT, regs[i].val);
-
-	return ret;
-}
-
-/* Read registers up to 4 at a time */
-static int ov4689_read_reg(struct i2c_client *client, u16 reg, unsigned int len,
-			   u32 *val)
-{
-	__be16 reg_addr_be = cpu_to_be16(reg);
-	struct i2c_msg msgs[2];
-	__be32 data_be = 0;
-	u8 *data_be_p;
-	int ret;
-
-	if (len > 4 || !len)
-		return -EINVAL;
-
-	data_be_p = (u8 *)&data_be;
-	/* Write register address */
-	msgs[0].addr = client->addr;
-	msgs[0].flags = 0;
-	msgs[0].len = 2;
-	msgs[0].buf = (u8 *)&reg_addr_be;
-
-	/* Read data from register */
-	msgs[1].addr = client->addr;
-	msgs[1].flags = I2C_M_RD;
-	msgs[1].len = len;
-	msgs[1].buf = &data_be_p[4 - len];
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret != ARRAY_SIZE(msgs))
-		return -EIO;
-
-	*val = be32_to_cpu(data_be);
-
-	return 0;
-}
-
 static void ov4689_fill_fmt(const struct ov4689_mode *mode,
 			    struct v4l2_mbus_framefmt *fmt)
 {
@@ -376,19 +342,6 @@ static int ov4689_set_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ov4689_get_fmt(struct v4l2_subdev *sd,
-			  struct v4l2_subdev_state *sd_state,
-			  struct v4l2_subdev_format *fmt)
-{
-	struct v4l2_mbus_framefmt *mbus_fmt = &fmt->format;
-	struct ov4689 *ov4689 = to_ov4689(sd);
-
-	/* only one mode supported for now */
-	ov4689_fill_fmt(ov4689->cur_mode, mbus_fmt);
-
-	return 0;
-}
-
 static int ov4689_enum_mbus_code(struct v4l2_subdev *sd,
 				 struct v4l2_subdev_state *sd_state,
 				 struct v4l2_subdev_mbus_code_enum *code)
@@ -427,16 +380,14 @@ static int ov4689_enable_test_pattern(struct ov4689 *ov4689, u32 pattern)
 	else
 		val = OV4689_TEST_PATTERN_DISABLE;
 
-	return ov4689_write_reg(ov4689->client, OV4689_REG_TEST_PATTERN,
-				OV4689_REG_VALUE_08BIT, val);
+	return cci_write(ov4689->regmap, OV4689_REG_TEST_PATTERN,
+			 val, NULL);
 }
 
 static int ov4689_get_selection(struct v4l2_subdev *sd,
 				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_selection *sel)
 {
-	const struct ov4689_mode *mode = to_ov4689(sd)->cur_mode;
-
 	if (sel->which != V4L2_SUBDEV_FORMAT_ACTIVE)
 		return -EINVAL;
 
@@ -444,63 +395,114 @@ static int ov4689_get_selection(struct v4l2_subdev *sd,
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 		sel->r.top = 0;
 		sel->r.left = 0;
-		sel->r.width = mode->sensor_width;
-		sel->r.height = mode->sensor_height;
+		sel->r.width = OV4689_PIXEL_ARRAY_WIDTH;
+		sel->r.height = OV4689_PIXEL_ARRAY_HEIGHT;
 		return 0;
 	case V4L2_SEL_TGT_CROP:
 	case V4L2_SEL_TGT_CROP_DEFAULT:
-		sel->r.top = mode->crop_top;
-		sel->r.left = mode->crop_left;
-		sel->r.width = mode->width;
-		sel->r.height = mode->height;
+		sel->r.top = OV4689_DUMMY_ROWS;
+		sel->r.left = OV4689_DUMMY_COLUMNS;
+		sel->r.width =
+			OV4689_PIXEL_ARRAY_WIDTH - 2 * OV4689_DUMMY_COLUMNS;
+		sel->r.height =
+			OV4689_PIXEL_ARRAY_HEIGHT - 2 * OV4689_DUMMY_ROWS;
 		return 0;
 	}
 
 	return -EINVAL;
 }
 
+static int ov4689_setup_timings(struct ov4689 *ov4689)
+{
+	const struct ov4689_mode *mode = ov4689->cur_mode;
+	struct regmap *rm = ov4689->regmap;
+	int ret = 0;
+
+	cci_write(rm, OV4689_REG_H_CROP_START, 8, &ret);
+	cci_write(rm, OV4689_REG_V_CROP_START, 8, &ret);
+	cci_write(rm, OV4689_REG_H_CROP_END, 2711, &ret);
+	cci_write(rm, OV4689_REG_V_CROP_END, 1531, &ret);
+
+	cci_write(rm, OV4689_REG_H_OUTPUT_SIZE, mode->width, &ret);
+	cci_write(rm, OV4689_REG_V_OUTPUT_SIZE, mode->height, &ret);
+
+	cci_write(rm, OV4689_REG_H_WIN_OFF, 8, &ret);
+	cci_write(rm, OV4689_REG_V_WIN_OFF, 4, &ret);
+
+	cci_write(rm, OV4689_REG_VFIFO_CTRL_01, 167, &ret);
+
+	return ret;
+}
+
+static int ov4689_setup_blc_anchors(struct ov4689 *ov4689)
+{
+	struct regmap *rm = ov4689->regmap;
+	int ret = 0;
+
+	cci_write(rm, OV4689_REG_ANCHOR_LEFT_START, 16, &ret);
+	cci_write(rm, OV4689_REG_ANCHOR_LEFT_END, 1999, &ret);
+	cci_write(rm, OV4689_REG_ANCHOR_RIGHT_START, 2400, &ret);
+	cci_write(rm, OV4689_REG_ANCHOR_RIGHT_END, 2415, &ret);
+
+	return ret;
+}
+
 static int ov4689_s_stream(struct v4l2_subdev *sd, int on)
 {
 	struct ov4689 *ov4689 = to_ov4689(sd);
-	struct i2c_client *client = ov4689->client;
+	struct v4l2_subdev_state *sd_state;
+	struct device *dev = ov4689->dev;
 	int ret = 0;
 
-	mutex_lock(&ov4689->mutex);
+	sd_state = v4l2_subdev_lock_and_get_active_state(&ov4689->subdev);
 
 	if (on) {
-		ret = pm_runtime_resume_and_get(&client->dev);
+		ret = pm_runtime_resume_and_get(dev);
 		if (ret < 0)
 			goto unlock_and_return;
 
-		ret = ov4689_write_array(ov4689->client,
-					 ov4689->cur_mode->reg_list);
+		ret = cci_multi_reg_write(ov4689->regmap,
+					  ov4689->cur_mode->reg_list,
+					  ov4689->cur_mode->num_regs,
+					  NULL);
+		if (ret) {
+			pm_runtime_put(dev);
+			goto unlock_and_return;
+		}
+
+		ret = ov4689_setup_timings(ov4689);
 		if (ret) {
-			pm_runtime_put(&client->dev);
+			pm_runtime_put(dev);
+			goto unlock_and_return;
+		}
+
+		ret = ov4689_setup_blc_anchors(ov4689);
+		if (ret) {
+			pm_runtime_put(dev);
 			goto unlock_and_return;
 		}
 
 		ret = __v4l2_ctrl_handler_setup(&ov4689->ctrl_handler);
 		if (ret) {
-			pm_runtime_put(&client->dev);
+			pm_runtime_put(dev);
 			goto unlock_and_return;
 		}
 
-		ret = ov4689_write_reg(ov4689->client, OV4689_REG_CTRL_MODE,
-				       OV4689_REG_VALUE_08BIT,
-				       OV4689_MODE_STREAMING);
+		ret = cci_write(ov4689->regmap, OV4689_REG_CTRL_MODE,
+				OV4689_MODE_STREAMING, NULL);
 		if (ret) {
-			pm_runtime_put(&client->dev);
+			pm_runtime_put(dev);
 			goto unlock_and_return;
 		}
 	} else {
-		ov4689_write_reg(ov4689->client, OV4689_REG_CTRL_MODE,
-				 OV4689_REG_VALUE_08BIT,
-				 OV4689_MODE_SW_STANDBY);
-		pm_runtime_put(&client->dev);
+		cci_write(ov4689->regmap, OV4689_REG_CTRL_MODE,
+			  OV4689_MODE_SW_STANDBY, NULL);
+		pm_runtime_mark_last_busy(dev);
+		pm_runtime_put_autosuspend(dev);
 	}
 
 unlock_and_return:
-	mutex_unlock(&ov4689->mutex);
+	v4l2_subdev_unlock_state(sd_state);
 
 	return ret;
 }
@@ -563,18 +565,13 @@ static int __maybe_unused ov4689_power_off(struct device *dev)
 	return 0;
 }
 
-static int ov4689_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
+static int ov4689_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
 {
-	struct ov4689 *ov4689 = to_ov4689(sd);
-	struct v4l2_mbus_framefmt *try_fmt;
-
-	mutex_lock(&ov4689->mutex);
-
-	try_fmt = v4l2_subdev_state_get_format(fh->state, 0);
-	/* Initialize try_fmt */
-	ov4689_fill_fmt(&supported_modes[OV4689_MODE_2688_1520], try_fmt);
+	struct v4l2_mbus_framefmt *fmt =
+		v4l2_subdev_state_get_format(sd_state, 0);
 
-	mutex_unlock(&ov4689->mutex);
+	ov4689_fill_fmt(&supported_modes[OV4689_MODE_2688_1520], fmt);
 
 	return 0;
 }
@@ -583,10 +580,6 @@ static const struct dev_pm_ops ov4689_pm_ops = {
 	SET_RUNTIME_PM_OPS(ov4689_power_off, ov4689_power_on, NULL)
 };
 
-static const struct v4l2_subdev_internal_ops ov4689_internal_ops = {
-	.open = ov4689_open,
-};
-
 static const struct v4l2_subdev_video_ops ov4689_video_ops = {
 	.s_stream = ov4689_s_stream,
 };
@@ -594,11 +587,15 @@ static const struct v4l2_subdev_video_ops ov4689_video_ops = {
 static const struct v4l2_subdev_pad_ops ov4689_pad_ops = {
 	.enum_mbus_code = ov4689_enum_mbus_code,
 	.enum_frame_size = ov4689_enum_frame_sizes,
-	.get_fmt = ov4689_get_fmt,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = ov4689_set_fmt,
 	.get_selection = ov4689_get_selection,
 };
 
+static const struct v4l2_subdev_internal_ops ov4689_internal_ops = {
+	.init_state = ov4689_init_state,
+};
+
 static const struct v4l2_subdev_ops ov4689_subdev_ops = {
 	.video = &ov4689_video_ops,
 	.pad = &ov4689_pad_ops,
@@ -610,7 +607,6 @@ static const struct v4l2_subdev_ops ov4689_subdev_ops = {
  */
 static int ov4689_map_gain(struct ov4689 *ov4689, int logical_gain, int *result)
 {
-	const struct device *dev = &ov4689->client->dev;
 	const struct ov4689_gain_range *range;
 	unsigned int n;
 
@@ -621,7 +617,8 @@ static int ov4689_map_gain(struct ov4689 *ov4689, int logical_gain, int *result)
 	}
 
 	if (n == ARRAY_SIZE(ov4689_gain_ranges)) {
-		dev_warn_ratelimited(dev, "no mapping found for gain %d\n",
+		dev_warn_ratelimited(ov4689->dev,
+				     "no mapping found for gain %d\n",
 				     logical_gain);
 		return -EINVAL;
 	}
@@ -637,10 +634,11 @@ static int ov4689_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct ov4689 *ov4689 =
 		container_of(ctrl->handler, struct ov4689, ctrl_handler);
-	struct i2c_client *client = ov4689->client;
-	int sensor_gain;
+	struct regmap *regmap = ov4689->regmap;
+	struct device *dev = ov4689->dev;
+	int sensor_gain = 0;
 	s64 max_expo;
-	int ret;
+	int ret = 0;
 
 	/* Propagate change of current control to all related controls */
 	switch (ctrl->id) {
@@ -654,44 +652,58 @@ static int ov4689_set_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 	}
 
-	if (!pm_runtime_get_if_in_use(&client->dev))
+	if (!pm_runtime_get_if_in_use(dev))
 		return 0;
 
 	switch (ctrl->id) {
 	case V4L2_CID_EXPOSURE:
-		/* 4 least significant bits of expsoure are fractional part */
-		ret = ov4689_write_reg(ov4689->client, OV4689_REG_EXPOSURE,
-				       OV4689_REG_VALUE_24BIT, ctrl->val << 4);
+		/* 4 least significant bits of exposure are fractional part */
+		cci_write(regmap, OV4689_REG_EXPOSURE, ctrl->val << 4, &ret);
 		break;
 	case V4L2_CID_ANALOGUE_GAIN:
 		ret = ov4689_map_gain(ov4689, ctrl->val, &sensor_gain);
-
-		ret = ret ?:
-			ov4689_write_reg(ov4689->client, OV4689_REG_GAIN_H,
-					 OV4689_REG_VALUE_08BIT,
-					 (sensor_gain >> OV4689_GAIN_H_SHIFT) &
-					 OV4689_GAIN_H_MASK);
-		ret = ret ?:
-			ov4689_write_reg(ov4689->client, OV4689_REG_GAIN_L,
-					 OV4689_REG_VALUE_08BIT,
-					 sensor_gain & OV4689_GAIN_L_MASK);
+		cci_write(regmap, OV4689_REG_GAIN, sensor_gain, &ret);
 		break;
 	case V4L2_CID_VBLANK:
-		ret = ov4689_write_reg(ov4689->client, OV4689_REG_VTS,
-				       OV4689_REG_VALUE_16BIT,
-				       ctrl->val + ov4689->cur_mode->height);
+		cci_write(regmap, OV4689_REG_VTS,
+			  ctrl->val + ov4689->cur_mode->height, &ret);
 		break;
 	case V4L2_CID_TEST_PATTERN:
 		ret = ov4689_enable_test_pattern(ov4689, ctrl->val);
 		break;
+	case V4L2_CID_HBLANK:
+		cci_write(regmap, OV4689_REG_HTS,
+			  (ctrl->val + ov4689->cur_mode->width) /
+			  OV4689_HTS_DIVIDER, &ret);
+		break;
+	case V4L2_CID_VFLIP:
+		cci_update_bits(regmap, OV4689_REG_TIMING_FORMAT1,
+				OV4689_TIMING_FLIP_MASK,
+				ctrl->val ? OV4689_TIMING_FLIP_BOTH : 0, &ret);
+		break;
+	case V4L2_CID_HFLIP:
+		cci_update_bits(regmap, OV4689_REG_TIMING_FORMAT2,
+				OV4689_TIMING_FLIP_MASK,
+				ctrl->val ? 0 : OV4689_TIMING_FLIP_BOTH, &ret);
+		break;
+	case V4L2_CID_DIGITAL_GAIN:
+		cci_write(regmap, OV4689_REG_DIG_GAIN, ctrl->val, &ret);
+		break;
+	case V4L2_CID_RED_BALANCE:
+		cci_write(regmap, OV4689_REG_WB_GAIN_RED, ctrl->val, &ret);
+		break;
+	case V4L2_CID_BLUE_BALANCE:
+		cci_write(regmap, OV4689_REG_WB_GAIN_BLUE, ctrl->val, &ret);
+		break;
 	default:
-		dev_warn(&client->dev, "%s Unhandled id:0x%x, val:0x%x\n",
+		dev_warn(dev, "%s Unhandled id:0x%x, val:0x%x\n",
 			 __func__, ctrl->id, ctrl->val);
 		ret = -EINVAL;
 		break;
 	}
 
-	pm_runtime_put(&client->dev);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
 
 	return ret;
 }
@@ -707,16 +719,15 @@ static int ov4689_initialize_controls(struct ov4689 *ov4689)
 	struct v4l2_ctrl_handler *handler;
 	const struct ov4689_mode *mode;
 	s64 exposure_max, vblank_def;
+	s64 hblank_def, hblank_min;
 	struct v4l2_ctrl *ctrl;
-	s64 h_blank_def;
 	int ret;
 
 	handler = &ov4689->ctrl_handler;
 	mode = ov4689->cur_mode;
-	ret = v4l2_ctrl_handler_init(handler, 10);
+	ret = v4l2_ctrl_handler_init(handler, 15);
 	if (ret)
 		return ret;
-	handler->lock = &ov4689->mutex;
 
 	ctrl = v4l2_ctrl_new_int_menu(handler, NULL, V4L2_CID_LINK_FREQ, 0, 0,
 				      link_freq_menu_items);
@@ -726,11 +737,11 @@ static int ov4689_initialize_controls(struct ov4689 *ov4689)
 	v4l2_ctrl_new_std(handler, NULL, V4L2_CID_PIXEL_RATE, 0,
 			  mode->pixel_rate, 1, mode->pixel_rate);
 
-	h_blank_def = mode->hts_def - mode->width;
-	ctrl = v4l2_ctrl_new_std(handler, NULL, V4L2_CID_HBLANK, h_blank_def,
-				 h_blank_def, 1, h_blank_def);
-	if (ctrl)
-		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	hblank_def = mode->hts_def - mode->width;
+	hblank_min = mode->hts_min - mode->width;
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_HBLANK,
+			  hblank_min, OV4689_HTS_MAX - mode->width,
+			  OV4689_HTS_DIVIDER, hblank_def);
 
 	vblank_def = mode->vts_def - mode->height;
 	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_VBLANK,
@@ -754,10 +765,24 @@ static int ov4689_initialize_controls(struct ov4689 *ov4689)
 				     ARRAY_SIZE(ov4689_test_pattern_menu) - 1,
 				     0, 0, ov4689_test_pattern_menu);
 
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
+			  OV4689_DIG_GAIN_MIN, OV4689_DIG_GAIN_MAX,
+			  OV4689_DIG_GAIN_STEP, OV4689_DIG_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_RED_BALANCE,
+			  OV4689_WB_GAIN_MIN, OV4689_WB_GAIN_MAX,
+			  OV4689_WB_GAIN_STEP, OV4689_WB_GAIN_DEFAULT);
+
+	v4l2_ctrl_new_std(handler, &ov4689_ctrl_ops, V4L2_CID_BLUE_BALANCE,
+			  OV4689_WB_GAIN_MIN, OV4689_WB_GAIN_MAX,
+			  OV4689_WB_GAIN_STEP, OV4689_WB_GAIN_DEFAULT);
+
 	if (handler->error) {
 		ret = handler->error;
-		dev_err(&ov4689->client->dev, "Failed to init controls(%d)\n",
-			ret);
+		dev_err(ov4689->dev, "Failed to init controls(%d)\n", ret);
 		goto err_free_handler;
 	}
 
@@ -783,19 +808,18 @@ err_free_handler:
 static int ov4689_check_sensor_id(struct ov4689 *ov4689,
 				  struct i2c_client *client)
 {
-	struct device *dev = &ov4689->client->dev;
-	u32 id = 0;
+	struct device *dev = ov4689->dev;
+	u64 id = 0;
 	int ret;
 
-	ret = ov4689_read_reg(client, OV4689_REG_CHIP_ID,
-			      OV4689_REG_VALUE_16BIT, &id);
+	ret = cci_read(ov4689->regmap, OV4689_REG_CHIP_ID, &id, NULL);
 	if (ret) {
 		dev_err(dev, "Cannot read sensor ID\n");
 		return ret;
 	}
 
 	if (id != CHIP_ID) {
-		dev_err(dev, "Unexpected sensor ID %06x, expected %06x\n",
+		dev_err(dev, "Unexpected sensor ID %06llx, expected %06x\n",
 			id, CHIP_ID);
 		return -ENODEV;
 	}
@@ -812,7 +836,7 @@ static int ov4689_configure_regulators(struct ov4689 *ov4689)
 	for (i = 0; i < ARRAY_SIZE(ov4689_supply_names); i++)
 		ov4689->supplies[i].supply = ov4689_supply_names[i];
 
-	return devm_regulator_bulk_get(&ov4689->client->dev,
+	return devm_regulator_bulk_get(ov4689->dev,
 				       ARRAY_SIZE(ov4689_supply_names),
 				       ov4689->supplies);
 }
@@ -881,7 +905,8 @@ static int ov4689_probe(struct i2c_client *client)
 	if (!ov4689)
 		return -ENOMEM;
 
-	ov4689->client = client;
+	ov4689->dev = dev;
+
 	ov4689->cur_mode = &supported_modes[OV4689_MODE_2688_1520];
 
 	ov4689->xvclk = devm_clk_get_optional(dev, NULL);
@@ -905,6 +930,13 @@ static int ov4689_probe(struct i2c_client *client)
 		return -EINVAL;
 	}
 
+	ov4689->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(ov4689->regmap)) {
+		ret = PTR_ERR(ov4689->regmap);
+		dev_err(dev, "failed to initialize CCI: %d\n", ret);
+		return ret;
+	}
+
 	ov4689->reset_gpio = devm_gpiod_get_optional(dev, "reset",
 						     GPIOD_OUT_LOW);
 	if (IS_ERR(ov4689->reset_gpio)) {
@@ -923,13 +955,15 @@ static int ov4689_probe(struct i2c_client *client)
 		return dev_err_probe(dev, ret,
 				     "Failed to get power regulators\n");
 
-	mutex_init(&ov4689->mutex);
-
 	sd = &ov4689->subdev;
 	v4l2_i2c_subdev_init(sd, client, &ov4689_subdev_ops);
+	sd->internal_ops = &ov4689_internal_ops;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	ret = ov4689_initialize_controls(ov4689);
-	if (ret)
-		goto err_destroy_mutex;
+	if (ret) {
+		dev_err(dev, "Failed to initialize controls\n");
+		return ret;
+	}
 
 	ret = ov4689_power_on(dev);
 	if (ret)
@@ -939,35 +973,47 @@ static int ov4689_probe(struct i2c_client *client)
 	if (ret)
 		goto err_power_off;
 
-	sd->internal_ops = &ov4689_internal_ops;
-	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
-	ov4689->pad.flags = MEDIA_PAD_FL_SOURCE;
 	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ov4689->pad.flags = MEDIA_PAD_FL_SOURCE;
 	ret = media_entity_pads_init(&sd->entity, 1, &ov4689->pad);
 	if (ret < 0)
 		goto err_power_off;
 
-	ret = v4l2_async_register_subdev_sensor(sd);
+	sd->state_lock = ov4689->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(sd);
 	if (ret) {
-		dev_err(dev, "v4l2 async register subdev failed\n");
+		dev_err(dev, "Could not register v4l2 device\n");
 		goto err_clean_entity;
 	}
 
 	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
 	pm_runtime_enable(dev);
-	pm_runtime_idle(dev);
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	ret = v4l2_async_register_subdev_sensor(sd);
+	if (ret) {
+		dev_err(dev, "v4l2 async register subdev failed\n");
+		goto err_clean_subdev_pm;
+	}
+
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
 
 	return 0;
 
+err_clean_subdev_pm:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	v4l2_subdev_cleanup(sd);
 err_clean_entity:
 	media_entity_cleanup(&sd->entity);
 err_power_off:
 	ov4689_power_off(dev);
 err_free_handler:
 	v4l2_ctrl_handler_free(&ov4689->ctrl_handler);
-err_destroy_mutex:
-	mutex_destroy(&ov4689->mutex);
 
 	return ret;
 }
@@ -979,9 +1025,8 @@ static void ov4689_remove(struct i2c_client *client)
 
 	v4l2_async_unregister_subdev(sd);
 	media_entity_cleanup(&sd->entity);
-
+	v4l2_subdev_cleanup(sd);
 	v4l2_ctrl_handler_free(&ov4689->ctrl_handler);
-	mutex_destroy(&ov4689->mutex);
 
 	pm_runtime_disable(&client->dev);
 	if (!pm_runtime_status_suspended(&client->dev))
diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
index 5162d45fe73b..0dae0438aa80 100644
--- a/drivers/media/i2c/ov5640.c
+++ b/drivers/media/i2c/ov5640.c
@@ -377,7 +377,7 @@ struct reg_value {
 struct ov5640_timings {
 	/* Analog crop rectangle. */
 	struct v4l2_rect analog_crop;
-	/* Visibile crop: from analog crop top-left corner. */
+	/* Visible crop: from analog crop top-left corner. */
 	struct v4l2_rect crop;
 	/* Total pixels per line: width + fixed hblank. */
 	u32 htot;
@@ -1982,6 +1982,7 @@ static int ov5640_get_light_freq(struct ov5640_dev *sensor)
 			light_freq = 50;
 		} else {
 			/* 60Hz */
+			light_freq = 60;
 		}
 	}
 
@@ -4003,8 +4004,8 @@ static const struct dev_pm_ops ov5640_pm_ops = {
 };
 
 static const struct i2c_device_id ov5640_id[] = {
-	{"ov5640", 0},
-	{},
+	{ "ov5640" },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, ov5640_id);
 
diff --git a/drivers/media/i2c/ov5645.c b/drivers/media/i2c/ov5645.c
index 3b22b9e12787..004d0ee5c3f5 100644
--- a/drivers/media/i2c/ov5645.c
+++ b/drivers/media/i2c/ov5645.c
@@ -88,7 +88,6 @@ struct ov5645 {
 	struct v4l2_subdev sd;
 	struct media_pad pad;
 	struct v4l2_fwnode_endpoint ep;
-	struct v4l2_mbus_framefmt fmt;
 	struct v4l2_rect crop;
 	struct clk *xclk;
 
@@ -105,8 +104,6 @@ struct ov5645 {
 	u8 timing_tc_reg20;
 	u8 timing_tc_reg21;
 
-	struct mutex power_lock; /* lock to protect power state */
-
 	struct gpio_desc *enable_gpio;
 	struct gpio_desc *rst_gpio;
 };
@@ -635,7 +632,7 @@ static int ov5645_set_register_array(struct ov5645 *ov5645,
 	return 0;
 }
 
-static int ov5645_set_power_off(struct device *dev)
+static void __ov5645_set_power_off(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct ov5645 *ov5645 = to_ov5645(sd);
@@ -643,8 +640,16 @@ static int ov5645_set_power_off(struct device *dev)
 	ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x58);
 	gpiod_set_value_cansleep(ov5645->rst_gpio, 1);
 	gpiod_set_value_cansleep(ov5645->enable_gpio, 0);
-	clk_disable_unprepare(ov5645->xclk);
 	regulator_bulk_disable(OV5645_NUM_SUPPLIES, ov5645->supplies);
+}
+
+static int ov5645_set_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct ov5645 *ov5645 = to_ov5645(sd);
+
+	__ov5645_set_power_off(dev);
+	clk_disable_unprepare(ov5645->xclk);
 
 	return 0;
 }
@@ -686,7 +691,8 @@ static int ov5645_set_power_on(struct device *dev)
 	return 0;
 
 exit:
-	ov5645_set_power_off(dev);
+	__ov5645_set_power_off(dev);
+	clk_disable_unprepare(ov5645->xclk);
 	return ret;
 }
 
@@ -772,11 +778,8 @@ static int ov5645_s_ctrl(struct v4l2_ctrl *ctrl)
 					     struct ov5645, ctrls);
 	int ret;
 
-	mutex_lock(&ov5645->power_lock);
-	if (!pm_runtime_get_if_in_use(ov5645->dev)) {
-		mutex_unlock(&ov5645->power_lock);
+	if (!pm_runtime_get_if_in_use(ov5645->dev))
 		return 0;
-	}
 
 	switch (ctrl->id) {
 	case V4L2_CID_SATURATION:
@@ -807,7 +810,6 @@ static int ov5645_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	pm_runtime_mark_last_busy(ov5645->dev);
 	pm_runtime_put_autosuspend(ov5645->dev);
-	mutex_unlock(&ov5645->power_lock);
 
 	return ret;
 }
@@ -846,49 +848,6 @@ static int ov5645_enum_frame_size(struct v4l2_subdev *subdev,
 	return 0;
 }
 
-static struct v4l2_mbus_framefmt *
-__ov5645_get_pad_format(struct ov5645 *ov5645,
-			struct v4l2_subdev_state *sd_state,
-			unsigned int pad,
-			enum v4l2_subdev_format_whence which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_state_get_format(sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &ov5645->fmt;
-	default:
-		return NULL;
-	}
-}
-
-static int ov5645_get_format(struct v4l2_subdev *sd,
-			     struct v4l2_subdev_state *sd_state,
-			     struct v4l2_subdev_format *format)
-{
-	struct ov5645 *ov5645 = to_ov5645(sd);
-
-	format->format = *__ov5645_get_pad_format(ov5645, sd_state,
-						  format->pad,
-						  format->which);
-	return 0;
-}
-
-static struct v4l2_rect *
-__ov5645_get_pad_crop(struct ov5645 *ov5645,
-		      struct v4l2_subdev_state *sd_state,
-		      unsigned int pad, enum v4l2_subdev_format_whence which)
-{
-	switch (which) {
-	case V4L2_SUBDEV_FORMAT_TRY:
-		return v4l2_subdev_state_get_crop(sd_state, pad);
-	case V4L2_SUBDEV_FORMAT_ACTIVE:
-		return &ov5645->crop;
-	default:
-		return NULL;
-	}
-}
-
 static int ov5645_set_format(struct v4l2_subdev *sd,
 			     struct v4l2_subdev_state *sd_state,
 			     struct v4l2_subdev_format *format)
@@ -899,33 +858,30 @@ static int ov5645_set_format(struct v4l2_subdev *sd,
 	const struct ov5645_mode_info *new_mode;
 	int ret;
 
-	__crop = __ov5645_get_pad_crop(ov5645, sd_state, format->pad,
-				       format->which);
-
+	__crop = v4l2_subdev_state_get_crop(sd_state, 0);
 	new_mode = v4l2_find_nearest_size(ov5645_mode_info_data,
-			       ARRAY_SIZE(ov5645_mode_info_data),
-			       width, height,
-			       format->format.width, format->format.height);
+					  ARRAY_SIZE(ov5645_mode_info_data),
+					  width, height, format->format.width,
+					  format->format.height);
 
 	__crop->width = new_mode->width;
 	__crop->height = new_mode->height;
 
 	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		ret = v4l2_ctrl_s_ctrl_int64(ov5645->pixel_clock,
-					     new_mode->pixel_clock);
+		ret = __v4l2_ctrl_s_ctrl_int64(ov5645->pixel_clock,
+					       new_mode->pixel_clock);
 		if (ret < 0)
 			return ret;
 
-		ret = v4l2_ctrl_s_ctrl(ov5645->link_freq,
-				       new_mode->link_freq);
+		ret = __v4l2_ctrl_s_ctrl(ov5645->link_freq,
+					 new_mode->link_freq);
 		if (ret < 0)
 			return ret;
 
 		ov5645->current_mode = new_mode;
 	}
 
-	__format = __ov5645_get_pad_format(ov5645, sd_state, format->pad,
-					   format->which);
+	__format = v4l2_subdev_state_get_format(sd_state, 0);
 	__format->width = __crop->width;
 	__format->height = __crop->height;
 	__format->code = MEDIA_BUS_FMT_UYVY8_1X16;
@@ -940,11 +896,15 @@ static int ov5645_set_format(struct v4l2_subdev *sd,
 static int ov5645_init_state(struct v4l2_subdev *subdev,
 			     struct v4l2_subdev_state *sd_state)
 {
-	struct v4l2_subdev_format fmt = { 0 };
-
-	fmt.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
-	fmt.format.width = 1920;
-	fmt.format.height = 1080;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.pad = 0,
+		.format = {
+			.code = MEDIA_BUS_FMT_UYVY8_1X16,
+			.width = ov5645_mode_info_data[1].width,
+			.height = ov5645_mode_info_data[1].height,
+		},
+	};
 
 	ov5645_set_format(subdev, sd_state, &fmt);
 
@@ -955,82 +915,88 @@ static int ov5645_get_selection(struct v4l2_subdev *sd,
 			   struct v4l2_subdev_state *sd_state,
 			   struct v4l2_subdev_selection *sel)
 {
-	struct ov5645 *ov5645 = to_ov5645(sd);
-
 	if (sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
-	sel->r = *__ov5645_get_pad_crop(ov5645, sd_state, sel->pad,
-					sel->which);
+	sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
 	return 0;
 }
 
-static int ov5645_s_stream(struct v4l2_subdev *subdev, int enable)
+static int ov5645_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
 {
-	struct ov5645 *ov5645 = to_ov5645(subdev);
+	struct ov5645 *ov5645 = to_ov5645(sd);
 	int ret;
 
-	if (enable) {
-		ret = pm_runtime_resume_and_get(ov5645->dev);
-		if (ret < 0)
-			return ret;
+	ret = pm_runtime_resume_and_get(ov5645->dev);
+	if (ret < 0)
+		return ret;
 
-		ret = ov5645_set_register_array(ov5645,
+	ret = ov5645_set_register_array(ov5645,
 					ov5645->current_mode->data,
 					ov5645->current_mode->data_size);
-		if (ret < 0) {
-			dev_err(ov5645->dev, "could not set mode %dx%d\n",
-				ov5645->current_mode->width,
-				ov5645->current_mode->height);
-			goto err_rpm_put;
-		}
-		ret = v4l2_ctrl_handler_setup(&ov5645->ctrls);
-		if (ret < 0) {
-			dev_err(ov5645->dev, "could not sync v4l2 controls\n");
-			goto err_rpm_put;
-		}
-
-		ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x45);
-		if (ret < 0)
-			goto err_rpm_put;
-
-		ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
-				       OV5645_SYSTEM_CTRL0_START);
-		if (ret < 0)
-			goto err_rpm_put;
-	} else {
-		ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x40);
-		if (ret < 0)
-			goto stream_off_rpm_put;
+	if (ret < 0) {
+		dev_err(ov5645->dev, "could not set mode %dx%d\n",
+			ov5645->current_mode->width,
+			ov5645->current_mode->height);
+		goto err_rpm_put;
+	}
+	ret = __v4l2_ctrl_handler_setup(&ov5645->ctrls);
+	if (ret < 0) {
+		dev_err(ov5645->dev, "could not sync v4l2 controls\n");
+		goto err_rpm_put;
+	}
 
-		ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
-				       OV5645_SYSTEM_CTRL0_STOP);
+	ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x45);
+	if (ret < 0)
+		goto err_rpm_put;
 
-		goto stream_off_rpm_put;
-	}
+	ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+			       OV5645_SYSTEM_CTRL0_START);
+	if (ret < 0)
+		goto err_rpm_put;
 
 	return 0;
 
 err_rpm_put:
 	pm_runtime_put_sync(ov5645->dev);
 	return ret;
+}
+
+static int ov5645_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
+{
+	struct ov5645 *ov5645 = to_ov5645(sd);
+	int ret;
+
+	ret = ov5645_write_reg(ov5645, OV5645_IO_MIPI_CTRL00, 0x40);
+	if (ret < 0)
+		goto rpm_put;
+
+	ret = ov5645_write_reg(ov5645, OV5645_SYSTEM_CTRL0,
+			       OV5645_SYSTEM_CTRL0_STOP);
 
-stream_off_rpm_put:
+rpm_put:
 	pm_runtime_mark_last_busy(ov5645->dev);
 	pm_runtime_put_autosuspend(ov5645->dev);
+
 	return ret;
 }
 
 static const struct v4l2_subdev_video_ops ov5645_video_ops = {
-	.s_stream = ov5645_s_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_pad_ops ov5645_subdev_pad_ops = {
 	.enum_mbus_code = ov5645_enum_mbus_code,
 	.enum_frame_size = ov5645_enum_frame_size,
-	.get_fmt = ov5645_get_format,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = ov5645_set_format,
 	.get_selection = ov5645_get_selection,
+	.enable_streams = ov5645_enable_streams,
+	.disable_streams = ov5645_disable_streams,
 };
 
 static const struct v4l2_subdev_ops ov5645_subdev_ops = {
@@ -1060,51 +1026,44 @@ static int ov5645_probe(struct i2c_client *client)
 	ov5645->dev = dev;
 
 	endpoint = of_graph_get_endpoint_by_regs(dev->of_node, 0, -1);
-	if (!endpoint) {
-		dev_err(dev, "endpoint node not found\n");
-		return -EINVAL;
-	}
+	if (!endpoint)
+		return dev_err_probe(dev, -EINVAL,
+				     "endpoint node not found\n");
 
 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(endpoint),
 					 &ov5645->ep);
 
 	of_node_put(endpoint);
 
-	if (ret < 0) {
-		dev_err(dev, "parsing endpoint node failed\n");
-		return ret;
-	}
+	if (ret < 0)
+		return dev_err_probe(dev, ret,
+				     "parsing endpoint node failed\n");
 
-	if (ov5645->ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
-		dev_err(dev, "invalid bus type, must be CSI2\n");
-		return -EINVAL;
-	}
+	if (ov5645->ep.bus_type != V4L2_MBUS_CSI2_DPHY)
+		return dev_err_probe(dev, -EINVAL,
+				     "invalid bus type, must be CSI2\n");
 
 	/* get system clock (xclk) */
 	ov5645->xclk = devm_clk_get(dev, NULL);
-	if (IS_ERR(ov5645->xclk)) {
-		dev_err(dev, "could not get xclk");
-		return PTR_ERR(ov5645->xclk);
-	}
+	if (IS_ERR(ov5645->xclk))
+		return dev_err_probe(dev, PTR_ERR(ov5645->xclk),
+				     "could not get xclk");
 
 	ret = of_property_read_u32(dev->of_node, "clock-frequency", &xclk_freq);
-	if (ret) {
-		dev_err(dev, "could not get xclk frequency\n");
-		return ret;
-	}
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "could not get xclk frequency\n");
 
 	/* external clock must be 24MHz, allow 1% tolerance */
-	if (xclk_freq < 23760000 || xclk_freq > 24240000) {
-		dev_err(dev, "external clock frequency %u is not supported\n",
-			xclk_freq);
-		return -EINVAL;
-	}
+	if (xclk_freq < 23760000 || xclk_freq > 24240000)
+		return dev_err_probe(dev, -EINVAL,
+				     "unsupported xclk frequency %u\n",
+				     xclk_freq);
 
 	ret = clk_set_rate(ov5645->xclk, xclk_freq);
-	if (ret) {
-		dev_err(dev, "could not set xclk frequency\n");
-		return ret;
-	}
+	if (ret)
+		return dev_err_probe(dev, ret,
+				     "could not set xclk frequency\n");
 
 	for (i = 0; i < OV5645_NUM_SUPPLIES; i++)
 		ov5645->supplies[i].supply = ov5645_supply_name[i];
@@ -1115,18 +1074,14 @@ static int ov5645_probe(struct i2c_client *client)
 		return ret;
 
 	ov5645->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_HIGH);
-	if (IS_ERR(ov5645->enable_gpio)) {
-		dev_err(dev, "cannot get enable gpio\n");
-		return PTR_ERR(ov5645->enable_gpio);
-	}
+	if (IS_ERR(ov5645->enable_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov5645->enable_gpio),
+				     "cannot get enable gpio\n");
 
 	ov5645->rst_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
-	if (IS_ERR(ov5645->rst_gpio)) {
-		dev_err(dev, "cannot get reset gpio\n");
-		return PTR_ERR(ov5645->rst_gpio);
-	}
-
-	mutex_init(&ov5645->power_lock);
+	if (IS_ERR(ov5645->rst_gpio))
+		return dev_err_probe(dev, PTR_ERR(ov5645->rst_gpio),
+				     "cannot get reset gpio\n");
 
 	v4l2_ctrl_handler_init(&ov5645->ctrls, 9);
 	v4l2_ctrl_new_std(&ov5645->ctrls, &ov5645_ctrl_ops,
@@ -1161,9 +1116,8 @@ static int ov5645_probe(struct i2c_client *client)
 	ov5645->sd.ctrl_handler = &ov5645->ctrls;
 
 	if (ov5645->ctrls.error) {
-		dev_err(dev, "%s: control initialization error %d\n",
-		       __func__, ov5645->ctrls.error);
 		ret = ov5645->ctrls.error;
+		dev_err_probe(dev, ret, "failed to add controls\n");
 		goto free_ctrl;
 	}
 
@@ -1171,12 +1125,12 @@ static int ov5645_probe(struct i2c_client *client)
 	ov5645->sd.internal_ops = &ov5645_internal_ops;
 	ov5645->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	ov5645->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ov5645->sd.dev = &client->dev;
+	ov5645->sd.dev = dev;
 	ov5645->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	ret = media_entity_pads_init(&ov5645->sd.entity, 1, &ov5645->pad);
 	if (ret < 0) {
-		dev_err(dev, "could not register media entity\n");
+		dev_err_probe(dev, ret, "could not register media entity\n");
 		goto free_ctrl;
 	}
 
@@ -1186,14 +1140,14 @@ static int ov5645_probe(struct i2c_client *client)
 
 	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_HIGH, &chip_id_high);
 	if (ret < 0 || chip_id_high != OV5645_CHIP_ID_HIGH_BYTE) {
-		dev_err(dev, "could not read ID high\n");
 		ret = -ENODEV;
+		dev_err_probe(dev, ret, "could not read ID high\n");
 		goto power_down;
 	}
 	ret = ov5645_read_reg(ov5645, OV5645_CHIP_ID_LOW, &chip_id_low);
 	if (ret < 0 || chip_id_low != OV5645_CHIP_ID_LOW_BYTE) {
-		dev_err(dev, "could not read ID low\n");
 		ret = -ENODEV;
+		dev_err_probe(dev, ret, "could not read ID low\n");
 		goto power_down;
 	}
 
@@ -1202,24 +1156,31 @@ static int ov5645_probe(struct i2c_client *client)
 	ret = ov5645_read_reg(ov5645, OV5645_AEC_PK_MANUAL,
 			      &ov5645->aec_pk_manual);
 	if (ret < 0) {
-		dev_err(dev, "could not read AEC/AGC mode\n");
 		ret = -ENODEV;
+		dev_err_probe(dev, ret, "could not read AEC/AGC mode\n");
 		goto power_down;
 	}
 
 	ret = ov5645_read_reg(ov5645, OV5645_TIMING_TC_REG20,
 			      &ov5645->timing_tc_reg20);
 	if (ret < 0) {
-		dev_err(dev, "could not read vflip value\n");
 		ret = -ENODEV;
+		dev_err_probe(dev, ret, "could not read vflip value\n");
 		goto power_down;
 	}
 
 	ret = ov5645_read_reg(ov5645, OV5645_TIMING_TC_REG21,
 			      &ov5645->timing_tc_reg21);
 	if (ret < 0) {
-		dev_err(dev, "could not read hflip value\n");
 		ret = -ENODEV;
+		dev_err_probe(dev, ret, "could not read hflip value\n");
+		goto power_down;
+	}
+
+	ov5645->sd.state_lock = ov5645->ctrls.lock;
+	ret = v4l2_subdev_init_finalize(&ov5645->sd);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "subdev init error\n");
 		goto power_down;
 	}
 
@@ -1227,11 +1188,9 @@ static int ov5645_probe(struct i2c_client *client)
 	pm_runtime_get_noresume(dev);
 	pm_runtime_enable(dev);
 
-	ov5645_init_state(&ov5645->sd, NULL);
-
-	ret = v4l2_async_register_subdev(&ov5645->sd);
+	ret = v4l2_async_register_subdev_sensor(&ov5645->sd);
 	if (ret < 0) {
-		dev_err(dev, "could not register v4l2 device\n");
+		dev_err_probe(dev, ret, "could not register v4l2 device\n");
 		goto err_pm_runtime;
 	}
 
@@ -1245,13 +1204,13 @@ static int ov5645_probe(struct i2c_client *client)
 err_pm_runtime:
 	pm_runtime_disable(dev);
 	pm_runtime_put_noidle(dev);
+	v4l2_subdev_cleanup(&ov5645->sd);
 power_down:
 	ov5645_set_power_off(dev);
 free_entity:
 	media_entity_cleanup(&ov5645->sd.entity);
 free_ctrl:
 	v4l2_ctrl_handler_free(&ov5645->ctrls);
-	mutex_destroy(&ov5645->power_lock);
 
 	return ret;
 }
@@ -1262,17 +1221,17 @@ static void ov5645_remove(struct i2c_client *client)
 	struct ov5645 *ov5645 = to_ov5645(sd);
 
 	v4l2_async_unregister_subdev(&ov5645->sd);
+	v4l2_subdev_cleanup(sd);
 	media_entity_cleanup(&ov5645->sd.entity);
 	v4l2_ctrl_handler_free(&ov5645->ctrls);
 	pm_runtime_disable(ov5645->dev);
 	if (!pm_runtime_status_suspended(ov5645->dev))
 		ov5645_set_power_off(ov5645->dev);
 	pm_runtime_set_suspended(ov5645->dev);
-	mutex_destroy(&ov5645->power_lock);
 }
 
 static const struct i2c_device_id ov5645_id[] = {
-	{ "ov5645", 0 },
+	{ "ov5645" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, ov5645_id);
diff --git a/drivers/media/i2c/ov5647.c b/drivers/media/i2c/ov5647.c
index 7e1ecdf2485f..a727beb9d57e 100644
--- a/drivers/media/i2c/ov5647.c
+++ b/drivers/media/i2c/ov5647.c
@@ -1360,24 +1360,21 @@ static int ov5647_parse_dt(struct ov5647 *sensor, struct device_node *np)
 	struct v4l2_fwnode_endpoint bus_cfg = {
 		.bus_type = V4L2_MBUS_CSI2_DPHY,
 	};
-	struct device_node *ep;
+	struct device_node *ep __free(device_node) =
+		of_graph_get_endpoint_by_regs(np, 0, -1);
 	int ret;
 
-	ep = of_graph_get_endpoint_by_regs(np, 0, -1);
 	if (!ep)
 		return -EINVAL;
 
 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
 	if (ret)
-		goto out;
+		return ret;
 
 	sensor->clock_ncont = bus_cfg.bus.mipi_csi2.flags &
 			      V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
 
-out:
-	of_node_put(ep);
-
-	return ret;
+	return 0;
 }
 
 static int ov5647_probe(struct i2c_client *client)
@@ -1490,7 +1487,7 @@ static const struct dev_pm_ops ov5647_pm_ops = {
 };
 
 static const struct i2c_device_id ov5647_id[] = {
-	{ "ov5647", 0 },
+	{ "ov5647" },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, ov5647_id);
diff --git a/drivers/media/i2c/ov5670.c b/drivers/media/i2c/ov5670.c
index 2aee85965cf7..c54bbc207189 100644
--- a/drivers/media/i2c/ov5670.c
+++ b/drivers/media/i2c/ov5670.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2017 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -1879,7 +1879,7 @@ struct ov5670 {
 	struct gpio_desc *pwdn_gpio; /* PWDNB pin. */
 	struct gpio_desc *reset_gpio; /* XSHUTDOWN pin. */
 
-	/* To serialize asynchronus callbacks */
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 
 	/* True if the device has been identified */
diff --git a/drivers/media/i2c/ov5675.c b/drivers/media/i2c/ov5675.c
index 3641911bc73f..e7aec281e9a4 100644
--- a/drivers/media/i2c/ov5675.c
+++ b/drivers/media/i2c/ov5675.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2019 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -510,7 +510,7 @@ struct ov5675 {
 	/* Current mode */
 	const struct ov5675_mode *cur_mode;
 
-	/* To serialize asynchronus callbacks */
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 
 	/* True if the device has been identified */
@@ -732,7 +732,7 @@ static int ov5675_set_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_EXPOSURE:
 		/* 4 least significant bits of expsoure are fractional part
 		 * val = val << 4
-		 * for ov5675, the unit of exposure is differnt from other
+		 * for ov5675, the unit of exposure is different from other
 		 * OmniVision sensors, its exposure value is twice of the
 		 * register value, the exposure should be divided by 2 before
 		 * set register, e.g. val << 3.
@@ -972,12 +972,10 @@ static int ov5675_set_stream(struct v4l2_subdev *sd, int enable)
 
 static int ov5675_power_off(struct device *dev)
 {
-	/* 512 xvclk cycles after the last SCCB transation or MIPI frame end */
-	u32 delay_us = DIV_ROUND_UP(512, OV5675_XVCLK_19_2 / 1000 / 1000);
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct ov5675 *ov5675 = to_ov5675(sd);
 
-	usleep_range(delay_us, delay_us * 2);
+	usleep_range(90, 100);
 
 	clk_disable_unprepare(ov5675->xvclk);
 	gpiod_set_value_cansleep(ov5675->reset_gpio, 1);
@@ -988,7 +986,6 @@ static int ov5675_power_off(struct device *dev)
 
 static int ov5675_power_on(struct device *dev)
 {
-	u32 delay_us = DIV_ROUND_UP(8192, OV5675_XVCLK_19_2 / 1000 / 1000);
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct ov5675 *ov5675 = to_ov5675(sd);
 	int ret;
@@ -1014,8 +1011,11 @@ static int ov5675_power_on(struct device *dev)
 
 	gpiod_set_value_cansleep(ov5675->reset_gpio, 0);
 
-	/* 8192 xvclk cycles prior to the first SCCB transation */
-	usleep_range(delay_us, delay_us * 2);
+	/* Worst case quiesence gap is 1.365 milliseconds @ 6MHz XVCLK
+	 * Add an additional threshold grace period to ensure reset
+	 * completion before initiating our first I2C transaction.
+	 */
+	usleep_range(1500, 1600);
 
 	return 0;
 }
@@ -1295,11 +1295,8 @@ static int ov5675_probe(struct i2c_client *client)
 		return -ENOMEM;
 
 	ret = ov5675_get_hwcfg(ov5675, &client->dev);
-	if (ret) {
-		dev_err(&client->dev, "failed to get HW configuration: %d",
-			ret);
+	if (ret)
 		return ret;
-	}
 
 	v4l2_i2c_subdev_init(&ov5675->sd, client, &ov5675_subdev_ops);
 
diff --git a/drivers/media/i2c/ov5693.c b/drivers/media/i2c/ov5693.c
index 8deb28b55983..46b9ce111676 100644
--- a/drivers/media/i2c/ov5693.c
+++ b/drivers/media/i2c/ov5693.c
@@ -141,7 +141,6 @@ struct ov5693_device {
 
 	struct gpio_desc *reset;
 	struct gpio_desc *powerdown;
-	struct gpio_desc *privacy_led;
 	struct regulator_bulk_data supplies[OV5693_NUM_SUPPLIES];
 	struct clk *xvclk;
 
@@ -657,7 +656,6 @@ static int ov5693_sensor_init(struct ov5693_device *ov5693)
 
 static void ov5693_sensor_powerdown(struct ov5693_device *ov5693)
 {
-	gpiod_set_value_cansleep(ov5693->privacy_led, 0);
 	gpiod_set_value_cansleep(ov5693->reset, 1);
 	gpiod_set_value_cansleep(ov5693->powerdown, 1);
 
@@ -687,7 +685,6 @@ static int ov5693_sensor_powerup(struct ov5693_device *ov5693)
 
 	gpiod_set_value_cansleep(ov5693->powerdown, 0);
 	gpiod_set_value_cansleep(ov5693->reset, 0);
-	gpiod_set_value_cansleep(ov5693->privacy_led, 1);
 
 	usleep_range(5000, 7500);
 
@@ -1201,13 +1198,6 @@ static int ov5693_configure_gpios(struct ov5693_device *ov5693)
 		return PTR_ERR(ov5693->powerdown);
 	}
 
-	ov5693->privacy_led = devm_gpiod_get_optional(ov5693->dev, "privacy-led",
-						      GPIOD_OUT_LOW);
-	if (IS_ERR(ov5693->privacy_led)) {
-		dev_err(ov5693->dev, "Error fetching privacy-led GPIO\n");
-		return PTR_ERR(ov5693->privacy_led);
-	}
-
 	return 0;
 }
 
diff --git a/drivers/media/i2c/ov64a40.c b/drivers/media/i2c/ov64a40.c
index 541bf74581d2..a5da4fe47e0b 100644
--- a/drivers/media/i2c/ov64a40.c
+++ b/drivers/media/i2c/ov64a40.c
@@ -18,7 +18,6 @@
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-subdev.h>
@@ -3200,13 +3199,7 @@ static const struct v4l2_subdev_pad_ops ov64a40_pad_ops = {
 	.get_selection = ov64a40_get_selection,
 };
 
-static const struct v4l2_subdev_core_ops ov64a40_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
 static const struct v4l2_subdev_ops ov64a40_subdev_ops = {
-	.core = &ov64a40_core_ops,
 	.video = &ov64a40_video_ops,
 	.pad = &ov64a40_pad_ops,
 };
@@ -3605,8 +3598,7 @@ static int ov64a40_probe(struct i2c_client *client)
 
 	/* Initialize subdev */
 	ov64a40->sd.internal_ops = &ov64a40_internal_ops;
-	ov64a40->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE
-			  | V4L2_SUBDEV_FL_HAS_EVENTS;
+	ov64a40->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 	ov64a40->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	ov64a40->pad.flags = MEDIA_PAD_FL_SOURCE;
diff --git a/drivers/media/i2c/ov6650.c b/drivers/media/i2c/ov6650.c
index b65befb22a79..9c7627161142 100644
--- a/drivers/media/i2c/ov6650.c
+++ b/drivers/media/i2c/ov6650.c
@@ -1128,7 +1128,7 @@ static void ov6650_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov6650_id[] = {
-	{ "ov6650", 0 },
+	{ "ov6650" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov6650_id);
diff --git a/drivers/media/i2c/ov7251.c b/drivers/media/i2c/ov7251.c
index 30f61e04ecaf..3226888d77e9 100644
--- a/drivers/media/i2c/ov7251.c
+++ b/drivers/media/i2c/ov7251.c
@@ -922,6 +922,8 @@ static int ov7251_set_power_on(struct device *dev)
 		return ret;
 	}
 
+	usleep_range(1000, 1100);
+
 	gpiod_set_value_cansleep(ov7251->enable_gpio, 1);
 
 	/* wait at least 65536 external clock cycles */
@@ -1696,7 +1698,7 @@ static int ov7251_probe(struct i2c_client *client)
 		return PTR_ERR(ov7251->analog_regulator);
 	}
 
-	ov7251->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_HIGH);
+	ov7251->enable_gpio = devm_gpiod_get(dev, "enable", GPIOD_OUT_LOW);
 	if (IS_ERR(ov7251->enable_gpio)) {
 		dev_err(dev, "cannot get enable gpio\n");
 		return PTR_ERR(ov7251->enable_gpio);
diff --git a/drivers/media/i2c/ov7640.c b/drivers/media/i2c/ov7640.c
index 293f5f404358..9f68d89936eb 100644
--- a/drivers/media/i2c/ov7640.c
+++ b/drivers/media/i2c/ov7640.c
@@ -77,7 +77,7 @@ static void ov7640_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov7640_id[] = {
-	{ "ov7640", 0 },
+	{ "ov7640" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov7640_id);
diff --git a/drivers/media/i2c/ov772x.c b/drivers/media/i2c/ov772x.c
index 3e36a55274ef..062e1023a411 100644
--- a/drivers/media/i2c/ov772x.c
+++ b/drivers/media/i2c/ov772x.c
@@ -269,7 +269,7 @@
 #define AF_8x           0x08	/*     Add frame when AGC reaches  8x gain */
 #define AF_16x          0x0c	/* Add frame when AGC reaches 16x gain */
 				/* AEC max step control */
-#define AEC_NO_LIMIT    0x01	/*   0 : AEC incease step has limit */
+#define AEC_NO_LIMIT    0x01	/*   0 : AEC increase step has limit */
 				/*   1 : No limit to AEC increase step */
 /* CLKRC */
 				/* Input clock divider register */
@@ -1546,7 +1546,7 @@ static void ov772x_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov772x_id[] = {
-	{ "ov772x", 0 },
+	{ "ov772x" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov772x_id);
diff --git a/drivers/media/i2c/ov7740.c b/drivers/media/i2c/ov7740.c
index 47b1b14d8796..1f1c0de8e510 100644
--- a/drivers/media/i2c/ov7740.c
+++ b/drivers/media/i2c/ov7740.c
@@ -117,7 +117,7 @@ struct ov7740 {
 	struct v4l2_ctrl *brightness;
 	struct v4l2_ctrl *contrast;
 
-	struct mutex mutex;	/* To serialize asynchronus callbacks */
+	struct mutex mutex;	/* To serialize asynchronous callbacks */
 
 	struct gpio_desc *resetb_gpio;
 	struct gpio_desc *pwdn_gpio;
@@ -1152,7 +1152,7 @@ static int __maybe_unused ov7740_runtime_resume(struct device *dev)
 }
 
 static const struct i2c_device_id ov7740_id[] = {
-	{ "ov7740", 0 },
+	{ "ov7740" },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, ov7740_id);
diff --git a/drivers/media/i2c/ov8856.c b/drivers/media/i2c/ov8856.c
index 6ffe10e57b5b..4b6874d2a104 100644
--- a/drivers/media/i2c/ov8856.c
+++ b/drivers/media/i2c/ov8856.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2019 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -1435,7 +1435,7 @@ struct ov8856 {
 	/* Application specified mbus format */
 	u32 cur_mbus_index;
 
-	/* To serialize asynchronus callbacks */
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 
 	/* lanes index */
@@ -2276,8 +2276,8 @@ static int ov8856_get_hwcfg(struct ov8856 *ov8856, struct device *dev)
 	if (!is_acpi_node(fwnode)) {
 		ov8856->xvclk = devm_clk_get(dev, "xvclk");
 		if (IS_ERR(ov8856->xvclk)) {
-			dev_err(dev, "could not get xvclk clock (%pe)\n",
-				ov8856->xvclk);
+			dev_err_probe(dev, PTR_ERR(ov8856->xvclk),
+				      "could not get xvclk clock\n");
 			return PTR_ERR(ov8856->xvclk);
 		}
 
@@ -2382,11 +2382,8 @@ static int ov8856_probe(struct i2c_client *client)
 		return -ENOMEM;
 
 	ret = ov8856_get_hwcfg(ov8856, &client->dev);
-	if (ret) {
-		dev_err(&client->dev, "failed to get HW configuration: %d",
-			ret);
+	if (ret)
 		return ret;
-	}
 
 	v4l2_i2c_subdev_init(&ov8856->sd, client, &ov8856_subdev_ops);
 
diff --git a/drivers/media/i2c/ov8858.c b/drivers/media/i2c/ov8858.c
index 174c65f76886..95f9ae794846 100644
--- a/drivers/media/i2c/ov8858.c
+++ b/drivers/media/i2c/ov8858.c
@@ -5,7 +5,7 @@
  * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -24,7 +24,6 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-subdev.h>
@@ -1500,13 +1499,7 @@ static const struct v4l2_subdev_pad_ops ov8858_pad_ops = {
 	.set_fmt = ov8858_set_fmt,
 };
 
-static const struct v4l2_subdev_core_ops ov8858_core_ops = {
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
-};
-
 static const struct v4l2_subdev_ops ov8858_subdev_ops = {
-	.core	= &ov8858_core_ops,
 	.video	= &ov8858_video_ops,
 	.pad	= &ov8858_pad_ops,
 };
@@ -1917,7 +1910,7 @@ static int ov8858_probe(struct i2c_client *client)
 		return ret;
 
 	sd = &ov8858->subdev;
-	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	ov8858->pad.flags = MEDIA_PAD_FL_SOURCE;
 	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
 	ret = media_entity_pads_init(&sd->entity, 1, &ov8858->pad);
diff --git a/drivers/media/i2c/ov9282.c b/drivers/media/i2c/ov9282.c
index 251a4b534914..c882a021cf18 100644
--- a/drivers/media/i2c/ov9282.c
+++ b/drivers/media/i2c/ov9282.c
@@ -4,7 +4,7 @@
  *
  * Copyright (C) 2021 Intel Corporation
  */
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -40,10 +40,19 @@
 /* Exposure control */
 #define OV9282_REG_EXPOSURE	0x3500
 #define OV9282_EXPOSURE_MIN	1
-#define OV9282_EXPOSURE_OFFSET	12
+#define OV9282_EXPOSURE_OFFSET	25
 #define OV9282_EXPOSURE_STEP	1
 #define OV9282_EXPOSURE_DEFAULT	0x0282
 
+/* AEC/AGC manual */
+#define OV9282_REG_AEC_MANUAL		0x3503
+#define OV9282_DIGFRAC_GAIN_DELAY	BIT(6)
+#define OV9282_GAIN_CHANGE_DELAY	BIT(5)
+#define OV9282_GAIN_DELAY		BIT(4)
+#define OV9282_GAIN_PREC16_EN		BIT(3)
+#define OV9282_GAIN_MANUAL_AS_SENSGAIN	BIT(2)
+#define OV9282_AEC_MANUAL_DEFAULT	0x00
+
 /* Analog gain control */
 #define OV9282_REG_AGAIN	0x3509
 #define OV9282_AGAIN_MIN	0x10
@@ -214,7 +223,7 @@ static const struct ov9282_reg common_regs[] = {
 	{0x3030, 0x10},
 	{0x3039, 0x32},
 	{0x303a, 0x00},
-	{0x3503, 0x08},
+	{OV9282_REG_AEC_MANUAL, OV9282_GAIN_PREC16_EN},
 	{0x3505, 0x8c},
 	{0x3507, 0x03},
 	{0x3508, 0x00},
@@ -296,8 +305,8 @@ static const struct ov9282_reg mode_1280x800_regs[] = {
 	{0x3813, 0x08},
 	{0x3814, 0x11},
 	{0x3815, 0x11},
-	{0x3820, 0x40},
-	{0x3821, 0x00},
+	{OV9282_REG_TIMING_FORMAT_1, 0x40},
+	{OV9282_REG_TIMING_FORMAT_2, 0x00},
 	{0x4003, 0x40},
 	{0x4008, 0x04},
 	{0x4009, 0x0b},
@@ -327,8 +336,8 @@ static const struct ov9282_reg mode_1280x720_regs[] = {
 	{0x3813, 0x08},
 	{0x3814, 0x11},
 	{0x3815, 0x11},
-	{0x3820, 0x3c},
-	{0x3821, 0x84},
+	{OV9282_REG_TIMING_FORMAT_1, 0x3c},
+	{OV9282_REG_TIMING_FORMAT_2, 0x84},
 	{0x4003, 0x40},
 	{0x4008, 0x02},
 	{0x4009, 0x05},
@@ -358,8 +367,8 @@ static const struct ov9282_reg mode_640x400_regs[] = {
 	{0x3813, 0x04},
 	{0x3814, 0x31},
 	{0x3815, 0x22},
-	{0x3820, 0x60},
-	{0x3821, 0x01},
+	{OV9282_REG_TIMING_FORMAT_1, 0x60},
+	{OV9282_REG_TIMING_FORMAT_2, 0x01},
 	{0x4008, 0x02},
 	{0x4009, 0x05},
 	{0x400c, 0x00},
diff --git a/drivers/media/i2c/ov9640.c b/drivers/media/i2c/ov9640.c
index e9a52a8a9dc0..01dbc0ba89c8 100644
--- a/drivers/media/i2c/ov9640.c
+++ b/drivers/media/i2c/ov9640.c
@@ -751,7 +751,7 @@ static void ov9640_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov9640_id[] = {
-	{ "ov9640", 0 },
+	{ "ov9640" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, ov9640_id);
diff --git a/drivers/media/i2c/ov9650.c b/drivers/media/i2c/ov9650.c
index 66cd0e9ddc9a..026ea34d6291 100644
--- a/drivers/media/i2c/ov9650.c
+++ b/drivers/media/i2c/ov9650.c
@@ -286,7 +286,7 @@ static const struct i2c_rv ov965x_init_regs[] = {
 	{ REG_COM5, 0x00 },	/* System clock options */
 	{ REG_COM2, 0x01 },	/* Output drive, soft sleep mode */
 	{ REG_COM10, 0x00 },	/* Slave mode, HREF vs HSYNC, signals negate */
-	{ REG_EDGE, 0xa6 },	/* Edge enhancement treshhold and factor */
+	{ REG_EDGE, 0xa6 },	/* Edge enhancement threshold and factor */
 	{ REG_COM16, 0x02 },	/* Color matrix coeff double option */
 	{ REG_COM17, 0x08 },	/* Single frame out, banding filter */
 	{ 0x16, 0x06 },
@@ -1566,8 +1566,8 @@ static void ov965x_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ov965x_id[] = {
-	{ "OV9650", 0 },
-	{ "OV9652", 0 },
+	{ "OV9650" },
+	{ "OV9652" },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(i2c, ov965x_id);
diff --git a/drivers/media/i2c/ov9734.c b/drivers/media/i2c/ov9734.c
index d99728597431..cae3aeefb616 100644
--- a/drivers/media/i2c/ov9734.c
+++ b/drivers/media/i2c/ov9734.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 // Copyright (c) 2020 Intel Corporation.
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
@@ -335,7 +335,7 @@ struct ov9734 {
 	/* Current mode */
 	const struct ov9734_mode *cur_mode;
 
-	/* To serialize asynchronus callbacks */
+	/* To serialize asynchronous callbacks */
 	struct mutex mutex;
 };
 
diff --git a/drivers/media/i2c/rdacm20.c b/drivers/media/i2c/rdacm20.c
index b4647bda8c21..52e8e2620b4d 100644
--- a/drivers/media/i2c/rdacm20.c
+++ b/drivers/media/i2c/rdacm20.c
@@ -16,10 +16,10 @@
  */
 
 #include <linux/delay.h>
-#include <linux/fwnode.h>
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/property.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
@@ -463,8 +463,8 @@ static int rdacm20_initialize(struct rdacm20_device *dev)
 		return ret;
 
 	/*
-	 *  Ensure that we have a good link configuration before attempting to
-	 *  identify the device.
+	 * Ensure that we have a good link configuration before attempting to
+	 * identify the device.
 	 */
 	ret = max9271_configure_i2c(&dev->serializer,
 				    MAX9271_I2CSLVSH_469NS_234NS |
@@ -575,10 +575,9 @@ static int rdacm20_probe(struct i2c_client *client)
 	dev->dev = &client->dev;
 	dev->serializer.client = client;
 
-	ret = of_property_read_u32_array(client->dev.of_node, "reg",
-					 dev->addrs, 2);
+	ret = device_property_read_u32_array(dev->dev, "reg", dev->addrs, 2);
 	if (ret < 0) {
-		dev_err(dev->dev, "Invalid DT reg property: %d\n", ret);
+		dev_err(dev->dev, "Invalid FW reg property: %d\n", ret);
 		return -EINVAL;
 	}
 
diff --git a/drivers/media/i2c/rdacm21.c b/drivers/media/i2c/rdacm21.c
index 3e22df36354f..bcab462708c7 100644
--- a/drivers/media/i2c/rdacm21.c
+++ b/drivers/media/i2c/rdacm21.c
@@ -11,10 +11,10 @@
  */
 
 #include <linux/delay.h>
-#include <linux/fwnode.h>
 #include <linux/init.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/property.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
@@ -551,10 +551,9 @@ static int rdacm21_probe(struct i2c_client *client)
 	dev->dev = &client->dev;
 	dev->serializer.client = client;
 
-	ret = of_property_read_u32_array(client->dev.of_node, "reg",
-					 dev->addrs, 2);
+	ret = device_property_read_u32_array(dev->dev, "reg", dev->addrs, 2);
 	if (ret < 0) {
-		dev_err(dev->dev, "Invalid DT reg property: %d\n", ret);
+		dev_err(dev->dev, "Invalid FW reg property: %d\n", ret);
 		return -EINVAL;
 	}
 
diff --git a/drivers/media/i2c/rj54n1cb0c.c b/drivers/media/i2c/rj54n1cb0c.c
index a59db10153cd..b7ca39f63dba 100644
--- a/drivers/media/i2c/rj54n1cb0c.c
+++ b/drivers/media/i2c/rj54n1cb0c.c
@@ -1410,7 +1410,7 @@ static void rj54n1_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id rj54n1_id[] = {
-	{ "rj54n1cb0c", 0 },
+	{ "rj54n1cb0c" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, rj54n1_id);
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index cf6be509af33..7716dfe2b8c9 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -1392,6 +1392,16 @@ err_reg_dis:
 	return ret;
 }
 
+/*
+ * This function has been created just to avoid a smatch warning,
+ * please do not merge into __s5c73m3_power_off() until you have
+ * confirmed that it does not introduce a new warning.
+ */
+static void s5c73m3_enable_clk(struct s5c73m3 *state)
+{
+	clk_prepare_enable(state->clock);
+}
+
 static int __s5c73m3_power_off(struct s5c73m3 *state)
 {
 	int i, ret;
@@ -1421,7 +1431,8 @@ err:
 				 state->supplies[i].supply, r);
 	}
 
-	clk_prepare_enable(state->clock);
+	s5c73m3_enable_clk(state);
+
 	return ret;
 }
 
@@ -1724,7 +1735,7 @@ static void s5c73m3_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id s5c73m3_id[] = {
-	{ DRIVER_NAME, 0 },
+	{ DRIVER_NAME },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, s5c73m3_id);
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
index 6b11039c3579..24f399cd2124 100644
--- a/drivers/media/i2c/s5k5baf.c
+++ b/drivers/media/i2c/s5k5baf.c
@@ -2018,8 +2018,8 @@ static void s5k5baf_remove(struct i2c_client *c)
 }
 
 static const struct i2c_device_id s5k5baf_id[] = {
-	{ S5K5BAF_DRIVER_NAME, 0 },
-	{ },
+	{ S5K5BAF_DRIVER_NAME },
+	{ }
 };
 MODULE_DEVICE_TABLE(i2c, s5k5baf_id);
 
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
index dea9fc09356f..fb09e4560d8a 100644
--- a/drivers/media/i2c/saa6588.c
+++ b/drivers/media/i2c/saa6588.c
@@ -496,7 +496,7 @@ static void saa6588_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id saa6588_id[] = {
-	{ "saa6588", 0 },
+	{ "saa6588" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, saa6588_id);
diff --git a/drivers/media/i2c/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
index 897eaa669b86..1ed8b5edb3fb 100644
--- a/drivers/media/i2c/saa6752hs.c
+++ b/drivers/media/i2c/saa6752hs.c
@@ -770,7 +770,7 @@ static void saa6752hs_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id saa6752hs_id[] = {
-	{ "saa6752hs", 0 },
+	{ "saa6752hs" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, saa6752hs_id);
diff --git a/drivers/media/i2c/saa7110.c b/drivers/media/i2c/saa7110.c
index 1520790338ce..942aeeb40c52 100644
--- a/drivers/media/i2c/saa7110.c
+++ b/drivers/media/i2c/saa7110.c
@@ -439,7 +439,7 @@ static void saa7110_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id saa7110_id[] = {
-	{ "saa7110", 0 },
+	{ "saa7110" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, saa7110_id);
diff --git a/drivers/media/i2c/saa717x.c b/drivers/media/i2c/saa717x.c
index 933ec0171430..b0793bb0c02a 100644
--- a/drivers/media/i2c/saa717x.c
+++ b/drivers/media/i2c/saa717x.c
@@ -1334,7 +1334,7 @@ static void saa717x_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id saa717x_id[] = {
-	{ "saa717x", 0 },
+	{ "saa717x" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, saa717x_id);
diff --git a/drivers/media/i2c/saa7185.c b/drivers/media/i2c/saa7185.c
index 5535d71f4860..c04e452a332b 100644
--- a/drivers/media/i2c/saa7185.c
+++ b/drivers/media/i2c/saa7185.c
@@ -334,7 +334,7 @@ static void saa7185_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id saa7185_id[] = {
-	{ "saa7185", 0 },
+	{ "saa7185" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, saa7185_id);
diff --git a/drivers/media/i2c/sony-btf-mpx.c b/drivers/media/i2c/sony-btf-mpx.c
index 0f53834f3ae4..16072a9f8247 100644
--- a/drivers/media/i2c/sony-btf-mpx.c
+++ b/drivers/media/i2c/sony-btf-mpx.c
@@ -366,7 +366,7 @@ static void sony_btf_mpx_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id sony_btf_mpx_id[] = {
-	{ "sony-btf-mpx", 0 },
+	{ "sony-btf-mpx" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, sony_btf_mpx_id);
diff --git a/drivers/media/i2c/st-mipid02.c b/drivers/media/i2c/st-mipid02.c
index f250640729ca..f4568e87f018 100644
--- a/drivers/media/i2c/st-mipid02.c
+++ b/drivers/media/i2c/st-mipid02.c
@@ -14,6 +14,7 @@
 #include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/pm_runtime.h>
 #include <linux/of_graph.h>
 #include <linux/regulator/consumer.h>
 #include <media/mipi-csi2.h>
@@ -67,9 +68,6 @@ static const u32 mipid02_supported_fmt_codes[] = {
 	MEDIA_BUS_FMT_YUYV8_1X16, MEDIA_BUS_FMT_YVYU8_1X16,
 	MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_VYUY8_1X16,
 	MEDIA_BUS_FMT_RGB565_1X16, MEDIA_BUS_FMT_BGR888_1X24,
-	MEDIA_BUS_FMT_RGB565_2X8_LE, MEDIA_BUS_FMT_RGB565_2X8_BE,
-	MEDIA_BUS_FMT_YUYV8_2X8, MEDIA_BUS_FMT_YVYU8_2X8,
-	MEDIA_BUS_FMT_UYVY8_2X8, MEDIA_BUS_FMT_VYUY8_2X8,
 	MEDIA_BUS_FMT_Y8_1X8, MEDIA_BUS_FMT_JPEG_1X8
 };
 
@@ -100,6 +98,7 @@ struct mipid02_dev {
 	/* remote source */
 	struct v4l2_async_notifier notifier;
 	struct v4l2_subdev *s_subdev;
+	u16 s_subdev_pad_id;
 	/* registers */
 	struct {
 		u8 clk_lane_reg1;
@@ -138,12 +137,6 @@ static int bpp_from_code(__u32 code)
 	case MEDIA_BUS_FMT_UYVY8_1X16:
 	case MEDIA_BUS_FMT_VYUY8_1X16:
 	case MEDIA_BUS_FMT_RGB565_1X16:
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-	case MEDIA_BUS_FMT_YVYU8_2X8:
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-	case MEDIA_BUS_FMT_VYUY8_2X8:
-	case MEDIA_BUS_FMT_RGB565_2X8_LE:
-	case MEDIA_BUS_FMT_RGB565_2X8_BE:
 		return 16;
 	case MEDIA_BUS_FMT_BGR888_1X24:
 		return 24;
@@ -175,16 +168,10 @@ static u8 data_type_from_code(__u32 code)
 	case MEDIA_BUS_FMT_YVYU8_1X16:
 	case MEDIA_BUS_FMT_UYVY8_1X16:
 	case MEDIA_BUS_FMT_VYUY8_1X16:
-	case MEDIA_BUS_FMT_YUYV8_2X8:
-	case MEDIA_BUS_FMT_YVYU8_2X8:
-	case MEDIA_BUS_FMT_UYVY8_2X8:
-	case MEDIA_BUS_FMT_VYUY8_2X8:
 		return MIPI_CSI2_DT_YUV422_8B;
 	case MEDIA_BUS_FMT_BGR888_1X24:
 		return MIPI_CSI2_DT_RGB888;
 	case MEDIA_BUS_FMT_RGB565_1X16:
-	case MEDIA_BUS_FMT_RGB565_2X8_LE:
-	case MEDIA_BUS_FMT_RGB565_2X8_BE:
 		return MIPI_CSI2_DT_RGB565;
 	default:
 		return 0;
@@ -248,8 +235,10 @@ static void mipid02_apply_reset(struct mipid02_dev *bridge)
 	usleep_range(5000, 10000);
 }
 
-static int mipid02_set_power_on(struct mipid02_dev *bridge)
+static int mipid02_set_power_on(struct device *dev)
 {
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
 	struct i2c_client *client = bridge->i2c_client;
 	int ret;
 
@@ -282,10 +271,15 @@ xclk_off:
 	return ret;
 }
 
-static void mipid02_set_power_off(struct mipid02_dev *bridge)
+static int mipid02_set_power_off(struct device *dev)
 {
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
+
 	regulator_bulk_disable(MIPID02_NUM_SUPPLIES, bridge->supplies);
 	clk_disable_unprepare(bridge->xclk);
+
+	return 0;
 }
 
 static int mipid02_detect(struct mipid02_dev *bridge)
@@ -307,8 +301,9 @@ static int mipid02_detect(struct mipid02_dev *bridge)
 static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge,
 					   struct v4l2_mbus_framefmt *fmt)
 {
+	struct media_pad *remote =
+		&bridge->s_subdev->entity.pads[bridge->s_subdev_pad_id];
 	struct i2c_client *client = bridge->i2c_client;
-	struct v4l2_subdev *subdev = bridge->s_subdev;
 	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
 	u32 bpp = bpp_from_code(fmt->code);
 	/*
@@ -318,7 +313,7 @@ static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge,
 	u64 ui_4 = 2000000000;
 	s64 link_freq;
 
-	link_freq = v4l2_get_link_freq(subdev->ctrl_handler, bpp,
+	link_freq = v4l2_get_link_freq(remote, bpp,
 				       2 * ep->bus.mipi_csi2.num_data_lanes);
 	if (link_freq < 0) {
 		dev_err(&client->dev, "Failed to get link frequency");
@@ -326,7 +321,7 @@ static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge,
 	}
 
 	dev_dbg(&client->dev, "detect link_freq = %lld Hz", link_freq);
-	do_div(ui_4, link_freq);
+	ui_4 = div64_u64(ui_4, link_freq);
 	bridge->r.clk_lane_reg1 |= ui_4 << 2;
 
 	return 0;
@@ -447,15 +442,19 @@ static int mipid02_configure_from_code(struct mipid02_dev *bridge,
 	return 0;
 }
 
-static int mipid02_stream_disable(struct mipid02_dev *bridge)
+static int mipid02_disable_streams(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state, u32 pad,
+				   u64 streams_mask)
 {
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
 	struct i2c_client *client = bridge->i2c_client;
 	int ret = -EINVAL;
 
 	if (!bridge->s_subdev)
 		goto error;
 
-	ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 0);
+	ret = v4l2_subdev_disable_streams(bridge->s_subdev,
+					  bridge->s_subdev_pad_id, BIT(0));
 	if (ret)
 		goto error;
 
@@ -465,6 +464,10 @@ static int mipid02_stream_disable(struct mipid02_dev *bridge)
 	cci_write(bridge->regmap, MIPID02_DATA_LANE1_REG1, 0, &ret);
 	if (ret)
 		goto error;
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
 error:
 	if (ret)
 		dev_err(&client->dev, "failed to stream off %d", ret);
@@ -472,33 +475,36 @@ error:
 	return ret;
 }
 
-static int mipid02_stream_enable(struct mipid02_dev *bridge)
+static int mipid02_enable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
 {
+	struct mipid02_dev *bridge = to_mipid02_dev(sd);
 	struct i2c_client *client = bridge->i2c_client;
-	struct v4l2_subdev_state *state;
 	struct v4l2_mbus_framefmt *fmt;
 	int ret = -EINVAL;
 
 	if (!bridge->s_subdev)
-		goto error;
+		return ret;
 
 	memset(&bridge->r, 0, sizeof(bridge->r));
 
-	state = v4l2_subdev_lock_and_get_active_state(&bridge->sd);
 	fmt = v4l2_subdev_state_get_format(state, MIPID02_SINK_0);
 
 	/* build registers content */
 	ret = mipid02_configure_from_rx(bridge, fmt);
 	if (ret)
-		goto error;
+		return ret;
 	ret = mipid02_configure_from_tx(bridge);
 	if (ret)
-		goto error;
+		return ret;
 	ret = mipid02_configure_from_code(bridge, fmt);
 	if (ret)
-		goto error;
+		return ret;
 
-	v4l2_subdev_unlock_state(state);
+	ret = pm_runtime_resume_and_get(&client->dev);
+	if (ret < 0)
+		return ret;
 
 	/* write mipi registers */
 	cci_write(bridge->regmap, MIPID02_CLK_LANE_REG1,
@@ -524,33 +530,20 @@ static int mipid02_stream_enable(struct mipid02_dev *bridge)
 	if (ret)
 		goto error;
 
-	ret = v4l2_subdev_call(bridge->s_subdev, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(bridge->s_subdev,
+					 bridge->s_subdev_pad_id, BIT(0));
 	if (ret)
 		goto error;
 
 	return 0;
 
 error:
-	dev_err(&client->dev, "failed to stream on %d", ret);
-	mipid02_stream_disable(bridge);
-
-	return ret;
-}
-
-static int mipid02_s_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct mipid02_dev *bridge = to_mipid02_dev(sd);
-	struct i2c_client *client = bridge->i2c_client;
-	int ret = 0;
-
-	dev_dbg(&client->dev, "%s : requested %d\n", __func__, enable);
-
-	ret = enable ? mipid02_stream_enable(bridge) :
-		       mipid02_stream_disable(bridge);
-	if (ret)
-		dev_err(&client->dev, "failed to stream %s (%d)\n",
-			enable ? "enable" : "disable", ret);
+	cci_write(bridge->regmap, MIPID02_CLK_LANE_REG1, 0, &ret);
+	cci_write(bridge->regmap, MIPID02_DATA_LANE0_REG1, 0, &ret);
+	cci_write(bridge->regmap, MIPID02_DATA_LANE1_REG1, 0, &ret);
 
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
 	return ret;
 }
 
@@ -640,13 +633,15 @@ static int mipid02_set_fmt(struct v4l2_subdev *sd,
 }
 
 static const struct v4l2_subdev_video_ops mipid02_video_ops = {
-	.s_stream = mipid02_s_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_pad_ops mipid02_pad_ops = {
 	.enum_mbus_code = mipid02_enum_mbus_code,
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = mipid02_set_fmt,
+	.enable_streams = mipid02_enable_streams,
+	.disable_streams = mipid02_disable_streams,
 };
 
 static const struct v4l2_subdev_ops mipid02_subdev_ops = {
@@ -692,6 +687,7 @@ static int mipid02_async_bound(struct v4l2_async_notifier *notifier,
 	}
 
 	bridge->s_subdev = s_subdev;
+	bridge->s_subdev_pad_id = source_pad;
 
 	return 0;
 }
@@ -875,7 +871,7 @@ static int mipid02_probe(struct i2c_client *client)
 	}
 
 	/* enable clock, power and reset device if available */
-	ret = mipid02_set_power_on(bridge);
+	ret = mipid02_set_power_on(&client->dev);
 	if (ret)
 		goto entity_cleanup;
 
@@ -897,6 +893,15 @@ static int mipid02_probe(struct i2c_client *client)
 		goto power_off;
 	}
 
+	/* Enable runtime PM and turn off the device */
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(&client->dev);
+	pm_runtime_enable(dev);
+
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
+
 	ret = v4l2_async_register_subdev(&bridge->sd);
 	if (ret < 0) {
 		dev_err(&client->dev, "v4l2_async_register_subdev failed %d",
@@ -911,8 +916,10 @@ static int mipid02_probe(struct i2c_client *client)
 unregister_notifier:
 	v4l2_async_nf_unregister(&bridge->notifier);
 	v4l2_async_nf_cleanup(&bridge->notifier);
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
 power_off:
-	mipid02_set_power_off(bridge);
+	mipid02_set_power_off(&client->dev);
 entity_cleanup:
 	media_entity_cleanup(&bridge->sd.entity);
 
@@ -927,7 +934,11 @@ static void mipid02_remove(struct i2c_client *client)
 	v4l2_async_nf_unregister(&bridge->notifier);
 	v4l2_async_nf_cleanup(&bridge->notifier);
 	v4l2_async_unregister_subdev(&bridge->sd);
-	mipid02_set_power_off(bridge);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		mipid02_set_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
 	media_entity_cleanup(&bridge->sd.entity);
 }
 
@@ -937,10 +948,15 @@ static const struct of_device_id mipid02_dt_ids[] = {
 };
 MODULE_DEVICE_TABLE(of, mipid02_dt_ids);
 
+static const struct dev_pm_ops mipid02_pm_ops = {
+	RUNTIME_PM_OPS(mipid02_set_power_off, mipid02_set_power_on, NULL)
+};
+
 static struct i2c_driver mipid02_i2c_driver = {
 	.driver = {
 		.name  = "st-mipid02",
 		.of_match_table = mipid02_dt_ids,
+		.pm = pm_ptr(&mipid02_pm_ops),
 	},
 	.probe = mipid02_probe,
 	.remove = mipid02_remove,
diff --git a/drivers/media/i2c/tc358743.c b/drivers/media/i2c/tc358743.c
index 3192a334aaab..3d6703b75bfa 100644
--- a/drivers/media/i2c/tc358743.c
+++ b/drivers/media/i2c/tc358743.c
@@ -87,6 +87,10 @@ struct tc358743_state {
 	struct timer_list timer;
 	struct work_struct work_i2c_poll;
 
+	/* debugfs */
+	struct dentry *debugfs_dir;
+	struct v4l2_debugfs_if *infoframes;
+
 	/* edid  */
 	u8 edid_blocks_written;
 
@@ -309,6 +313,10 @@ static int tc358743_get_detected_timings(struct v4l2_subdev *sd,
 
 	memset(timings, 0, sizeof(struct v4l2_dv_timings));
 
+	/* if HPD is low, ignore any video */
+	if (!(i2c_rd8(sd, HPD_CTL) & MASK_HPD_OUT0))
+		return -ENOLINK;
+
 	if (no_signal(sd)) {
 		v4l2_dbg(1, debug, sd, "%s: no valid signal\n", __func__);
 		return -ENOLINK;
@@ -430,12 +438,35 @@ static void tc358743_erase_bksv(struct v4l2_subdev *sd)
 
 /* --------------- AVI infoframe --------------- */
 
+static ssize_t
+tc358743_debugfs_if_read(u32 type, void *priv, struct file *filp,
+			 char __user *ubuf, size_t count, loff_t *ppos)
+{
+	u8 buf[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	struct v4l2_subdev *sd = priv;
+	int len;
+
+	if (!is_hdmi(sd))
+		return 0;
+
+	if (type != V4L2_DEBUGFS_IF_AVI)
+		return 0;
+
+	i2c_rd(sd, PK_AVI_0HEAD, buf, PK_AVI_16BYTE - PK_AVI_0HEAD + 1);
+	len = buf[2] + 4;
+	if (len > V4L2_DEBUGFS_IF_MAX_LEN)
+		len = -ENOENT;
+	if (len > 0)
+		len = simple_read_from_buffer(ubuf, count, ppos, buf, len);
+	return len < 0 ? 0 : len;
+}
+
 static void print_avi_infoframe(struct v4l2_subdev *sd)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(sd);
 	struct device *dev = &client->dev;
 	union hdmi_infoframe frame;
-	u8 buffer[HDMI_INFOFRAME_SIZE(AVI)];
+	u8 buffer[HDMI_INFOFRAME_SIZE(AVI)] = {};
 
 	if (!is_hdmi(sd)) {
 		v4l2_info(sd, "DVI-D signal - AVI infoframe not supported\n");
@@ -1474,7 +1505,7 @@ static irqreturn_t tc358743_irq_handler(int irq, void *dev_id)
 
 static void tc358743_irq_poll_timer(struct timer_list *t)
 {
-	struct tc358743_state *state = from_timer(state, t, timer);
+	struct tc358743_state *state = timer_container_of(state, t, timer);
 	unsigned int msecs;
 
 	schedule_work(&state->work_i2c_poll);
@@ -1521,11 +1552,14 @@ static int tc358743_g_input_status(struct v4l2_subdev *sd, u32 *status)
 	return 0;
 }
 
-static int tc358743_s_dv_timings(struct v4l2_subdev *sd,
+static int tc358743_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				 struct v4l2_dv_timings *timings)
 {
 	struct tc358743_state *state = to_state(sd);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (!timings)
 		return -EINVAL;
 
@@ -1553,11 +1587,14 @@ static int tc358743_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tc358743_g_dv_timings(struct v4l2_subdev *sd,
+static int tc358743_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				 struct v4l2_dv_timings *timings)
 {
 	struct tc358743_state *state = to_state(sd);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	*timings = state->timings;
 
 	return 0;
@@ -1573,11 +1610,14 @@ static int tc358743_enum_dv_timings(struct v4l2_subdev *sd,
 			&tc358743_timings_cap, NULL, NULL);
 }
 
-static int tc358743_query_dv_timings(struct v4l2_subdev *sd,
-		struct v4l2_dv_timings *timings)
+static int tc358743_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				     struct v4l2_dv_timings *timings)
 {
 	int ret;
 
+	if (pad != 0)
+		return -EINVAL;
+
 	ret = tc358743_get_detected_timings(sd, timings);
 	if (ret)
 		return ret;
@@ -1822,9 +1862,6 @@ static const struct v4l2_subdev_core_ops tc358743_core_ops = {
 
 static const struct v4l2_subdev_video_ops tc358743_video_ops = {
 	.g_input_status = tc358743_g_input_status,
-	.s_dv_timings = tc358743_s_dv_timings,
-	.g_dv_timings = tc358743_g_dv_timings,
-	.query_dv_timings = tc358743_query_dv_timings,
 	.s_stream = tc358743_s_stream,
 };
 
@@ -1834,6 +1871,9 @@ static const struct v4l2_subdev_pad_ops tc358743_pad_ops = {
 	.get_fmt = tc358743_get_fmt,
 	.get_edid = tc358743_g_edid,
 	.set_edid = tc358743_s_edid,
+	.s_dv_timings = tc358743_s_dv_timings,
+	.g_dv_timings = tc358743_g_dv_timings,
+	.query_dv_timings = tc358743_query_dv_timings,
 	.enum_dv_timings = tc358743_enum_dv_timings,
 	.dv_timings_cap = tc358743_dv_timings_cap,
 	.get_mbus_config = tc358743_get_mbus_config,
@@ -2110,7 +2150,7 @@ static int tc358743_probe(struct i2c_client *client)
 
 	tc358743_initial_setup(sd);
 
-	tc358743_s_dv_timings(sd, &default_timing);
+	tc358743_s_dv_timings(sd, 0, &default_timing);
 
 	tc358743_set_csi_color_space(sd);
 
@@ -2152,6 +2192,11 @@ static int tc358743_probe(struct i2c_client *client)
 	if (err < 0)
 		goto err_work_queues;
 
+	state->debugfs_dir = debugfs_create_dir(sd->name, v4l2_debugfs_root());
+	state->infoframes = v4l2_debugfs_if_alloc(state->debugfs_dir,
+						  V4L2_DEBUGFS_IF_AVI, sd,
+						  tc358743_debugfs_if_read);
+
 	v4l2_info(sd, "%s found @ 0x%x (%s)\n", client->name,
 		  client->addr << 1, client->adapter->name);
 
@@ -2159,8 +2204,10 @@ static int tc358743_probe(struct i2c_client *client)
 
 err_work_queues:
 	cec_unregister_adapter(state->cec_adap);
-	if (!state->i2c_client->irq)
+	if (!state->i2c_client->irq) {
+		timer_delete(&state->timer);
 		flush_work(&state->work_i2c_poll);
+	}
 	cancel_delayed_work(&state->delayed_work_enable_hotplug);
 	mutex_destroy(&state->confctl_mutex);
 err_hdl:
@@ -2175,10 +2222,12 @@ static void tc358743_remove(struct i2c_client *client)
 	struct tc358743_state *state = to_state(sd);
 
 	if (!state->i2c_client->irq) {
-		del_timer_sync(&state->timer);
+		timer_delete_sync(&state->timer);
 		flush_work(&state->work_i2c_poll);
 	}
 	cancel_delayed_work_sync(&state->delayed_work_enable_hotplug);
+	v4l2_debugfs_if_free(state->infoframes);
+	debugfs_remove_recursive(state->debugfs_dir);
 	cec_unregister_adapter(state->cec_adap);
 	v4l2_async_unregister_subdev(sd);
 	v4l2_device_unregister_subdev(sd);
@@ -2188,7 +2237,7 @@ static void tc358743_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tc358743_id[] = {
-	{"tc358743", 0},
+	{ "tc358743" },
 	{}
 };
 
diff --git a/drivers/media/i2c/tc358746.c b/drivers/media/i2c/tc358746.c
index d676adc4401b..143aa1359aba 100644
--- a/drivers/media/i2c/tc358746.c
+++ b/drivers/media/i2c/tc358746.c
@@ -161,10 +161,6 @@ struct tc358746 {
 	u16				pll_pre_div;
 	u16				pll_mul;
 
-#define TC358746_VB_MAX_SIZE		(511 * 32)
-#define TC358746_VB_DEFAULT_SIZE	  (1 * 32)
-	unsigned int			vb_size; /* Video buffer size in bits */
-
 	struct phy_configure_opts_mipi_dphy dphy_cfg;
 };
 
@@ -202,6 +198,15 @@ enum {
 	PDFORMAT_YUV444,
 };
 
+#define TC358746_FORMAT_RAW(_bpp, _code)		\
+{							\
+	.code = _code,					\
+	.bus_width = _bpp,				\
+	.bpp = _bpp,					\
+	.pdformat = PDFORMAT_RAW##_bpp,			\
+	.pdataf = PDATAF_MODE0, /* don't care */	\
+}
+
 /* Check tc358746_src_mbus_code() if you add new formats */
 static const struct tc358746_format tc358746_formats[] = {
 	{
@@ -230,7 +235,23 @@ static const struct tc358746_format tc358746_formats[] = {
 		.bpp = 20,
 		.pdformat = PDFORMAT_YUV422_10BIT,
 		.pdataf = PDATAF_MODE0, /* don't care */
-	}
+	},
+	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SBGGR8_1X8),
+	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SGBRG8_1X8),
+	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SGRBG8_1X8),
+	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SRGGB8_1X8),
+	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SBGGR10_1X10),
+	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SGBRG10_1X10),
+	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SGRBG10_1X10),
+	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SRGGB10_1X10),
+	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SBGGR12_1X12),
+	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SGBRG12_1X12),
+	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SGRBG12_1X12),
+	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SRGGB12_1X12),
+	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SBGGR14_1X14),
+	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SGBRG14_1X14),
+	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SGRBG14_1X14),
+	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SRGGB14_1X14),
 };
 
 /* Get n-th format for pad */
@@ -415,6 +436,70 @@ tc358746_apply_pll_config(struct tc358746 *tc358746)
 	return tc358746_set_bits(tc358746, PLLCTL1_REG, CKEN);
 }
 
+#define TC358746_VB_PRECISION		10
+#define TC358746_VB_MAX_SIZE		(511 * 32)
+#define TC358746_VB_DEFAULT_SIZE	(1 * 32)
+
+static int tc358746_calc_vb_size(struct tc358746 *tc358746,
+				 s64 source_link_freq,
+				 const struct v4l2_mbus_framefmt *mbusfmt,
+				 const struct tc358746_format *fmt)
+{
+	unsigned long csi_bitrate, source_bitrate;
+	unsigned int fifo_sz, tmp, n;
+	int vb_size; /* Video buffer size in bits */
+
+	source_bitrate = source_link_freq * fmt->bus_width;
+
+	csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
+
+	dev_dbg(tc358746->sd.dev,
+		"Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
+		source_bitrate, csi_bitrate);
+
+	/* Avoid possible FIFO overflows */
+	if (csi_bitrate < source_bitrate)
+		return -EINVAL;
+
+	/* Best case */
+	if (csi_bitrate == source_bitrate) {
+		fifo_sz = TC358746_VB_DEFAULT_SIZE;
+		vb_size = TC358746_VB_DEFAULT_SIZE;
+	} else {
+		/*
+		 * Avoid possible FIFO underflow in case of
+		 * csi_bitrate > source_bitrate. For such case the chip has a internal
+		 * fifo which can be used to delay the line output.
+		 *
+		 * Fifo size calculation (excluding precision):
+		 *
+		 * fifo-sz, image-width - in bits
+		 * sbr                  - source_bitrate in bits/s
+		 * csir                 - csi_bitrate in bits/s
+		 *
+		 * image-width / csir >= (image-width - fifo-sz) / sbr
+		 * image-width * sbr / csir >= image-width - fifo-sz
+		 * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
+		 * fifo-sz >= image-width - image-width / n
+		 */
+		source_bitrate /= TC358746_VB_PRECISION;
+		n = csi_bitrate / source_bitrate;
+		tmp = (mbusfmt->width * TC358746_VB_PRECISION) / n;
+		fifo_sz = mbusfmt->width - tmp;
+		fifo_sz *= fmt->bpp;
+		vb_size = round_up(fifo_sz, 32);
+	}
+
+	dev_dbg(tc358746->sd.dev,
+		"Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
+		fifo_sz, vb_size);
+
+	if (vb_size > TC358746_VB_MAX_SIZE)
+		return -EINVAL;
+
+	return vb_size;
+}
+
 static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 {
 	const struct v4l2_mbus_framefmt *mbusfmt;
@@ -422,6 +507,9 @@ static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 	struct v4l2_subdev_state *sink_state;
 	const struct tc358746_format *fmt;
 	struct device *dev = sd->dev;
+	struct media_pad *source_pad;
+	s64 source_link_freq;
+	int vb_size;
 	u32 val;
 	int err;
 
@@ -430,6 +518,21 @@ static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 	mbusfmt = v4l2_subdev_state_get_format(sink_state, TC358746_SINK);
 	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
 
+	source_pad = media_entity_remote_source_pad_unique(&sd->entity);
+	if (IS_ERR(source_pad)) {
+		dev_err(dev, "Failed to get source pad of %s\n", sd->name);
+		err = PTR_ERR(source_pad);
+		goto out;
+	}
+	source_link_freq = v4l2_get_link_freq(source_pad, 0, 0);
+	if (source_link_freq <= 0) {
+		dev_err(dev,
+			"Failed to query or invalid source link frequency\n");
+		/* Return -EINVAL in case of source_link_freq is 0 */
+		err = source_link_freq ?: -EINVAL;
+		goto out;
+	}
+
 	/* Self defined CSI user data type id's are not supported yet */
 	val = PDFMT(fmt->pdformat);
 	dev_dbg(dev, "DATAFMT: 0x%x\n", val);
@@ -443,7 +546,13 @@ static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 	if (err)
 		goto out;
 
-	val = tc358746->vb_size / 32;
+	vb_size = tc358746_calc_vb_size(tc358746, source_link_freq, mbusfmt, fmt);
+	if (vb_size < 0) {
+		err = vb_size;
+		goto out;
+	}
+
+	val = vb_size / 32;
 	dev_dbg(dev, "FIFOCTL: %u (0x%x)\n", val, val);
 	err = tc358746_write(tc358746, FIFOCTL_REG, val);
 	if (err)
@@ -460,24 +569,20 @@ out:
 	return err;
 }
 
-/* Use MHz as base so the div needs no u64 */
-static u32 tc358746_cfg_to_cnt(unsigned int cfg_val,
-			       unsigned int clk_mhz,
-			       unsigned int time_base)
+static u32 tc358746_cfg_to_cnt(unsigned long cfg_val, unsigned long clk_hz,
+			       unsigned long long time_base)
 {
-	return DIV_ROUND_UP(cfg_val * clk_mhz, time_base);
+	return div64_u64((u64)cfg_val * clk_hz + time_base - 1, time_base);
 }
 
-static u32 tc358746_ps_to_cnt(unsigned int cfg_val,
-			      unsigned int clk_mhz)
+static u32 tc358746_ps_to_cnt(unsigned long cfg_val, unsigned long clk_hz)
 {
-	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, USEC_PER_SEC);
+	return tc358746_cfg_to_cnt(cfg_val, clk_hz, PSEC_PER_SEC);
 }
 
-static u32 tc358746_us_to_cnt(unsigned int cfg_val,
-			      unsigned int clk_mhz)
+static u32 tc358746_us_to_cnt(unsigned long cfg_val, unsigned long clk_hz)
 {
-	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, 1);
+	return tc358746_cfg_to_cnt(cfg_val, clk_hz, USEC_PER_SEC);
 }
 
 static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
@@ -492,7 +597,6 @@ static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
 
 	/* The hs_byte_clk is also called SYSCLK in the excel sheet */
 	hs_byte_clk = cfg->hs_clk_rate / 8;
-	hs_byte_clk /= HZ_PER_MHZ;
 	hf_clk = hs_byte_clk / 2;
 
 	val = tc358746_us_to_cnt(cfg->init, hf_clk) - 1;
@@ -844,8 +948,7 @@ static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
 			continue;
 
 		tmp = fout * postdiv;
-		do_div(tmp, fin);
-		mul = tmp;
+		mul = div64_ul(tmp, fin);
 		if (mul > 511)
 			continue;
 
@@ -883,97 +986,6 @@ static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
 	return best_freq;
 }
 
-#define TC358746_PRECISION 10
-
-static int
-tc358746_link_validate(struct v4l2_subdev *sd, struct media_link *link,
-		       struct v4l2_subdev_format *source_fmt,
-		       struct v4l2_subdev_format *sink_fmt)
-{
-	struct tc358746 *tc358746 = to_tc358746(sd);
-	unsigned long csi_bitrate, source_bitrate;
-	struct v4l2_subdev_state *sink_state;
-	struct v4l2_mbus_framefmt *mbusfmt;
-	const struct tc358746_format *fmt;
-	unsigned int fifo_sz, tmp, n;
-	struct v4l2_subdev *source;
-	s64 source_link_freq;
-	int err;
-
-	err = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
-	if (err)
-		return err;
-
-	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
-	mbusfmt = v4l2_subdev_state_get_format(sink_state, TC358746_SINK);
-
-	/* Check the FIFO settings */
-	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
-
-	source = media_entity_to_v4l2_subdev(link->source->entity);
-	source_link_freq = v4l2_get_link_freq(source->ctrl_handler, 0, 0);
-	if (source_link_freq <= 0) {
-		dev_err(tc358746->sd.dev,
-			"Failed to query or invalid source link frequency\n");
-		v4l2_subdev_unlock_state(sink_state);
-		/* Return -EINVAL in case of source_link_freq is 0 */
-		return source_link_freq ? : -EINVAL;
-	}
-	source_bitrate = source_link_freq * fmt->bus_width;
-
-	csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
-
-	dev_dbg(tc358746->sd.dev,
-		"Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
-		source_bitrate, csi_bitrate);
-
-	/* Avoid possible FIFO overflows */
-	if (csi_bitrate < source_bitrate) {
-		v4l2_subdev_unlock_state(sink_state);
-		return -EINVAL;
-	}
-
-	/* Best case */
-	if (csi_bitrate == source_bitrate) {
-		fifo_sz = TC358746_VB_DEFAULT_SIZE;
-		tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
-		goto out;
-	}
-
-	/*
-	 * Avoid possible FIFO underflow in case of
-	 * csi_bitrate > source_bitrate. For such case the chip has a internal
-	 * fifo which can be used to delay the line output.
-	 *
-	 * Fifo size calculation (excluding precision):
-	 *
-	 * fifo-sz, image-width - in bits
-	 * sbr                  - source_bitrate in bits/s
-	 * csir                 - csi_bitrate in bits/s
-	 *
-	 * image-width / csir >= (image-width - fifo-sz) / sbr
-	 * image-width * sbr / csir >= image-width - fifo-sz
-	 * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
-	 * fifo-sz >= image-width - image-width / n
-	 */
-
-	source_bitrate /= TC358746_PRECISION;
-	n = csi_bitrate / source_bitrate;
-	tmp = (mbusfmt->width * TC358746_PRECISION) / n;
-	fifo_sz = mbusfmt->width - tmp;
-	fifo_sz *= fmt->bpp;
-	tc358746->vb_size = round_up(fifo_sz, 32);
-
-out:
-	dev_dbg(tc358746->sd.dev,
-		"Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
-		fifo_sz, tc358746->vb_size);
-
-	v4l2_subdev_unlock_state(sink_state);
-
-	return tc358746->vb_size > TC358746_VB_MAX_SIZE ? -EINVAL : 0;
-}
-
 static int tc358746_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 				    struct v4l2_mbus_config *config)
 {
@@ -1041,7 +1053,7 @@ static const struct v4l2_subdev_pad_ops tc358746_pad_ops = {
 	.enum_mbus_code = tc358746_enum_mbus_code,
 	.set_fmt = tc358746_set_fmt,
 	.get_fmt = v4l2_subdev_get_fmt,
-	.link_validate = tc358746_link_validate,
+	.link_validate = v4l2_subdev_link_validate_default,
 	.get_mbus_config = tc358746_get_mbus_config,
 };
 
@@ -1353,8 +1365,6 @@ tc358746_init_output_port(struct tc358746 *tc358746, unsigned long refclk)
 	if (err)
 		goto err;
 
-	tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
-
 	return 0;
 
 err:
@@ -1617,6 +1627,16 @@ static void tc358746_remove(struct i2c_client *client)
 	pm_runtime_dont_use_autosuspend(sd->dev);
 }
 
+/*
+ * This function has been created just to avoid a smatch warning,
+ * please do not merge it into tc358746_suspend until you have
+ * confirmed that it does not introduce a new warning.
+ */
+static void tc358746_clk_enable(struct tc358746 *tc358746)
+{
+	clk_prepare_enable(tc358746->refclk);
+}
+
 static int tc358746_suspend(struct device *dev)
 {
 	struct tc358746 *tc358746 = dev_get_drvdata(dev);
@@ -1627,7 +1647,7 @@ static int tc358746_suspend(struct device *dev)
 	err = regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
 				     tc358746->supplies);
 	if (err)
-		clk_prepare_enable(tc358746->refclk);
+		tc358746_clk_enable(tc358746);
 
 	return err;
 }
diff --git a/drivers/media/i2c/tda1997x.c b/drivers/media/i2c/tda1997x.c
index 8e4a0718c4b6..959590afc80f 100644
--- a/drivers/media/i2c/tda1997x.c
+++ b/drivers/media/i2c/tda1997x.c
@@ -1669,8 +1669,8 @@ tda1997x_g_input_status(struct v4l2_subdev *sd, u32 *status)
 	return 0;
 };
 
-static int tda1997x_s_dv_timings(struct v4l2_subdev *sd,
-				struct v4l2_dv_timings *timings)
+static int tda1997x_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				 struct v4l2_dv_timings *timings)
 {
 	struct tda1997x_state *state = to_state(sd);
 
@@ -1694,7 +1694,7 @@ static int tda1997x_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tda1997x_g_dv_timings(struct v4l2_subdev *sd,
+static int tda1997x_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				 struct v4l2_dv_timings *timings)
 {
 	struct tda1997x_state *state = to_state(sd);
@@ -1707,7 +1707,7 @@ static int tda1997x_g_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int tda1997x_query_dv_timings(struct v4l2_subdev *sd,
+static int tda1997x_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				     struct v4l2_dv_timings *timings)
 {
 	struct tda1997x_state *state = to_state(sd);
@@ -1724,9 +1724,6 @@ static int tda1997x_query_dv_timings(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_video_ops tda1997x_video_ops = {
 	.g_input_status = tda1997x_g_input_status,
-	.s_dv_timings = tda1997x_s_dv_timings,
-	.g_dv_timings = tda1997x_g_dv_timings,
-	.query_dv_timings = tda1997x_query_dv_timings,
 };
 
 
@@ -1930,6 +1927,9 @@ static const struct v4l2_subdev_pad_ops tda1997x_pad_ops = {
 	.set_fmt = tda1997x_set_format,
 	.get_edid = tda1997x_get_edid,
 	.set_edid = tda1997x_set_edid,
+	.s_dv_timings = tda1997x_s_dv_timings,
+	.g_dv_timings = tda1997x_g_dv_timings,
+	.query_dv_timings = tda1997x_query_dv_timings,
 	.dv_timings_cap = tda1997x_get_dv_timings_cap,
 	.enum_dv_timings = tda1997x_enum_dv_timings,
 };
@@ -2315,11 +2315,10 @@ static int tda1997x_parse_dt(struct tda1997x_state *state)
 		return -EINVAL;
 
 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), &bus_cfg);
-	if (ret) {
-		of_node_put(ep);
-		return ret;
-	}
 	of_node_put(ep);
+	if (ret)
+		return ret;
+
 	pdata->vidout_bus_type = bus_cfg.bus_type;
 
 	/* polarity of HS/VS/DE */
@@ -2514,7 +2513,7 @@ static void tda1997x_codec_remove(struct snd_soc_component *component)
 {
 }
 
-static struct snd_soc_component_driver tda1997x_codec_driver = {
+static const struct snd_soc_component_driver tda1997x_codec_driver = {
 	.probe			= tda1997x_codec_probe,
 	.remove			= tda1997x_codec_remove,
 	.idle_bias_on		= 1,
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
index 6ecdc8e2e0c6..76ef0fdddf76 100644
--- a/drivers/media/i2c/tda7432.c
+++ b/drivers/media/i2c/tda7432.c
@@ -400,7 +400,7 @@ static void tda7432_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tda7432_id[] = {
-	{ "tda7432", 0 },
+	{ "tda7432" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tda7432_id);
diff --git a/drivers/media/i2c/tda9840.c b/drivers/media/i2c/tda9840.c
index 1911ef2126be..d61da811c9da 100644
--- a/drivers/media/i2c/tda9840.c
+++ b/drivers/media/i2c/tda9840.c
@@ -182,7 +182,7 @@ static void tda9840_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tda9840_id[] = {
-	{ "tda9840", 0 },
+	{ "tda9840" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tda9840_id);
diff --git a/drivers/media/i2c/tea6415c.c b/drivers/media/i2c/tea6415c.c
index 3ed6e441d515..4aaf66353610 100644
--- a/drivers/media/i2c/tea6415c.c
+++ b/drivers/media/i2c/tea6415c.c
@@ -141,7 +141,7 @@ static void tea6415c_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tea6415c_id[] = {
-	{ "tea6415c", 0 },
+	{ "tea6415c" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tea6415c_id);
diff --git a/drivers/media/i2c/tea6420.c b/drivers/media/i2c/tea6420.c
index 63f23784bb41..5c5ea3973251 100644
--- a/drivers/media/i2c/tea6420.c
+++ b/drivers/media/i2c/tea6420.c
@@ -123,7 +123,7 @@ static void tea6420_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tea6420_id[] = {
-	{ "tea6420", 0 },
+	{ "tea6420" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tea6420_id);
diff --git a/drivers/media/i2c/thp7312.c b/drivers/media/i2c/thp7312.c
index 19bd923a7315..8852c56431fe 100644
--- a/drivers/media/i2c/thp7312.c
+++ b/drivers/media/i2c/thp7312.c
@@ -4,7 +4,7 @@
  * Copyright (C) 2023 Ideas on Board Oy
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <linux/clk.h>
 #include <linux/delay.h>
@@ -27,7 +27,6 @@
 #include <media/v4l2-cci.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-subdev.h>
 
@@ -879,8 +878,6 @@ static int thp7312_init_state(struct v4l2_subdev *sd,
 
 static const struct v4l2_subdev_core_ops thp7312_core_ops = {
 	.log_status = v4l2_ctrl_subdev_log_status,
-	.subscribe_event = v4l2_ctrl_subdev_subscribe_event,
-	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_video_ops thp7312_video_ops = {
@@ -1503,7 +1500,7 @@ static int __thp7312_flash_reg_read(struct thp7312_device *thp7312,
 
 	msgs[0].addr = client->addr;
 	msgs[0].flags = 0;
-	msgs[0].len = sizeof(thp7312_cmd_read_reg),
+	msgs[0].len = sizeof(thp7312_cmd_read_reg);
 	msgs[0].buf = (u8 *)thp7312_cmd_read_reg;
 
 	msgs[1].addr = client->addr;
@@ -2127,7 +2124,7 @@ static int thp7312_probe(struct i2c_client *client)
 
 	v4l2_i2c_subdev_init(&thp7312->sd, client, &thp7312_subdev_ops);
 	thp7312->sd.internal_ops = &thp7312_internal_ops;
-	thp7312->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	thp7312->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	thp7312->pad.flags = MEDIA_PAD_FL_SOURCE;
 	thp7312->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index ea70c1c13872..b7cedc5b3e8e 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -7,7 +7,7 @@
  * Author: Chaithrika U S <chaithrika@ti.com>
  *
  * Contributors:
- *     Hans Verkuil <hans.verkuil@cisco.com>
+ *     Hans Verkuil <hansverk@cisco.com>
  *     Lad, Prabhakar <prabhakar.lad@ti.com>
  *     Martin Bugge <marbugge@cisco.com>
  *
@@ -193,8 +193,8 @@ static int ths7303_s_stream(struct v4l2_subdev *sd, int enable)
 }
 
 /* for setting filter for HD output */
-static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
-			       struct v4l2_dv_timings *dv_timings)
+static int ths7303_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *dv_timings)
 {
 	struct ths7303_state *state = to_state(sd);
 
@@ -210,7 +210,6 @@ static int ths7303_s_dv_timings(struct v4l2_subdev *sd,
 static const struct v4l2_subdev_video_ops ths7303_video_ops = {
 	.s_stream	= ths7303_s_stream,
 	.s_std_output	= ths7303_s_std_output,
-	.s_dv_timings   = ths7303_s_dv_timings,
 };
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -317,9 +316,14 @@ static const struct v4l2_subdev_core_ops ths7303_core_ops = {
 #endif
 };
 
+static const struct v4l2_subdev_pad_ops ths7303_pad_ops = {
+	.s_dv_timings = ths7303_s_dv_timings,
+};
+
 static const struct v4l2_subdev_ops ths7303_ops = {
 	.core	= &ths7303_core_ops,
 	.video	= &ths7303_video_ops,
+	.pad	= &ths7303_pad_ops,
 };
 
 static int ths7303_probe(struct i2c_client *client)
@@ -365,9 +369,9 @@ static void ths7303_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ths7303_id[] = {
-	{"ths7303", 0},
-	{"ths7353", 0},
-	{},
+	{ "ths7303" },
+	{ "ths7353" },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, ths7303_id);
diff --git a/drivers/media/i2c/ths8200.c b/drivers/media/i2c/ths8200.c
index 0e0f676cd221..686f10641c7a 100644
--- a/drivers/media/i2c/ths8200.c
+++ b/drivers/media/i2c/ths8200.c
@@ -358,13 +358,16 @@ static void ths8200_setup(struct v4l2_subdev *sd, struct v4l2_bt_timings *bt)
 		 bt->hsync, bt->vsync);
 }
 
-static int ths8200_s_dv_timings(struct v4l2_subdev *sd,
+static int ths8200_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_dv_timings *timings)
 {
 	struct ths8200_state *state = to_state(sd);
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (!v4l2_valid_dv_timings(timings, &ths8200_timings_cap,
 				NULL, NULL))
 		return -EINVAL;
@@ -385,13 +388,16 @@ static int ths8200_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int ths8200_g_dv_timings(struct v4l2_subdev *sd,
+static int ths8200_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_dv_timings *timings)
 {
 	struct ths8200_state *state = to_state(sd);
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	*timings = state->dv_timings;
 
 	return 0;
@@ -420,11 +426,11 @@ static int ths8200_dv_timings_cap(struct v4l2_subdev *sd,
 /* Specific video subsystem operation handlers */
 static const struct v4l2_subdev_video_ops ths8200_video_ops = {
 	.s_stream = ths8200_s_stream,
-	.s_dv_timings = ths8200_s_dv_timings,
-	.g_dv_timings = ths8200_g_dv_timings,
 };
 
 static const struct v4l2_subdev_pad_ops ths8200_pad_ops = {
+	.s_dv_timings = ths8200_s_dv_timings,
+	.g_dv_timings = ths8200_g_dv_timings,
 	.enum_dv_timings = ths8200_enum_dv_timings,
 	.dv_timings_cap = ths8200_dv_timings_cap,
 };
@@ -481,8 +487,8 @@ static void ths8200_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id ths8200_id[] = {
-	{ "ths8200", 0 },
-	{},
+	{ "ths8200" },
+	{}
 };
 MODULE_DEVICE_TABLE(i2c, ths8200_id);
 
diff --git a/drivers/media/i2c/tlv320aic23b.c b/drivers/media/i2c/tlv320aic23b.c
index d800ff8af1ff..b7b31b6192af 100644
--- a/drivers/media/i2c/tlv320aic23b.c
+++ b/drivers/media/i2c/tlv320aic23b.c
@@ -188,7 +188,7 @@ static void tlv320aic23b_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id tlv320aic23b_id[] = {
-	{ "tlv320aic23b", 0 },
+	{ "tlv320aic23b" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tlv320aic23b_id);
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index ba20f35cafd5..6267e9ad39c0 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -318,7 +318,7 @@ static int chip_cmd(struct CHIPSTATE *chip, char *name, audiocmd *cmd)
 
 static void chip_thread_wake(struct timer_list *t)
 {
-	struct CHIPSTATE *chip = from_timer(chip, t, wt);
+	struct CHIPSTATE *chip = timer_container_of(chip, t, wt);
 	wake_up_process(chip->thread);
 }
 
@@ -1787,7 +1787,7 @@ static int tvaudio_s_radio(struct v4l2_subdev *sd)
 	struct CHIPSTATE *chip = to_state(sd);
 
 	chip->radio = 1;
-	/* del_timer(&chip->wt); */
+	/* timer_delete(&chip->wt); */
 	return 0;
 }
 
@@ -2071,7 +2071,7 @@ static void tvaudio_remove(struct i2c_client *client)
 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
 	struct CHIPSTATE *chip = to_state(sd);
 
-	del_timer_sync(&chip->wt);
+	timer_delete_sync(&chip->wt);
 	if (chip->thread) {
 		/* shutdown async thread */
 		kthread_stop(chip->thread);
@@ -2086,7 +2086,7 @@ static void tvaudio_remove(struct i2c_client *client)
    detect which device is present. So rather than listing all supported
    devices here, we pretend to support a single, fake device type. */
 static const struct i2c_device_id tvaudio_id[] = {
-	{ "tvaudio", 0 },
+	{ "tvaudio" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tvaudio_id);
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 64b91aa3c82a..e3675c744d9e 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -514,7 +514,7 @@ struct i2c_vbi_ram_value {
  * and so on. There are 16 possible locations from 0 to 15.
  */
 
-static struct i2c_vbi_ram_value vbi_ram_default[] = {
+static const struct i2c_vbi_ram_value vbi_ram_default[] = {
 
 	/*
 	 * FIXME: Current api doesn't handle all VBI types, those not
@@ -1812,7 +1812,7 @@ static const struct regmap_access_table tvp5150_readable_table = {
 	.n_yes_ranges = ARRAY_SIZE(tvp5150_readable_ranges),
 };
 
-static struct regmap_config tvp5150_config = {
+static const struct regmap_config tvp5150_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 	.max_register = 0xff,
@@ -2265,7 +2265,7 @@ static const struct dev_pm_ops tvp5150_pm_ops = {
 };
 
 static const struct i2c_device_id tvp5150_id[] = {
-	{ "tvp5150", 0 },
+	{ "tvp5150" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tvp5150_id);
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 6a04ffae5343..c09a5bd71fd0 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -546,13 +546,16 @@ static int tvp7002_write_inittab(struct v4l2_subdev *sd,
 	return error;
 }
 
-static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
-					struct v4l2_dv_timings *dv_timings)
+static int tvp7002_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *dv_timings)
 {
 	struct tvp7002 *device = to_tvp7002(sd);
 	const struct v4l2_bt_timings *bt = &dv_timings->bt;
 	int i;
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (dv_timings->type != V4L2_DV_BT_656_1120)
 		return -EINVAL;
 	for (i = 0; i < NUM_TIMINGS; i++) {
@@ -566,11 +569,14 @@ static int tvp7002_s_dv_timings(struct v4l2_subdev *sd,
 	return -EINVAL;
 }
 
-static int tvp7002_g_dv_timings(struct v4l2_subdev *sd,
-					struct v4l2_dv_timings *dv_timings)
+static int tvp7002_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				struct v4l2_dv_timings *dv_timings)
 {
 	struct tvp7002 *device = to_tvp7002(sd);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	*dv_timings = device->current_timings->timings;
 	return 0;
 }
@@ -659,12 +665,16 @@ static int tvp7002_query_dv(struct v4l2_subdev *sd, int *index)
 	return 0;
 }
 
-static int tvp7002_query_dv_timings(struct v4l2_subdev *sd,
-					struct v4l2_dv_timings *timings)
+static int tvp7002_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				    struct v4l2_dv_timings *timings)
 {
 	int index;
-	int err = tvp7002_query_dv(sd, &index);
+	int err;
+
+	if (pad != 0)
+		return -EINVAL;
 
+	err = tvp7002_query_dv(sd, &index);
 	if (err)
 		return err;
 	*timings = tvp7002_timings[index].timings;
@@ -861,9 +871,6 @@ static const struct v4l2_subdev_core_ops tvp7002_core_ops = {
 
 /* Specific video subsystem operation handlers */
 static const struct v4l2_subdev_video_ops tvp7002_video_ops = {
-	.g_dv_timings = tvp7002_g_dv_timings,
-	.s_dv_timings = tvp7002_s_dv_timings,
-	.query_dv_timings = tvp7002_query_dv_timings,
 	.s_stream = tvp7002_s_stream,
 };
 
@@ -872,6 +879,9 @@ static const struct v4l2_subdev_pad_ops tvp7002_pad_ops = {
 	.enum_mbus_code = tvp7002_enum_mbus_code,
 	.get_fmt = tvp7002_get_pad_format,
 	.set_fmt = tvp7002_set_pad_format,
+	.g_dv_timings = tvp7002_g_dv_timings,
+	.s_dv_timings = tvp7002_s_dv_timings,
+	.query_dv_timings = tvp7002_query_dv_timings,
 	.enum_dv_timings = tvp7002_enum_dv_timings,
 };
 
@@ -1001,7 +1011,7 @@ static int tvp7002_probe(struct i2c_client *c)
 
 	/* Set registers according to default video mode */
 	timings = device->current_timings->timings;
-	error = tvp7002_s_dv_timings(sd, &timings);
+	error = tvp7002_s_dv_timings(sd, 0, &timings);
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	device->pad.flags = MEDIA_PAD_FL_SOURCE;
@@ -1060,7 +1070,7 @@ static void tvp7002_remove(struct i2c_client *c)
 
 /* I2C Device ID table */
 static const struct i2c_device_id tvp7002_id[] = {
-	{ "tvp7002", 0 },
+	{ "tvp7002" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tvp7002_id);
diff --git a/drivers/media/i2c/tw2804.c b/drivers/media/i2c/tw2804.c
index 6a2521e3a25c..3d154f4fb5f9 100644
--- a/drivers/media/i2c/tw2804.c
+++ b/drivers/media/i2c/tw2804.c
@@ -414,7 +414,7 @@ static void tw2804_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tw2804_id[] = {
-	{ "tw2804", 0 },
+	{ "tw2804" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tw2804_id);
diff --git a/drivers/media/i2c/tw9900.c b/drivers/media/i2c/tw9900.c
index bc7623ec46e5..53efdeaed1db 100644
--- a/drivers/media/i2c/tw9900.c
+++ b/drivers/media/i2c/tw9900.c
@@ -753,7 +753,7 @@ static const struct dev_pm_ops tw9900_pm_ops = {
 };
 
 static const struct i2c_device_id tw9900_id[] = {
-	{ "tw9900", 0 },
+	{ "tw9900" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tw9900_id);
diff --git a/drivers/media/i2c/tw9903.c b/drivers/media/i2c/tw9903.c
index 996be3960af3..b996a05e56f2 100644
--- a/drivers/media/i2c/tw9903.c
+++ b/drivers/media/i2c/tw9903.c
@@ -245,7 +245,7 @@ static void tw9903_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id tw9903_id[] = {
-	{ "tw9903", 0 },
+	{ "tw9903" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tw9903_id);
diff --git a/drivers/media/i2c/tw9906.c b/drivers/media/i2c/tw9906.c
index 25c625f6d6e4..6220f4fddbab 100644
--- a/drivers/media/i2c/tw9906.c
+++ b/drivers/media/i2c/tw9906.c
@@ -213,7 +213,7 @@ static void tw9906_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id tw9906_id[] = {
-	{ "tw9906", 0 },
+	{ "tw9906" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tw9906_id);
diff --git a/drivers/media/i2c/tw9910.c b/drivers/media/i2c/tw9910.c
index 905af98c7d53..f3e400304e04 100644
--- a/drivers/media/i2c/tw9910.c
+++ b/drivers/media/i2c/tw9910.c
@@ -212,11 +212,6 @@
  * structure
  */
 
-struct regval_list {
-	unsigned char reg_num;
-	unsigned char value;
-};
-
 struct tw9910_scale_ctrl {
 	char           *name;
 	unsigned short  width;
@@ -1001,7 +996,7 @@ static void tw9910_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tw9910_id[] = {
-	{ "tw9910", 0 },
+	{ "tw9910" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, tw9910_id);
diff --git a/drivers/media/i2c/uda1342.c b/drivers/media/i2c/uda1342.c
index da7bc4700bed..2e4540ee2df2 100644
--- a/drivers/media/i2c/uda1342.c
+++ b/drivers/media/i2c/uda1342.c
@@ -79,7 +79,7 @@ static void uda1342_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id uda1342_id[] = {
-	{ "uda1342", 0 },
+	{ "uda1342" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, uda1342_id);
@@ -95,4 +95,5 @@ static struct i2c_driver uda1342_driver = {
 
 module_i2c_driver(uda1342_driver);
 
+MODULE_DESCRIPTION("Philips UDA1342 audio codec driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/i2c/upd64031a.c b/drivers/media/i2c/upd64031a.c
index 54c2ba0ba375..9d0b72a213be 100644
--- a/drivers/media/i2c/upd64031a.c
+++ b/drivers/media/i2c/upd64031a.c
@@ -219,7 +219,7 @@ static void upd64031a_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id upd64031a_id[] = {
-	{ "upd64031a", 0 },
+	{ "upd64031a" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, upd64031a_id);
diff --git a/drivers/media/i2c/upd64083.c b/drivers/media/i2c/upd64083.c
index 2a820589a4cb..2e99ed5da42c 100644
--- a/drivers/media/i2c/upd64083.c
+++ b/drivers/media/i2c/upd64083.c
@@ -190,7 +190,7 @@ static void upd64083_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id upd64083_id[] = {
-	{ "upd64083", 0 },
+	{ "upd64083" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, upd64083_id);
diff --git a/drivers/media/i2c/vd55g1.c b/drivers/media/i2c/vd55g1.c
new file mode 100644
index 000000000000..25e2fc88a036
--- /dev/null
+++ b/drivers/media/i2c/vd55g1.c
@@ -0,0 +1,1965 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for VD55G1 global shutter sensor family driver
+ *
+ * Copyright (C) 2025 STMicroelectronics SA
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/unaligned.h>
+#include <linux/units.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Register Map */
+#define VD55G1_REG_MODEL_ID				CCI_REG32_LE(0x0000)
+#define VD55G1_MODEL_ID					0x53354731
+#define VD55G1_REG_REVISION				CCI_REG16_LE(0x0004)
+#define VD55G1_REVISION_CCB				0x2020
+#define VD55G1_REG_FWPATCH_REVISION			CCI_REG16_LE(0x0012)
+#define VD55G1_REG_FWPATCH_START_ADDR			CCI_REG8(0x2000)
+#define VD55G1_REG_SYSTEM_FSM				CCI_REG8(0x001c)
+#define VD55G1_SYSTEM_FSM_READY_TO_BOOT			0x01
+#define VD55G1_SYSTEM_FSM_SW_STBY			0x02
+#define VD55G1_SYSTEM_FSM_STREAMING			0x03
+#define VD55G1_REG_BOOT					CCI_REG8(0x0200)
+#define VD55G1_BOOT_PATCH_SETUP				2
+#define VD55G1_REG_STBY					CCI_REG8(0x0201)
+#define VD55G1_STBY_START_STREAM			1
+#define VD55G1_REG_STREAMING				CCI_REG8(0x0202)
+#define VD55G1_STREAMING_STOP_STREAM			1
+#define VD55G1_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
+#define VD55G1_REG_LINE_LENGTH				CCI_REG16_LE(0x0300)
+#define VD55G1_REG_ORIENTATION				CCI_REG8(0x0302)
+#define VD55G1_REG_FORMAT_CTRL				CCI_REG8(0x030a)
+#define VD55G1_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
+#define VD55G1_REG_ISL_ENABLE				CCI_REG16_LE(0x326)
+#define VD55G1_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
+#define VD55G1_REG_MIPI_DATA_RATE			CCI_REG32_LE(0x0224)
+#define VD55G1_REG_PATGEN_CTRL				CCI_REG16_LE(0x0304)
+#define VD55G1_PATGEN_TYPE_SHIFT			4
+#define VD55G1_PATGEN_ENABLE				BIT(0)
+#define VD55G1_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x0501)
+#define VD55G1_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x0502)
+#define VD55G1_REG_MANUAL_DIGITAL_GAIN			CCI_REG16_LE(0x0504)
+#define VD55G1_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x00e8)
+#define VD55G1_REG_APPLIED_ANALOG_GAIN			CCI_REG16_LE(0x00ea)
+#define VD55G1_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x00ec)
+#define VD55G1_REG_AE_FORCE_COLDSTART			CCI_REG8(0x0308)
+#define VD55G1_REG_AE_COLDSTART_EXP_TIME		CCI_REG32_LE(0x0374)
+#define VD55G1_REG_READOUT_CTRL				CCI_REG8(0x052e)
+#define VD55G1_READOUT_CTRL_BIN_MODE_NORMAL		0
+#define VD55G1_READOUT_CTRL_BIN_MODE_DIGITAL_X2		1
+#define VD55G1_REG_DUSTER_CTRL				CCI_REG8(0x03ea)
+#define VD55G1_DUSTER_ENABLE				BIT(0)
+#define VD55G1_DUSTER_DISABLE				0
+#define VD55G1_DUSTER_DYN_ENABLE			BIT(1)
+#define VD55G1_DUSTER_RING_ENABLE			BIT(4)
+#define VD55G1_REG_AE_TARGET_PERCENTAGE			CCI_REG8(0x0486)
+#define VD55G1_REG_NEXT_CTX				CCI_REG16_LE(0x03e4)
+#define VD55G1_REG_EXPOSURE_USE_CASES			CCI_REG8(0x0312)
+#define VD55G1_EXPOSURE_USE_CASES_MULTI_CONTEXT		BIT(2)
+#define VD55G1_REG_EXPOSURE_MAX_COARSE			CCI_REG16_LE(0x0372)
+#define VD55G1_EXPOSURE_MAX_COARSE_DEF			0x7fff
+#define VD55G1_EXPOSURE_MAX_COARSE_SUB			446
+#define VD55G1_REG_CTX_REPEAT_COUNT_CTX0		CCI_REG16_LE(0x03dc)
+#define VD55G1_REG_CTX_REPEAT_COUNT_CTX1		CCI_REG16_LE(0x03de)
+
+#define VD55G1_REG_EXP_MODE(ctx) \
+	CCI_REG8(0x0500 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_FRAME_LENGTH(ctx) \
+	CCI_REG32_LE(0x050c + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_X_START(ctx) \
+	CCI_REG16_LE(0x0514 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_X_WIDTH(ctx) \
+	CCI_REG16_LE(0x0516 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_Y_START(ctx) \
+	CCI_REG16_LE(0x0510 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_Y_HEIGHT(ctx) \
+	CCI_REG16_LE(0x0512 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_REG_GPIO_0_CTRL(ctx) \
+	CCI_REG8(0x051d + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_GPIO_MODE_FSYNC_OUT			0x00
+#define VD55G1_GPIO_MODE_IN				0x01
+#define VD55G1_GPIO_MODE_STROBE				0x02
+#define VD55G1_REG_VT_MODE(ctx) \
+	CCI_REG8(0x0536 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_VT_MODE_NORMAL				0
+#define VD55G1_VT_MODE_SUBTRACTION			1
+#define VD55G1_REG_MASK_FRAME_CTRL(ctx) \
+	CCI_REG8(0x0537 + VD55G1_CTX_OFFSET * (ctx))
+#define VD55G1_MASK_FRAME_CTRL_OUTPUT			0
+#define VD55G1_MASK_FRAME_CTRL_MASK			1
+#define VD55G1_REG_EXPOSURE_INSTANCE(ctx) \
+	CCI_REG32_LE(0x52D + VD55G1_CTX_OFFSET * (ctx))
+
+#define VD55G1_WIDTH					804
+#define VD55G1_HEIGHT					704
+#define VD55G1_DEFAULT_MODE				0
+#define VD55G1_NB_GPIOS					4
+#define VD55G1_MEDIA_BUS_FMT_DEF			MEDIA_BUS_FMT_Y8_1X8
+#define VD55G1_DGAIN_DEF				256
+#define VD55G1_AGAIN_DEF				19
+#define VD55G1_EXPO_MAX_TERM				64
+#define VD55G1_EXPO_DEF					500
+#define VD55G1_LINE_LENGTH_MIN				1128
+#define VD55G1_LINE_LENGTH_SUB_MIN			1344
+#define VD55G1_VBLANK_MIN				86
+#define VD55G1_VBLANK_MAX				0xffff
+#define VD55G1_FRAME_LENGTH_DEF				1860 /* 60 fps */
+#define VD55G1_MIPI_MARGIN				900
+#define VD55G1_CTX_OFFSET				0x50
+#define VD55G1_FWPATCH_REVISION_MAJOR			2
+#define VD55G1_FWPATCH_REVISION_MINOR			9
+#define VD55G1_XCLK_FREQ_MIN				(6 * HZ_PER_MHZ)
+#define VD55G1_XCLK_FREQ_MAX				(27 * HZ_PER_MHZ)
+#define VD55G1_MIPI_RATE_MIN				(250 * HZ_PER_MHZ)
+#define VD55G1_MIPI_RATE_MAX				(1200 * HZ_PER_MHZ)
+
+static const u8 patch_array[] = {
+	0x44, 0x03, 0x09, 0x02, 0xe6, 0x01, 0x42, 0x00, 0xea, 0x01, 0x42, 0x00,
+	0xf0, 0x01, 0x42, 0x00, 0xe6, 0x01, 0x42, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x4c, 0x00, 0x00, 0xfa, 0x68, 0x40, 0x00, 0xe8,
+	0x09, 0xbe, 0x4c, 0x08, 0x00, 0xf2, 0x93, 0xdd, 0x1c, 0x00, 0xc0, 0xe2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa, 0x6b, 0x80, 0x98, 0x7f,
+	0xfc, 0xef, 0x11, 0xc1, 0x0f, 0x82, 0x69, 0xbe, 0x0f, 0xac, 0x58, 0x40,
+	0x00, 0xe8, 0x0c, 0x0c, 0x00, 0xf2, 0x93, 0xdd, 0x1c, 0x00, 0x40, 0xe3,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x84, 0xfa, 0x46, 0x0e, 0xe8, 0xe0,
+	0x08, 0xde, 0x4a, 0x40, 0x84, 0xe0, 0xa5, 0x86, 0xa8, 0x7d, 0xfc, 0xef,
+	0x6b, 0x80, 0x01, 0xbf, 0x28, 0x77, 0x0c, 0xef, 0x0b, 0x0e, 0x21, 0x78,
+	0x06, 0xc0, 0x0b, 0xa5, 0xb5, 0x84, 0x06, 0x42, 0x98, 0xe1, 0x01, 0x81,
+	0x01, 0x42, 0x38, 0xe0, 0x0c, 0xc4, 0x0e, 0x84, 0x46, 0x02, 0x84, 0xe0,
+	0x0c, 0x84, 0x11, 0x81, 0x21, 0x81, 0x31, 0x81, 0x41, 0x81, 0x51, 0x81,
+	0xc1, 0x81, 0x05, 0x83, 0x0c, 0x0c, 0x84, 0xf2, 0x93, 0xdd, 0x06, 0x40,
+	0x98, 0xe1, 0xc8, 0x80, 0x58, 0x82, 0x48, 0xc0, 0x38, 0xc2, 0x29, 0x00,
+	0x10, 0xe0, 0x19, 0x00, 0x14, 0xe0, 0x09, 0x00, 0x38, 0xe0, 0x5f, 0xb8,
+	0x5f, 0xa8, 0x5f, 0xa6, 0x5f, 0xa4, 0x5f, 0xa2, 0x5f, 0xa0, 0x56, 0x41,
+	0x98, 0xe1, 0x18, 0x82, 0x28, 0x80, 0x38, 0xc0, 0x5f, 0xa2, 0x19, 0x00,
+	0x20, 0xf8, 0x5f, 0xa4, 0x28, 0xc2, 0x5f, 0xa6, 0x39, 0x00, 0x10, 0xe0,
+	0x5f, 0xa2, 0x19, 0x00, 0x14, 0xe0, 0x5f, 0xa4, 0x29, 0x00, 0x18, 0xe0,
+	0x5f, 0xa6, 0x39, 0x00, 0x40, 0xe0, 0x5f, 0xa2, 0x19, 0x00, 0x44, 0xe0,
+	0x5f, 0xa4, 0x29, 0x00, 0x1c, 0xe0, 0x5f, 0xa6, 0x39, 0x00, 0x38, 0xe0,
+	0x5f, 0xa2, 0x19, 0x00, 0x20, 0xe0, 0x5f, 0xa4, 0x29, 0x00, 0x24, 0xe0,
+	0x5f, 0xa6, 0x39, 0x00, 0x28, 0xe0, 0x5f, 0xa2, 0x19, 0x00, 0x2c, 0xe0,
+	0x5f, 0xa4, 0x29, 0x00, 0x30, 0xe0, 0x5f, 0xa6, 0x09, 0x00, 0x34, 0xe0,
+	0x5f, 0xa2, 0x5f, 0xa4, 0x5f, 0xa0, 0x4a, 0x0a, 0xfc, 0xfb, 0xe5, 0x82,
+	0x08, 0xde, 0x4a, 0x40, 0x88, 0xe0, 0xf6, 0x40, 0x00, 0xe0, 0x01, 0x4e,
+	0x99, 0x78, 0x0a, 0xc0, 0x85, 0x80, 0x98, 0x40, 0x00, 0xe8, 0x35, 0x81,
+	0xa8, 0x40, 0x00, 0xe8, 0x0b, 0x8c, 0x0c, 0x0c, 0x84, 0xf2, 0xd5, 0xed,
+	0x83, 0xc1, 0x13, 0xc5, 0x93, 0xdd, 0xc3, 0xc1, 0x83, 0xc1, 0x13, 0xc3,
+	0x93, 0xdd, 0xc3, 0xc1, 0x4c, 0x04, 0x04, 0xfa, 0xc6, 0x0f, 0x94, 0xe0,
+	0x19, 0x0e, 0xc9, 0x65, 0x01, 0xc0, 0x28, 0xde, 0x0a, 0x42, 0x80, 0xe0,
+	0x24, 0x02, 0x00, 0xfc, 0x16, 0xde, 0xa5, 0x8a, 0x19, 0x00, 0xb8, 0xe0,
+	0x10, 0x02, 0x0c, 0xec, 0x1d, 0xe6, 0x14, 0x02, 0x88, 0x80, 0x4e, 0x04,
+	0x01, 0x00, 0x10, 0x80, 0x25, 0x02, 0x08, 0x9c, 0x86, 0x02, 0x00, 0x80,
+	0x08, 0x44, 0x00, 0x98, 0x55, 0x81, 0x11, 0x85, 0x45, 0x81, 0x11, 0x89,
+	0x25, 0x81, 0x11, 0x83, 0x2b, 0x00, 0x24, 0xe0, 0x64, 0xc2, 0x0b, 0x84,
+	0x08, 0x51, 0x00, 0xef, 0x2b, 0x80, 0x01, 0x83, 0x1b, 0x8c, 0x38, 0x7d,
+	0x5c, 0xef, 0x18, 0xde, 0x0b, 0xa1, 0x25, 0x82, 0x0b, 0x0e, 0x88, 0xf9,
+	0x0a, 0x00, 0x00, 0xe8, 0x10, 0x42, 0x04, 0x9c, 0x11, 0x4e, 0x0c, 0x80,
+	0x10, 0x40, 0x04, 0xf0, 0x4e, 0x05, 0x01, 0x60, 0x10, 0xc0, 0x06, 0x88,
+	0x10, 0x40, 0xf8, 0xf3, 0x06, 0xde, 0x4c, 0x0c, 0x04, 0xf2, 0x93, 0xdd,
+	0x0c, 0x04, 0x1c, 0xfe, 0xf6, 0x0f, 0x94, 0xe0, 0x38, 0x9c, 0x46, 0x51,
+	0xfc, 0xe0, 0x46, 0x49, 0x38, 0xe2, 0x30, 0x46, 0xf8, 0xf3, 0x36, 0x9c,
+	0xc6, 0x46, 0x0c, 0xe1, 0x34, 0x8c, 0x94, 0xa0, 0x4e, 0xa0, 0x39, 0x06,
+	0x80, 0xe0, 0x4a, 0x46, 0x94, 0xe0, 0x05, 0x8c, 0x6a, 0x40, 0x80, 0xe0,
+	0x2c, 0x0c, 0x00, 0xe2, 0x0b, 0x8c, 0xb8, 0x7c, 0x5c, 0xef, 0x0b, 0x8c,
+	0x9e, 0xa0, 0xf8, 0x40, 0x60, 0xef, 0x0b, 0xa1, 0x5a, 0x40, 0x80, 0xe0,
+	0x65, 0x88, 0x28, 0x02, 0x01, 0x40, 0x00, 0x80, 0x2a, 0x42, 0x9c, 0xe1,
+	0x28, 0x49, 0x60, 0xef, 0x96, 0x4d, 0x9c, 0xe1, 0x01, 0x81, 0x06, 0x98,
+	0xd5, 0x81, 0x09, 0x0e, 0xa1, 0x64, 0x01, 0xc0, 0x4a, 0x40, 0x88, 0xe0,
+	0x85, 0x80, 0xb8, 0x77, 0xfc, 0xef, 0x35, 0x81, 0xc8, 0x77, 0xfc, 0xef,
+	0x08, 0x98, 0x4a, 0x00, 0xfc, 0xfb, 0x55, 0xfc, 0xe8, 0x4a, 0x60, 0xef,
+	0x1a, 0x44, 0x9c, 0xe1, 0x35, 0x81, 0x1a, 0x4e, 0x9c, 0xe9, 0x1c, 0x00,
+	0x00, 0xe2, 0x0c, 0x0c, 0x1c, 0xf6, 0x93, 0xdd, 0x0d, 0xc3, 0x1a, 0x41,
+	0x08, 0xe4, 0x0a, 0x40, 0x84, 0xe1, 0x0c, 0x00, 0x00, 0xe2, 0x93, 0xdd,
+	0x4c, 0x04, 0x1c, 0xfa, 0x86, 0x52, 0xec, 0xe1, 0x08, 0xa6, 0x65, 0x12,
+	0x24, 0xf8, 0x0e, 0x02, 0x99, 0x7a, 0x00, 0xc0, 0x00, 0x40, 0xa0, 0xf3,
+	0x06, 0xa6, 0x0b, 0x8c, 0x08, 0x49, 0x00, 0xef, 0x85, 0x12, 0x28, 0xf8,
+	0x02, 0x02, 0xfc, 0xed, 0xf6, 0x47, 0xfd, 0x6f, 0xe0, 0xff, 0x04, 0xe2,
+	0x14, 0x04, 0xc0, 0xe0, 0x0f, 0x86, 0x2f, 0xa0, 0x0b, 0x8c, 0x2e, 0xe2,
+	0x08, 0x48, 0x00, 0xef, 0x86, 0x02, 0x84, 0xfe, 0x0e, 0x05, 0x09, 0x7d,
+	0x00, 0xc0, 0x05, 0x52, 0x08, 0xf8, 0x18, 0x7d, 0xfc, 0xef, 0x4a, 0x40,
+	0x80, 0xe0, 0x09, 0x12, 0x04, 0xc0, 0x65, 0x12, 0x20, 0xf8, 0x00, 0x40,
+	0x40, 0xdc, 0x01, 0x52, 0x04, 0xc0, 0x0e, 0x00, 0x41, 0x78, 0xf5, 0xc5,
+	0x6d, 0xc0, 0xb5, 0x82, 0x05, 0x10, 0x10, 0xe0, 0x11, 0xf1, 0x0f, 0x82,
+	0x05, 0x50, 0x10, 0xe0, 0x05, 0x10, 0x10, 0xe0, 0xfe, 0x02, 0xf0, 0xff,
+	0x0f, 0x82, 0x85, 0x83, 0x15, 0x10, 0x10, 0xe0, 0x16, 0x00, 0x91, 0x6e,
+	0x69, 0xcd, 0x21, 0xf1, 0x6d, 0xc1, 0x01, 0x83, 0x2f, 0x82, 0x26, 0x00,
+	0x00, 0x80, 0x2f, 0xa0, 0x25, 0x50, 0x10, 0xe0, 0x05, 0x10, 0x10, 0xe0,
+	0x11, 0xa1, 0xfe, 0x04, 0xf0, 0xff, 0x06, 0x42, 0x00, 0x80, 0x0f, 0x84,
+	0x0f, 0xa2, 0x05, 0x50, 0x10, 0xe0, 0x16, 0x00, 0x91, 0x6e, 0x69, 0xcd,
+	0x6d, 0xc1, 0x71, 0x8d, 0x16, 0x00, 0x79, 0x61, 0x2d, 0xcb, 0x86, 0x0e,
+	0x00, 0x80, 0x6d, 0xc1, 0x56, 0x0e, 0x00, 0xc0, 0x0b, 0x8c, 0x1b, 0x8e,
+	0x71, 0x52, 0x0c, 0xf8, 0x08, 0x43, 0x00, 0xef, 0x05, 0x52, 0x14, 0xf8,
+	0x15, 0x10, 0x28, 0xe0, 0x70, 0x04, 0x04, 0xec, 0x31, 0xe1, 0x29, 0x9e,
+	0x1f, 0x86, 0x1f, 0xa4, 0x15, 0x50, 0x28, 0xe0, 0x86, 0x42, 0x3c, 0xe0,
+	0x0e, 0x04, 0x9d, 0x64, 0x9b, 0xc2, 0x05, 0x52, 0x1c, 0xf8, 0x78, 0xa6,
+	0x48, 0x77, 0xfc, 0xef, 0x4a, 0x40, 0x80, 0xe0, 0x70, 0x4e, 0x10, 0xdc,
+	0x1e, 0x00, 0x81, 0x70, 0xeb, 0xcb, 0x70, 0x4e, 0xec, 0x93, 0x6d, 0xc1,
+	0x11, 0x85, 0x36, 0x02, 0x00, 0x80, 0x76, 0xa6, 0x11, 0x52, 0x10, 0xf8,
+	0x05, 0x10, 0x40, 0xe0, 0xfe, 0x47, 0x0c, 0xff, 0x14, 0x04, 0xa0, 0xe0,
+	0x0f, 0x86, 0x0f, 0xa4, 0x05, 0x50, 0x40, 0xe0, 0x05, 0x10, 0x28, 0xe0,
+	0xfe, 0x47, 0xfd, 0x7f, 0xe3, 0xff, 0x14, 0x04, 0xd0, 0xe0, 0x0f, 0x86,
+	0x2f, 0xa0, 0x20, 0x00, 0x01, 0x6c, 0x00, 0xd0, 0x05, 0x50, 0x28, 0xe0,
+	0x0b, 0x8c, 0xf8, 0x7e, 0xfc, 0xee, 0x0e, 0x03, 0x59, 0x78, 0xf5, 0xc5,
+	0x0d, 0xc2, 0x05, 0x52, 0x0c, 0xf8, 0x08, 0xa6, 0x46, 0x42, 0xb4, 0xe0,
+	0x18, 0x84, 0x00, 0x40, 0xf4, 0x93, 0x00, 0x40, 0x08, 0xdc, 0x1b, 0xa1,
+	0x06, 0xa6, 0x05, 0x10, 0x40, 0x80, 0x04, 0x00, 0x50, 0x9c, 0x65, 0x8a,
+	0x05, 0x10, 0x44, 0xe0, 0xf6, 0x43, 0xfd, 0x6f, 0x00, 0xf8, 0x0f, 0x82,
+	0x06, 0x02, 0x01, 0x60, 0x1e, 0xc0, 0x0f, 0xa2, 0x05, 0x50, 0x44, 0xe0,
+	0x05, 0x10, 0x44, 0xe0, 0x0e, 0x02, 0x00, 0xf8, 0x0f, 0x82, 0x09, 0xf6,
+	0x05, 0x50, 0x44, 0xe0, 0x05, 0x10, 0x40, 0xe0, 0x04, 0x00, 0x54, 0xfc,
+	0x05, 0x50, 0x40, 0xe0, 0x05, 0x10, 0x40, 0xe0, 0x04, 0x00, 0xcc, 0xfc,
+	0x05, 0x50, 0x40, 0xe0, 0x05, 0x10, 0x40, 0xe0, 0x04, 0x00, 0x4c, 0xfc,
+	0x05, 0x50, 0x40, 0xe0, 0x05, 0x10, 0x40, 0xe0, 0x04, 0x00, 0xd0, 0xfc,
+	0x05, 0x50, 0x40, 0xe0, 0x4c, 0x0c, 0x1c, 0xf2, 0x93, 0xdd, 0xc3, 0xc1,
+	0xc6, 0x40, 0xfc, 0xe0, 0x04, 0x80, 0xc6, 0x44, 0x0c, 0xe1, 0x15, 0x04,
+	0x0c, 0xf8, 0x0a, 0x80, 0x06, 0x07, 0x04, 0xe0, 0x03, 0x42, 0x48, 0xe1,
+	0x46, 0x02, 0x40, 0xe2, 0x08, 0xc6, 0x44, 0x88, 0x06, 0x46, 0x0e, 0xe0,
+	0x86, 0x01, 0x84, 0xe0, 0x33, 0x80, 0x39, 0x06, 0xd8, 0xef, 0x0a, 0x46,
+	0x80, 0xe0, 0x31, 0xbf, 0x06, 0x06, 0x00, 0xc0, 0x31, 0x48, 0x60, 0xe0,
+	0x34, 0x88, 0x49, 0x06, 0x40, 0xe1, 0x40, 0x48, 0x7c, 0xf3, 0x41, 0x46,
+	0x40, 0xe1, 0x24, 0x8a, 0x39, 0x04, 0x10, 0xe0, 0x39, 0xc2, 0x31, 0x44,
+	0x10, 0xe0, 0x14, 0xc4, 0x1b, 0xa5, 0x11, 0x83, 0x11, 0x40, 0x25, 0x6a,
+	0x01, 0xc0, 0x08, 0x5c, 0x00, 0xda, 0x15, 0x00, 0xcc, 0xe0, 0x25, 0x00,
+	0xf8, 0xe0, 0x1b, 0x85, 0x08, 0x5c, 0x00, 0x9a, 0x4e, 0x03, 0x01, 0x60,
+	0x10, 0xc0, 0x29, 0x00, 0x1c, 0xe4, 0x18, 0x84, 0x20, 0x44, 0xf8, 0xf3,
+	0x2f, 0xa2, 0x21, 0x40, 0x1c, 0xe4, 0x93, 0xdd, 0x0c, 0x00, 0x80, 0xfa,
+	0x15, 0x00, 0x3c, 0xe0, 0x21, 0x81, 0x31, 0x85, 0x21, 0x42, 0x60, 0xe0,
+	0x15, 0x00, 0x44, 0xe0, 0x31, 0x42, 0x40, 0xe1, 0x15, 0x00, 0x34, 0xe0,
+	0x21, 0x42, 0x20, 0xe0, 0x15, 0x00, 0x34, 0xe0, 0xd6, 0x04, 0x10, 0xe0,
+	0x23, 0x42, 0x30, 0xe0, 0x15, 0x00, 0x34, 0xe0, 0x86, 0x44, 0x04, 0xe0,
+	0x23, 0x42, 0x38, 0xe0, 0x05, 0x00, 0x30, 0xe0, 0xc6, 0x02, 0x08, 0xe0,
+	0x13, 0x40, 0x10, 0xe3, 0xe8, 0x56, 0x40, 0xef, 0x06, 0x40, 0x0c, 0xe1,
+	0x04, 0x80, 0x06, 0x02, 0x94, 0xe0, 0x2b, 0x02, 0xc4, 0xea, 0x3b, 0x00,
+	0x78, 0xe2, 0x20, 0x44, 0xfd, 0x73, 0x07, 0xc0, 0x30, 0x46, 0x01, 0x70,
+	0xf8, 0xc0, 0x3f, 0xa4, 0x33, 0x40, 0x78, 0xe2, 0x0a, 0x84, 0x0c, 0x08,
+	0x80, 0xf2, 0xf8, 0x3b, 0x3c, 0xff, 0xc3, 0xc1, 0x06, 0x40, 0x0c, 0xe1,
+	0x04, 0x80, 0x1b, 0x00, 0x40, 0xe4, 0x19, 0xc2, 0x13, 0x40, 0x40, 0xe4,
+	0x1b, 0x00, 0x40, 0xe4, 0x19, 0xc4, 0x13, 0x40, 0x40, 0xe4, 0x93, 0xdd,
+	0xc6, 0x43, 0xec, 0xe0, 0x46, 0x41, 0xfc, 0xe0, 0x24, 0x84, 0x04, 0x80,
+	0x31, 0x81, 0x4a, 0x44, 0x80, 0xe0, 0x86, 0x44, 0x0c, 0xe1, 0x09, 0x00,
+	0x6c, 0xe0, 0xc4, 0x8a, 0x8e, 0x47, 0xfc, 0x9f, 0x01, 0x42, 0x51, 0x78,
+	0x0c, 0xc0, 0x31, 0x58, 0x90, 0xe0, 0x34, 0x8a, 0x41, 0xbf, 0x06, 0x08,
+	0x00, 0xc0, 0x41, 0x46, 0xa0, 0xe0, 0x34, 0x8a, 0x51, 0x81, 0xf6, 0x0b,
+	0x00, 0xc0, 0x51, 0x46, 0xd0, 0xe0, 0x34, 0x8a, 0x01, 0xbf, 0x51, 0x46,
+	0xe0, 0xe0, 0x44, 0x84, 0x0a, 0x48, 0x84, 0xe0, 0x75, 0x86, 0x54, 0xca,
+	0x49, 0x88, 0x44, 0x06, 0x88, 0xe1, 0x36, 0x94, 0x4a, 0x46, 0x80, 0xe0,
+	0x34, 0xca, 0x47, 0xc6, 0x11, 0x8d, 0x41, 0x46, 0xd0, 0xe0, 0x34, 0x88,
+	0x76, 0x02, 0x00, 0xc0, 0x06, 0x00, 0x00, 0xc0, 0x16, 0x8c, 0x14, 0x88,
+	0x01, 0x42, 0xc0, 0xe1, 0x01, 0x42, 0xe0, 0xe1, 0x01, 0x42, 0xf0, 0xe1,
+	0x93, 0xdd, 0x34, 0xca, 0x41, 0x85, 0x46, 0x8c, 0x34, 0xca, 0x06, 0x48,
+	0x00, 0xe0, 0x41, 0x46, 0xd0, 0xe0, 0x34, 0x88, 0x41, 0x83, 0x46, 0x8c,
+	0x34, 0x88, 0x01, 0x46, 0xc0, 0xe1, 0x01, 0x46, 0xe0, 0xe1, 0x01, 0x46,
+	0xf0, 0xe1, 0x09, 0x02, 0x20, 0xe0, 0x14, 0xca, 0x03, 0x42, 0x58, 0xe0,
+	0x93, 0xdd, 0xc3, 0xc1, 0x4c, 0x04, 0x04, 0xfa, 0x46, 0x4e, 0x08, 0xe1,
+	0x06, 0x4c, 0x0c, 0xe1, 0x0a, 0x9e, 0x14, 0x98, 0x05, 0x42, 0x44, 0xe0,
+	0x10, 0x00, 0xe1, 0x65, 0x03, 0xc0, 0x78, 0x41, 0x00, 0xe8, 0x08, 0x9c,
+	0x0b, 0xa1, 0x04, 0x98, 0x06, 0x02, 0x10, 0x80, 0x13, 0x40, 0xf8, 0x86,
+	0x65, 0x82, 0x00, 0x00, 0xe1, 0x65, 0x03, 0xc0, 0xa8, 0x40, 0x00, 0xe8,
+	0x14, 0x98, 0x04, 0x00, 0xa0, 0xfc, 0x03, 0x42, 0x00, 0xe7, 0x4c, 0x0c,
+	0x04, 0xf2, 0x93, 0xdd, 0x0a, 0x80, 0x93, 0xdd, 0x0c, 0x04, 0x00, 0xfa,
+	0x06, 0x02, 0xec, 0xe1, 0x64, 0x84, 0x15, 0x0c, 0x2c, 0xe0, 0x14, 0x02,
+	0xa0, 0xfc, 0x15, 0x4c, 0x2c, 0xe0, 0xd8, 0x40, 0x00, 0xe8, 0x14, 0xd8,
+	0x09, 0x82, 0x14, 0x02, 0x00, 0xfc, 0x1f, 0xa0, 0x1e, 0xd8, 0x01, 0x85,
+	0x0c, 0x0c, 0x00, 0xf2, 0xe8, 0x32, 0x2c, 0xff, 0x93, 0xdd, 0xc3, 0xc1,
+	0x0c, 0x04, 0x00, 0xfa, 0x6b, 0x80, 0xf6, 0x01, 0x94, 0xe0, 0x08, 0x80,
+	0x4a, 0x40, 0x80, 0xe0, 0x45, 0x86, 0x06, 0x40, 0x0c, 0xe1, 0x04, 0x80,
+	0xc6, 0x02, 0x40, 0xe2, 0x09, 0x00, 0xd0, 0xe0, 0x14, 0x84, 0x1b, 0xa5,
+	0x15, 0x84, 0x07, 0xc5, 0x09, 0x82, 0x18, 0x41, 0x00, 0xe8, 0x46, 0x43,
+	0xfc, 0xe0, 0x14, 0x84, 0x19, 0x02, 0xd8, 0xe0, 0x19, 0x82, 0x0b, 0x83,
+	0x16, 0x00, 0x00, 0xc0, 0x01, 0x4c, 0x00, 0xc0, 0x0c, 0x0c, 0x00, 0xf2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x4a, 0x00, 0x00, 0xe0, 0x0c, 0x00, 0x00, 0xe2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x46, 0x40, 0x84, 0xe0, 0x11, 0xaf, 0x13, 0x40,
+	0x6c, 0xec, 0x11, 0xb3, 0x13, 0x40, 0x70, 0xec, 0xc6, 0x43, 0xf0, 0xe0,
+	0x13, 0x40, 0xdc, 0xec, 0xc6, 0x02, 0x24, 0xe0, 0x1c, 0x80, 0x93, 0xdd,
+	0x4c, 0x00, 0x00, 0xfa, 0xc8, 0x60, 0x7c, 0xef, 0xe8, 0x61, 0x7c, 0xef,
+	0x28, 0x7e, 0x80, 0xef, 0xc6, 0x40, 0x98, 0xe1, 0x11, 0x83, 0x16, 0x80,
+	0x46, 0x01, 0x10, 0xe1, 0x11, 0x81, 0x16, 0x80, 0x4c, 0x08, 0x00, 0xf2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x0c, 0xfa, 0x6b, 0x80, 0x04, 0x98,
+	0x7b, 0x82, 0x56, 0x42, 0xb4, 0xe0, 0x88, 0x84, 0x05, 0x00, 0x10, 0xe0,
+	0x09, 0x86, 0x0b, 0xa5, 0x46, 0x02, 0x00, 0x80, 0x06, 0x05, 0x00, 0x80,
+	0x25, 0x82, 0x0b, 0xa3, 0xa5, 0x80, 0x0b, 0xa1, 0x06, 0x00, 0xf4, 0xef,
+	0xd5, 0x84, 0x11, 0x85, 0x21, 0x91, 0x0b, 0x8e, 0x88, 0x74, 0x10, 0xef,
+	0x0b, 0xa1, 0xf5, 0x82, 0x0a, 0x9e, 0x1a, 0x9c, 0x24, 0x98, 0x07, 0xe0,
+	0x0f, 0xa2, 0x0e, 0xca, 0x0a, 0xde, 0x1a, 0xdc, 0x24, 0x98, 0x03, 0xb0,
+	0x07, 0xe0, 0x0f, 0xa2, 0x0e, 0xc8, 0x01, 0x81, 0x0c, 0x0c, 0x0c, 0xf2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x7c, 0xfa, 0x46, 0x42, 0x9c, 0xe0,
+	0x0b, 0x02, 0x04, 0xe3, 0xf0, 0x1e, 0x30, 0xec, 0x0b, 0xa3, 0x35, 0x96,
+	0x8e, 0x01, 0x01, 0x60, 0x10, 0xc0, 0x0e, 0xfc, 0xc6, 0x05, 0xd0, 0xe1,
+	0x0b, 0x82, 0x31, 0x81, 0x10, 0x16, 0x00, 0xe5, 0x20, 0x10, 0x20, 0xe7,
+	0x0e, 0xbe, 0xb5, 0x85, 0x94, 0xfc, 0xa4, 0xbe, 0x82, 0x4c, 0x9c, 0xf0,
+	0x05, 0x0c, 0x40, 0xe0, 0x11, 0x89, 0x93, 0x8e, 0xa3, 0x8e, 0x58, 0x44,
+	0x00, 0xe8, 0x15, 0x0c, 0xc0, 0xf8, 0x04, 0x0c, 0x80, 0xfb, 0x0c, 0xed,
+	0x0b, 0x82, 0x1b, 0x8c, 0x48, 0x44, 0x00, 0xe8, 0x15, 0x10, 0x1c, 0xfc,
+	0x0e, 0xa8, 0x0b, 0x82, 0x1b, 0x8c, 0xd8, 0x43, 0x00, 0xe8, 0x71, 0x88,
+	0x0e, 0xa4, 0x0a, 0x0e, 0x40, 0xe0, 0x35, 0xf8, 0x04, 0xbe, 0x14, 0xbc,
+	0x81, 0xa0, 0x03, 0x8e, 0x0e, 0xbe, 0x04, 0xfc, 0x11, 0x82, 0x3b, 0x82,
+	0x03, 0x8e, 0x0e, 0xfc, 0x3b, 0xa9, 0x06, 0x0e, 0x00, 0xc0, 0x35, 0x5e,
+	0x00, 0xc0, 0xd5, 0xfa, 0xc6, 0x01, 0xd0, 0xe1, 0x7b, 0x80, 0x04, 0x9e,
+	0x11, 0x91, 0x98, 0x41, 0x00, 0xe8, 0x24, 0x9c, 0x46, 0x42, 0x9c, 0xe0,
+	0x6b, 0x82, 0x03, 0x4c, 0xc4, 0xe0, 0x11, 0x91, 0x0b, 0x84, 0xf8, 0x40,
+	0x00, 0xe8, 0x19, 0x0e, 0x20, 0xe5, 0x03, 0x4c, 0xc0, 0xe0, 0x0b, 0x82,
+	0x08, 0x72, 0xfc, 0xef, 0x01, 0x4c, 0x24, 0xf9, 0xf1, 0x98, 0x0c, 0x0c,
+	0x7c, 0xf2, 0x93, 0xdd, 0x4c, 0x00, 0x00, 0xfa, 0x48, 0x65, 0x2c, 0xef,
+	0x4c, 0x08, 0x00, 0xf2, 0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x00, 0xfa,
+	0x6b, 0x82, 0x78, 0x6e, 0xfc, 0xee, 0x46, 0x42, 0xec, 0xe0, 0x24, 0x84,
+	0x24, 0x02, 0x80, 0xfa, 0x1d, 0xcc, 0x11, 0x83, 0xf5, 0x82, 0x24, 0x02,
+	0xa0, 0xe1, 0x14, 0x02, 0x80, 0xfa, 0x1d, 0xcc, 0x11, 0x85, 0x15, 0x82,
+	0x27, 0xe1, 0x24, 0x02, 0x80, 0xfa, 0x1d, 0xcc, 0x11, 0x89, 0x86, 0x02,
+	0x00, 0x80, 0x0c, 0x0c, 0x00, 0xf2, 0x18, 0x17, 0xfc, 0xfe, 0xc3, 0xc1,
+	0x0c, 0x04, 0x00, 0xfa, 0x06, 0x41, 0x8c, 0xe0, 0x1b, 0x00, 0xec, 0xe4,
+	0x1b, 0xa3, 0x75, 0x84, 0x11, 0x81, 0x8e, 0x05, 0x01, 0x60, 0x10, 0xc0,
+	0x00, 0x06, 0xc0, 0xe5, 0x95, 0x81, 0x44, 0x88, 0x1d, 0xee, 0x75, 0x80,
+	0x4e, 0xc1, 0x25, 0x81, 0x4e, 0xcd, 0x21, 0x88, 0x11, 0x82, 0x0a, 0x02,
+	0x40, 0xe0, 0xd5, 0xfc, 0x56, 0x00, 0x00, 0xe1, 0x18, 0x80, 0x1b, 0xa1,
+	0xc5, 0x84, 0x08, 0x82, 0x4a, 0x00, 0xfc, 0xfb, 0x45, 0x84, 0x86, 0x4d,
+	0x84, 0xe1, 0x04, 0x98, 0x05, 0x00, 0x10, 0xe0, 0x4a, 0x40, 0x80, 0xe0,
+	0x45, 0x82, 0x11, 0x81, 0x0b, 0x8c, 0x58, 0x76, 0x28, 0xef, 0x0b, 0x8c,
+	0x0c, 0x0c, 0x00, 0xf2, 0x88, 0x35, 0x28, 0xff, 0x0c, 0x0c, 0x00, 0xf2,
+	0x93, 0xdd, 0xc3, 0xc1, 0x46, 0x41, 0xfc, 0xe0, 0x04, 0x80, 0x09, 0x00,
+	0x80, 0xe0, 0x09, 0x9e, 0x0b, 0xa3, 0x75, 0x82, 0x46, 0x41, 0x80, 0xe1,
+	0x04, 0x80, 0xc6, 0x42, 0x8c, 0xe0, 0x04, 0xc2, 0x00, 0x40, 0x00, 0xf2,
+	0x07, 0xcf, 0x06, 0x84, 0x06, 0x40, 0x84, 0xe0, 0x15, 0x00, 0x28, 0xe5,
+	0x1c, 0xc2, 0x93, 0xdd, 0x0b, 0xa1, 0xc6, 0x00, 0xa0, 0xe1, 0x15, 0x00,
+	0x04, 0xf8, 0x05, 0x84, 0x21, 0x8b, 0x2c, 0x84, 0x14, 0x80, 0x2c, 0x84,
+	0x14, 0x82, 0x2c, 0x84, 0x15, 0x00, 0x10, 0xe0, 0x21, 0xa1, 0x21, 0x42,
+	0x10, 0xe0, 0x05, 0x00, 0x14, 0xe0, 0x01, 0x88, 0x75, 0x83, 0x21, 0x85,
+	0x2c, 0x84, 0x14, 0x80, 0x06, 0x46, 0x00, 0xe0, 0x2c, 0x84, 0x14, 0x82,
+	0x2c, 0x84, 0x14, 0xc0, 0x21, 0xa1, 0x21, 0x42, 0x20, 0xe0, 0x14, 0xc2,
+	0x31, 0x42, 0x20, 0xe0, 0x15, 0x00, 0x10, 0xe0, 0x21, 0x42, 0x20, 0xe0,
+	0x05, 0x00, 0x14, 0xe0, 0x01, 0x90, 0x06, 0x42, 0x00, 0xe0, 0x16, 0x80,
+	0x93, 0xdd, 0xc3, 0xc1, 0x0c, 0x04, 0x7c, 0xfa, 0x4a, 0x40, 0x80, 0xe0,
+	0xf0, 0x1e, 0x30, 0xec, 0xe5, 0x82, 0xa6, 0x40, 0x00, 0xe1, 0x1a, 0x80,
+	0x2a, 0xc0, 0x3a, 0xc2, 0x13, 0x40, 0x10, 0xe0, 0x1a, 0x82, 0x23, 0x40,
+	0x18, 0xe0, 0x33, 0x40, 0x1c, 0xe0, 0x13, 0x40, 0x14, 0xe0, 0xf8, 0x61,
+	0x68, 0xef, 0xc6, 0x13, 0x00, 0xe1, 0x15, 0x12, 0x28, 0xf8, 0x0b, 0x02,
+	0x2c, 0xe0, 0x1b, 0x02, 0x24, 0xe0, 0x8a, 0x00, 0xa5, 0x64, 0x03, 0xc0,
+	0x35, 0x82, 0x0a, 0x4e, 0x9c, 0xe1, 0x1a, 0x03, 0x11, 0x6f, 0x02, 0xc0,
+	0xe8, 0x13, 0x01, 0x20, 0x00, 0xc0, 0x1f, 0xa0, 0x5a, 0x42, 0x80, 0xe0,
+	0x0a, 0x4e, 0x9c, 0xe1, 0x68, 0x13, 0x00, 0xa0, 0x09, 0x12, 0x78, 0xf8,
+	0xa1, 0x81, 0xf0, 0x02, 0x10, 0xe4, 0x07, 0xc4, 0x0c, 0xfc, 0xf0, 0x00,
+	0x20, 0xe4, 0xa6, 0x91, 0xa8, 0x53, 0x74, 0xef, 0x05, 0x12, 0x30, 0xf8,
+	0x25, 0x12, 0x28, 0xf8, 0x61, 0x87, 0x09, 0x00, 0x48, 0xe0, 0x81, 0x85,
+	0x09, 0x86, 0x0b, 0xa7, 0x26, 0x0c, 0x00, 0xc0, 0x0b, 0xa1, 0x0b, 0x04,
+	0x28, 0xe0, 0x16, 0x0c, 0x00, 0x80, 0x03, 0x52, 0x04, 0xf8, 0x0b, 0x04,
+	0x20, 0xe0, 0x0c, 0xa6, 0x1b, 0x04, 0x2c, 0xe0, 0x3b, 0x04, 0x28, 0xe0,
+	0x4b, 0x04, 0x20, 0xe0, 0x13, 0x86, 0x3b, 0x04, 0x24, 0xe0, 0x10, 0x0a,
+	0x04, 0xec, 0x1a, 0xfc, 0x33, 0x88, 0x30, 0x06, 0x04, 0xec, 0x12, 0x4e,
+	0x94, 0xf0, 0x32, 0x48, 0x84, 0xf0, 0x4c, 0xe4, 0x7c, 0xa4, 0xcb, 0x04,
+	0x28, 0xe0, 0x14, 0x08, 0x84, 0xe1, 0xcd, 0xc9, 0xc2, 0x58, 0x90, 0x91,
+	0x42, 0x4e, 0x94, 0x90, 0xc3, 0x52, 0x04, 0x98, 0x73, 0x52, 0x00, 0x80,
+	0x5b, 0x04, 0x20, 0xe0, 0x5d, 0xc9, 0x52, 0x40, 0x90, 0x91, 0x42, 0x48,
+	0x8c, 0x90, 0x03, 0x52, 0x04, 0x80, 0x43, 0x52, 0x08, 0x80, 0x3b, 0x04,
+	0x2c, 0xe0, 0x49, 0x04, 0xb8, 0xe0, 0x33, 0x52, 0x1c, 0xf8, 0x2b, 0x04,
+	0x24, 0xe0, 0x4b, 0xab, 0x23, 0x52, 0x18, 0xf8, 0x65, 0x8a, 0x4b, 0xa9,
+	0xe5, 0x90, 0x4b, 0xa7, 0x22, 0x44, 0x84, 0xd0, 0x32, 0x46, 0x84, 0xd0,
+	0x33, 0x52, 0x1c, 0xd8, 0x23, 0x52, 0x18, 0xd8, 0x95, 0x96, 0x20, 0x44,
+	0xf9, 0x73, 0xff, 0xc0, 0x27, 0xc3, 0x23, 0x82, 0x23, 0x52, 0x18, 0xf8,
+	0x24, 0x02, 0x80, 0xfb, 0x04, 0x00, 0x80, 0xfb, 0x2b, 0x8c, 0x58, 0x52,
+	0x74, 0xef, 0x1b, 0x12, 0x1c, 0xf8, 0x2a, 0xfc, 0x0c, 0xe4, 0x17, 0xc3,
+	0x13, 0x84, 0x13, 0x52, 0x1c, 0xf8, 0x0b, 0x12, 0x04, 0xf8, 0x14, 0x02,
+	0x80, 0xfb, 0x2b, 0x8c, 0x68, 0x51, 0x74, 0xef, 0xc5, 0x87, 0x20, 0x44,
+	0xe1, 0x73, 0xff, 0xc0, 0x27, 0xc7, 0x23, 0x82, 0x23, 0x52, 0x18, 0xf8,
+	0x24, 0x02, 0x80, 0xfb, 0x04, 0x00, 0x80, 0xfb, 0x2b, 0x8c, 0x78, 0x57,
+	0x74, 0xef, 0x1b, 0x12, 0x1c, 0xf8, 0x2a, 0xfc, 0x0c, 0xe4, 0x17, 0xc7,
+	0x13, 0x84, 0x13, 0x52, 0x1c, 0xf8, 0x0b, 0x12, 0x04, 0xf8, 0x14, 0x02,
+	0x80, 0xfb, 0x2b, 0x8c, 0x88, 0x56, 0x74, 0xef, 0xe5, 0x83, 0x20, 0x44,
+	0xf1, 0x73, 0xff, 0xc0, 0x27, 0xc5, 0x23, 0x82, 0x23, 0x52, 0x18, 0xf8,
+	0x24, 0x02, 0x80, 0xfb, 0x04, 0x00, 0x80, 0xfb, 0x2b, 0x8c, 0x18, 0x52,
+	0x74, 0xef, 0x1b, 0x12, 0x1c, 0xf8, 0x2a, 0xfc, 0x0c, 0xe4, 0x17, 0xc5,
+	0x13, 0x84, 0x13, 0x52, 0x1c, 0xf8, 0x0b, 0x12, 0x04, 0xf8, 0x14, 0x02,
+	0x80, 0xfb, 0x2b, 0x8c, 0x28, 0x51, 0x74, 0xef, 0x7b, 0x80, 0x7c, 0xa4,
+	0x08, 0x91, 0xa3, 0x52, 0x1c, 0xe0, 0xa3, 0x52, 0x24, 0xe0, 0x0b, 0xa1,
+	0x83, 0x52, 0x1c, 0x80, 0x83, 0x52, 0x24, 0x80, 0x89, 0x12, 0x78, 0xf8,
+	0xf6, 0x57, 0xfc, 0xef, 0x6b, 0x12, 0x1c, 0xf8, 0xab, 0x12, 0x18, 0xf8,
+	0xd6, 0x57, 0xfc, 0x8f, 0x8b, 0xa3, 0xa0, 0x40, 0x00, 0x9c, 0xa5, 0x86,
+	0x64, 0x00, 0x80, 0xfb, 0x1b, 0x90, 0xf8, 0x7d, 0xf8, 0xee, 0x6b, 0x80,
+	0xa4, 0x00, 0x80, 0xfb, 0x1b, 0x90, 0x98, 0x7d, 0xf8, 0xee, 0x15, 0x12,
+	0x28, 0xf8, 0x19, 0x02, 0xb8, 0xe0, 0x1b, 0xad, 0x95, 0x82, 0x1a, 0xa6,
+	0xa0, 0x44, 0xf9, 0x73, 0xff, 0xc0, 0x27, 0xc3, 0x13, 0x94, 0x10, 0x02,
+	0x08, 0xec, 0x1c, 0xe4, 0x23, 0x52, 0x18, 0xf8, 0x1b, 0x12, 0x04, 0xf8,
+	0x03, 0x96, 0x03, 0x52, 0x28, 0xe0, 0x1c, 0xe6, 0x0a, 0xa6, 0x1a, 0xe4,
+	0x63, 0x96, 0x63, 0x52, 0x20, 0xe0, 0x73, 0x52, 0x10, 0xe0, 0x03, 0x52,
+	0x14, 0xe0, 0x13, 0x52, 0x18, 0xe0, 0x98, 0x52, 0x74, 0xef, 0x09, 0x12,
+	0x8c, 0xe0, 0x0b, 0xa1, 0x01, 0x81, 0x01, 0x52, 0x90, 0xe0, 0x65, 0x82,
+	0x05, 0x12, 0x30, 0xf8, 0x09, 0x00, 0xa8, 0xe0, 0x0a, 0x00, 0x0c, 0xf8,
+	0x16, 0x00, 0x00, 0xc0, 0x01, 0x52, 0x90, 0xc0, 0x46, 0x41, 0x84, 0xe0,
+	0x0a, 0x80, 0x0a, 0x4e, 0x9c, 0xe9, 0x1a, 0x00, 0x08, 0xe0, 0x38, 0x01,
+	0x01, 0x20, 0x00, 0xc0, 0x0b, 0x12, 0x1c, 0xe0, 0x1b, 0x12, 0x24, 0xe0,
+	0x2b, 0x12, 0x28, 0xe0, 0x03, 0x52, 0x2c, 0xe0, 0x0b, 0x12, 0x20, 0xe0,
+	0x13, 0x52, 0x34, 0xe0, 0x23, 0x52, 0x38, 0xe0, 0x03, 0x52, 0x30, 0xe0,
+	0x0c, 0x00, 0x00, 0xe2, 0xf1, 0x98, 0x0c, 0x0c, 0x7c, 0xf2, 0x93, 0xdd,
+	0x13, 0xa9, 0x00, 0x00, 0xa8, 0xc1, 0x40, 0x00, 0x68, 0x04, 0xa0, 0xe0,
+	0x40, 0x6c, 0x40, 0x00, 0xe8, 0x34, 0xc8, 0xe0, 0xfc, 0x91, 0x40, 0x00,
+	0x68, 0x1f, 0xb8, 0xe0, 0x30, 0x16, 0x41, 0x00, 0x28, 0x39, 0x74, 0xe0,
+	0xb0, 0x7e, 0x40, 0x00, 0xe8, 0x38, 0xc0, 0xe0, 0x30, 0x04, 0x41, 0x00,
+	0x48, 0x1b, 0x80, 0xe0, 0x30, 0x2e, 0x40, 0x00, 0x88, 0x0c, 0xec, 0xe0,
+	0x10, 0x9f, 0x40, 0x00, 0x88, 0x08, 0xb4, 0xe0, 0x10, 0x01, 0x41, 0x00,
+	0x68, 0x01, 0x84, 0xe0, 0x54, 0xd6, 0x40, 0x00, 0xc8, 0x1a, 0x98, 0xe0,
+	0xd0, 0xc8, 0x40, 0x00, 0x68, 0x08, 0xa0, 0xe0, 0x80, 0xdb, 0x40, 0x00,
+	0xe8, 0x35, 0x94, 0xe0, 0x74, 0xff, 0x40, 0x00, 0xa8, 0x11, 0x80, 0xe0,
+	0xf8, 0x89, 0x40, 0x00, 0x88, 0x16, 0xbc, 0xe0, 0x00, 0x90, 0x40, 0x00,
+	0x08, 0x35, 0xb8, 0xe0, 0x7c, 0x73, 0x40, 0x00, 0x88, 0x1b, 0xc8, 0xe0,
+	0xf4, 0xff, 0x40, 0x00, 0x68, 0x39, 0x80, 0xe0, 0xa4, 0xa4, 0x40, 0x00,
+	0xa8, 0x16, 0xb0, 0xe0, 0x50, 0xc9, 0x40, 0x00, 0x28, 0x3a, 0x98, 0xe0,
+	0x00, 0xb9, 0x00, 0x00, 0xb6, 0x85, 0x00, 0x00,
+};
+
+static const char * const vd55g1_tp_menu[] = {
+	"Disabled",
+	"Diagonal Grey Scale",
+	"Pseudo-random Noise",
+};
+
+static const s64 vd55g1_ev_bias_menu[] = {
+	-3000, -2500, -2000, -1500, -1000, -500,
+	    0,
+	  500,  1000,  1500,  2000,  2500, 3000,
+};
+
+static const char * const vd55g1_hdr_menu[] = {
+	"No HDR",
+	/*
+	 * This mode acquires 2 frames on the sensor, the first one is ditched
+	 * out and only used for auto exposure data, the second one is output to
+	 * the host
+	 */
+	"Internal subtraction",
+};
+
+static const char * const vd55g1_supply_name[] = {
+	"vcore",
+	"vddio",
+	"vana",
+};
+
+enum vd55g1_hdr_mode {
+	VD55G1_NO_HDR,
+	VD55G1_HDR_SUB,
+};
+
+struct vd55g1_mode {
+	u32 width;
+	u32 height;
+};
+
+struct vd55g1_fmt_desc {
+	u32 code;
+	u8 bpp;
+	u8 data_type;
+};
+
+static const struct vd55g1_fmt_desc vd55g1_mbus_codes[] = {
+	{
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.bpp = 8,
+		.data_type = MIPI_CSI2_DT_RAW8,
+	},
+	{
+		.code = MEDIA_BUS_FMT_Y10_1X10,
+		.bpp = 10,
+		.data_type = MIPI_CSI2_DT_RAW10,
+	},
+};
+
+static const struct vd55g1_mode vd55g1_supported_modes[] = {
+	{
+		.width = VD55G1_WIDTH,
+		.height = VD55G1_HEIGHT,
+	},
+	{
+		.width = 800,
+		.height = VD55G1_HEIGHT,
+	},
+	{
+		.width = 800,
+		.height = 600,
+	},
+	{
+		.width = 640,
+		.height = 480,
+	},
+	{
+		.width = 320,
+		.height = 240,
+	},
+};
+
+enum vd55g1_expo_state {
+	VD55G1_EXP_AUTO,
+	VD55G1_EXP_FREEZE,
+	VD55G1_EXP_MANUAL,
+	VD55G1_EXP_SINGLE_STEP,
+	VD55G1_EXP_BYPASS,
+};
+
+struct vd55g1_vblank_limits {
+	u16 min;
+	u16 def;
+	u16 max;
+};
+
+struct vd55g1 {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(vd55g1_supply_name)];
+	struct gpio_desc *reset_gpio;
+	struct clk *xclk;
+	struct regmap *regmap;
+	u32 xclk_freq;
+	u16 oif_ctrl;
+	u8 gpios[VD55G1_NB_GPIOS];
+	unsigned long ext_leds_mask;
+	u32 mipi_rate;
+	u32 pixel_clock;
+	u64 link_freq;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *pixel_rate_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct {
+		struct v4l2_ctrl *hflip_ctrl;
+		struct v4l2_ctrl *vflip_ctrl;
+	};
+	struct v4l2_ctrl *patgen_ctrl;
+	struct {
+		struct v4l2_ctrl *ae_ctrl;
+		struct v4l2_ctrl *expo_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+		struct v4l2_ctrl *dgain_ctrl;
+	};
+	struct v4l2_ctrl *ae_lock_ctrl;
+	struct v4l2_ctrl *ae_bias_ctrl;
+	struct v4l2_ctrl *led_ctrl;
+	struct v4l2_ctrl *hdr_ctrl;
+};
+
+static inline struct vd55g1 *to_vd55g1(struct v4l2_subdev *sd)
+{
+	return container_of_const(sd, struct vd55g1, sd);
+}
+
+static inline struct vd55g1 *ctrl_to_vd55g1(struct v4l2_ctrl *ctrl)
+{
+	struct v4l2_subdev *sd = &container_of_const(ctrl->handler,
+						     struct vd55g1,
+						     ctrl_handler)->sd;
+
+	return to_vd55g1(sd);
+}
+
+static const struct vd55g1_fmt_desc *vd55g1_get_fmt_desc(struct vd55g1 *sensor,
+							 u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vd55g1_mbus_codes); i++) {
+		if (vd55g1_mbus_codes[i].code == code)
+			return &vd55g1_mbus_codes[i];
+	}
+
+	/* Should never happen */
+	dev_warn(sensor->dev, "Unsupported code %d. default to 8 bpp\n", code);
+
+	return &vd55g1_mbus_codes[0];
+}
+
+static s32 vd55g1_get_pixel_rate(struct vd55g1 *sensor,
+				 struct v4l2_mbus_framefmt *format)
+{
+	return sensor->mipi_rate /
+			vd55g1_get_fmt_desc(sensor, format->code)->bpp;
+}
+
+static unsigned int vd55g1_get_hblank_min(struct vd55g1 *sensor,
+					  struct v4l2_mbus_framefmt *format,
+					  struct v4l2_rect *crop)
+{
+	u32 mipi_req_line_time;
+	u32 mipi_req_line_length;
+	u32 min_line_length;
+
+	/* MIPI required time */
+	mipi_req_line_time = (crop->width *
+			      vd55g1_get_fmt_desc(sensor, format->code)->bpp +
+			      VD55G1_MIPI_MARGIN) /
+			      (sensor->mipi_rate / MEGA);
+	mipi_req_line_length = mipi_req_line_time * sensor->pixel_clock /
+			       HZ_PER_MHZ;
+
+	/* Absolute time required for ADCs to convert pixels */
+	min_line_length = VD55G1_LINE_LENGTH_MIN;
+	if (sensor->hdr_ctrl->val == VD55G1_HDR_SUB)
+		min_line_length = VD55G1_LINE_LENGTH_SUB_MIN;
+
+	/* Respect both constraint */
+	min_line_length = max(min_line_length, mipi_req_line_length);
+
+	return min_line_length - crop->width;
+}
+
+static void vd55g1_get_vblank_limits(struct vd55g1 *sensor,
+				     struct v4l2_rect *crop,
+				     struct vd55g1_vblank_limits *limits)
+{
+	limits->min = VD55G1_VBLANK_MIN;
+	limits->def = VD55G1_FRAME_LENGTH_DEF - crop->height;
+	limits->max = VD55G1_VBLANK_MAX - crop->height;
+}
+
+#define vd55g1_read(sensor, reg, val, err) \
+	cci_read((sensor)->regmap, reg, val, err)
+
+#define vd55g1_write(sensor, reg, val, err) \
+	cci_write((sensor)->regmap, reg, val, err)
+
+static int vd55g1_write_array(struct vd55g1 *sensor, u32 reg, unsigned int len,
+			      const u8 *array, int *err)
+{
+	unsigned int chunk_sz = 1024;
+	unsigned int sz;
+	int ret = 0;
+
+	if (err && *err)
+		return *err;
+
+	/*
+	 * This loop isn't necessary but in certains conditions (platforms, cpu
+	 * load, etc.) it has been observed that the bulk write could timeout.
+	 */
+	while (len) {
+		sz = min(len, chunk_sz);
+		ret = regmap_bulk_write(sensor->regmap, reg, array, sz);
+		if (ret < 0)
+			goto out;
+		len -= sz;
+		reg += sz;
+		array += sz;
+	}
+
+out:
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int vd55g1_poll_reg(struct vd55g1 *sensor, u32 reg, u8 poll_val,
+			   int *err)
+{
+	unsigned int val = 0;
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	ret = regmap_read_poll_timeout(sensor->regmap, CCI_REG_ADDR(reg), val,
+				       (val == poll_val), 2000,
+				       500 * USEC_PER_MSEC);
+
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int vd55g1_wait_state(struct vd55g1 *sensor, int state, int *err)
+{
+	return vd55g1_poll_reg(sensor, VD55G1_REG_SYSTEM_FSM, state, err);
+}
+
+static int vd55g1_prepare_clock_tree(struct vd55g1 *sensor)
+{
+	u32 sys_clk, mipi_div, pixel_div;
+
+	if (sensor->xclk_freq < VD55G1_XCLK_FREQ_MIN ||
+	    sensor->xclk_freq > VD55G1_XCLK_FREQ_MAX) {
+		dev_err(sensor->dev,
+			"Only %luMhz-%luMhz clock range supported. Provided %lu MHz\n",
+			VD55G1_XCLK_FREQ_MIN / HZ_PER_MHZ,
+			VD55G1_XCLK_FREQ_MAX / HZ_PER_MHZ,
+			sensor->xclk_freq / HZ_PER_MHZ);
+		return -EINVAL;
+	}
+
+	/* MIPI bus is double data rate */
+	sensor->mipi_rate = sensor->link_freq * 2;
+
+	if (sensor->mipi_rate < VD55G1_MIPI_RATE_MIN ||
+	    sensor->mipi_rate > VD55G1_MIPI_RATE_MAX) {
+		dev_err(sensor->dev,
+			"Only %luMbps-%luMbps data rate range supported. Provided %lu Mbps\n",
+			VD55G1_MIPI_RATE_MIN / MEGA,
+			VD55G1_MIPI_RATE_MAX / MEGA,
+			sensor->mipi_rate / MEGA);
+		return -EINVAL;
+	}
+
+	if (sensor->mipi_rate <= 300 * MEGA)
+		mipi_div = 4;
+	else if (sensor->mipi_rate <= 600 * MEGA)
+		mipi_div = 2;
+	else
+		mipi_div = 1;
+
+	sys_clk = sensor->mipi_rate * mipi_div;
+
+	if (sys_clk <= 780 * HZ_PER_MHZ)
+		pixel_div = 5;
+	else if (sys_clk <= 900 * HZ_PER_MHZ)
+		pixel_div = 6;
+	else
+		pixel_div = 8;
+
+	sensor->pixel_clock = sys_clk / pixel_div;
+
+	return 0;
+}
+
+static int vd55g1_update_patgen(struct vd55g1 *sensor, u32 patgen_index)
+{
+	static const u8 index2val[] = {
+		0x0, 0x22, 0x28
+	};
+	u32 pattern = index2val[patgen_index];
+	u32 reg = pattern << VD55G1_PATGEN_TYPE_SHIFT;
+	u8 duster = VD55G1_DUSTER_RING_ENABLE | VD55G1_DUSTER_DYN_ENABLE |
+		    VD55G1_DUSTER_ENABLE;
+	int ret = 0;
+
+	BUILD_BUG_ON(ARRAY_SIZE(index2val) != ARRAY_SIZE(vd55g1_tp_menu));
+
+	if (pattern != 0) {
+		reg |= VD55G1_PATGEN_ENABLE;
+		/* Take care of duster to not mess up the test pattern output */
+		duster = VD55G1_DUSTER_DISABLE;
+	}
+
+	vd55g1_write(sensor, VD55G1_REG_DUSTER_CTRL, duster, &ret);
+	vd55g1_write(sensor, VD55G1_REG_PATGEN_CTRL, reg, &ret);
+
+	return ret;
+}
+
+static int vd55g1_update_expo_cluster(struct vd55g1 *sensor, bool is_auto)
+{
+	enum vd55g1_expo_state expo_state = is_auto ? VD55G1_EXP_AUTO :
+						      VD55G1_EXP_MANUAL;
+	int ret = 0;
+
+	if (sensor->ae_ctrl->is_new)
+		vd55g1_write(sensor, VD55G1_REG_EXP_MODE(0), expo_state, &ret);
+
+	if (sensor->hdr_ctrl->val == VD55G1_HDR_SUB &&
+	    sensor->hdr_ctrl->is_new) {
+		vd55g1_write(sensor, VD55G1_REG_EXP_MODE(1), VD55G1_EXP_BYPASS,
+			     &ret);
+		if (ret)
+			return ret;
+	}
+
+	if (!is_auto && sensor->expo_ctrl->is_new)
+		vd55g1_write(sensor, VD55G1_REG_MANUAL_COARSE_EXPOSURE,
+			     sensor->expo_ctrl->val, &ret);
+
+	if (!is_auto && sensor->again_ctrl->is_new)
+		vd55g1_write(sensor, VD55G1_REG_MANUAL_ANALOG_GAIN,
+			     sensor->again_ctrl->val, &ret);
+
+	if (!is_auto && sensor->dgain_ctrl->is_new)
+		vd55g1_write(sensor, VD55G1_REG_MANUAL_DIGITAL_GAIN,
+			     sensor->dgain_ctrl->val, &ret);
+
+	return ret;
+}
+
+static int vd55g1_lock_exposure(struct vd55g1 *sensor, u32 lock_val)
+{
+	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
+	enum vd55g1_expo_state expo_state = ae_lock ? VD55G1_EXP_FREEZE :
+						      VD55G1_EXP_AUTO;
+	int ret = 0;
+
+	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
+		vd55g1_write(sensor, VD55G1_REG_EXP_MODE(0), expo_state, &ret);
+
+	return ret;
+}
+
+static int vd55g1_read_expo_cluster(struct vd55g1 *sensor)
+{
+	u64 exposure = 0;
+	u64 again = 0;
+	u64 dgain = 0;
+	int ret = 0;
+
+	vd55g1_read(sensor, VD55G1_REG_APPLIED_COARSE_EXPOSURE, &exposure,
+		    &ret);
+	vd55g1_read(sensor, VD55G1_REG_APPLIED_ANALOG_GAIN, &again, &ret);
+	vd55g1_read(sensor, VD55G1_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
+	if (ret)
+		return ret;
+
+	sensor->expo_ctrl->cur.val = exposure;
+	sensor->again_ctrl->cur.val = again;
+	sensor->dgain_ctrl->cur.val = dgain;
+
+	return 0;
+}
+
+static int vd55g1_update_frame_length(struct vd55g1 *sensor,
+				      unsigned int frame_length)
+{
+	int ret = 0;
+
+	if (sensor->hdr_ctrl->val == VD55G1_HDR_SUB)
+		vd55g1_write(sensor, VD55G1_REG_FRAME_LENGTH(1), frame_length,
+			     &ret);
+	vd55g1_write(sensor, VD55G1_REG_FRAME_LENGTH(0), frame_length, &ret);
+
+	return ret;
+}
+
+static int vd55g1_update_exposure_target(struct vd55g1 *sensor, int index)
+{
+	/*
+	 * Find auto exposure target with: default target exposure * 2^EV
+	 * Defaut target exposure being 27 for the sensor.
+	 */
+	static const unsigned int index2exposure_target[] = {
+		3, 5, 7, 10, 14, 19, 27, 38, 54, 76, 108, 153, 216,
+	};
+	int exposure_target = index2exposure_target[index];
+
+	return vd55g1_write(sensor, VD55G1_REG_AE_TARGET_PERCENTAGE,
+			    exposure_target, NULL);
+}
+
+static int vd55g1_apply_cold_start(struct vd55g1 *sensor,
+				   struct v4l2_rect *crop)
+{
+	/*
+	 * Cold start register is a single register expressed as exposure time
+	 * in us. This differ from status registers being a combination of
+	 * exposure, digital gain, and analog gain, requiring the following
+	 * format conversion.
+	 */
+	unsigned int line_length = crop->width + sensor->hblank_ctrl->val;
+	unsigned int line_time_us = DIV_ROUND_UP(line_length * MEGA,
+						 sensor->pixel_clock);
+	u8 d_gain = DIV_ROUND_CLOSEST(sensor->dgain_ctrl->val, 1 << 8);
+	u8 a_gain = DIV_ROUND_CLOSEST(32, (32 - sensor->again_ctrl->val));
+	unsigned int expo_us = sensor->expo_ctrl->val * d_gain * a_gain *
+			       line_time_us;
+	int ret = 0;
+
+	vd55g1_write(sensor, VD55G1_REG_AE_FORCE_COLDSTART, 1, &ret);
+	vd55g1_write(sensor, VD55G1_REG_AE_COLDSTART_EXP_TIME, expo_us, &ret);
+
+	return ret;
+}
+
+static void vd55g1_update_img_pad_format(struct vd55g1 *sensor,
+					 const struct vd55g1_mode *mode,
+					 u32 code,
+					 struct v4l2_mbus_framefmt *fmt)
+{
+	fmt->code = code;
+	fmt->width = mode->width;
+	fmt->height = mode->height;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	fmt->quantization = V4L2_QUANTIZATION_DEFAULT;
+	fmt->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+}
+
+static int vd55g1_update_hdr_mode(struct vd55g1 *sensor)
+{
+	int ret = 0;
+
+	switch (sensor->hdr_ctrl->val) {
+	case VD55G1_NO_HDR:
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_MAX_COARSE,
+			     VD55G1_EXPOSURE_MAX_COARSE_DEF, &ret);
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_USE_CASES, 0, &ret);
+		vd55g1_write(sensor, VD55G1_REG_NEXT_CTX, 0x0, &ret);
+
+		vd55g1_write(sensor, VD55G1_REG_CTX_REPEAT_COUNT_CTX0, 0, &ret);
+
+		vd55g1_write(sensor, VD55G1_REG_VT_MODE(0),
+			     VD55G1_VT_MODE_NORMAL, &ret);
+		vd55g1_write(sensor, VD55G1_REG_MASK_FRAME_CTRL(0),
+			     VD55G1_MASK_FRAME_CTRL_OUTPUT, &ret);
+		break;
+	case VD55G1_HDR_SUB:
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_MAX_COARSE,
+			     VD55G1_EXPOSURE_MAX_COARSE_SUB, &ret);
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_USE_CASES,
+			     VD55G1_EXPOSURE_USE_CASES_MULTI_CONTEXT, &ret);
+		vd55g1_write(sensor, VD55G1_REG_NEXT_CTX, 0x0001, &ret);
+
+		vd55g1_write(sensor, VD55G1_REG_CTX_REPEAT_COUNT_CTX0, 1, &ret);
+		vd55g1_write(sensor, VD55G1_REG_CTX_REPEAT_COUNT_CTX1, 1, &ret);
+
+		vd55g1_write(sensor, VD55G1_REG_VT_MODE(0),
+			     VD55G1_VT_MODE_NORMAL, &ret);
+		vd55g1_write(sensor, VD55G1_REG_MASK_FRAME_CTRL(0),
+			     VD55G1_MASK_FRAME_CTRL_MASK, &ret);
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_INSTANCE(0), 0, &ret);
+		vd55g1_write(sensor, VD55G1_REG_VT_MODE(1),
+			     VD55G1_VT_MODE_SUBTRACTION, &ret);
+		vd55g1_write(sensor, VD55G1_REG_MASK_FRAME_CTRL(1),
+			     VD55G1_MASK_FRAME_CTRL_OUTPUT, &ret);
+		vd55g1_write(sensor, VD55G1_REG_EXPOSURE_INSTANCE(1), 1, &ret);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int vd55g1_set_framefmt(struct vd55g1 *sensor,
+			       struct v4l2_mbus_framefmt *format,
+			       struct v4l2_rect *crop)
+{
+	u8 binning;
+	int ret = 0;
+
+	vd55g1_write(sensor, VD55G1_REG_FORMAT_CTRL,
+		     vd55g1_get_fmt_desc(sensor, format->code)->bpp, &ret);
+	vd55g1_write(sensor, VD55G1_REG_OIF_IMG_CTRL,
+		     vd55g1_get_fmt_desc(sensor, format->code)->data_type,
+		     &ret);
+
+	switch (crop->width / format->width) {
+	case 1:
+	default:
+		binning = VD55G1_READOUT_CTRL_BIN_MODE_NORMAL;
+		break;
+	case 2:
+		binning = VD55G1_READOUT_CTRL_BIN_MODE_DIGITAL_X2;
+		break;
+	}
+	vd55g1_write(sensor, VD55G1_REG_READOUT_CTRL, binning, &ret);
+
+	vd55g1_write(sensor, VD55G1_REG_X_START(0), crop->left, &ret);
+	vd55g1_write(sensor, VD55G1_REG_X_WIDTH(0), crop->width, &ret);
+	vd55g1_write(sensor, VD55G1_REG_Y_START(0), crop->top, &ret);
+	vd55g1_write(sensor, VD55G1_REG_Y_HEIGHT(0), crop->height, &ret);
+
+	vd55g1_write(sensor, VD55G1_REG_X_START(1), crop->left, &ret);
+	vd55g1_write(sensor, VD55G1_REG_X_WIDTH(1), crop->width, &ret);
+	vd55g1_write(sensor, VD55G1_REG_Y_START(1), crop->top, &ret);
+	vd55g1_write(sensor, VD55G1_REG_Y_HEIGHT(1), crop->height, &ret);
+
+	return ret;
+}
+
+static int vd55g1_update_gpios(struct vd55g1 *sensor, unsigned long gpio_mask)
+{
+	unsigned long io;
+	u8 gpio_val;
+	int ret = 0;
+
+	for_each_set_bit(io, &gpio_mask, VD55G1_NB_GPIOS) {
+		gpio_val = sensor->gpios[io];
+
+		if (gpio_val == VD55G1_GPIO_MODE_STROBE &&
+		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE) {
+			gpio_val = VD55G1_GPIO_MODE_IN;
+			if (sensor->hdr_ctrl->val == VD55G1_HDR_SUB) {
+				/* Make its context 1 counterpart strobe too */
+				vd55g1_write(sensor,
+					     VD55G1_REG_GPIO_0_CTRL(1) + io,
+					     gpio_val, &ret);
+			}
+		}
+
+		ret = vd55g1_write(sensor, VD55G1_REG_GPIO_0_CTRL(0) + io,
+				   gpio_val, &ret);
+	}
+
+	return ret;
+}
+
+static int vd55g1_ro_ctrls_setup(struct vd55g1 *sensor, struct v4l2_rect *crop)
+{
+	return vd55g1_write(sensor, VD55G1_REG_LINE_LENGTH,
+			    crop->width + sensor->hblank_ctrl->val, NULL);
+}
+
+static void vd55g1_grab_ctrls(struct vd55g1 *sensor, bool enable)
+{
+	/* These settings cannot change during stream */
+	v4l2_ctrl_grab(sensor->hflip_ctrl, enable);
+	v4l2_ctrl_grab(sensor->vflip_ctrl, enable);
+	v4l2_ctrl_grab(sensor->patgen_ctrl, enable);
+	v4l2_ctrl_grab(sensor->hdr_ctrl, enable);
+}
+
+static int vd55g1_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
+{
+	struct vd55g1 *sensor = to_vd55g1(sd);
+	struct v4l2_rect *crop =
+		v4l2_subdev_state_get_crop(state, 0);
+	struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	int ret;
+
+	ret = pm_runtime_resume_and_get(sensor->dev);
+	if (ret < 0)
+		return ret;
+
+	vd55g1_write(sensor, VD55G1_REG_EXT_CLOCK, sensor->xclk_freq, &ret);
+
+	/* Configure output */
+	vd55g1_write(sensor, VD55G1_REG_MIPI_DATA_RATE,
+		     sensor->mipi_rate, &ret);
+	vd55g1_write(sensor, VD55G1_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
+	vd55g1_write(sensor, VD55G1_REG_ISL_ENABLE, 0, &ret);
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vd55g1_set_framefmt(sensor, format, crop);
+	if (ret)
+		goto err_rpm_put;
+
+	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
+	ret = vd55g1_update_gpios(sensor, GENMASK(VD55G1_NB_GPIOS - 1, 0));
+	if (ret)
+		goto err_rpm_put;
+
+	ret = vd55g1_apply_cold_start(sensor, crop);
+	if (ret)
+		goto err_rpm_put;
+
+	/* Apply settings from V4L2 ctrls */
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
+	if (ret)
+		goto err_rpm_put;
+
+	/* Also apply settings from read-only V4L2 ctrls */
+	ret = vd55g1_ro_ctrls_setup(sensor, crop);
+	if (ret)
+		goto err_rpm_put;
+
+	/* Start streaming */
+	vd55g1_write(sensor, VD55G1_REG_STBY, VD55G1_STBY_START_STREAM, &ret);
+	vd55g1_poll_reg(sensor, VD55G1_REG_STBY, 0, &ret);
+	vd55g1_wait_state(sensor, VD55G1_SYSTEM_FSM_STREAMING, &ret);
+	if (ret)
+		goto err_rpm_put;
+
+	vd55g1_grab_ctrls(sensor, true);
+
+	return 0;
+
+err_rpm_put:
+	pm_runtime_put(sensor->dev);
+	return 0;
+}
+
+static int vd55g1_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
+{
+	struct vd55g1 *sensor = to_vd55g1(sd);
+	int ret = 0;
+
+	/* Retrieve Expo cluster to enable coldstart of AE */
+	ret = vd55g1_read_expo_cluster(sensor);
+
+	vd55g1_write(sensor, VD55G1_REG_STREAMING, VD55G1_STREAMING_STOP_STREAM,
+		     &ret);
+	vd55g1_poll_reg(sensor, VD55G1_REG_STREAMING, 0, &ret);
+	vd55g1_wait_state(sensor, VD55G1_SYSTEM_FSM_SW_STBY, &ret);
+
+	if (ret)
+		dev_warn(sensor->dev, "Can't disable stream\n");
+
+	vd55g1_grab_ctrls(sensor, false);
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static int vd55g1_patch(struct vd55g1 *sensor)
+{
+	u64 patch;
+	int ret = 0;
+
+	vd55g1_write_array(sensor, VD55G1_REG_FWPATCH_START_ADDR,
+			   sizeof(patch_array), patch_array, &ret);
+	vd55g1_write(sensor, VD55G1_REG_BOOT, VD55G1_BOOT_PATCH_SETUP, &ret);
+	vd55g1_poll_reg(sensor, VD55G1_REG_BOOT, 0, &ret);
+	if (ret) {
+		dev_err(sensor->dev, "Failed to apply patch\n");
+		return ret;
+	}
+
+	vd55g1_read(sensor, VD55G1_REG_FWPATCH_REVISION, &patch, &ret);
+	if (patch != (VD55G1_FWPATCH_REVISION_MAJOR << 8) +
+	    VD55G1_FWPATCH_REVISION_MINOR) {
+		dev_err(sensor->dev, "Bad patch version expected %d.%d got %d.%d\n",
+			VD55G1_FWPATCH_REVISION_MAJOR,
+			VD55G1_FWPATCH_REVISION_MINOR,
+			(u8)(patch >> 8), (u8)(patch & 0xff));
+		return -ENODEV;
+	}
+	dev_dbg(sensor->dev, "patch %d.%d applied\n",
+		(u8)(patch >> 8), (u8)(patch & 0xff));
+
+	return 0;
+}
+
+static int vd55g1_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(sd_state, 0);
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *crop;
+		return 0;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = VD55G1_WIDTH;
+		sel->r.height = VD55G1_HEIGHT;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int vd55g1_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(vd55g1_mbus_codes))
+		return -EINVAL;
+
+	code->code = vd55g1_mbus_codes[code->index].code;
+
+	return 0;
+}
+
+static int vd55g1_new_format_change_controls(struct vd55g1 *sensor,
+					     struct v4l2_mbus_framefmt *format,
+					     struct v4l2_rect *crop)
+{
+	struct vd55g1_vblank_limits vblank;
+	unsigned int hblank;
+	unsigned int frame_length = 0;
+	unsigned int expo_max;
+	int ret;
+
+	/* Reset vblank and frame length to default */
+	vd55g1_get_vblank_limits(sensor, crop, &vblank);
+	ret = __v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank.min,
+				       vblank.max, 1, vblank.def);
+	if (ret)
+		return ret;
+
+	/* Max exposure changes with vblank */
+	frame_length = crop->height + sensor->vblank_ctrl->val;
+	expo_max = frame_length - VD55G1_EXPO_MAX_TERM;
+	ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max, 1,
+				       VD55G1_EXPO_DEF);
+	if (ret)
+		return ret;
+
+	/* Update pixel rate to reflect new bpp */
+	ret = __v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_ctrl,
+				       vd55g1_get_pixel_rate(sensor, format));
+	if (ret)
+		return ret;
+
+	/* Update hblank according to new width */
+	hblank = vd55g1_get_hblank_min(sensor, format, crop);
+	ret = __v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
+				       hblank);
+
+	return ret;
+}
+
+static int vd55g1_set_pad_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *sd_fmt)
+{
+	struct vd55g1 *sensor = to_vd55g1(sd);
+	const struct vd55g1_mode *new_mode;
+	struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect pad_crop;
+	unsigned int binning;
+
+	new_mode = v4l2_find_nearest_size(vd55g1_supported_modes,
+					  ARRAY_SIZE(vd55g1_supported_modes),
+					  width, height, sd_fmt->format.width,
+					  sd_fmt->format.height);
+
+	vd55g1_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
+				     &sd_fmt->format);
+
+	/*
+	 * Use binning to maximize the crop rectangle size, and centre it in the
+	 * sensor.
+	 */
+	binning = min(VD55G1_WIDTH / sd_fmt->format.width,
+		      VD55G1_HEIGHT / sd_fmt->format.height);
+	binning = min(binning, 2U);
+	pad_crop.width = sd_fmt->format.width * binning;
+	pad_crop.height = sd_fmt->format.height * binning;
+	pad_crop.left = (VD55G1_WIDTH - pad_crop.width) / 2;
+	pad_crop.top = (VD55G1_HEIGHT - pad_crop.height) / 2;
+
+	format = v4l2_subdev_state_get_format(sd_state, sd_fmt->pad);
+
+	*format = sd_fmt->format;
+
+	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
+	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return vd55g1_new_format_change_controls(sensor,
+							 &sd_fmt->format,
+							 &pad_crop);
+
+	return 0;
+}
+
+static int vd55g1_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
+{
+	unsigned int def_mode = VD55G1_DEFAULT_MODE;
+	struct vd55g1 *sensor = to_vd55g1(sd);
+	struct v4l2_subdev_format fmt = { 0 };
+	struct v4l2_subdev_route routes[] = {
+		{ .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE }
+	};
+	struct v4l2_subdev_krouting routing = {
+	    .num_routes = ARRAY_SIZE(routes),
+	    .routes = routes,
+	};
+	int ret;
+
+	/* Needed by v4l2_subdev_s_stream_helper(), even with 1 stream only */
+	ret = v4l2_subdev_set_routing(sd, sd_state, &routing);
+	if (ret)
+		return ret;
+
+	vd55g1_update_img_pad_format(sensor, &vd55g1_supported_modes[def_mode],
+				     VD55G1_MEDIA_BUS_FMT_DEF, &fmt.format);
+
+	return vd55g1_set_pad_fmt(sd, sd_state, &fmt);
+}
+
+static int vd55g1_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(vd55g1_supported_modes))
+		return -EINVAL;
+
+	fse->min_width = vd55g1_supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = vd55g1_supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops vd55g1_internal_ops = {
+	.init_state = vd55g1_init_state,
+};
+
+static const struct v4l2_subdev_pad_ops vd55g1_pad_ops = {
+	.enum_mbus_code = vd55g1_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = vd55g1_set_pad_fmt,
+	.get_selection = vd55g1_get_selection,
+	.enum_frame_size = vd55g1_enum_frame_size,
+	.enable_streams = vd55g1_enable_streams,
+	.disable_streams = vd55g1_disable_streams,
+};
+
+static const struct v4l2_subdev_video_ops vd55g1_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_ops vd55g1_subdev_ops = {
+	.video = &vd55g1_video_ops,
+	.pad = &vd55g1_pad_ops,
+};
+
+static int vd55g1_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vd55g1 *sensor = ctrl_to_vd55g1(ctrl);
+	int ret = 0;
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd55g1_read_expo_cluster(sensor);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static int vd55g1_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vd55g1 *sensor = ctrl_to_vd55g1(ctrl);
+	unsigned int frame_length = 0;
+	unsigned int expo_max;
+	struct v4l2_subdev_state *state =
+		v4l2_subdev_get_locked_active_state(&sensor->sd);
+	struct v4l2_rect *crop =
+		v4l2_subdev_state_get_crop(state, 0);
+	struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	unsigned int hblank = vd55g1_get_hblank_min(sensor, format, crop);
+	bool is_auto = false;
+	int ret = 0;
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
+		return 0;
+
+	/* Update controls state, range, etc. whatever the state of the HW */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		frame_length = crop->height + ctrl->val;
+		expo_max = frame_length - VD55G1_EXPO_MAX_TERM;
+		ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, 0, expo_max,
+					       1, VD55G1_EXPO_DEF);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
+		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
+		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
+		break;
+	case V4L2_CID_HDR_SENSOR_MODE:
+		/* Discriminate if the userspace changed the control value */
+		if (ctrl->val != ctrl->cur.val) {
+			/* Max horizontal blanking changes with hdr mode */
+			ret = __v4l2_ctrl_modify_range(sensor->hblank_ctrl,
+						       hblank, hblank, 1,
+						       hblank);
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* Don't modify hardware if controls modification failed */
+	if (ret)
+		return ret;
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ret = vd55g1_write(sensor, VD55G1_REG_ORIENTATION,
+				   sensor->hflip_ctrl->val |
+					   (sensor->vflip_ctrl->val << 1),
+				   NULL);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = vd55g1_update_patgen(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd55g1_update_expo_cluster(sensor, is_auto);
+		break;
+	case V4L2_CID_3A_LOCK:
+		ret = vd55g1_lock_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:
+		/*
+		 * We use auto exposure target percentage register to control
+		 * exposure bias for more precision.
+		 */
+		ret = vd55g1_update_exposure_target(sensor, ctrl->val);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = vd55g1_update_frame_length(sensor, frame_length);
+		break;
+	case V4L2_CID_FLASH_LED_MODE:
+		ret = vd55g1_update_gpios(sensor, sensor->ext_leds_mask);
+		break;
+	case V4L2_CID_HDR_SENSOR_MODE:
+		ret = vd55g1_update_hdr_mode(sensor);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vd55g1_ctrl_ops = {
+	.g_volatile_ctrl = vd55g1_g_volatile_ctrl,
+	.s_ctrl = vd55g1_s_ctrl,
+};
+
+static int vd55g1_init_ctrls(struct vd55g1 *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &vd55g1_ctrl_ops;
+	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
+	struct v4l2_ctrl *ctrl;
+	struct v4l2_fwnode_device_properties fwnode_props;
+	struct vd55g1_vblank_limits vblank;
+	unsigned int hblank;
+	struct v4l2_subdev_state *state =
+		v4l2_subdev_lock_and_get_active_state(&sensor->sd);
+	struct v4l2_rect *crop =
+		v4l2_subdev_state_get_crop(state, 0);
+	struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	s32 pixel_rate = vd55g1_get_pixel_rate(sensor, format);
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 16);
+
+	/* Flip cluster */
+	sensor->hflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
+					       0, 1, 1, 0);
+	sensor->vflip_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
+					       0, 1, 1, 0);
+	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
+
+	/* Exposition cluster */
+	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO, 1,
+						 ~0x3, V4L2_EXPOSURE_AUTO);
+	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					       0, 0x1c, 1, VD55G1_AGAIN_DEF);
+	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
+					       256, 0xffff, 1,
+					       VD55G1_DGAIN_DEF);
+	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE, 0,
+					      VD55G1_FRAME_LENGTH_DEF -
+					      VD55G1_EXPO_MAX_TERM,
+					      1, VD55G1_EXPO_DEF);
+	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
+
+	sensor->patgen_ctrl =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(vd55g1_tp_menu) - 1, 0,
+					     0, vd55g1_tp_menu);
+	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+				      0, 0, &sensor->link_freq);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->pixel_rate_ctrl = v4l2_ctrl_new_std(hdl, ops,
+						    V4L2_CID_PIXEL_RATE, 1,
+						    INT_MAX, 1,
+						    pixel_rate);
+	if (sensor->pixel_rate_ctrl)
+		sensor->pixel_rate_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK,
+						 0, 1, 0, 0);
+	sensor->ae_bias_ctrl =
+		v4l2_ctrl_new_int_menu(hdl, ops,
+				       V4L2_CID_AUTO_EXPOSURE_BIAS,
+				       ARRAY_SIZE(vd55g1_ev_bias_menu) - 1,
+				       ARRAY_SIZE(vd55g1_ev_bias_menu) / 2,
+				       vd55g1_ev_bias_menu);
+	sensor->hdr_ctrl =
+		v4l2_ctrl_new_std_menu_items(hdl, ops,
+					     V4L2_CID_HDR_SENSOR_MODE,
+					     ARRAY_SIZE(vd55g1_hdr_menu) - 1, 0,
+					     VD55G1_NO_HDR, vd55g1_hdr_menu);
+	hblank = vd55g1_get_hblank_min(sensor, format, crop);
+	sensor->hblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK,
+						hblank, hblank, 1, hblank);
+	if (sensor->hblank_ctrl)
+		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	vd55g1_get_vblank_limits(sensor, crop, &vblank);
+	sensor->vblank_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
+						vblank.min, vblank.max,
+						1, vblank.def);
+
+	/* Additional controls based on device tree properties */
+	if (sensor->ext_leds_mask) {
+		sensor->led_ctrl =
+			v4l2_ctrl_new_std_menu(hdl, ops,
+					       V4L2_CID_FLASH_LED_MODE,
+					       V4L2_FLASH_LED_MODE_FLASH, 0,
+					       V4L2_FLASH_LED_MODE_NONE);
+	}
+
+	ret = v4l2_fwnode_device_parse(sensor->dev, &fwnode_props);
+	if (ret)
+		goto free_ctrls;
+
+	ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &fwnode_props);
+	if (ret)
+		goto free_ctrls;
+
+	sensor->sd.ctrl_handler = hdl;
+	goto unlock_state;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+unlock_state:
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
+static int vd55g1_detect(struct vd55g1 *sensor)
+{
+	u64 device_rev;
+	u64 id;
+	int ret;
+
+	ret = vd55g1_read(sensor, VD55G1_REG_MODEL_ID, &id, NULL);
+	if (ret)
+		return ret;
+
+	if (id != VD55G1_MODEL_ID) {
+		dev_warn(sensor->dev, "Unsupported sensor id %x\n", (u32)id);
+		return -ENODEV;
+	}
+
+	ret = vd55g1_read(sensor, VD55G1_REG_REVISION, &device_rev, NULL);
+	if (ret)
+		return ret;
+
+	if (device_rev != VD55G1_REVISION_CCB) {
+		dev_err(sensor->dev, "Unsupported sensor revision (0x%x)\n",
+			(u16)device_rev);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int vd55g1_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd55g1 *sensor = to_vd55g1(sd);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(vd55g1_supply_name),
+				    sensor->supplies);
+	if (ret) {
+		dev_err(dev, "Failed to enable regulators %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(dev, "Failed to enable clock %d\n", ret);
+		goto disable_bulk;
+	}
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(5000, 10000);
+	ret = vd55g1_wait_state(sensor, VD55G1_SYSTEM_FSM_READY_TO_BOOT, NULL);
+	if (ret) {
+		dev_err(dev, "Sensor reset failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	ret = vd55g1_detect(sensor);
+	if (ret) {
+		dev_err(dev, "Sensor detect failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	ret = vd55g1_patch(sensor);
+	if (ret) {
+		dev_err(dev, "Sensor patch failed %d\n", ret);
+		goto disable_clock;
+	}
+
+	ret = vd55g1_wait_state(sensor, VD55G1_SYSTEM_FSM_SW_STBY, NULL);
+	if (ret) {
+		dev_err(dev, "Sensor waiting after patch failed %d\n",
+			ret);
+		goto disable_clock;
+	}
+
+	return 0;
+
+disable_clock:
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	clk_disable_unprepare(sensor->xclk);
+disable_bulk:
+	regulator_bulk_disable(ARRAY_SIZE(vd55g1_supply_name),
+			       sensor->supplies);
+
+	return ret;
+}
+
+static int vd55g1_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd55g1 *sensor = to_vd55g1(sd);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	clk_disable_unprepare(sensor->xclk);
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+
+	return 0;
+}
+
+static int vd55g1_check_csi_conf(struct vd55g1 *sensor,
+				 struct fwnode_handle *endpoint)
+{
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	u8 n_lanes;
+	int ret;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+	if (ret)
+		return -EINVAL;
+
+	/* Check lanes number */
+	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	if (n_lanes != 1) {
+		dev_err(sensor->dev, "Sensor only supports 1 lane, found %d\n",
+			n_lanes);
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Clock lane must be first */
+	if (ep.bus.mipi_csi2.clock_lane != 0) {
+		dev_err(sensor->dev, "Clock lane must be mapped to lane 0\n");
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/* Handle polarities in sensor configuration */
+	sensor->oif_ctrl = (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
+			   (ep.bus.mipi_csi2.lane_polarities[1] << 6);
+
+	/* Check the link frequency set in device tree */
+	if (!ep.nr_of_link_frequencies) {
+		dev_err(sensor->dev, "link-frequency property not found in DT\n");
+		ret = -EINVAL;
+		goto done;
+	}
+	if (ep.nr_of_link_frequencies != 1) {
+		dev_err(sensor->dev, "Multiple link frequencies not supported\n");
+		ret = -EINVAL;
+		goto done;
+	}
+	sensor->link_freq = ep.link_frequencies[0];
+
+done:
+	v4l2_fwnode_endpoint_free(&ep);
+
+	return ret;
+}
+
+static int vd55g1_parse_dt_gpios_array(struct vd55g1 *sensor,
+				       char *prop_name, u32 *array, int *nb)
+{
+	unsigned int i;
+	int ret;
+
+	*nb = device_property_count_u32(sensor->dev, prop_name);
+	if (*nb == -EINVAL) {
+		/* Property not found */
+		*nb = 0;
+		return 0;
+	}
+
+	ret = device_property_read_u32_array(sensor->dev,
+					     prop_name, array, *nb);
+	if (ret) {
+		dev_err(sensor->dev, "Failed to read %s prop\n", prop_name);
+		return ret;
+	}
+	for (i = 0; i < *nb;  i++) {
+		if (array[i] >= VD55G1_NB_GPIOS) {
+			dev_err(sensor->dev, "Invalid GPIO number %d\n",
+				array[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int vd55g1_parse_dt_gpios(struct vd55g1 *sensor)
+{
+	u32 led_gpios[VD55G1_NB_GPIOS];
+	int nb_gpios_leds;
+	unsigned int i;
+	int ret;
+
+	/* Initialize GPIOs to default */
+	for (i = 0; i < VD55G1_NB_GPIOS; i++)
+		sensor->gpios[i] = VD55G1_GPIO_MODE_IN;
+	sensor->ext_leds_mask = 0;
+
+	/* Take into account optional 'st,leds' output for GPIOs */
+	ret = vd55g1_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
+					  &nb_gpios_leds);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < nb_gpios_leds; i++) {
+		sensor->gpios[led_gpios[i]] = VD55G1_GPIO_MODE_STROBE;
+		set_bit(led_gpios[i], &sensor->ext_leds_mask);
+	}
+
+	return 0;
+}
+
+static int vd55g1_parse_dt(struct vd55g1 *sensor)
+{
+	struct fwnode_handle *endpoint;
+	int ret;
+
+	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(sensor->dev),
+						   0, 0, 0);
+	if (!endpoint) {
+		dev_err(sensor->dev, "Endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = vd55g1_check_csi_conf(sensor, endpoint);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return ret;
+
+	return vd55g1_parse_dt_gpios(sensor);
+}
+
+static int vd55g1_subdev_init(struct vd55g1 *sensor)
+{
+	int ret;
+
+	/* Init sub device */
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->sd.internal_ops = &vd55g1_internal_ops;
+
+	/* Init source pad */
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret) {
+		dev_err(sensor->dev, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+	sensor->sd.state_lock = sensor->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&sensor->sd);
+	if (ret) {
+		dev_err(sensor->dev, "Subdev init error: %d\n", ret);
+		goto err_ctrls;
+	}
+
+	/*
+	 * Initialize controls after v4l2_subdev_init_finalize() to make sure
+	 * active state is set
+	 */
+	ret = vd55g1_init_ctrls(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "Controls initialization failed %d\n",
+			ret);
+		goto err_media;
+	}
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+
+err_media:
+	media_entity_cleanup(&sensor->sd.entity);
+	return ret;
+}
+
+static void vd55g1_subdev_cleanup(struct vd55g1 *sensor)
+{
+	v4l2_async_unregister_subdev(&sensor->sd);
+	v4l2_subdev_cleanup(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+}
+
+static int vd55g1_get_regulators(struct vd55g1 *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(vd55g1_supply_name); i++)
+		sensor->supplies[i].supply = vd55g1_supply_name[i];
+
+	return devm_regulator_bulk_get(sensor->dev,
+				       ARRAY_SIZE(vd55g1_supply_name),
+				       sensor->supplies);
+}
+
+static int vd55g1_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct vd55g1 *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+	sensor->dev = &client->dev;
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &vd55g1_subdev_ops);
+
+	ret = vd55g1_parse_dt(sensor);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to parse Device Tree\n");
+
+	/* Get (and check) resources : power regs, ext clock, reset gpio */
+	ret = vd55g1_get_regulators(sensor);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get regulators\n");
+
+	sensor->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xclk))
+		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
+				     "Failed to get xclk\n");
+
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	ret = vd55g1_prepare_clock_tree(sensor);
+	if (ret)
+		return ret;
+
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
+				     "Failed to get reset gpio\n");
+
+	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(sensor->regmap))
+		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
+				     "Failed to init regmap\n");
+
+	/* Detect if sensor is present and if its revision is supported */
+	ret = vd55g1_power_on(dev);
+	if (ret)
+		return ret;
+
+	/* Enable pm_runtime and power off the sensor */
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_set_autosuspend_delay(dev, 4000);
+	pm_runtime_use_autosuspend(dev);
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	ret = vd55g1_subdev_init(sensor);
+	if (ret) {
+		dev_err(dev, "V4l2 init failed: %d\n", ret);
+		goto err_power_off;
+	}
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret) {
+		dev_err(dev, "async subdev register failed %d\n", ret);
+		goto err_subdev;
+	}
+
+	return 0;
+
+err_subdev:
+	vd55g1_subdev_cleanup(sensor);
+err_power_off:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	pm_runtime_dont_use_autosuspend(dev);
+	vd55g1_power_off(dev);
+
+	return ret;
+}
+
+static void vd55g1_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vd55g1 *sensor = to_vd55g1(sd);
+
+	vd55g1_subdev_cleanup(sensor);
+
+	pm_runtime_disable(&client->dev);
+	if (!pm_runtime_status_suspended(&client->dev))
+		vd55g1_power_off(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_dont_use_autosuspend(&client->dev);
+}
+
+static const struct of_device_id vd55g1_dt_ids[] = {
+	{ .compatible = "st,vd55g1" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, vd55g1_dt_ids);
+
+static const struct dev_pm_ops vd55g1_pm_ops = {
+	SET_RUNTIME_PM_OPS(vd55g1_power_off, vd55g1_power_on, NULL)
+};
+
+static struct i2c_driver vd55g1_i2c_driver = {
+	.driver = {
+		.name  = "vd55g1",
+		.of_match_table = vd55g1_dt_ids,
+		.pm = &vd55g1_pm_ops,
+	},
+	.probe = vd55g1_probe,
+	.remove = vd55g1_remove,
+};
+
+module_i2c_driver(vd55g1_i2c_driver);
+
+MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
+MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
+MODULE_DESCRIPTION("VD55G1 camera subdev driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/vd56g3.c b/drivers/media/i2c/vd56g3.c
new file mode 100644
index 000000000000..5d951ad0b478
--- /dev/null
+++ b/drivers/media/i2c/vd56g3.c
@@ -0,0 +1,1586 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * A V4L2 driver for ST VD56G3 (Mono) and VD66GY (RGB) global shutter cameras.
+ * Copyright (C) 2024, STMicroelectronics SA
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/i2c.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+#include <linux/unaligned.h>
+#include <linux/units.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-cci.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+/* Register Map */
+#define VD56G3_REG_MODEL_ID				CCI_REG16_LE(0x0000)
+#define VD56G3_MODEL_ID					0x5603
+#define VD56G3_REG_REVISION				CCI_REG16_LE(0x0002)
+#define VD56G3_REVISION_CUT3				0x31
+#define VD56G3_REG_OPTICAL_REVISION			CCI_REG8(0x001a)
+#define VD56G3_OPTICAL_REVISION_MONO			0
+#define VD56G3_OPTICAL_REVISION_BAYER			1
+#define VD56G3_REG_SYSTEM_FSM				CCI_REG8(0x0028)
+#define VD56G3_SYSTEM_FSM_READY_TO_BOOT			0x01
+#define VD56G3_SYSTEM_FSM_SW_STBY			0x02
+#define VD56G3_SYSTEM_FSM_STREAMING			0x03
+#define VD56G3_REG_APPLIED_COARSE_EXPOSURE		CCI_REG16_LE(0x0064)
+#define VD56G3_REG_APPLIED_ANALOG_GAIN			CCI_REG8(0x0068)
+#define VD56G3_REG_APPLIED_DIGITAL_GAIN			CCI_REG16_LE(0x006a)
+#define VD56G3_REG_BOOT					CCI_REG8(0x0200)
+#define VD56G3_CMD_ACK					0
+#define VD56G3_CMD_BOOT					1
+#define VD56G3_REG_STBY					CCI_REG8(0x0201)
+#define VD56G3_CMD_START_STREAM				1
+#define VD56G3_REG_STREAMING				CCI_REG8(0x0202)
+#define VD56G3_CMD_STOP_STREAM				1
+#define VD56G3_REG_EXT_CLOCK				CCI_REG32_LE(0x0220)
+#define VD56G3_REG_CLK_PLL_PREDIV			CCI_REG8(0x0224)
+#define VD56G3_REG_CLK_SYS_PLL_MULT			CCI_REG8(0x0226)
+#define VD56G3_REG_ORIENTATION				CCI_REG8(0x0302)
+#define VD56G3_REG_FORMAT_CTRL				CCI_REG8(0x030a)
+#define VD56G3_REG_OIF_CTRL				CCI_REG16_LE(0x030c)
+#define VD56G3_REG_OIF_IMG_CTRL				CCI_REG8(0x030f)
+#define VD56G3_REG_OIF_CSI_BITRATE			CCI_REG16_LE(0x0312)
+#define VD56G3_REG_DUSTER_CTRL				CCI_REG8(0x0318)
+#define VD56G3_DUSTER_DISABLE				0
+#define VD56G3_DUSTER_ENABLE_DEF_MODULES		0x13
+#define VD56G3_REG_ISL_ENABLE				CCI_REG8(0x0333)
+#define VD56G3_REG_DARKCAL_CTRL				CCI_REG8(0x0340)
+#define VD56G3_DARKCAL_ENABLE				1
+#define VD56G3_DARKCAL_DISABLE_DARKAVG			2
+#define VD56G3_REG_PATGEN_CTRL				CCI_REG16_LE(0x0400)
+#define VD56G3_PATGEN_ENABLE				1
+#define VD56G3_PATGEN_TYPE_SHIFT			4
+#define VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE		CCI_REG16_LE(0x042a)
+#define VD56G3_REG_AE_COLDSTART_ANALOG_GAIN		CCI_REG8(0x042c)
+#define VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN		CCI_REG16_LE(0x042e)
+#define VD56G3_REG_AE_ROI_START_H			CCI_REG16_LE(0x0432)
+#define VD56G3_REG_AE_ROI_START_V			CCI_REG16_LE(0x0434)
+#define VD56G3_REG_AE_ROI_END_H				CCI_REG16_LE(0x0436)
+#define VD56G3_REG_AE_ROI_END_V				CCI_REG16_LE(0x0438)
+#define VD56G3_REG_AE_COMPENSATION			CCI_REG16_LE(0x043a)
+#define VD56G3_REG_EXP_MODE				CCI_REG8(0x044c)
+#define VD56G3_EXP_MODE_AUTO				0
+#define VD56G3_EXP_MODE_FREEZE				1
+#define VD56G3_EXP_MODE_MANUAL				2
+#define VD56G3_REG_MANUAL_ANALOG_GAIN			CCI_REG8(0x044d)
+#define VD56G3_REG_MANUAL_COARSE_EXPOSURE		CCI_REG16_LE(0x044e)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0		CCI_REG16_LE(0x0450)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1		CCI_REG16_LE(0x0452)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2		CCI_REG16_LE(0x0454)
+#define VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3		CCI_REG16_LE(0x0456)
+#define VD56G3_REG_FRAME_LENGTH				CCI_REG16_LE(0x0458)
+#define VD56G3_REG_Y_START				CCI_REG16_LE(0x045a)
+#define VD56G3_REG_Y_END				CCI_REG16_LE(0x045c)
+#define VD56G3_REG_OUT_ROI_X_START			CCI_REG16_LE(0x045e)
+#define VD56G3_REG_OUT_ROI_X_END			CCI_REG16_LE(0x0460)
+#define VD56G3_REG_OUT_ROI_Y_START			CCI_REG16_LE(0x0462)
+#define VD56G3_REG_OUT_ROI_Y_END			CCI_REG16_LE(0x0464)
+#define VD56G3_REG_GPIO_0_CTRL				CCI_REG8(0x0467)
+#define VD56G3_GPIOX_GPIO_IN				0x01
+#define VD56G3_GPIOX_STROBE_MODE			0x02
+#define VD56G3_REG_READOUT_CTRL				CCI_REG8(0x047e)
+#define READOUT_NORMAL					0x00
+#define READOUT_DIGITAL_BINNING_X2			0x01
+
+/* The VD56G3 is a portrait image sensor with native resolution of 1124x1364. */
+#define VD56G3_NATIVE_WIDTH				1124
+#define VD56G3_NATIVE_HEIGHT				1364
+#define VD56G3_DEFAULT_MODE				0
+
+/* PLL settings */
+#define VD56G3_TARGET_PLL				804000000UL
+#define VD56G3_VT_CLOCK_DIV				5
+
+/* External clock must be in [6Mhz-27Mhz] */
+#define VD56G3_XCLK_FREQ_MIN				 (6 * HZ_PER_MHZ)
+#define VD56G3_XCLK_FREQ_MAX				 (27 * HZ_PER_MHZ)
+
+/* Line length and Frame length (settings are for standard 10bits ADC mode) */
+#define VD56G3_LINE_LENGTH_MIN				1236
+#define VD56G3_VBLANK_MIN				110
+#define VD56G3_FRAME_LENGTH_DEF_60FPS			2168
+#define VD56G3_FRAME_LENGTH_MAX				0xffff
+
+/* Exposure settings */
+#define VD56G3_EXPOSURE_MARGIN				75
+#define VD56G3_EXPOSURE_MIN				5
+#define VD56G3_EXPOSURE_DEFAULT				1420
+
+/* Output Interface settings */
+#define VD56G3_MAX_CSI_DATA_LANES			2
+#define VD56G3_LINK_FREQ_DEF_1LANE			750000000UL
+#define VD56G3_LINK_FREQ_DEF_2LANES			402000000UL
+
+/* GPIOs */
+#define VD56G3_NB_GPIOS					8
+
+/* regulator supplies */
+static const char *const vd56g3_supply_names[] = {
+	"vcore",
+	"vddio",
+	"vana",
+};
+
+/* -----------------------------------------------------------------------------
+ * Models (VD56G3: Mono, VD66GY: Bayer RGB), Modes and formats
+ */
+
+enum vd56g3_models {
+	VD56G3_MODEL_VD56G3,
+	VD56G3_MODEL_VD66GY,
+};
+
+struct vd56g3_mode {
+	u32 width;
+	u32 height;
+};
+
+static const struct vd56g3_mode vd56g3_supported_modes[] = {
+	{
+		.width = VD56G3_NATIVE_WIDTH,
+		.height = VD56G3_NATIVE_HEIGHT,
+	},
+	{
+		.width = 1120,
+		.height = 1360,
+	},
+	{
+		.width = 1024,
+		.height = 1280,
+	},
+	{
+		.width = 1024,
+		.height = 768,
+	},
+	{
+		.width = 768,
+		.height = 1024,
+	},
+	{
+		.width = 720,
+		.height = 1280,
+	},
+	{
+		.width = 640,
+		.height = 480,
+	},
+	{
+		.width = 480,
+		.height = 640,
+	},
+	{
+		.width = 320,
+		.height = 240,
+	},
+};
+
+/*
+ * Sensor support 8bits and 10bits output in both variants
+ *  - Monochrome
+ *  - RGB (with all H/V flip variations)
+ */
+static const unsigned int vd56g3_mbus_codes[2][5] = {
+	{
+		MEDIA_BUS_FMT_Y8_1X8,
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+	},
+};
+
+struct vd56g3 {
+	struct device *dev;
+	struct v4l2_subdev sd;
+	struct media_pad pad;
+	struct regulator_bulk_data supplies[ARRAY_SIZE(vd56g3_supply_names)];
+	struct gpio_desc *reset_gpio;
+	struct clk *xclk;
+	struct regmap *regmap;
+	u32 xclk_freq;
+	u32 pll_prediv;
+	u32 pll_mult;
+	u32 pixel_clock;
+	u16 oif_ctrl;
+	u8 nb_of_lane;
+	u32 gpios[VD56G3_NB_GPIOS];
+	unsigned long ext_leds_mask;
+	bool is_mono;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *hblank_ctrl;
+	struct v4l2_ctrl *vblank_ctrl;
+	struct {
+		struct v4l2_ctrl *hflip_ctrl;
+		struct v4l2_ctrl *vflip_ctrl;
+	};
+	struct v4l2_ctrl *patgen_ctrl;
+	struct {
+		struct v4l2_ctrl *ae_ctrl;
+		struct v4l2_ctrl *expo_ctrl;
+		struct v4l2_ctrl *again_ctrl;
+		struct v4l2_ctrl *dgain_ctrl;
+	};
+	struct v4l2_ctrl *ae_lock_ctrl;
+	struct v4l2_ctrl *ae_bias_ctrl;
+	struct v4l2_ctrl *led_ctrl;
+};
+
+static inline struct vd56g3 *to_vd56g3(struct v4l2_subdev *sd)
+{
+	return container_of_const(sd, struct vd56g3, sd);
+}
+
+static inline struct vd56g3 *ctrl_to_vd56g3(struct v4l2_ctrl *ctrl)
+{
+	return container_of_const(ctrl->handler, struct vd56g3, ctrl_handler);
+}
+
+/* -----------------------------------------------------------------------------
+ * Additional i2c register helpers
+ */
+
+static int vd56g3_poll_reg(struct vd56g3 *sensor, u32 reg, u8 poll_val,
+			   int *err)
+{
+	unsigned int val = 0;
+	int ret;
+
+	if (err && *err)
+		return *err;
+
+	/*
+	 * Timeout must be higher than longuest frame duration. With current
+	 * blanking constraints, frame duration can take up to 504ms.
+	 */
+	ret = regmap_read_poll_timeout(sensor->regmap, CCI_REG_ADDR(reg), val,
+				       (val == poll_val), 2000,
+				       600 * USEC_PER_MSEC);
+
+	if (ret && err)
+		*err = ret;
+
+	return ret;
+}
+
+static int vd56g3_wait_state(struct vd56g3 *sensor, int state, int *err)
+{
+	return vd56g3_poll_reg(sensor, VD56G3_REG_SYSTEM_FSM, state, err);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls: definitions, helpers and handlers
+ */
+
+static const char *const vd56g3_tp_menu[] = { "Disabled",
+					      "Solid Color",
+					      "Vertical Color Bars",
+					      "Horizontal Gray Scale",
+					      "Vertical Gray Scale",
+					      "Diagonal Gray Scale",
+					      "Pseudo Random" };
+
+static const s64 vd56g3_ev_bias_qmenu[] = { -4000, -3500, -3000, -2500, -2000,
+					    -1500, -1000, -500,	 0,	500,
+					    1000,  1500,  2000,	 2500,	3000,
+					    3500,  4000 };
+
+static const s64 vd56g3_link_freq_1lane[] = { VD56G3_LINK_FREQ_DEF_1LANE };
+
+static const s64 vd56g3_link_freq_2lanes[] = { VD56G3_LINK_FREQ_DEF_2LANES };
+
+static u8 vd56g3_get_bpp(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	default:
+		return 8;
+	case MEDIA_BUS_FMT_Y10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		return 10;
+	}
+}
+
+static u8 vd56g3_get_datatype(__u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_Y8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	default:
+		return MIPI_CSI2_DT_RAW8;
+	case MEDIA_BUS_FMT_Y10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		return MIPI_CSI2_DT_RAW10;
+	}
+}
+
+static int vd56g3_read_expo_cluster(struct vd56g3 *sensor, bool force_cur_val)
+{
+	u64 exposure;
+	u64 again;
+	u64 dgain;
+	int ret = 0;
+
+	/*
+	 * When 'force_cur_val' is enabled, save the ctrl value in 'cur.val'
+	 * instead of the normal 'val', this is used during poweroff to cache
+	 * volatile ctrls and enable coldstart.
+	 */
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_COARSE_EXPOSURE, &exposure,
+		 &ret);
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_ANALOG_GAIN, &again, &ret);
+	cci_read(sensor->regmap, VD56G3_REG_APPLIED_DIGITAL_GAIN, &dgain, &ret);
+	if (ret)
+		return ret;
+
+	if (force_cur_val) {
+		sensor->expo_ctrl->cur.val = exposure;
+		sensor->again_ctrl->cur.val = again;
+		sensor->dgain_ctrl->cur.val = dgain;
+	} else {
+		sensor->expo_ctrl->val = exposure;
+		sensor->again_ctrl->val = again;
+		sensor->dgain_ctrl->val = dgain;
+	}
+
+	return ret;
+}
+
+static int vd56g3_update_patgen(struct vd56g3 *sensor, u32 patgen_index)
+{
+	u32 pattern = patgen_index <= 2 ? patgen_index : patgen_index + 13;
+	u16 patgen = pattern << VD56G3_PATGEN_TYPE_SHIFT;
+	u8 duster = VD56G3_DUSTER_ENABLE_DEF_MODULES;
+	u8 darkcal = VD56G3_DARKCAL_ENABLE;
+	int ret = 0;
+
+	if (patgen_index) {
+		patgen |= VD56G3_PATGEN_ENABLE;
+		duster = VD56G3_DUSTER_DISABLE;
+		darkcal = VD56G3_DARKCAL_DISABLE_DARKAVG;
+	}
+
+	cci_write(sensor->regmap, VD56G3_REG_DUSTER_CTRL, duster, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_DARKCAL_CTRL, darkcal, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_PATGEN_CTRL, patgen, &ret);
+
+	return ret;
+}
+
+static int vd56g3_update_expo_cluster(struct vd56g3 *sensor, bool is_auto)
+{
+	u8 expo_state = is_auto ? VD56G3_EXP_MODE_AUTO : VD56G3_EXP_MODE_MANUAL;
+	int ret = 0;
+
+	if (sensor->ae_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_EXP_MODE, expo_state,
+			  &ret);
+
+	/* In Auto expo, set coldstart parameters */
+	if (is_auto && sensor->ae_ctrl->is_new) {
+		cci_write(sensor->regmap,
+			  VD56G3_REG_AE_COLDSTART_COARSE_EXPOSURE,
+			  sensor->expo_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_ANALOG_GAIN,
+			  sensor->again_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_AE_COLDSTART_DIGITAL_GAIN,
+			  sensor->dgain_ctrl->val, &ret);
+	}
+
+	/* In Manual expo, set exposure, analog and digital gains */
+	if (!is_auto && sensor->expo_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_COARSE_EXPOSURE,
+			  sensor->expo_ctrl->val, &ret);
+
+	if (!is_auto && sensor->again_ctrl->is_new)
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_ANALOG_GAIN,
+			  sensor->again_ctrl->val, &ret);
+
+	if (!is_auto && sensor->dgain_ctrl->is_new) {
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH0,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH1,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH2,
+			  sensor->dgain_ctrl->val, &ret);
+		cci_write(sensor->regmap, VD56G3_REG_MANUAL_DIGITAL_GAIN_CH3,
+			  sensor->dgain_ctrl->val, &ret);
+	}
+
+	return ret;
+}
+
+static int vd56g3_lock_exposure(struct vd56g3 *sensor, u32 lock_val)
+{
+	bool ae_lock = lock_val & V4L2_LOCK_EXPOSURE;
+	u8 expo_state = ae_lock ? VD56G3_EXP_MODE_FREEZE : VD56G3_EXP_MODE_AUTO;
+
+	if (sensor->ae_ctrl->val == V4L2_EXPOSURE_AUTO)
+		return cci_write(sensor->regmap, VD56G3_REG_EXP_MODE,
+				 expo_state, NULL);
+
+	return 0;
+}
+
+static int vd56g3_write_gpiox(struct vd56g3 *sensor, unsigned long gpio_mask)
+{
+	unsigned long io;
+	u32 gpio_val;
+	int ret = 0;
+
+	for_each_set_bit(io, &gpio_mask, VD56G3_NB_GPIOS) {
+		gpio_val = sensor->gpios[io];
+
+		if (gpio_val == VD56G3_GPIOX_STROBE_MODE &&
+		    sensor->led_ctrl->val == V4L2_FLASH_LED_MODE_NONE)
+			gpio_val = VD56G3_GPIOX_GPIO_IN;
+
+		cci_write(sensor->regmap, VD56G3_REG_GPIO_0_CTRL + io, gpio_val,
+			  &ret);
+	}
+
+	return ret;
+}
+
+static int vd56g3_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
+	int ret = 0;
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd56g3_read_expo_cluster(sensor, false);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static int vd56g3_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vd56g3 *sensor = ctrl_to_vd56g3(ctrl);
+	struct v4l2_subdev_state *state;
+	const struct v4l2_rect *crop;
+	unsigned int frame_length = 0;
+	unsigned int expo_max;
+	unsigned int ae_compensation;
+	bool is_auto = false;
+	int ret = 0;
+
+	state = v4l2_subdev_get_locked_active_state(&sensor->sd);
+	crop = v4l2_subdev_state_get_crop(state, 0);
+
+	if (ctrl->flags & V4L2_CTRL_FLAG_READ_ONLY)
+		return 0;
+
+	/* Update controls state, range, etc. whatever the state of the HW */
+	switch (ctrl->id) {
+	case V4L2_CID_VBLANK:
+		frame_length = crop->height + ctrl->val;
+		expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
+		ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl,
+					       VD56G3_EXPOSURE_MIN, expo_max, 1,
+					       min(VD56G3_EXPOSURE_DEFAULT,
+						   expo_max));
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		is_auto = (ctrl->val == V4L2_EXPOSURE_AUTO);
+		__v4l2_ctrl_grab(sensor->ae_lock_ctrl, !is_auto);
+		__v4l2_ctrl_grab(sensor->ae_bias_ctrl, !is_auto);
+		break;
+	default:
+		break;
+	}
+
+	if (ret)
+		return ret;
+
+	/* Interact with HW only when it is powered ON */
+	if (!pm_runtime_get_if_in_use(sensor->dev))
+		return 0;
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		ret = cci_write(sensor->regmap, VD56G3_REG_ORIENTATION,
+				sensor->hflip_ctrl->val |
+					(sensor->vflip_ctrl->val << 1),
+				NULL);
+		break;
+	case V4L2_CID_TEST_PATTERN:
+		ret = vd56g3_update_patgen(sensor, ctrl->val);
+		break;
+	case V4L2_CID_EXPOSURE_AUTO:
+		ret = vd56g3_update_expo_cluster(sensor, is_auto);
+		break;
+	case V4L2_CID_3A_LOCK:
+		ret = vd56g3_lock_exposure(sensor, ctrl->val);
+		break;
+	case V4L2_CID_AUTO_EXPOSURE_BIAS:
+		ae_compensation =
+			DIV_ROUND_CLOSEST((int)vd56g3_ev_bias_qmenu[ctrl->val] *
+					  256, 1000);
+		ret = cci_write(sensor->regmap, VD56G3_REG_AE_COMPENSATION,
+				ae_compensation, NULL);
+		break;
+	case V4L2_CID_VBLANK:
+		ret = cci_write(sensor->regmap, VD56G3_REG_FRAME_LENGTH,
+				frame_length, NULL);
+		break;
+	case V4L2_CID_FLASH_LED_MODE:
+		ret = vd56g3_write_gpiox(sensor, sensor->ext_leds_mask);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops vd56g3_ctrl_ops = {
+	.g_volatile_ctrl = vd56g3_g_volatile_ctrl,
+	.s_ctrl = vd56g3_s_ctrl,
+};
+
+static int vd56g3_update_controls(struct vd56g3 *sensor)
+{
+	struct v4l2_subdev_state *state;
+	const struct v4l2_rect *crop;
+	unsigned int hblank;
+	unsigned int vblank_min, vblank, vblank_max;
+	unsigned int frame_length;
+	unsigned int expo_max;
+	int ret;
+
+	state = v4l2_subdev_get_locked_active_state(&sensor->sd);
+	crop = v4l2_subdev_state_get_crop(state, 0);
+	hblank = VD56G3_LINE_LENGTH_MIN - crop->width;
+	vblank_min = VD56G3_VBLANK_MIN;
+	vblank = VD56G3_FRAME_LENGTH_DEF_60FPS - crop->height;
+	vblank_max = VD56G3_FRAME_LENGTH_MAX - crop->height;
+	frame_length = crop->height + vblank;
+	expo_max = frame_length - VD56G3_EXPOSURE_MARGIN;
+
+	/* Update blanking and exposure (ranges + values) */
+	ret = __v4l2_ctrl_modify_range(sensor->hblank_ctrl, hblank, hblank, 1,
+				       hblank);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_modify_range(sensor->vblank_ctrl, vblank_min,
+				       vblank_max, 1, vblank);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_s_ctrl(sensor->vblank_ctrl, vblank);
+	if (ret)
+		return ret;
+
+	ret = __v4l2_ctrl_modify_range(sensor->expo_ctrl, VD56G3_EXPOSURE_MIN,
+				       expo_max, 1, VD56G3_EXPOSURE_DEFAULT);
+	if (ret)
+		return ret;
+
+	return __v4l2_ctrl_s_ctrl(sensor->expo_ctrl, VD56G3_EXPOSURE_DEFAULT);
+}
+
+static int vd56g3_init_controls(struct vd56g3 *sensor)
+{
+	const struct v4l2_ctrl_ops *ops = &vd56g3_ctrl_ops;
+	struct v4l2_ctrl_handler *hdl = &sensor->ctrl_handler;
+	struct v4l2_fwnode_device_properties fwnode_props;
+	struct v4l2_ctrl *ctrl;
+	int ret;
+
+	v4l2_ctrl_handler_init(hdl, 25);
+
+	/* Horizontal & vertical flips modify bayer code on RGB variant */
+	sensor->hflip_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+	if (sensor->hflip_ctrl)
+		sensor->hflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	sensor->vflip_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+	if (sensor->vflip_ctrl)
+		sensor->vflip_ctrl->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
+
+	sensor->patgen_ctrl =
+		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
+					     ARRAY_SIZE(vd56g3_tp_menu) - 1, 0,
+					     0, vd56g3_tp_menu);
+
+	ctrl = v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_LINK_FREQ,
+				      ARRAY_SIZE(vd56g3_link_freq_1lane) - 1, 0,
+				      (sensor->nb_of_lane == 2) ?
+					      vd56g3_link_freq_2lanes :
+					      vd56g3_link_freq_1lane);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
+				 sensor->pixel_clock, sensor->pixel_clock, 1,
+				 sensor->pixel_clock);
+	if (ctrl)
+		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	sensor->ae_ctrl = v4l2_ctrl_new_std_menu(hdl, ops,
+						 V4L2_CID_EXPOSURE_AUTO,
+						 V4L2_EXPOSURE_MANUAL, 0,
+						 V4L2_EXPOSURE_AUTO);
+
+	sensor->ae_lock_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_3A_LOCK, 0,
+						 GENMASK(2, 0), 0, 0);
+
+	sensor->ae_bias_ctrl =
+		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
+				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) - 1,
+				       ARRAY_SIZE(vd56g3_ev_bias_qmenu) / 2,
+				       vd56g3_ev_bias_qmenu);
+
+	/*
+	 * Analog gain [1, 8] is computed with the following logic :
+	 * 32/(32 - again_reg), with again_reg in the range [0:28]
+	 * Digital gain [1.00, 8.00] is coded as a Fixed Point 5.8
+	 */
+	sensor->again_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
+					       0, 28, 1, 0);
+	sensor->dgain_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
+					       0x100, 0x800, 1, 0x100);
+
+	/*
+	 * Set the exposure, horizontal and vertical blanking ctrls
+	 * to hardcoded values, they will be updated in vd56g3_update_controls.
+	 * Exposure being in an auto-cluster, set a significant value here.
+	 */
+	sensor->expo_ctrl = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
+					      VD56G3_EXPOSURE_DEFAULT,
+					      VD56G3_EXPOSURE_DEFAULT, 1,
+					      VD56G3_EXPOSURE_DEFAULT);
+	sensor->hblank_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, 1, 1, 1, 1);
+	if (sensor->hblank_ctrl)
+		sensor->hblank_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	sensor->vblank_ctrl =
+		v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK, 1, 1, 1, 1);
+
+	/* Additional control based on device tree properties */
+	if (sensor->ext_leds_mask)
+		sensor->led_ctrl =
+			v4l2_ctrl_new_std_menu(hdl, ops,
+					       V4L2_CID_FLASH_LED_MODE,
+					       V4L2_FLASH_LED_MODE_FLASH, 0,
+					       V4L2_FLASH_LED_MODE_NONE);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		goto free_ctrls;
+	}
+
+	v4l2_ctrl_cluster(2, &sensor->hflip_ctrl);
+	v4l2_ctrl_auto_cluster(4, &sensor->ae_ctrl, V4L2_EXPOSURE_MANUAL, true);
+
+	/* Optional controls coming from fwnode (e.g. rotation, orientation). */
+	ret = v4l2_fwnode_device_parse(sensor->dev, &fwnode_props);
+	if (ret)
+		goto free_ctrls;
+
+	ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &fwnode_props);
+	if (ret)
+		goto free_ctrls;
+
+	sensor->sd.ctrl_handler = hdl;
+
+	return 0;
+
+free_ctrls:
+	v4l2_ctrl_handler_free(hdl);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Pad ops
+ */
+
+/* Media bus code is dependent of :
+ *      - 8bits or 10bits output
+ *      - variant : Mono or RGB
+ *      - H/V flips parameters in case of RGB
+ */
+static u32 vd56g3_get_mbus_code(struct vd56g3 *sensor, u32 code)
+{
+	unsigned int i_bpp;
+	unsigned int j;
+
+	for (i_bpp = 0; i_bpp < ARRAY_SIZE(vd56g3_mbus_codes); i_bpp++) {
+		for (j = 0; j < ARRAY_SIZE(vd56g3_mbus_codes[i_bpp]); j++) {
+			if (vd56g3_mbus_codes[i_bpp][j] == code)
+				goto endloops;
+		}
+	}
+
+endloops:
+	if (i_bpp >= ARRAY_SIZE(vd56g3_mbus_codes))
+		i_bpp = 0;
+
+	if (sensor->is_mono)
+		j = 0;
+	else
+		j = 1 + (sensor->hflip_ctrl->val ? 1 : 0) +
+		    (sensor->vflip_ctrl->val ? 2 : 0);
+
+	return vd56g3_mbus_codes[i_bpp][j];
+}
+
+static int vd56g3_enum_mbus_code(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state,
+				 struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	if (code->index >= ARRAY_SIZE(vd56g3_mbus_codes))
+		return -EINVAL;
+
+	code->code =
+		vd56g3_get_mbus_code(sensor, vd56g3_mbus_codes[code->index][0]);
+
+	return 0;
+}
+
+static int vd56g3_enum_frame_size(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *sd_state,
+				  struct v4l2_subdev_frame_size_enum *fse)
+{
+	if (fse->index >= ARRAY_SIZE(vd56g3_supported_modes))
+		return -EINVAL;
+
+	fse->min_width = vd56g3_supported_modes[fse->index].width;
+	fse->max_width = fse->min_width;
+	fse->min_height = vd56g3_supported_modes[fse->index].height;
+	fse->max_height = fse->min_height;
+
+	return 0;
+}
+
+static void vd56g3_update_img_pad_format(struct vd56g3 *sensor,
+					 const struct vd56g3_mode *mode,
+					 u32 mbus_code,
+					 struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	mbus_fmt->width = mode->width;
+	mbus_fmt->height = mode->height;
+	mbus_fmt->code = vd56g3_get_mbus_code(sensor, mbus_code);
+	mbus_fmt->colorspace = V4L2_COLORSPACE_RAW;
+	mbus_fmt->field = V4L2_FIELD_NONE;
+	mbus_fmt->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	mbus_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	mbus_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+}
+
+static int vd56g3_set_pad_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *sd_fmt)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	const struct vd56g3_mode *new_mode;
+	struct v4l2_rect pad_crop;
+	unsigned int binning;
+
+	new_mode = v4l2_find_nearest_size(vd56g3_supported_modes,
+					  ARRAY_SIZE(vd56g3_supported_modes),
+					  width, height, sd_fmt->format.width,
+					  sd_fmt->format.height);
+
+	vd56g3_update_img_pad_format(sensor, new_mode, sd_fmt->format.code,
+				     &sd_fmt->format);
+	*v4l2_subdev_state_get_format(sd_state, sd_fmt->pad) = sd_fmt->format;
+
+	/* Compute and update crop rectangle (maximized via binning) */
+	binning = min(VD56G3_NATIVE_WIDTH / sd_fmt->format.width,
+		      VD56G3_NATIVE_HEIGHT / sd_fmt->format.height);
+	binning = min(binning, 2U);
+	pad_crop.width = sd_fmt->format.width * binning;
+	pad_crop.height = sd_fmt->format.height * binning;
+	pad_crop.left = (VD56G3_NATIVE_WIDTH - pad_crop.width) / 2;
+	pad_crop.top = (VD56G3_NATIVE_HEIGHT - pad_crop.height) / 2;
+	*v4l2_subdev_state_get_crop(sd_state, sd_fmt->pad) = pad_crop;
+
+	/* Update controls in case of active state */
+	if (sd_fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+		return vd56g3_update_controls(sensor);
+
+	return 0;
+}
+
+static int vd56g3_get_selection(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *sd_state,
+				struct v4l2_subdev_selection *sel)
+{
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, 0);
+		break;
+	case V4L2_SEL_TGT_NATIVE_SIZE:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.top = 0;
+		sel->r.left = 0;
+		sel->r.width = VD56G3_NATIVE_WIDTH;
+		sel->r.height = VD56G3_NATIVE_HEIGHT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vd56g3_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				 struct v4l2_mbus_frame_desc *fd)
+{
+	struct v4l2_subdev_state *state;
+	const struct v4l2_mbus_framefmt *format;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	format = v4l2_subdev_state_get_format(state, pad);
+	v4l2_subdev_unlock_state(state);
+
+	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
+	fd->num_entries = 1;
+	fd->entry[0].pixelcode = format->code;
+	fd->entry[0].stream = 0;
+	fd->entry[0].bus.csi2.vc = 0;
+	fd->entry[0].bus.csi2.dt = vd56g3_get_datatype(format->code);
+
+	return 0;
+}
+
+static int vd56g3_enable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state, u32 pad,
+				 u64 streams_mask)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	const struct v4l2_mbus_framefmt *format =
+		v4l2_subdev_state_get_format(state, 0);
+	const struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
+	unsigned int csi_mbps = ((sensor->nb_of_lane == 2) ?
+					 VD56G3_LINK_FREQ_DEF_2LANES :
+					 VD56G3_LINK_FREQ_DEF_1LANE) *
+				2 / MEGA;
+	unsigned int binning;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(sensor->dev);
+	if (ret < 0)
+		return ret;
+
+	/* configure clocks */
+	cci_write(sensor->regmap, VD56G3_REG_EXT_CLOCK, sensor->xclk_freq,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_CLK_PLL_PREDIV, sensor->pll_prediv,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_CLK_SYS_PLL_MULT, sensor->pll_mult,
+		  &ret);
+
+	/* configure output */
+	cci_write(sensor->regmap, VD56G3_REG_FORMAT_CTRL,
+		  vd56g3_get_bpp(format->code), &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_CTRL, sensor->oif_ctrl, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_CSI_BITRATE, csi_mbps, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OIF_IMG_CTRL,
+		  vd56g3_get_datatype(format->code), &ret);
+	cci_write(sensor->regmap, VD56G3_REG_ISL_ENABLE, 0, &ret);
+
+	/* configure binning mode */
+	switch (crop->width / format->width) {
+	case 1:
+	default:
+		binning = READOUT_NORMAL;
+		break;
+	case 2:
+		binning = READOUT_DIGITAL_BINNING_X2;
+		break;
+	}
+	cci_write(sensor->regmap, VD56G3_REG_READOUT_CTRL, binning, &ret);
+
+	/* configure ROIs */
+	cci_write(sensor->regmap, VD56G3_REG_Y_START, crop->top, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_Y_END,
+		  crop->top + crop->height - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_START, crop->left, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_X_END,
+		  crop->left + crop->width - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_START, 0, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_OUT_ROI_Y_END, crop->height - 1,
+		  &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_H, crop->left, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_H,
+		  crop->left + crop->width - 1, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_START_V, 0, &ret);
+	cci_write(sensor->regmap, VD56G3_REG_AE_ROI_END_V, crop->height - 1,
+		  &ret);
+	if (ret)
+		goto rpm_put;
+
+	/* Setup default GPIO values; could be overridden by V4L2 ctrl setup */
+	ret = vd56g3_write_gpiox(sensor, GENMASK(VD56G3_NB_GPIOS - 1, 0));
+	if (ret)
+		goto rpm_put;
+
+	/* Apply settings from V4L2 ctrls */
+	ret = __v4l2_ctrl_handler_setup(&sensor->ctrl_handler);
+	if (ret)
+		goto rpm_put;
+
+	/* start streaming */
+	cci_write(sensor->regmap, VD56G3_REG_STBY, VD56G3_CMD_START_STREAM,
+		  &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_STBY, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_STREAMING, &ret);
+	if (ret)
+		goto rpm_put;
+
+	/* some controls are locked during streaming */
+	__v4l2_ctrl_grab(sensor->hflip_ctrl, true);
+	__v4l2_ctrl_grab(sensor->vflip_ctrl, true);
+	__v4l2_ctrl_grab(sensor->patgen_ctrl, true);
+
+	return ret;
+
+rpm_put:
+	dev_err(sensor->dev, "Failed to start streaming\n");
+	pm_runtime_put_sync(sensor->dev);
+
+	return ret;
+}
+
+static int vd56g3_disable_streams(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state, u32 pad,
+				  u64 streams_mask)
+{
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	int ret;
+
+	/* Retrieve Expo cluster to enable coldstart of AE */
+	ret = vd56g3_read_expo_cluster(sensor, true);
+
+	cci_write(sensor->regmap, VD56G3_REG_STREAMING, VD56G3_CMD_STOP_STREAM,
+		  &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_STREAMING, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
+
+	/* locked controls must be unlocked */
+	__v4l2_ctrl_grab(sensor->hflip_ctrl, false);
+	__v4l2_ctrl_grab(sensor->vflip_ctrl, false);
+	__v4l2_ctrl_grab(sensor->patgen_ctrl, false);
+
+	pm_runtime_mark_last_busy(sensor->dev);
+	pm_runtime_put_autosuspend(sensor->dev);
+
+	return ret;
+}
+
+static int vd56g3_init_state(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *sd_state)
+{
+	unsigned int def_mode = VD56G3_DEFAULT_MODE;
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_TRY,
+		.pad = 0,
+		.format = {
+			.code = vd56g3_mbus_codes[0][0],
+			.width = vd56g3_supported_modes[def_mode].width,
+			.height = vd56g3_supported_modes[def_mode].height,
+		},
+	};
+
+	return vd56g3_set_pad_fmt(sd, sd_state, &fmt);
+}
+
+static const struct v4l2_subdev_video_ops vd56g3_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_pad_ops vd56g3_pad_ops = {
+	.enum_mbus_code = vd56g3_enum_mbus_code,
+	.enum_frame_size = vd56g3_enum_frame_size,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = vd56g3_set_pad_fmt,
+	.get_selection = vd56g3_get_selection,
+	.get_frame_desc = vd56g3_get_frame_desc,
+	.enable_streams = vd56g3_enable_streams,
+	.disable_streams = vd56g3_disable_streams,
+};
+
+static const struct v4l2_subdev_ops vd56g3_subdev_ops = {
+	.video = &vd56g3_video_ops,
+	.pad = &vd56g3_pad_ops,
+};
+
+static const struct media_entity_operations vd56g3_subdev_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static const struct v4l2_subdev_internal_ops vd56g3_internal_ops = {
+	.init_state = vd56g3_init_state,
+};
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int vd56g3_power_on(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+	int ret;
+
+	/* power on */
+	ret = regulator_bulk_enable(ARRAY_SIZE(sensor->supplies),
+				    sensor->supplies);
+	if (ret) {
+		dev_err(dev, "Failed to enable regulators: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(sensor->xclk);
+	if (ret) {
+		dev_err(dev, "Failed to enable clock: %d\n", ret);
+		goto disable_reg;
+	}
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 0);
+	usleep_range(3500, 4000);
+	ret = vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_READY_TO_BOOT, NULL);
+	if (ret) {
+		dev_err(dev, "Sensor reset failed: %d\n", ret);
+		goto disable_clock;
+	}
+
+	/* boot sensor */
+	cci_write(sensor->regmap, VD56G3_REG_BOOT, VD56G3_CMD_BOOT, &ret);
+	vd56g3_poll_reg(sensor, VD56G3_REG_BOOT, VD56G3_CMD_ACK, &ret);
+	vd56g3_wait_state(sensor, VD56G3_SYSTEM_FSM_SW_STBY, &ret);
+	if (ret) {
+		dev_err(dev, "Sensor boot failed: %d\n", ret);
+		goto disable_clock;
+	}
+
+	return 0;
+
+disable_clock:
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	clk_disable_unprepare(sensor->xclk);
+disable_reg:
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+
+	return ret;
+}
+
+static int vd56g3_power_off(struct device *dev)
+{
+	struct v4l2_subdev *sd = dev_get_drvdata(dev);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	gpiod_set_value_cansleep(sensor->reset_gpio, 1);
+	clk_disable_unprepare(sensor->xclk);
+	regulator_bulk_disable(ARRAY_SIZE(sensor->supplies), sensor->supplies);
+
+	return 0;
+}
+
+static const struct dev_pm_ops vd56g3_pm_ops = {
+	SET_RUNTIME_PM_OPS(vd56g3_power_off, vd56g3_power_on, NULL)
+};
+
+/* -----------------------------------------------------------------------------
+ * Probe and initialization
+ */
+
+static int vd56g3_check_csi_conf(struct vd56g3 *sensor,
+				 struct fwnode_handle *endpoint)
+{
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	u32 phy_data_lanes[VD56G3_MAX_CSI_DATA_LANES] = { ~0, ~0 };
+	u8 n_lanes;
+	u64 frequency;
+	int p, l;
+	int ret = 0;
+
+	ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep);
+	if (ret)
+		return -EINVAL;
+
+	/* Check lanes number */
+	n_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	if (n_lanes != 1 && n_lanes != 2) {
+		dev_err(sensor->dev, "Invalid data lane number: %d\n", n_lanes);
+		ret = -EINVAL;
+		goto done;
+	}
+	sensor->nb_of_lane = n_lanes;
+
+	/* Clock lane must be first */
+	if (ep.bus.mipi_csi2.clock_lane != 0) {
+		dev_err(sensor->dev, "Clock lane must be mapped to lane 0\n");
+		ret = -EINVAL;
+		goto done;
+	}
+
+	/*
+	 * Prepare Output Interface conf based on lane settings
+	 * logical to physical lane conversion (+ pad remaining slots)
+	 */
+	for (l = 0; l < n_lanes; l++)
+		phy_data_lanes[ep.bus.mipi_csi2.data_lanes[l] - 1] = l;
+	for (p = 0; p < VD56G3_MAX_CSI_DATA_LANES; p++) {
+		if (phy_data_lanes[p] != ~0)
+			continue;
+		phy_data_lanes[p] = l;
+		l++;
+	}
+	sensor->oif_ctrl = n_lanes |
+			   (ep.bus.mipi_csi2.lane_polarities[0] << 3) |
+			   ((phy_data_lanes[0]) << 4) |
+			   (ep.bus.mipi_csi2.lane_polarities[1] << 6) |
+			   ((phy_data_lanes[1]) << 7) |
+			   (ep.bus.mipi_csi2.lane_polarities[2] << 9);
+
+	/* Check link frequency */
+	if (!ep.nr_of_link_frequencies) {
+		dev_err(sensor->dev, "link-frequency not found in DT\n");
+		ret = -EINVAL;
+		goto done;
+	}
+	frequency = (n_lanes == 2) ? VD56G3_LINK_FREQ_DEF_2LANES :
+				     VD56G3_LINK_FREQ_DEF_1LANE;
+	if (ep.nr_of_link_frequencies != 1 ||
+	    ep.link_frequencies[0] != frequency) {
+		dev_err(sensor->dev, "Link frequency not supported: %lld\n",
+			ep.link_frequencies[0]);
+		ret = -EINVAL;
+		goto done;
+	}
+
+done:
+	v4l2_fwnode_endpoint_free(&ep);
+
+	return ret;
+}
+
+static int vd56g3_parse_dt_gpios_array(struct vd56g3 *sensor, char *prop_name,
+				       u32 *array, unsigned int *nb)
+{
+	struct device *dev = sensor->dev;
+	unsigned int i;
+	int ret;
+
+	if (!device_property_present(dev, prop_name)) {
+		*nb = 0;
+		return 0;
+	}
+
+	ret = device_property_count_u32(dev, prop_name);
+	if (ret < 0) {
+		dev_err(dev, "Failed to read %s count\n", prop_name);
+		return ret;
+	}
+
+	*nb = ret;
+	ret = device_property_read_u32_array(dev, prop_name, array, *nb);
+	if (ret) {
+		dev_err(dev, "Failed to read %s prop\n", prop_name);
+		return ret;
+	}
+
+	for (i = 0; i < *nb; i++) {
+		if (array[i] >= VD56G3_NB_GPIOS) {
+			dev_err(dev, "Invalid GPIO: %d\n", array[i]);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int vd56g3_parse_dt_gpios(struct vd56g3 *sensor)
+{
+	u32 led_gpios[VD56G3_NB_GPIOS];
+	unsigned int nb_gpios_leds;
+	unsigned int i;
+	int ret;
+
+	/* Initialize GPIOs to default */
+	for (i = 0; i < VD56G3_NB_GPIOS; i++)
+		sensor->gpios[i] = VD56G3_GPIOX_GPIO_IN;
+	sensor->ext_leds_mask = 0;
+
+	/* Take into account optional 'st,leds' output for GPIOs */
+	ret = vd56g3_parse_dt_gpios_array(sensor, "st,leds", led_gpios,
+					  &nb_gpios_leds);
+	if (ret)
+		return ret;
+	for (i = 0; i < nb_gpios_leds; i++) {
+		sensor->gpios[led_gpios[i]] = VD56G3_GPIOX_STROBE_MODE;
+		set_bit(led_gpios[i], &sensor->ext_leds_mask);
+	}
+
+	return 0;
+}
+
+static int vd56g3_parse_dt(struct vd56g3 *sensor)
+{
+	struct fwnode_handle *endpoint;
+	int ret;
+
+	endpoint = fwnode_graph_get_endpoint_by_id(dev_fwnode(sensor->dev), 0,
+						   0, 0);
+	if (!endpoint) {
+		dev_err(sensor->dev, "Endpoint node not found\n");
+		return -EINVAL;
+	}
+
+	ret = vd56g3_check_csi_conf(sensor, endpoint);
+	fwnode_handle_put(endpoint);
+	if (ret)
+		return ret;
+
+	return vd56g3_parse_dt_gpios(sensor);
+}
+
+static int vd56g3_get_regulators(struct vd56g3 *sensor)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(sensor->supplies); i++)
+		sensor->supplies[i].supply = vd56g3_supply_names[i];
+
+	return devm_regulator_bulk_get(sensor->dev,
+				       ARRAY_SIZE(sensor->supplies),
+				       sensor->supplies);
+}
+
+static int vd56g3_prepare_clock_tree(struct vd56g3 *sensor)
+{
+	const unsigned int predivs[] = { 1, 2, 4 };
+	u32 pll_out;
+	int i;
+
+	/* External clock must be in [6Mhz-27Mhz] */
+	if (sensor->xclk_freq < VD56G3_XCLK_FREQ_MIN ||
+	    sensor->xclk_freq > VD56G3_XCLK_FREQ_MAX) {
+		dev_err(sensor->dev,
+			"Only 6Mhz-27Mhz clock range supported. Provided %lu MHz\n",
+			sensor->xclk_freq / HZ_PER_MHZ);
+		return -EINVAL;
+	}
+
+	/* PLL input should be in [6Mhz-12Mhz[ */
+	for (i = 0; i < ARRAY_SIZE(predivs); i++) {
+		sensor->pll_prediv = predivs[i];
+		if (sensor->xclk_freq / sensor->pll_prediv < 12 * HZ_PER_MHZ)
+			break;
+	}
+
+	/* PLL output clock must be as close as possible to 804Mhz */
+	sensor->pll_mult = (VD56G3_TARGET_PLL * sensor->pll_prediv +
+			    sensor->xclk_freq / 2) /
+			   sensor->xclk_freq;
+	pll_out = sensor->xclk_freq * sensor->pll_mult / sensor->pll_prediv;
+
+	/* Target Pixel Clock for standard 10bit ADC mode : 160.8Mhz */
+	sensor->pixel_clock = pll_out / VD56G3_VT_CLOCK_DIV;
+
+	return 0;
+}
+
+static int vd56g3_detect(struct vd56g3 *sensor)
+{
+	struct device *dev = sensor->dev;
+	unsigned int model;
+	u64 model_id;
+	u64 device_revision;
+	u64 optical_revision;
+	int ret = 0;
+
+	model = (uintptr_t)device_get_match_data(dev);
+
+	ret = cci_read(sensor->regmap, VD56G3_REG_MODEL_ID, &model_id, NULL);
+	if (ret)
+		return ret;
+
+	if (model_id != VD56G3_MODEL_ID) {
+		dev_err(dev, "Unsupported sensor id: %x\n", (u16)model_id);
+		return -ENODEV;
+	}
+
+	ret = cci_read(sensor->regmap, VD56G3_REG_REVISION, &device_revision,
+		       NULL);
+	if (ret)
+		return ret;
+
+	if ((device_revision >> 8) != VD56G3_REVISION_CUT3) {
+		dev_err(dev, "Unsupported version: %x\n", (u16)device_revision);
+		return -ENODEV;
+	}
+
+	ret = cci_read(sensor->regmap, VD56G3_REG_OPTICAL_REVISION,
+		       &optical_revision, NULL);
+	if (ret)
+		return ret;
+
+	sensor->is_mono =
+		((optical_revision & 1) == VD56G3_OPTICAL_REVISION_MONO);
+	if ((sensor->is_mono && model == VD56G3_MODEL_VD66GY) ||
+	    (!sensor->is_mono && model == VD56G3_MODEL_VD56G3)) {
+		dev_err(dev, "Found %s sensor, while %s model is defined in DT\n",
+			(sensor->is_mono) ? "Mono" : "Bayer",
+			(model == VD56G3_MODEL_VD56G3) ? "vd56g3" : "vd66gy");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int vd56g3_subdev_init(struct vd56g3 *sensor)
+{
+	struct v4l2_subdev_state *state;
+	int ret;
+
+	/* Init remaining sub device ops */
+	sensor->sd.internal_ops = &vd56g3_internal_ops;
+	sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sensor->sd.entity.ops = &vd56g3_subdev_entity_ops;
+
+	/* Init source pad */
+	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
+	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
+	if (ret) {
+		dev_err(sensor->dev, "Failed to init media entity: %d\n", ret);
+		return ret;
+	}
+
+	/* Init controls */
+	ret = vd56g3_init_controls(sensor);
+	if (ret) {
+		dev_err(sensor->dev, "Controls initialization failed: %d\n",
+			ret);
+		goto err_media;
+	}
+
+	/* Init vd56g3 struct : default resolution + raw8 */
+	sensor->sd.state_lock = sensor->ctrl_handler.lock;
+	ret = v4l2_subdev_init_finalize(&sensor->sd);
+	if (ret) {
+		dev_err(sensor->dev, "Subdev init failed: %d\n", ret);
+		goto err_ctrls;
+	}
+
+	/* Update controls according to the resolution set */
+	state = v4l2_subdev_lock_and_get_active_state(&sensor->sd);
+	ret = vd56g3_update_controls(sensor);
+	v4l2_subdev_unlock_state(state);
+	if (ret) {
+		dev_err(sensor->dev, "Controls update failed: %d\n", ret);
+		goto err_ctrls;
+	}
+
+	return 0;
+
+err_ctrls:
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+
+err_media:
+	media_entity_cleanup(&sensor->sd.entity);
+
+	return ret;
+}
+
+static void vd56g3_subdev_cleanup(struct vd56g3 *sensor)
+{
+	v4l2_async_unregister_subdev(&sensor->sd);
+	v4l2_subdev_cleanup(&sensor->sd);
+	media_entity_cleanup(&sensor->sd.entity);
+	v4l2_ctrl_handler_free(sensor->sd.ctrl_handler);
+}
+
+static int vd56g3_probe(struct i2c_client *client)
+{
+	struct device *dev = &client->dev;
+	struct vd56g3 *sensor;
+	int ret;
+
+	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
+	if (!sensor)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(&sensor->sd, client, &vd56g3_subdev_ops);
+	sensor->dev = dev;
+
+	ret = vd56g3_parse_dt(sensor);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to parse Device Tree\n");
+
+	/* Get (and check) resources : power regs, ext clock, reset gpio */
+	ret = vd56g3_get_regulators(sensor);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get regulators\n");
+
+	sensor->xclk = devm_clk_get(dev, NULL);
+	if (IS_ERR(sensor->xclk))
+		return dev_err_probe(dev, PTR_ERR(sensor->xclk),
+				     "Failed to get xclk\n");
+	sensor->xclk_freq = clk_get_rate(sensor->xclk);
+	ret = vd56g3_prepare_clock_tree(sensor);
+	if (ret)
+		return ret;
+
+	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
+						     GPIOD_OUT_HIGH);
+	if (IS_ERR(sensor->reset_gpio))
+		return dev_err_probe(dev, PTR_ERR(sensor->reset_gpio),
+				     "Failed to get reset gpio\n");
+
+	sensor->regmap = devm_cci_regmap_init_i2c(client, 16);
+	if (IS_ERR(sensor->regmap))
+		return dev_err_probe(dev, PTR_ERR(sensor->regmap),
+				     "Failed to init regmap\n");
+
+	/* Power ON */
+	ret = vd56g3_power_on(dev);
+	if (ret)
+		return dev_err_probe(dev, ret, "Sensor power on failed\n");
+
+	/* Enable PM runtime with autosuspend (sensor being ON, set active) */
+	pm_runtime_set_active(dev);
+	pm_runtime_get_noresume(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_set_autosuspend_delay(dev, 1000);
+	pm_runtime_use_autosuspend(dev);
+
+	/* Check HW model/version */
+	ret = vd56g3_detect(sensor);
+	if (ret) {
+		dev_err(dev, "Sensor detect failed: %d\n", ret);
+		goto err_power_off;
+	}
+
+	/* Initialize & register subdev (v4l2_i2c subdev already initialized) */
+	ret = vd56g3_subdev_init(sensor);
+	if (ret) {
+		dev_err(dev, "V4l2 init failed: %d\n", ret);
+		goto err_power_off;
+	}
+
+	ret = v4l2_async_register_subdev(&sensor->sd);
+	if (ret) {
+		dev_err(dev, "Async subdev register failed: %d\n", ret);
+		goto err_subdev;
+	}
+
+	/* Sensor could now be powered off (after the autosuspend delay) */
+	pm_runtime_mark_last_busy(dev);
+	pm_runtime_put_autosuspend(dev);
+
+	dev_dbg(dev, "Successfully probe %s sensor\n",
+		(sensor->is_mono) ? "vd56g3" : "vd66gy");
+
+	return 0;
+
+err_subdev:
+	vd56g3_subdev_cleanup(sensor);
+err_power_off:
+	pm_runtime_disable(dev);
+	pm_runtime_put_noidle(dev);
+	pm_runtime_dont_use_autosuspend(dev);
+	vd56g3_power_off(dev);
+
+	return ret;
+}
+
+static void vd56g3_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+	struct vd56g3 *sensor = to_vd56g3(sd);
+
+	vd56g3_subdev_cleanup(sensor);
+
+	pm_runtime_disable(sensor->dev);
+	if (!pm_runtime_status_suspended(sensor->dev))
+		vd56g3_power_off(sensor->dev);
+	pm_runtime_set_suspended(sensor->dev);
+	pm_runtime_dont_use_autosuspend(sensor->dev);
+}
+
+static const struct of_device_id vd56g3_dt_ids[] = {
+	{ .compatible = "st,vd56g3", .data = (void *)VD56G3_MODEL_VD56G3 },
+	{ .compatible = "st,vd66gy", .data = (void *)VD56G3_MODEL_VD66GY },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, vd56g3_dt_ids);
+
+static struct i2c_driver vd56g3_i2c_driver = {
+	.driver = {
+		.name  = "vd56g3",
+		.of_match_table = vd56g3_dt_ids,
+		.pm = &vd56g3_pm_ops,
+	},
+	.probe = vd56g3_probe,
+	.remove = vd56g3_remove,
+};
+
+module_i2c_driver(vd56g3_i2c_driver);
+
+MODULE_AUTHOR("Benjamin Mugnier <benjamin.mugnier@foss.st.com>");
+MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
+MODULE_AUTHOR("Sylvain Petinot <sylvain.petinot@foss.st.com>");
+MODULE_DESCRIPTION("ST VD56G3 sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/i2c/st-vgxy61.c b/drivers/media/i2c/vgxy61.c
index b9e7c57027b1..5b0479f3a3c0 100644
--- a/drivers/media/i2c/st-vgxy61.c
+++ b/drivers/media/i2c/vgxy61.c
@@ -16,7 +16,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/units.h>
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/mipi-csi2.h>
 #include <media/v4l2-async.h>
@@ -892,8 +892,8 @@ static u32 vgxy61_get_expo_long_max(struct vgxy61_dev *sensor,
 	third_rot_max_expo = (sensor->frame_length / 71) * short_expo_ratio;
 
 	/* Take the minimum from all rules */
-	return min(min(first_rot_max_expo, second_rot_max_expo),
-		   third_rot_max_expo);
+	return min3(first_rot_max_expo, second_rot_max_expo,
+		    third_rot_max_expo);
 }
 
 static int vgxy61_update_exposure(struct vgxy61_dev *sensor, u16 new_expo_long,
@@ -1617,7 +1617,7 @@ static int vgxy61_detect(struct vgxy61_dev *sensor)
 
 	ret = cci_read(sensor->regmap, VGXY61_REG_NVM, &st, NULL);
 	if (ret < 0)
-		return st;
+		return ret;
 	if (st != VGXY61_NVM_OK)
 		dev_warn(&client->dev, "Bad nvm state got %u\n", (u8)st);
 
@@ -1878,7 +1878,7 @@ static const struct dev_pm_ops vgxy61_pm_ops = {
 
 static struct i2c_driver vgxy61_i2c_driver = {
 	.driver = {
-		.name  = "st-vgxy61",
+		.name  = "vgxy61",
 		.of_match_table = vgxy61_dt_ids,
 		.pm = &vgxy61_pm_ops,
 	},
diff --git a/drivers/media/i2c/video-i2c.c b/drivers/media/i2c/video-i2c.c
index 56dbe07a1c99..0dd991d70d53 100644
--- a/drivers/media/i2c/video-i2c.c
+++ b/drivers/media/i2c/video-i2c.c
@@ -264,18 +264,8 @@ static int amg88xx_set_power(struct video_i2c_data *data, bool on)
 
 #if IS_REACHABLE(CONFIG_HWMON)
 
-static const u32 amg88xx_temp_config[] = {
-	HWMON_T_INPUT,
-	0
-};
-
-static const struct hwmon_channel_info amg88xx_temp = {
-	.type = hwmon_temp,
-	.config = amg88xx_temp_config,
-};
-
 static const struct hwmon_channel_info * const amg88xx_info[] = {
-	&amg88xx_temp,
+	HWMON_CHANNEL_INFO(temp, HWMON_T_INPUT),
 	NULL
 };
 
@@ -566,8 +556,6 @@ static const struct vb2_ops video_i2c_video_qops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int video_i2c_querycap(struct file *file, void  *priv,
@@ -798,13 +786,13 @@ static int video_i2c_probe(struct i2c_client *client)
 	queue->min_queued_buffers = 1;
 	queue->ops = &video_i2c_video_qops;
 	queue->mem_ops = &vb2_vmalloc_memops;
+	queue->lock = &data->queue_lock;
 
 	ret = vb2_queue_init(queue);
 	if (ret < 0)
 		goto error_unregister_device;
 
 	data->vdev.queue = queue;
-	data->vdev.queue->lock = &data->queue_lock;
 
 	snprintf(data->vdev.name, sizeof(data->vdev.name),
 				 "I2C %d-%d Transport Video",
diff --git a/drivers/media/i2c/vp27smpx.c b/drivers/media/i2c/vp27smpx.c
index 0ba3c2b68037..06fd46a63c72 100644
--- a/drivers/media/i2c/vp27smpx.c
+++ b/drivers/media/i2c/vp27smpx.c
@@ -172,7 +172,7 @@ static void vp27smpx_remove(struct i2c_client *client)
 /* ----------------------------------------------------------------------- */
 
 static const struct i2c_device_id vp27smpx_id[] = {
-	{ "vp27smpx", 0 },
+	{ "vp27smpx" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, vp27smpx_id);
diff --git a/drivers/media/i2c/vpx3220.c b/drivers/media/i2c/vpx3220.c
index 1eaae886f217..5f1a22284168 100644
--- a/drivers/media/i2c/vpx3220.c
+++ b/drivers/media/i2c/vpx3220.c
@@ -535,9 +535,9 @@ static void vpx3220_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id vpx3220_id[] = {
-	{ "vpx3220a", 0 },
-	{ "vpx3216b", 0 },
-	{ "vpx3214c", 0 },
+	{ "vpx3220a" },
+	{ "vpx3216b" },
+	{ "vpx3214c" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, vpx3220_id);
diff --git a/drivers/media/i2c/wm8739.c b/drivers/media/i2c/wm8739.c
index 19bf7a00dff9..c091b78a5b41 100644
--- a/drivers/media/i2c/wm8739.c
+++ b/drivers/media/i2c/wm8739.c
@@ -243,7 +243,7 @@ static void wm8739_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id wm8739_id[] = {
-	{ "wm8739", 0 },
+	{ "wm8739" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, wm8739_id);
diff --git a/drivers/media/i2c/wm8775.c b/drivers/media/i2c/wm8775.c
index d1b716fd6f11..619b2988577c 100644
--- a/drivers/media/i2c/wm8775.c
+++ b/drivers/media/i2c/wm8775.c
@@ -289,7 +289,7 @@ static void wm8775_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id wm8775_id[] = {
-	{ "wm8775", 0 },
+	{ "wm8775" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, wm8775_id);
diff --git a/drivers/media/mc/mc-devnode.c b/drivers/media/mc/mc-devnode.c
index 7f67825c8757..56444edaf136 100644
--- a/drivers/media/mc/mc-devnode.c
+++ b/drivers/media/mc/mc-devnode.c
@@ -204,7 +204,6 @@ static const struct file_operations media_devnode_fops = {
 #endif /* CONFIG_COMPAT */
 	.release = media_release,
 	.poll = media_poll,
-	.llseek = no_llseek,
 };
 
 int __must_check media_devnode_register(struct media_device *mdev,
@@ -245,15 +244,14 @@ int __must_check media_devnode_register(struct media_device *mdev,
 	kobject_set_name(&devnode->cdev.kobj, "media%d", devnode->minor);
 
 	/* Part 3: Add the media and char device */
+	set_bit(MEDIA_FLAG_REGISTERED, &devnode->flags);
 	ret = cdev_device_add(&devnode->cdev, &devnode->dev);
 	if (ret < 0) {
+		clear_bit(MEDIA_FLAG_REGISTERED, &devnode->flags);
 		pr_err("%s: cdev_device_add failed\n", __func__);
 		goto cdev_add_error;
 	}
 
-	/* Part 4: Activate this minor. The char device can now be used. */
-	set_bit(MEDIA_FLAG_REGISTERED, &devnode->flags);
-
 	return 0;
 
 cdev_add_error:
diff --git a/drivers/media/mc/mc-entity.c b/drivers/media/mc/mc-entity.c
index 0e28b9a7936e..045590905582 100644
--- a/drivers/media/mc/mc-entity.c
+++ b/drivers/media/mc/mc-entity.c
@@ -619,6 +619,12 @@ static int media_pipeline_explore_next_link(struct media_pipeline *pipe,
 	link = list_entry(entry->links, typeof(*link), list);
 	last_link = media_pipeline_walk_pop(walk);
 
+	if ((link->flags & MEDIA_LNK_FL_LINK_TYPE) != MEDIA_LNK_FL_DATA_LINK) {
+		dev_dbg(walk->mdev->dev,
+			"media pipeline: skipping link (not data-link)\n");
+		return 0;
+	}
+
 	dev_dbg(walk->mdev->dev,
 		"media pipeline: exploring link '%s':%u -> '%s':%u\n",
 		link->source->entity->name, link->source->index,
@@ -762,10 +768,10 @@ done:
 	return ret;
 }
 
-__must_check int __media_pipeline_start(struct media_pad *pad,
+__must_check int __media_pipeline_start(struct media_pad *origin,
 					struct media_pipeline *pipe)
 {
-	struct media_device *mdev = pad->graph_obj.mdev;
+	struct media_device *mdev = origin->graph_obj.mdev;
 	struct media_pipeline_pad *err_ppad;
 	struct media_pipeline_pad *ppad;
 	int ret;
@@ -776,7 +782,7 @@ __must_check int __media_pipeline_start(struct media_pad *pad,
 	 * If the pad is already part of a pipeline, that pipeline must be the
 	 * same as the pipe given to media_pipeline_start().
 	 */
-	if (WARN_ON(pad->pipe && pad->pipe != pipe))
+	if (WARN_ON(origin->pipe && origin->pipe != pipe))
 		return -EINVAL;
 
 	/*
@@ -793,7 +799,7 @@ __must_check int __media_pipeline_start(struct media_pad *pad,
 	 * with media_pipeline_pad instances for each pad found during graph
 	 * walk.
 	 */
-	ret = media_pipeline_populate(pipe, pad);
+	ret = media_pipeline_populate(pipe, origin);
 	if (ret)
 		return ret;
 
@@ -908,14 +914,14 @@ error:
 }
 EXPORT_SYMBOL_GPL(__media_pipeline_start);
 
-__must_check int media_pipeline_start(struct media_pad *pad,
+__must_check int media_pipeline_start(struct media_pad *origin,
 				      struct media_pipeline *pipe)
 {
-	struct media_device *mdev = pad->graph_obj.mdev;
+	struct media_device *mdev = origin->graph_obj.mdev;
 	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
-	ret = __media_pipeline_start(pad, pipe);
+	ret = __media_pipeline_start(origin, pipe);
 	mutex_unlock(&mdev->graph_mutex);
 	return ret;
 }
diff --git a/drivers/media/mc/mc-request.c b/drivers/media/mc/mc-request.c
index addb8f2d8939..5edfc2791ce7 100644
--- a/drivers/media/mc/mc-request.c
+++ b/drivers/media/mc/mc-request.c
@@ -6,7 +6,7 @@
  * Copyright (C) 2018 Intel Corporation
  * Copyright (C) 2018 Google, Inc.
  *
- * Author: Hans Verkuil <hans.verkuil@cisco.com>
+ * Author: Hans Verkuil <hansverk@cisco.com>
  * Author: Sakari Ailus <sakari.ailus@linux.intel.com>
  */
 
@@ -246,22 +246,21 @@ static const struct file_operations request_fops = {
 struct media_request *
 media_request_get_by_fd(struct media_device *mdev, int request_fd)
 {
-	struct fd f;
 	struct media_request *req;
 
 	if (!mdev || !mdev->ops ||
 	    !mdev->ops->req_validate || !mdev->ops->req_queue)
 		return ERR_PTR(-EBADR);
 
-	f = fdget(request_fd);
-	if (!f.file)
-		goto err_no_req_fd;
+	CLASS(fd, f)(request_fd);
+	if (fd_empty(f))
+		goto err;
 
-	if (f.file->f_op != &request_fops)
-		goto err_fput;
-	req = f.file->private_data;
+	if (fd_file(f)->f_op != &request_fops)
+		goto err;
+	req = fd_file(f)->private_data;
 	if (req->mdev != mdev)
-		goto err_fput;
+		goto err;
 
 	/*
 	 * Note: as long as someone has an open filehandle of the request,
@@ -272,14 +271,9 @@ media_request_get_by_fd(struct media_device *mdev, int request_fd)
 	 * before media_request_get() is called.
 	 */
 	media_request_get(req);
-	fdput(f);
-
 	return req;
 
-err_fput:
-	fdput(f);
-
-err_no_req_fd:
+err:
 	dev_dbg(mdev->dev, "cannot find request_fd %d\n", request_fd);
 	return ERR_PTR(-EINVAL);
 }
diff --git a/drivers/media/mmc/siano/smssdio.c b/drivers/media/mmc/siano/smssdio.c
index 065b572e0272..8199077faf36 100644
--- a/drivers/media/mmc/siano/smssdio.c
+++ b/drivers/media/mmc/siano/smssdio.c
@@ -344,30 +344,7 @@ static struct sdio_driver smssdio_driver = {
 	.probe = smssdio_probe,
 	.remove = smssdio_remove,
 };
-
-/*******************************************************************/
-/* Module functions                                                */
-/*******************************************************************/
-
-static int __init smssdio_module_init(void)
-{
-	int ret = 0;
-
-	printk(KERN_INFO "smssdio: Siano SMS1xxx SDIO driver\n");
-	printk(KERN_INFO "smssdio: Copyright Pierre Ossman\n");
-
-	ret = sdio_register_driver(&smssdio_driver);
-
-	return ret;
-}
-
-static void __exit smssdio_module_exit(void)
-{
-	sdio_unregister_driver(&smssdio_driver);
-}
-
-module_init(smssdio_module_init);
-module_exit(smssdio_module_exit);
+module_sdio_driver(smssdio_driver);
 
 MODULE_DESCRIPTION("Siano SMS1xxx SDIO driver");
 MODULE_AUTHOR("Pierre Ossman");
diff --git a/drivers/media/pci/Kconfig b/drivers/media/pci/Kconfig
index 7f65aa609388..eebb16c58f3d 100644
--- a/drivers/media/pci/Kconfig
+++ b/drivers/media/pci/Kconfig
@@ -15,7 +15,6 @@ if MEDIA_CAMERA_SUPPORT
 
 source "drivers/media/pci/mgb4/Kconfig"
 source "drivers/media/pci/solo6x10/Kconfig"
-source "drivers/media/pci/sta2x11/Kconfig"
 source "drivers/media/pci/tw5864/Kconfig"
 source "drivers/media/pci/tw68/Kconfig"
 source "drivers/media/pci/tw686x/Kconfig"
diff --git a/drivers/media/pci/Makefile b/drivers/media/pci/Makefile
index f18c7e15abe3..02763ad88511 100644
--- a/drivers/media/pci/Makefile
+++ b/drivers/media/pci/Makefile
@@ -22,8 +22,6 @@ obj-y        +=	ttpci/		\
 # Please keep it alphabetically sorted by Kconfig name
 # (e. g. LC_ALL=C sort Makefile)
 
-obj-$(CONFIG_STA2X11_VIP) += sta2x11/
-
 obj-$(CONFIG_VIDEO_BT848) += bt8xx/
 obj-$(CONFIG_VIDEO_COBALT) += cobalt/
 obj-$(CONFIG_VIDEO_CX18) += cx18/
diff --git a/drivers/media/pci/b2c2/flexcop-dma.c b/drivers/media/pci/b2c2/flexcop-dma.c
index ff8058568240..2ef97be4dc54 100644
--- a/drivers/media/pci/b2c2/flexcop-dma.c
+++ b/drivers/media/pci/b2c2/flexcop-dma.c
@@ -123,23 +123,6 @@ static int flexcop_dma_remap(struct flexcop_device *fc,
 	return 0;
 }
 
-int flexcop_dma_control_size_irq(struct flexcop_device *fc,
-		flexcop_dma_index_t no,
-		int onoff)
-{
-	flexcop_ibi_value v = fc->read_ibi_reg(fc, ctrl_208);
-
-	if (no & FC_DMA_1)
-		v.ctrl_208.DMA1_IRQ_Enable_sig = onoff;
-
-	if (no & FC_DMA_2)
-		v.ctrl_208.DMA2_IRQ_Enable_sig = onoff;
-
-	fc->write_ibi_reg(fc, ctrl_208, v);
-	return 0;
-}
-EXPORT_SYMBOL(flexcop_dma_control_size_irq);
-
 int flexcop_dma_control_timer_irq(struct flexcop_device *fc,
 		flexcop_dma_index_t no,
 		int onoff)
diff --git a/drivers/media/pci/bt8xx/bt878.c b/drivers/media/pci/bt8xx/bt878.c
index 90972d6952f1..62a6c4a80bed 100644
--- a/drivers/media/pci/bt8xx/bt878.c
+++ b/drivers/media/pci/bt8xx/bt878.c
@@ -300,8 +300,8 @@ static irqreturn_t bt878_irq(int irq, void *dev_id)
 		}
 		if (astat & BT878_ARISCI) {
 			bt->finished_block = (stat & BT878_ARISCS) >> 28;
-			if (bt->tasklet.callback)
-				tasklet_schedule(&bt->tasklet);
+			if (bt->bh_work.func)
+				queue_work(system_bh_wq, &bt->bh_work);
 			break;
 		}
 		count++;
@@ -478,8 +478,8 @@ static int bt878_probe(struct pci_dev *dev, const struct pci_device_id *pci_id)
 	btwrite(0, BT878_AINT_MASK);
 	bt878_num++;
 
-	if (!bt->tasklet.func)
-		tasklet_disable(&bt->tasklet);
+	if (!bt->bh_work.func)
+		disable_work_sync(&bt->bh_work);
 
 	return 0;
 
@@ -563,4 +563,5 @@ static void __exit bt878_cleanup_module(void)
 module_init(bt878_init_module);
 module_exit(bt878_cleanup_module);
 
+MODULE_DESCRIPTION("DVB/ATSC Support for bt878 based TV cards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/pci/bt8xx/bt878.h b/drivers/media/pci/bt8xx/bt878.h
index fde8db293c54..5b1c7f56e553 100644
--- a/drivers/media/pci/bt8xx/bt878.h
+++ b/drivers/media/pci/bt8xx/bt878.h
@@ -14,6 +14,7 @@
 #include <linux/sched.h>
 #include <linux/spinlock.h>
 #include <linux/mutex.h>
+#include <linux/workqueue.h>
 
 #include "bt848.h"
 #include "bttv.h"
@@ -120,7 +121,7 @@ struct bt878 {
 	dma_addr_t risc_dma;
 	u32 risc_pos;
 
-	struct tasklet_struct tasklet;
+	struct work_struct bh_work;
 	int shutdown;
 };
 
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index 867c1308de23..365b04e5ae4d 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -24,7 +24,7 @@
 #include <linux/firmware.h>
 #include <net/checksum.h>
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <asm/io.h>
 
 #include "bttvp.h"
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index 511f013cc338..9ce67f515843 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -1584,8 +1584,6 @@ static const struct vb2_ops bttv_video_qops = {
 	.buf_cleanup    = buf_cleanup,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare   = vb2_ops_wait_prepare,
-	.wait_finish    = vb2_ops_wait_finish,
 };
 
 static void radio_enable(struct bttv *btv)
@@ -2800,7 +2798,7 @@ bttv_irq_wakeup_vbi(struct bttv *btv, struct bttv_buffer *wakeup,
 
 static void bttv_irq_timeout(struct timer_list *t)
 {
-	struct bttv *btv = from_timer(btv, t, timeout);
+	struct bttv *btv = timer_container_of(btv, t, timeout);
 	struct bttv_buffer_set old,new;
 	struct bttv_buffer *ovbi;
 	struct bttv_buffer *item;
@@ -3493,7 +3491,7 @@ static void bttv_remove(struct pci_dev *pci_dev)
 
 	/* free resources */
 	free_irq(btv->c.pci->irq,btv);
-	del_timer_sync(&btv->timeout);
+	timer_delete_sync(&btv->timeout);
 	iounmap(btv->bt848_mmio);
 	release_mem_region(pci_resource_start(btv->c.pci,0),
 			   pci_resource_len(btv->c.pci,0));
diff --git a/drivers/media/pci/bt8xx/bttv-gpio.c b/drivers/media/pci/bt8xx/bttv-gpio.c
index 6b7fea50328c..59a6f160aac7 100644
--- a/drivers/media/pci/bt8xx/bttv-gpio.c
+++ b/drivers/media/pci/bt8xx/bttv-gpio.c
@@ -28,9 +28,9 @@
 /* ----------------------------------------------------------------------- */
 /* internal: the bttv "bus"                                                */
 
-static int bttv_sub_bus_match(struct device *dev, struct device_driver *drv)
+static int bttv_sub_bus_match(struct device *dev, const struct device_driver *drv)
 {
-	struct bttv_sub_driver *sub = to_bttv_sub_drv(drv);
+	const struct bttv_sub_driver *sub = to_bttv_sub_drv(drv);
 	int len = strlen(sub->wanted);
 
 	if (0 == strncmp(dev_name(dev), sub->wanted, len))
diff --git a/drivers/media/pci/bt8xx/bttv-input.c b/drivers/media/pci/bt8xx/bttv-input.c
index 41226f1d0e5b..84aa269248fd 100644
--- a/drivers/media/pci/bt8xx/bttv-input.c
+++ b/drivers/media/pci/bt8xx/bttv-input.c
@@ -126,7 +126,7 @@ void bttv_input_irq(struct bttv *btv)
 
 static void bttv_input_timer(struct timer_list *t)
 {
-	struct bttv_ir *ir = from_timer(ir, t, timer);
+	struct bttv_ir *ir = timer_container_of(ir, t, timer);
 	struct bttv *btv = ir->btv;
 
 	if (btv->c.type == BTTV_BOARD_ENLTV_FM_2)
@@ -182,7 +182,7 @@ static u32 bttv_rc5_decode(unsigned int code)
 
 static void bttv_rc5_timer_end(struct timer_list *t)
 {
-	struct bttv_ir *ir = from_timer(ir, t, timer);
+	struct bttv_ir *ir = timer_container_of(ir, t, timer);
 	ktime_t tv;
 	u32 gap, rc5, scancode;
 	u8 toggle, command, system;
@@ -304,12 +304,12 @@ static void bttv_ir_start(struct bttv_ir *ir)
 static void bttv_ir_stop(struct bttv *btv)
 {
 	if (btv->remote->polling)
-		del_timer_sync(&btv->remote->timer);
+		timer_delete_sync(&btv->remote->timer);
 
 	if (btv->remote->rc5_gpio) {
 		u32 gpio;
 
-		del_timer_sync(&btv->remote->timer);
+		timer_delete_sync(&btv->remote->timer);
 
 		gpio = bttv_gpio_read(&btv->c);
 		bttv_gpio_write(&btv->c, gpio & ~(1 << 4));
diff --git a/drivers/media/pci/bt8xx/bttv-risc.c b/drivers/media/pci/bt8xx/bttv-risc.c
index 241a696e374a..79581cd7bd59 100644
--- a/drivers/media/pci/bt8xx/bttv-risc.c
+++ b/drivers/media/pci/bt8xx/bttv-risc.c
@@ -376,7 +376,7 @@ static void bttv_set_irq_timer(struct bttv *btv)
 	if (btv->curr.frame_irq || btv->loop_irq || btv->cvbi)
 		mod_timer(&btv->timeout, jiffies + BTTV_TIMEOUT);
 	else
-		del_timer(&btv->timeout);
+		timer_delete(&btv->timeout);
 }
 
 static int bttv_set_capture_control(struct bttv *btv, int start_capture)
diff --git a/drivers/media/pci/bt8xx/bttv-vbi.c b/drivers/media/pci/bt8xx/bttv-vbi.c
index e489a3acb4b9..a71440611e46 100644
--- a/drivers/media/pci/bt8xx/bttv-vbi.c
+++ b/drivers/media/pci/bt8xx/bttv-vbi.c
@@ -170,8 +170,6 @@ const struct vb2_ops bttv_vbi_qops = {
 	.buf_cleanup	= buf_cleanup_vbi,
 	.start_streaming = start_streaming_vbi,
 	.stop_streaming = stop_streaming_vbi,
-	.wait_prepare   = vb2_ops_wait_prepare,
-	.wait_finish    = vb2_ops_wait_finish,
 };
 
 /* ----------------------------------------------------------------------- */
diff --git a/drivers/media/pci/bt8xx/bttv.h b/drivers/media/pci/bt8xx/bttv.h
index eed7eeb3b963..97bbed980f98 100644
--- a/drivers/media/pci/bt8xx/bttv.h
+++ b/drivers/media/pci/bt8xx/bttv.h
@@ -341,7 +341,7 @@ struct bttv_sub_driver {
 	int                    (*probe)(struct bttv_sub_device *sub);
 	void                   (*remove)(struct bttv_sub_device *sub);
 };
-#define to_bttv_sub_drv(x) container_of((x), struct bttv_sub_driver, drv)
+#define to_bttv_sub_drv(x) container_of_const((x), struct bttv_sub_driver, drv)
 
 int bttv_sub_register(struct bttv_sub_driver *drv, char *wanted);
 int bttv_sub_unregister(struct bttv_sub_driver *drv);
diff --git a/drivers/media/pci/bt8xx/dvb-bt8xx.c b/drivers/media/pci/bt8xx/dvb-bt8xx.c
index 390cbba6c065..f0fbb468aea2 100644
--- a/drivers/media/pci/bt8xx/dvb-bt8xx.c
+++ b/drivers/media/pci/bt8xx/dvb-bt8xx.c
@@ -39,9 +39,9 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 #define IF_FREQUENCYx6 217    /* 6 * 36.16666666667MHz */
 
-static void dvb_bt8xx_task(struct tasklet_struct *t)
+static void dvb_bt8xx_work(struct work_struct *t)
 {
-	struct bt878 *bt = from_tasklet(bt, t, tasklet);
+	struct bt878 *bt = from_work(bt, t, bh_work);
 	struct dvb_bt8xx_card *card = dev_get_drvdata(&bt->adapter->dev);
 
 	dprintk("%d\n", card->bt->finished_block);
@@ -782,7 +782,7 @@ static int dvb_bt8xx_load_card(struct dvb_bt8xx_card *card, u32 type)
 		goto err_disconnect_frontend;
 	}
 
-	tasklet_setup(&card->bt->tasklet, dvb_bt8xx_task);
+	INIT_WORK(&card->bt->bh_work, dvb_bt8xx_work);
 
 	frontend_init(card, type);
 
@@ -922,7 +922,7 @@ static void dvb_bt8xx_remove(struct bttv_sub_device *sub)
 	dprintk("dvb_bt8xx: unloading card%d\n", card->bttv_nr);
 
 	bt878_stop(card->bt);
-	tasklet_kill(&card->bt->tasklet);
+	cancel_work_sync(&card->bt->bh_work);
 	dvb_net_release(&card->dvbnet);
 	card->demux.dmx.remove_frontend(&card->demux.dmx, &card->fe_mem);
 	card->demux.dmx.remove_frontend(&card->demux.dmx, &card->fe_hw);
diff --git a/drivers/media/pci/cobalt/cobalt-driver.c b/drivers/media/pci/cobalt/cobalt-driver.c
index 6e1a0614e6d0..39e25cc53edb 100644
--- a/drivers/media/pci/cobalt/cobalt-driver.c
+++ b/drivers/media/pci/cobalt/cobalt-driver.c
@@ -44,7 +44,7 @@ module_param_named(ignore_err, cobalt_ignore_err, int, 0644);
 MODULE_PARM_DESC(ignore_err,
 	"If set then ignore missing i2c adapters/receivers. Default: 0\n");
 
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com> & Morten Hestnes");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com> & Morten Hestnes");
 MODULE_DESCRIPTION("cobalt driver");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/media/pci/cobalt/cobalt-v4l2.c b/drivers/media/pci/cobalt/cobalt-v4l2.c
index 77ba08ace29f..ae82427e3479 100644
--- a/drivers/media/pci/cobalt/cobalt-v4l2.c
+++ b/drivers/media/pci/cobalt/cobalt-v4l2.c
@@ -424,8 +424,6 @@ static const struct vb2_ops cobalt_qops = {
 	.buf_queue = cobalt_buf_queue,
 	.start_streaming = cobalt_start_streaming,
 	.stop_streaming = cobalt_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 /* V4L2 ioctls */
@@ -633,7 +631,7 @@ static int cobalt_s_dv_timings(struct file *file, void *priv_fh,
 		return -EBUSY;
 
 	err = v4l2_subdev_call(s->sd,
-			video, s_dv_timings, timings);
+			pad, s_dv_timings, 0, timings);
 	if (!err) {
 		s->timings = *timings;
 		s->width = timings->bt.width;
@@ -653,7 +651,7 @@ static int cobalt_g_dv_timings(struct file *file, void *priv_fh,
 		return 0;
 	}
 	return v4l2_subdev_call(s->sd,
-			video, g_dv_timings, timings);
+			pad, g_dv_timings, 0, timings);
 }
 
 static int cobalt_query_dv_timings(struct file *file, void *priv_fh,
@@ -666,7 +664,7 @@ static int cobalt_query_dv_timings(struct file *file, void *priv_fh,
 		return 0;
 	}
 	return v4l2_subdev_call(s->sd,
-			video, query_dv_timings, timings);
+			pad, query_dv_timings, 0, timings);
 }
 
 static int cobalt_dv_timings_cap(struct file *file, void *priv_fh,
@@ -1080,7 +1078,7 @@ static int cobalt_g_pixelaspect(struct file *file, void *fh,
 	if (s->input == 1)
 		timings = cea1080p60;
 	else
-		err = v4l2_subdev_call(s->sd, video, g_dv_timings, &timings);
+		err = v4l2_subdev_call(s->sd, pad, g_dv_timings, 0, &timings);
 	if (!err)
 		*f = v4l2_dv_timings_aspect_ratio(&timings);
 	return err;
@@ -1099,7 +1097,7 @@ static int cobalt_g_selection(struct file *file, void *fh,
 	if (s->input == 1)
 		timings = cea1080p60;
 	else
-		err = v4l2_subdev_call(s->sd, video, g_dv_timings, &timings);
+		err = v4l2_subdev_call(s->sd, pad, g_dv_timings, 0, &timings);
 
 	if (err)
 		return err;
@@ -1243,7 +1241,7 @@ static int cobalt_node_register(struct cobalt *cobalt, int node)
 		if (s->sd)
 			vdev->ctrl_handler = s->sd->ctrl_handler;
 		s->timings = dv1080p60;
-		v4l2_subdev_call(s->sd, video, s_dv_timings, &s->timings);
+		v4l2_subdev_call(s->sd, pad, s_dv_timings, 0, &s->timings);
 		if (!s->is_output && s->sd)
 			cobalt_enable_input(s);
 		vdev->ioctl_ops = s->is_dummy ? &cobalt_ioctl_empty_ops :
diff --git a/drivers/media/pci/cx18/cx18-fileops.c b/drivers/media/pci/cx18/cx18-fileops.c
index 7e742733391b..315577d71d95 100644
--- a/drivers/media/pci/cx18/cx18-fileops.c
+++ b/drivers/media/pci/cx18/cx18-fileops.c
@@ -628,7 +628,7 @@ __poll_t cx18_v4l2_enc_poll(struct file *filp, poll_table *wait)
 
 void cx18_vb_timeout(struct timer_list *t)
 {
-	struct cx18_stream *s = from_timer(s, t, vb_timeout);
+	struct cx18_stream *s = timer_container_of(s, t, vb_timeout);
 
 	/*
 	 * Return all of the buffers in error state, so the vbi/vid inode
diff --git a/drivers/media/pci/cx18/cx18-gpio.c b/drivers/media/pci/cx18/cx18-gpio.c
index c85eb8d25837..485a6cbeb15a 100644
--- a/drivers/media/pci/cx18/cx18-gpio.c
+++ b/drivers/media/pci/cx18/cx18-gpio.c
@@ -305,21 +305,6 @@ int cx18_gpio_register(struct cx18 *cx, u32 hw)
 	return v4l2_device_register_subdev(&cx->v4l2_dev, sd);
 }
 
-void cx18_reset_ir_gpio(void *data)
-{
-	struct cx18 *cx = to_cx18(data);
-
-	if (cx->card->gpio_i2c_slave_reset.ir_reset_mask == 0)
-		return;
-
-	CX18_DEBUG_INFO("Resetting IR microcontroller\n");
-
-	v4l2_subdev_call(&cx->sd_resetctrl,
-			 core, reset, CX18_GPIO_RESET_Z8F0811);
-}
-EXPORT_SYMBOL(cx18_reset_ir_gpio);
-/* This symbol is exported for use by lirc_pvr150 for the IR-blaster */
-
 /* Xceive tuner reset function */
 int cx18_reset_tuner_gpio(void *dev, int component, int cmd, int value)
 {
diff --git a/drivers/media/pci/cx18/cx18-gpio.h b/drivers/media/pci/cx18/cx18-gpio.h
index 0fa4c7ad2286..8d5797dea7f5 100644
--- a/drivers/media/pci/cx18/cx18-gpio.h
+++ b/drivers/media/pci/cx18/cx18-gpio.h
@@ -17,5 +17,4 @@ enum cx18_gpio_reset_type {
 	CX18_GPIO_RESET_XC2028  = 2,
 };
 
-void cx18_reset_ir_gpio(void *data);
 int cx18_reset_tuner_gpio(void *dev, int component, int cmd, int value);
diff --git a/drivers/media/pci/cx18/cx18-scb.h b/drivers/media/pci/cx18/cx18-scb.h
index f7105421dd25..841edc0712ab 100644
--- a/drivers/media/pci/cx18/cx18-scb.h
+++ b/drivers/media/pci/cx18/cx18-scb.h
@@ -258,7 +258,7 @@ struct cx18_scb {
 	struct cx18_mailbox  ppu2epu_mb;
 
 	struct cx18_mdl_ack  cpu_mdl_ack[CX18_MAX_STREAMS][CX18_MAX_MDL_ACKS];
-	struct cx18_mdl_ent  cpu_mdl[1];
+	struct cx18_mdl_ent  cpu_mdl[];
 };
 
 void cx18_init_scb(struct cx18 *cx);
diff --git a/drivers/media/pci/cx18/cx18-streams.c b/drivers/media/pci/cx18/cx18-streams.c
index acc6418db425..42d6f7b90ede 100644
--- a/drivers/media/pci/cx18/cx18-streams.c
+++ b/drivers/media/pci/cx18/cx18-streams.c
@@ -229,8 +229,6 @@ static const struct vb2_ops cx18_vb2_qops = {
 	.buf_prepare		= cx18_buf_prepare,
 	.start_streaming	= cx18_start_streaming,
 	.stop_streaming		= cx18_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int cx18_stream_init(struct cx18 *cx, int type)
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index fdb96f80c036..219937a153b3 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -1210,8 +1210,6 @@ static const struct vb2_ops cx23885_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = cx23885_start_streaming,
 	.stop_streaming = cx23885_stop_streaming,
 };
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index 9244b4320558..da23e7dfeef5 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -860,6 +860,31 @@ struct cx23885_board cx23885_boards[] = {
 			.amux   = CX25840_AUDIO7,
 		} },
 	},
+	[CX23885_BOARD_AVERMEDIA_H789C] = {
+		.name		= "AVerMedia H789-C",
+		.porta		= CX23885_ANALOG_VIDEO,
+		.tuner_type	= TUNER_NXP_TDA18271,
+		.tuner_addr	= 0x63, /* 0xc0 >> 1 */
+		.tuner_bus	= 1,
+		.input		= {{
+			.type   = CX23885_VMUX_TELEVISION,
+			.vmux   = CX25840_VIN7_CH3 |
+				  CX25840_VIN5_CH2 |
+				  CX25840_VIN2_CH1 |
+				  CX25840_DIF_ON,
+			.amux   = CX25840_AUDIO8,
+		}, {
+			.type	= CX23885_VMUX_COMPOSITE1,
+			.vmux	= CX25840_VIN8_CH1,
+			.amux	= CX25840_AUDIO7,
+		}, {
+			.type	= CX23885_VMUX_SVIDEO,
+			.vmux	= CX25840_VIN8_CH1 |
+				  CX25840_VIN7_CH3 |
+				  CX25840_SVIDEO_ON,
+			.amux   = CX25840_AUDIO7,
+		}, },
+	},
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
 
@@ -1187,6 +1212,10 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x1461,
 		.subdevice = 0x3100,
 		.card      = CX23885_BOARD_AVERMEDIA_CE310B,
+	}, {
+		.subvendor = 0x1461,
+		.subdevice = 0xe139,
+		.card      = CX23885_BOARD_AVERMEDIA_H789C,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -2413,6 +2442,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_VIEWCAST_260E:
 	case CX23885_BOARD_VIEWCAST_460E:
 	case CX23885_BOARD_AVERMEDIA_CE310B:
+	case CX23885_BOARD_AVERMEDIA_H789C:
 		dev->sd_cx25840 = v4l2_i2c_new_subdev(&dev->v4l2_dev,
 				&dev->i2c_bus[2].i2c_adap,
 				"cx25840", 0x88 >> 1, NULL);
diff --git a/drivers/media/pci/cx23885/cx23885-core.c b/drivers/media/pci/cx23885/cx23885-core.c
index c8705d786cdd..a39f445ce22a 100644
--- a/drivers/media/pci/cx23885/cx23885-core.c
+++ b/drivers/media/pci/cx23885/cx23885-core.c
@@ -2231,6 +2231,28 @@ static void cx23885_finidev(struct pci_dev *pci_dev)
 	kfree(dev);
 }
 
+static int __maybe_unused cx23885_suspend(struct device *dev_d)
+{
+	struct pci_dev *pci_dev = to_pci_dev(dev_d);
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct cx23885_dev *dev = to_cx23885(v4l2_dev);
+
+	cx23885_shutdown(dev);
+
+	return 0;
+}
+
+static int __maybe_unused cx23885_resume(struct device *dev_d)
+{
+	struct pci_dev *pci_dev = to_pci_dev(dev_d);
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct cx23885_dev *dev = to_cx23885(v4l2_dev);
+
+	cx23885_reset(dev);
+
+	return 0;
+}
+
 static const struct pci_device_id cx23885_pci_tbl[] = {
 	{
 		/* CX23885 */
@@ -2250,11 +2272,14 @@ static const struct pci_device_id cx23885_pci_tbl[] = {
 };
 MODULE_DEVICE_TABLE(pci, cx23885_pci_tbl);
 
+static SIMPLE_DEV_PM_OPS(cx23885_pm_ops, cx23885_suspend, cx23885_resume);
+
 static struct pci_driver cx23885_pci_driver = {
-	.name     = "cx23885",
-	.id_table = cx23885_pci_tbl,
-	.probe    = cx23885_initdev,
-	.remove   = cx23885_finidev,
+	.name      = "cx23885",
+	.id_table  = cx23885_pci_tbl,
+	.probe     = cx23885_initdev,
+	.remove    = cx23885_finidev,
+	.driver.pm = &cx23885_pm_ops,
 };
 
 static int __init cx23885_init(void)
diff --git a/drivers/media/pci/cx23885/cx23885-dvb.c b/drivers/media/pci/cx23885/cx23885-dvb.c
index 3d01cdc4c7f3..05a7859cbe57 100644
--- a/drivers/media/pci/cx23885/cx23885-dvb.c
+++ b/drivers/media/pci/cx23885/cx23885-dvb.c
@@ -170,8 +170,6 @@ static const struct vb2_ops dvb_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = cx23885_start_streaming,
 	.stop_streaming = cx23885_stop_streaming,
 };
diff --git a/drivers/media/pci/cx23885/cx23885-vbi.c b/drivers/media/pci/cx23885/cx23885-vbi.c
index 4bdd2bea3713..40817cc52fc1 100644
--- a/drivers/media/pci/cx23885/cx23885-vbi.c
+++ b/drivers/media/pci/cx23885/cx23885-vbi.c
@@ -249,8 +249,6 @@ const struct vb2_ops cx23885_vbi_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = cx23885_start_streaming,
 	.stop_streaming = cx23885_stop_streaming,
 };
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 7d4a409c433e..14d219fd1d8a 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -261,7 +261,8 @@ static int cx23885_video_mux(struct cx23885_dev *dev, unsigned int input)
 		(dev->board == CX23885_BOARD_AVERMEDIA_HC81R) ||
 		(dev->board == CX23885_BOARD_VIEWCAST_260E) ||
 		(dev->board == CX23885_BOARD_VIEWCAST_460E) ||
-		(dev->board == CX23885_BOARD_AVERMEDIA_CE310B)) {
+		(dev->board == CX23885_BOARD_AVERMEDIA_CE310B) ||
+		(dev->board == CX23885_BOARD_AVERMEDIA_H789C)) {
 		/* Configure audio routing */
 		v4l2_subdev_call(dev->sd_cx25840, audio, s_routing,
 			INPUT(input)->amux, 0, 0);
@@ -519,8 +520,6 @@ static const struct vb2_ops cx23885_video_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = cx23885_start_streaming,
 	.stop_streaming = cx23885_stop_streaming,
 };
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index 349462ee2c48..8ba1f306238c 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -102,6 +102,7 @@
 #define CX23885_BOARD_HAUPPAUGE_QUADHD_DVB_885 60
 #define CX23885_BOARD_HAUPPAUGE_QUADHD_ATSC_885 61
 #define CX23885_BOARD_AVERMEDIA_CE310B         62
+#define CX23885_BOARD_AVERMEDIA_H789C          63
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
diff --git a/drivers/media/pci/cx23885/netup-eeprom.c b/drivers/media/pci/cx23885/netup-eeprom.c
index 26dd5b3884e6..335d150d81de 100644
--- a/drivers/media/pci/cx23885/netup-eeprom.c
+++ b/drivers/media/pci/cx23885/netup-eeprom.c
@@ -49,35 +49,6 @@ int netup_eeprom_read(struct i2c_adapter *i2c_adap, u8 addr)
 	return buf[1];
 };
 
-int netup_eeprom_write(struct i2c_adapter *i2c_adap, u8 addr, u8 data)
-{
-	int ret;
-	unsigned char bufw[2];
-
-	/* Write into EEPROM */
-	struct i2c_msg msg[] = {
-		{
-			.addr	= EEPROM_I2C_ADDR,
-			.flags	= 0,
-			.buf	= &bufw[0],
-			.len	= 2
-		}
-	};
-
-	bufw[0] = addr;
-	bufw[1] = data;
-
-	ret = i2c_transfer(i2c_adap, msg, 1);
-
-	if (ret != 1) {
-		pr_err("eeprom i2c write error, status=%d\n", ret);
-		return -1;
-	}
-
-	mdelay(10); /* prophylactic delay, datasheet write cycle time = 5 ms */
-	return 0;
-};
-
 void netup_get_card_info(struct i2c_adapter *i2c_adap,
 				struct netup_card_info *cinfo)
 {
diff --git a/drivers/media/pci/cx23885/netup-eeprom.h b/drivers/media/pci/cx23885/netup-eeprom.h
index 549a033679f7..fb8217eb455c 100644
--- a/drivers/media/pci/cx23885/netup-eeprom.h
+++ b/drivers/media/pci/cx23885/netup-eeprom.h
@@ -21,7 +21,6 @@ struct netup_card_info {
 };
 
 extern int netup_eeprom_read(struct i2c_adapter *i2c_adap, u8 addr);
-extern int netup_eeprom_write(struct i2c_adapter *i2c_adap, u8 addr, u8 data);
 extern void netup_get_card_info(struct i2c_adapter *i2c_adap,
 				struct netup_card_info *cinfo);
 
diff --git a/drivers/media/pci/cx25821/cx25821-video.c b/drivers/media/pci/cx25821/cx25821-video.c
index 0bee4b728a60..84aa1209e717 100644
--- a/drivers/media/pci/cx25821/cx25821-video.c
+++ b/drivers/media/pci/cx25821/cx25821-video.c
@@ -295,8 +295,6 @@ static const struct vb2_ops cx25821_video_qops = {
 	.buf_prepare  = cx25821_buffer_prepare,
 	.buf_finish = cx25821_buffer_finish,
 	.buf_queue    = cx25821_buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = cx25821_start_streaming,
 	.stop_streaming = cx25821_stop_streaming,
 };
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index d55df8fdb3b6..13b8cc46835b 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -781,8 +781,6 @@ static const struct vb2_ops blackbird_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
 };
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index b33b3a5e32ec..c9cfceed2f1b 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -152,8 +152,6 @@ static const struct vb2_ops dvb_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
 };
diff --git a/drivers/media/pci/cx88/cx88-input.c b/drivers/media/pci/cx88/cx88-input.c
index a04a1d33fadb..b9f2c14d62b4 100644
--- a/drivers/media/pci/cx88/cx88-input.c
+++ b/drivers/media/pci/cx88/cx88-input.c
@@ -190,8 +190,7 @@ static int __cx88_ir_start(void *priv)
 	ir = core->ir;
 
 	if (ir->polling) {
-		hrtimer_init(&ir->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
-		ir->timer.function = cx88_ir_work;
+		hrtimer_setup(&ir->timer, cx88_ir_work, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 		hrtimer_start(&ir->timer,
 			      ktime_set(0, ir->polling * 1000000),
 			      HRTIMER_MODE_REL);
diff --git a/drivers/media/pci/cx88/cx88-vbi.c b/drivers/media/pci/cx88/cx88-vbi.c
index 469aeaa725ad..e3e379e6f620 100644
--- a/drivers/media/pci/cx88/cx88-vbi.c
+++ b/drivers/media/pci/cx88/cx88-vbi.c
@@ -228,8 +228,6 @@ const struct vb2_ops cx8800_vbi_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
 };
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index cefb6b25e921..0c87327689d3 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -562,8 +562,6 @@ static const struct vb2_ops cx8800_video_qops = {
 	.buf_prepare  = buffer_prepare,
 	.buf_finish = buffer_finish,
 	.buf_queue    = buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
 };
diff --git a/drivers/media/pci/ddbridge/ddbridge.h b/drivers/media/pci/ddbridge/ddbridge.h
index f3699dbd193f..f01ecdb0b627 100644
--- a/drivers/media/pci/ddbridge/ddbridge.h
+++ b/drivers/media/pci/ddbridge/ddbridge.h
@@ -298,7 +298,7 @@ struct ddb_link {
 	spinlock_t             lock; /* lock link access */
 	struct mutex           flash_mutex; /* lock flash access */
 	struct ddb_lnb         lnb;
-	struct tasklet_struct  tasklet;
+	struct work_struct     bh_work;
 	struct ddb_ids         ids;
 
 	spinlock_t             temp_lock; /* lock temp chip access */
diff --git a/drivers/media/pci/dt3155/dt3155.c b/drivers/media/pci/dt3155/dt3155.c
index dff853e73fdc..7bddcbba4cf1 100644
--- a/drivers/media/pci/dt3155/dt3155.c
+++ b/drivers/media/pci/dt3155/dt3155.c
@@ -222,8 +222,6 @@ static void dt3155_buf_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops q_ops = {
 	.queue_setup = dt3155_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.buf_prepare = dt3155_buf_prepare,
 	.start_streaming = dt3155_start_streaming,
 	.stop_streaming = dt3155_stop_streaming,
diff --git a/drivers/media/pci/intel/Kconfig b/drivers/media/pci/intel/Kconfig
index ee4684159d3d..d9fcddce028b 100644
--- a/drivers/media/pci/intel/Kconfig
+++ b/drivers/media/pci/intel/Kconfig
@@ -1,11 +1,13 @@
 # SPDX-License-Identifier: GPL-2.0-only
 
 source "drivers/media/pci/intel/ipu3/Kconfig"
+source "drivers/media/pci/intel/ipu6/Kconfig"
 source "drivers/media/pci/intel/ivsc/Kconfig"
 
 config IPU_BRIDGE
 	tristate "Intel IPU Bridge"
-	depends on I2C && ACPI
+	depends on ACPI || COMPILE_TEST
+	depends on I2C
 	help
 	  The IPU bridge is a helper library for Intel IPU drivers to
 	  function on systems shipped with Windows.
diff --git a/drivers/media/pci/intel/Makefile b/drivers/media/pci/intel/Makefile
index f199a97e1d78..3a2cc6567159 100644
--- a/drivers/media/pci/intel/Makefile
+++ b/drivers/media/pci/intel/Makefile
@@ -5,3 +5,4 @@
 obj-$(CONFIG_IPU_BRIDGE) += ipu-bridge.o
 obj-y	+= ipu3/
 obj-y	+= ivsc/
+obj-$(CONFIG_VIDEO_INTEL_IPU6)	+= ipu6/
diff --git a/drivers/media/pci/intel/ipu-bridge.c b/drivers/media/pci/intel/ipu-bridge.c
index e994db4f4d91..83e682e1a4b7 100644
--- a/drivers/media/pci/intel/ipu-bridge.c
+++ b/drivers/media/pci/intel/ipu-bridge.c
@@ -2,6 +2,7 @@
 /* Author: Dan Scally <djrscally@gmail.com> */
 
 #include <linux/acpi.h>
+#include <acpi/acpi_bus.h>
 #include <linux/cleanup.h>
 #include <linux/device.h>
 #include <linux/i2c.h>
@@ -15,6 +16,8 @@
 #include <media/ipu-bridge.h>
 #include <media/v4l2-fwnode.h>
 
+#define ADEV_DEV(adev) ACPI_PTR(&((adev)->dev))
+
 /*
  * 92335fcf-3203-4472-af93-7b4453ac29da
  *
@@ -41,28 +44,48 @@
  * becoming apparent in the future.
  *
  * Do not add an entry for a sensor that is not actually supported.
+ *
+ * Please keep the list sorted by ACPI HID.
  */
 static const struct ipu_sensor_config ipu_supported_sensors[] = {
+	/* Himax HM11B1 */
+	IPU_SENSOR_CONFIG("HIMX11B1", 1, 384000000),
+	/* Himax HM2170 */
+	IPU_SENSOR_CONFIG("HIMX2170", 1, 384000000),
+	/* Himax HM2172 */
+	IPU_SENSOR_CONFIG("HIMX2172", 1, 384000000),
+	/* GalaxyCore GC0310 */
+	IPU_SENSOR_CONFIG("INT0310", 0),
 	/* Omnivision OV5693 */
 	IPU_SENSOR_CONFIG("INT33BE", 1, 419200000),
+	/* Omnivision OV2740 */
+	IPU_SENSOR_CONFIG("INT3474", 1, 180000000),
 	/* Omnivision OV8865 */
 	IPU_SENSOR_CONFIG("INT347A", 1, 360000000),
 	/* Omnivision OV7251 */
 	IPU_SENSOR_CONFIG("INT347E", 1, 319200000),
+	/* Hynix Hi-556 */
+	IPU_SENSOR_CONFIG("INT3537", 1, 437000000),
+	/* Lontium lt6911uxe */
+	IPU_SENSOR_CONFIG("INTC10C5", 0),
+	/* Omnivision OV01A10 / OV01A1S */
+	IPU_SENSOR_CONFIG("OVTI01A0", 1, 400000000),
+	IPU_SENSOR_CONFIG("OVTI01AS", 1, 400000000),
+	/* Omnivision OV02C10 */
+	IPU_SENSOR_CONFIG("OVTI02C1", 1, 400000000),
+	/* Omnivision OV02E10 */
+	IPU_SENSOR_CONFIG("OVTI02E1", 1, 360000000),
+	/* Omnivision OV08A10 */
+	IPU_SENSOR_CONFIG("OVTI08A1", 1, 500000000),
+	/* Omnivision OV08x40 */
+	IPU_SENSOR_CONFIG("OVTI08F4", 1, 400000000),
+	/* Omnivision OV13B10 */
+	IPU_SENSOR_CONFIG("OVTI13B1", 1, 560000000),
+	IPU_SENSOR_CONFIG("OVTIDB10", 1, 560000000),
 	/* Omnivision OV2680 */
 	IPU_SENSOR_CONFIG("OVTI2680", 1, 331200000),
-	/* Omnivision ov8856 */
+	/* Omnivision OV8856 */
 	IPU_SENSOR_CONFIG("OVTI8856", 3, 180000000, 360000000, 720000000),
-	/* Omnivision ov2740 */
-	IPU_SENSOR_CONFIG("INT3474", 1, 180000000),
-	/* Hynix hi556 */
-	IPU_SENSOR_CONFIG("INT3537", 1, 437000000),
-	/* Omnivision ov13b10 */
-	IPU_SENSOR_CONFIG("OVTIDB10", 1, 560000000),
-	/* GalaxyCore GC0310 */
-	IPU_SENSOR_CONFIG("INT0310", 0),
-	/* Omnivision ov01a10 */
-	IPU_SENSOR_CONFIG("OVTI01A0", 1, 400000000),
 };
 
 static const struct ipu_property_names prop_names = {
@@ -100,17 +123,17 @@ static const struct acpi_device_id ivsc_acpi_ids[] = {
 
 static struct acpi_device *ipu_bridge_get_ivsc_acpi_dev(struct acpi_device *adev)
 {
-	acpi_handle handle = acpi_device_handle(adev);
-	struct acpi_device *consumer, *ivsc_adev;
 	unsigned int i;
 
 	for (i = 0; i < ARRAY_SIZE(ivsc_acpi_ids); i++) {
 		const struct acpi_device_id *acpi_id = &ivsc_acpi_ids[i];
+		struct acpi_device *consumer, *ivsc_adev;
 
+		acpi_handle handle = acpi_device_handle(ACPI_PTR(adev));
 		for_each_acpi_dev_match(ivsc_adev, acpi_id->id, NULL, -1)
 			/* camera sensor depends on IVSC in DSDT if exist */
 			for_each_acpi_consumer_dev(ivsc_adev, consumer)
-				if (consumer->handle == handle) {
+				if (ACPI_PTR(consumer->handle) == handle) {
 					acpi_dev_put(consumer);
 					return ivsc_adev;
 				}
@@ -163,7 +186,7 @@ static int ipu_bridge_check_ivsc_dev(struct ipu_sensor *sensor,
 		csi_dev = ipu_bridge_get_ivsc_csi_dev(adev);
 		if (!csi_dev) {
 			acpi_dev_put(adev);
-			dev_err(&adev->dev, "Failed to find MEI CSI dev\n");
+			dev_err(ADEV_DEV(adev), "Failed to find MEI CSI dev\n");
 			return -ENODEV;
 		}
 
@@ -182,24 +205,25 @@ static int ipu_bridge_read_acpi_buffer(struct acpi_device *adev, char *id,
 	acpi_status status;
 	int ret = 0;
 
-	status = acpi_evaluate_object(adev->handle, id, NULL, &buffer);
+	status = acpi_evaluate_object(ACPI_PTR(adev->handle),
+				      id, NULL, &buffer);
 	if (ACPI_FAILURE(status))
 		return -ENODEV;
 
 	obj = buffer.pointer;
 	if (!obj) {
-		dev_err(&adev->dev, "Couldn't locate ACPI buffer\n");
+		dev_err(ADEV_DEV(adev), "Couldn't locate ACPI buffer\n");
 		return -ENODEV;
 	}
 
 	if (obj->type != ACPI_TYPE_BUFFER) {
-		dev_err(&adev->dev, "Not an ACPI buffer\n");
+		dev_err(ADEV_DEV(adev), "Not an ACPI buffer\n");
 		ret = -ENODEV;
 		goto out_free_buff;
 	}
 
 	if (obj->buffer.length > size) {
-		dev_err(&adev->dev, "Given buffer is too small\n");
+		dev_err(ADEV_DEV(adev), "Given buffer is too small\n");
 		ret = -EINVAL;
 		goto out_free_buff;
 	}
@@ -220,7 +244,7 @@ static u32 ipu_bridge_parse_rotation(struct acpi_device *adev,
 	case IPU_SENSOR_ROTATION_INVERTED:
 		return 180;
 	default:
-		dev_warn(&adev->dev,
+		dev_warn(ADEV_DEV(adev),
 			 "Unknown rotation %d. Assume 0 degree rotation\n",
 			 ssdb->degree);
 		return 0;
@@ -230,12 +254,10 @@ static u32 ipu_bridge_parse_rotation(struct acpi_device *adev,
 static enum v4l2_fwnode_orientation ipu_bridge_parse_orientation(struct acpi_device *adev)
 {
 	enum v4l2_fwnode_orientation orientation;
-	struct acpi_pld_info *pld;
-	acpi_status status;
+	struct acpi_pld_info *pld = NULL;
 
-	status = acpi_get_physical_device_location(adev->handle, &pld);
-	if (ACPI_FAILURE(status)) {
-		dev_warn(&adev->dev, "_PLD call failed, using default orientation\n");
+	if (!acpi_get_physical_device_location(ACPI_PTR(adev->handle), &pld)) {
+		dev_warn(ADEV_DEV(adev), "_PLD call failed, using default orientation\n");
 		return V4L2_FWNODE_ORIENTATION_EXTERNAL;
 	}
 
@@ -253,7 +275,8 @@ static enum v4l2_fwnode_orientation ipu_bridge_parse_orientation(struct acpi_dev
 		orientation = V4L2_FWNODE_ORIENTATION_EXTERNAL;
 		break;
 	default:
-		dev_warn(&adev->dev, "Unknown _PLD panel val %d\n", pld->panel);
+		dev_warn(ADEV_DEV(adev), "Unknown _PLD panel val %d\n",
+			 pld->panel);
 		orientation = V4L2_FWNODE_ORIENTATION_EXTERNAL;
 		break;
 	}
@@ -272,12 +295,12 @@ int ipu_bridge_parse_ssdb(struct acpi_device *adev, struct ipu_sensor *sensor)
 		return ret;
 
 	if (ssdb.vcmtype > ARRAY_SIZE(ipu_vcm_types)) {
-		dev_warn(&adev->dev, "Unknown VCM type %d\n", ssdb.vcmtype);
+		dev_warn(ADEV_DEV(adev), "Unknown VCM type %d\n", ssdb.vcmtype);
 		ssdb.vcmtype = 0;
 	}
 
 	if (ssdb.lanes > IPU_MAX_LANES) {
-		dev_err(&adev->dev, "Number of lanes in SSDB is invalid\n");
+		dev_err(ADEV_DEV(adev), "Number of lanes in SSDB is invalid\n");
 		return -EINVAL;
 	}
 
@@ -292,7 +315,7 @@ int ipu_bridge_parse_ssdb(struct acpi_device *adev, struct ipu_sensor *sensor)
 
 	return 0;
 }
-EXPORT_SYMBOL_NS_GPL(ipu_bridge_parse_ssdb, INTEL_IPU_BRIDGE);
+EXPORT_SYMBOL_NS_GPL(ipu_bridge_parse_ssdb, "INTEL_IPU_BRIDGE");
 
 static void ipu_bridge_create_fwnode_properties(
 	struct ipu_sensor *sensor,
@@ -465,8 +488,12 @@ static void ipu_bridge_create_connection_swnodes(struct ipu_bridge *bridge,
 						sensor->ipu_properties);
 
 	if (sensor->csi_dev) {
+		const char *device_hid = "";
+
+		device_hid = acpi_device_hid(sensor->ivsc_adev);
+
 		snprintf(sensor->ivsc_name, sizeof(sensor->ivsc_name), "%s-%u",
-			 acpi_device_hid(sensor->ivsc_adev), sensor->link);
+			 device_hid, sensor->link);
 
 		nodes[SWNODE_IVSC_HID] = NODE_SENSOR(sensor->ivsc_name,
 						     sensor->ivsc_properties);
@@ -594,7 +621,7 @@ int ipu_bridge_instantiate_vcm(struct device *sensor)
 
 	return 0;
 }
-EXPORT_SYMBOL_NS_GPL(ipu_bridge_instantiate_vcm, INTEL_IPU_BRIDGE);
+EXPORT_SYMBOL_NS_GPL(ipu_bridge_instantiate_vcm, "INTEL_IPU_BRIDGE");
 
 static int ipu_bridge_instantiate_ivsc(struct ipu_sensor *sensor)
 {
@@ -631,11 +658,11 @@ static int ipu_bridge_connect_sensor(const struct ipu_sensor_config *cfg,
 {
 	struct fwnode_handle *fwnode, *primary;
 	struct ipu_sensor *sensor;
-	struct acpi_device *adev;
+	struct acpi_device *adev = NULL;
 	int ret;
 
 	for_each_acpi_dev_match(adev, cfg->hid, NULL, -1) {
-		if (!adev->status.enabled)
+		if (!ACPI_PTR(adev->status.enabled))
 			continue;
 
 		if (bridge->n_sensors >= IPU_MAX_PORTS) {
@@ -671,7 +698,7 @@ static int ipu_bridge_connect_sensor(const struct ipu_sensor_config *cfg,
 			goto err_free_swnodes;
 		}
 
-		sensor->adev = acpi_dev_get(adev);
+		sensor->adev = ACPI_PTR(acpi_dev_get(adev));
 
 		primary = acpi_fwnode_handle(adev);
 		primary->secondary = fwnode;
@@ -731,7 +758,7 @@ static int ipu_bridge_ivsc_is_ready(void)
 			&ipu_supported_sensors[i];
 
 		for_each_acpi_dev_match(sensor_adev, cfg->hid, NULL, -1) {
-			if (!sensor_adev->status.enabled)
+			if (!ACPI_PTR(sensor_adev->status.enabled))
 				continue;
 
 			adev = ipu_bridge_get_ivsc_acpi_dev(sensor_adev);
@@ -836,7 +863,7 @@ err_free_bridge:
 
 	return ret;
 }
-EXPORT_SYMBOL_NS_GPL(ipu_bridge_init, INTEL_IPU_BRIDGE);
+EXPORT_SYMBOL_NS_GPL(ipu_bridge_init, "INTEL_IPU_BRIDGE");
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Intel IPU Sensors Bridge driver");
diff --git a/drivers/media/pci/intel/ipu3/ipu3-cio2.c b/drivers/media/pci/intel/ipu3/ipu3-cio2.c
index c42adc5a408d..16fde96c9fb2 100644
--- a/drivers/media/pci/intel/ipu3/ipu3-cio2.c
+++ b/drivers/media/pci/intel/ipu3/ipu3-cio2.c
@@ -4,9 +4,9 @@
  *
  * Based partially on Intel IPU4 driver written by
  *  Sakari Ailus <sakari.ailus@linux.intel.com>
- *  Samu Onkalo <samu.onkalo@intel.com>
+ *  Samu Onkalo
  *  Jouni Högander <jouni.hogander@intel.com>
- *  Jouni Ukkonen <jouni.ukkonen@intel.com>
+ *  Jouni Ukkonen
  *  Antti Laakso <antti.laakso@intel.com>
  * et al.
  */
@@ -309,12 +309,17 @@ static int cio2_csi2_calc_timing(struct cio2_device *cio2, struct cio2_queue *q,
 				 unsigned int bpp, unsigned int lanes)
 {
 	struct device *dev = &cio2->pci_dev->dev;
+	struct media_pad *src_pad;
 	s64 freq;
 
-	if (!q->sensor)
-		return -ENODEV;
+	src_pad = media_entity_remote_source_pad_unique(&q->subdev.entity);
+	if (IS_ERR(src_pad)) {
+		dev_err(dev, "can't get source pad of %s (%ld)\n",
+			q->subdev.name, PTR_ERR(src_pad));
+		return PTR_ERR(src_pad);
+	}
 
-	freq = v4l2_get_link_freq(q->sensor->ctrl_handler, bpp, lanes * 2);
+	freq = v4l2_get_link_freq(src_pad, bpp, lanes * 2);
 	if (freq < 0) {
 		dev_err(dev, "error %lld, invalid link_freq\n", freq);
 		return freq;
@@ -1045,8 +1050,6 @@ static const struct vb2_ops cio2_vb2_ops = {
 	.queue_setup = cio2_vb2_queue_setup,
 	.start_streaming = cio2_vb2_start_streaming,
 	.stop_streaming = cio2_vb2_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 /**************** V4L2 interface ****************/
@@ -1699,14 +1702,13 @@ static int cio2_pci_probe(struct pci_dev *pci_dev,
 	dev_info(dev, "device 0x%x (rev: 0x%x)\n",
 		 pci_dev->device, pci_dev->revision);
 
-	r = pcim_iomap_regions(pci_dev, 1 << CIO2_PCI_BAR, pci_name(pci_dev));
+	cio2->base = pcim_iomap_region(pci_dev, CIO2_PCI_BAR, CIO2_NAME);
+	r = PTR_ERR_OR_ZERO(cio2->base);
 	if (r) {
 		dev_err(dev, "failed to remap I/O memory (%d)\n", r);
 		return -ENODEV;
 	}
 
-	cio2->base = pcim_iomap_table(pci_dev)[CIO2_PCI_BAR];
-
 	pci_set_drvdata(pci_dev, cio2);
 
 	pci_set_master(pci_dev);
@@ -1752,11 +1754,6 @@ static int cio2_pci_probe(struct pci_dev *pci_dev,
 
 	v4l2_async_nf_init(&cio2->notifier, &cio2->v4l2_dev);
 
-	/* Register notifier for subdevices we care */
-	r = cio2_parse_firmware(cio2);
-	if (r)
-		goto fail_clean_notifier;
-
 	r = devm_request_irq(dev, pci_dev->irq, cio2_irq, IRQF_SHARED,
 			     CIO2_NAME, cio2);
 	if (r) {
@@ -1764,6 +1761,11 @@ static int cio2_pci_probe(struct pci_dev *pci_dev,
 		goto fail_clean_notifier;
 	}
 
+	/* Register notifier for subdevices we care */
+	r = cio2_parse_firmware(cio2);
+	if (r)
+		goto fail_clean_notifier;
+
 	pm_runtime_put_noidle(dev);
 	pm_runtime_allow(dev);
 
@@ -1807,16 +1809,10 @@ static int __maybe_unused cio2_runtime_suspend(struct device *dev)
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
 	void __iomem *const base = cio2->base;
-	u16 pm;
 
 	writel(CIO2_D0I3C_I3, base + CIO2_REG_D0I3C);
 	dev_dbg(dev, "cio2 runtime suspend.\n");
 
-	pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm);
-	pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT;
-	pm |= CIO2_PMCSR_D3;
-	pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm);
-
 	return 0;
 }
 
@@ -1825,15 +1821,10 @@ static int __maybe_unused cio2_runtime_resume(struct device *dev)
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct cio2_device *cio2 = pci_get_drvdata(pci_dev);
 	void __iomem *const base = cio2->base;
-	u16 pm;
 
 	writel(CIO2_D0I3C_RR, base + CIO2_REG_D0I3C);
 	dev_dbg(dev, "cio2 runtime resume.\n");
 
-	pci_read_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, &pm);
-	pm = (pm >> CIO2_PMCSR_D0D3_SHIFT) << CIO2_PMCSR_D0D3_SHIFT;
-	pci_write_config_word(pci_dev, pci_dev->pm_cap + CIO2_PMCSR_OFFSET, pm);
-
 	return 0;
 }
 
@@ -2006,11 +1997,11 @@ static struct pci_driver cio2_pci_driver = {
 
 module_pci_driver(cio2_pci_driver);
 
-MODULE_AUTHOR("Tuukka Toivonen <tuukka.toivonen@intel.com>");
+MODULE_AUTHOR("Tuukka Toivonen");
 MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
 MODULE_AUTHOR("Jian Xu Zheng");
-MODULE_AUTHOR("Yuning Pu <yuning.pu@intel.com>");
+MODULE_AUTHOR("Yuning Pu");
 MODULE_AUTHOR("Yong Zhi <yong.zhi@intel.com>");
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("IPU3 CIO2 driver");
-MODULE_IMPORT_NS(INTEL_IPU_BRIDGE);
+MODULE_IMPORT_NS("INTEL_IPU_BRIDGE");
diff --git a/drivers/media/pci/intel/ipu3/ipu3-cio2.h b/drivers/media/pci/intel/ipu3/ipu3-cio2.h
index d731ce8adbe3..d7cb7dae665b 100644
--- a/drivers/media/pci/intel/ipu3/ipu3-cio2.h
+++ b/drivers/media/pci/intel/ipu3/ipu3-cio2.h
@@ -320,10 +320,6 @@ struct pci_dev;
 #define CIO2_CSIRX_DLY_CNT_TERMEN_DEFAULT		0x4
 #define CIO2_CSIRX_DLY_CNT_SETTLE_DEFAULT		0x570
 
-#define CIO2_PMCSR_OFFSET				4U
-#define CIO2_PMCSR_D0D3_SHIFT				2U
-#define CIO2_PMCSR_D3					0x3
-
 struct cio2_csi2_timing {
 	s32 clk_termen;
 	s32 clk_settle;
diff --git a/drivers/media/pci/intel/ipu6/Kconfig b/drivers/media/pci/intel/ipu6/Kconfig
new file mode 100644
index 000000000000..1129e2beb4be
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/Kconfig
@@ -0,0 +1,18 @@
+config VIDEO_INTEL_IPU6
+	tristate "Intel IPU6 driver"
+	depends on ACPI || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on X86 && HAS_DMA
+	depends on IPU_BRIDGE || !IPU_BRIDGE
+	select AUXILIARY_BUS
+	select IOMMU_IOVA
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_SG
+	select V4L2_FWNODE
+	help
+	  This is the 6th Gen Intel Image Processing Unit, found in Intel SoCs
+	  and used for capturing images and video from camera sensors.
+
+	  To compile this driver, say Y here! It contains 2 modules -
+	  intel_ipu6 and intel_ipu6_isys.
diff --git a/drivers/media/pci/intel/ipu6/Makefile b/drivers/media/pci/intel/ipu6/Makefile
new file mode 100644
index 000000000000..a821b0a1567f
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/Makefile
@@ -0,0 +1,23 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+intel-ipu6-y			:= ipu6.o \
+				ipu6-bus.o \
+				ipu6-dma.o \
+				ipu6-mmu.o \
+				ipu6-buttress.o \
+				ipu6-cpd.o \
+				ipu6-fw-com.o
+
+obj-$(CONFIG_VIDEO_INTEL_IPU6)	+= intel-ipu6.o
+
+intel-ipu6-isys-y		:= ipu6-isys.o \
+				ipu6-isys-csi2.o \
+				ipu6-fw-isys.o \
+				ipu6-isys-video.o \
+				ipu6-isys-queue.o \
+				ipu6-isys-subdev.o \
+				ipu6-isys-mcd-phy.o \
+				ipu6-isys-jsl-phy.o \
+				ipu6-isys-dwc-phy.o
+
+obj-$(CONFIG_VIDEO_INTEL_IPU6)	+= intel-ipu6-isys.o
diff --git a/drivers/media/pci/intel/ipu6/ipu6-bus.c b/drivers/media/pci/intel/ipu6/ipu6-bus.c
new file mode 100644
index 000000000000..5cee2748983b
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-bus.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013 - 2024 Intel Corporation
+ */
+
+#include <linux/auxiliary_bus.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/pm_domain.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-buttress.h"
+#include "ipu6-dma.h"
+
+static int bus_pm_runtime_suspend(struct device *dev)
+{
+	struct ipu6_bus_device *adev = to_ipu6_bus_device(dev);
+	int ret;
+
+	ret = pm_generic_runtime_suspend(dev);
+	if (ret)
+		return ret;
+
+	ret = ipu6_buttress_power(dev, adev->ctrl, false);
+	if (!ret)
+		return 0;
+
+	dev_err(dev, "power down failed!\n");
+
+	/* Powering down failed, attempt to resume device now */
+	ret = pm_generic_runtime_resume(dev);
+	if (!ret)
+		return -EBUSY;
+
+	return -EIO;
+}
+
+static int bus_pm_runtime_resume(struct device *dev)
+{
+	struct ipu6_bus_device *adev = to_ipu6_bus_device(dev);
+	int ret;
+
+	ret = ipu6_buttress_power(dev, adev->ctrl, true);
+	if (ret)
+		return ret;
+
+	ret = pm_generic_runtime_resume(dev);
+	if (ret)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	ipu6_buttress_power(dev, adev->ctrl, false);
+
+	return -EBUSY;
+}
+
+static struct dev_pm_domain ipu6_bus_pm_domain = {
+	.ops = {
+		.runtime_suspend = bus_pm_runtime_suspend,
+		.runtime_resume = bus_pm_runtime_resume,
+	},
+};
+
+static DEFINE_MUTEX(ipu6_bus_mutex);
+
+static void ipu6_bus_release(struct device *dev)
+{
+	struct ipu6_bus_device *adev = to_ipu6_bus_device(dev);
+
+	kfree(adev->pdata);
+	kfree(adev);
+}
+
+struct ipu6_bus_device *
+ipu6_bus_initialize_device(struct pci_dev *pdev, struct device *parent,
+			   void *pdata, const struct ipu6_buttress_ctrl *ctrl,
+			   char *name)
+{
+	struct auxiliary_device *auxdev;
+	struct ipu6_bus_device *adev;
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+	int ret;
+
+	adev = kzalloc(sizeof(*adev), GFP_KERNEL);
+	if (!adev)
+		return ERR_PTR(-ENOMEM);
+
+	adev->isp = isp;
+	adev->ctrl = ctrl;
+	adev->pdata = pdata;
+	auxdev = &adev->auxdev;
+	auxdev->name = name;
+	auxdev->id = (pci_domain_nr(pdev->bus) << 16) |
+		      PCI_DEVID(pdev->bus->number, pdev->devfn);
+
+	auxdev->dev.parent = parent;
+	auxdev->dev.release = ipu6_bus_release;
+
+	ret = auxiliary_device_init(auxdev);
+	if (ret < 0) {
+		dev_err(&isp->pdev->dev, "auxiliary device init failed (%d)\n",
+			ret);
+		kfree(adev);
+		return ERR_PTR(ret);
+	}
+
+	dev_pm_domain_set(&auxdev->dev, &ipu6_bus_pm_domain);
+
+	pm_runtime_forbid(&adev->auxdev.dev);
+	pm_runtime_enable(&adev->auxdev.dev);
+
+	return adev;
+}
+
+int ipu6_bus_add_device(struct ipu6_bus_device *adev)
+{
+	struct auxiliary_device *auxdev = &adev->auxdev;
+	int ret;
+
+	ret = auxiliary_device_add(auxdev);
+	if (ret) {
+		auxiliary_device_uninit(auxdev);
+		return ret;
+	}
+
+	mutex_lock(&ipu6_bus_mutex);
+	list_add(&adev->list, &adev->isp->devices);
+	mutex_unlock(&ipu6_bus_mutex);
+
+	pm_runtime_allow(&auxdev->dev);
+
+	return 0;
+}
+
+void ipu6_bus_del_devices(struct pci_dev *pdev)
+{
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+	struct ipu6_bus_device *adev, *save;
+
+	mutex_lock(&ipu6_bus_mutex);
+
+	list_for_each_entry_safe(adev, save, &isp->devices, list) {
+		pm_runtime_disable(&adev->auxdev.dev);
+		list_del(&adev->list);
+		auxiliary_device_delete(&adev->auxdev);
+		auxiliary_device_uninit(&adev->auxdev);
+	}
+
+	mutex_unlock(&ipu6_bus_mutex);
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-bus.h b/drivers/media/pci/intel/ipu6/ipu6-bus.h
new file mode 100644
index 000000000000..a08c5468d536
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-bus.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013 - 2024 Intel Corporation */
+
+#ifndef IPU6_BUS_H
+#define IPU6_BUS_H
+
+#include <linux/auxiliary_bus.h>
+#include <linux/container_of.h>
+#include <linux/device.h>
+#include <linux/irqreturn.h>
+#include <linux/list.h>
+#include <linux/scatterlist.h>
+#include <linux/types.h>
+
+struct firmware;
+struct pci_dev;
+
+struct ipu6_buttress_ctrl;
+
+struct ipu6_bus_device {
+	struct auxiliary_device auxdev;
+	const struct auxiliary_driver *auxdrv;
+	const struct ipu6_auxdrv_data *auxdrv_data;
+	struct list_head list;
+	void *pdata;
+	struct ipu6_mmu *mmu;
+	struct ipu6_device *isp;
+	const struct ipu6_buttress_ctrl *ctrl;
+	const struct firmware *fw;
+	struct sg_table fw_sgt;
+	u64 *pkg_dir;
+	dma_addr_t pkg_dir_dma_addr;
+	unsigned int pkg_dir_size;
+};
+
+struct ipu6_auxdrv_data {
+	irqreturn_t (*isr)(struct ipu6_bus_device *adev);
+	irqreturn_t (*isr_threaded)(struct ipu6_bus_device *adev);
+	bool wake_isr_thread;
+};
+
+#define to_ipu6_bus_device(_dev) \
+	container_of(to_auxiliary_dev(_dev), struct ipu6_bus_device, auxdev)
+#define auxdev_to_adev(_auxdev) \
+	container_of(_auxdev, struct ipu6_bus_device, auxdev)
+#define ipu6_bus_get_drvdata(adev) dev_get_drvdata(&(adev)->auxdev.dev)
+
+struct ipu6_bus_device *
+ipu6_bus_initialize_device(struct pci_dev *pdev, struct device *parent,
+			   void *pdata, const struct ipu6_buttress_ctrl *ctrl,
+			   char *name);
+int ipu6_bus_add_device(struct ipu6_bus_device *adev);
+void ipu6_bus_del_devices(struct pci_dev *pdev);
+
+#endif
diff --git a/drivers/media/pci/intel/ipu6/ipu6-buttress.c b/drivers/media/pci/intel/ipu6/ipu6-buttress.c
new file mode 100644
index 000000000000..103386c4f6ae
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-buttress.c
@@ -0,0 +1,910 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/firmware.h>
+#include <linux/interrupt.h>
+#include <linux/iopoll.h>
+#include <linux/math64.h>
+#include <linux/mm.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/pfn.h>
+#include <linux/pm_runtime.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/time64.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-dma.h"
+#include "ipu6-buttress.h"
+#include "ipu6-platform-buttress-regs.h"
+
+#define BOOTLOADER_STATUS_OFFSET       0x15c
+
+#define BOOTLOADER_MAGIC_KEY		0xb00710ad
+
+#define ENTRY	BUTTRESS_IU2CSECSR_IPC_PEER_COMP_ACTIONS_RST_PHASE1
+#define EXIT	BUTTRESS_IU2CSECSR_IPC_PEER_COMP_ACTIONS_RST_PHASE2
+#define QUERY	BUTTRESS_IU2CSECSR_IPC_PEER_QUERIED_IP_COMP_ACTIONS_RST_PHASE
+
+#define BUTTRESS_TSC_SYNC_RESET_TRIAL_MAX	10
+
+#define BUTTRESS_POWER_TIMEOUT_US		(200 * USEC_PER_MSEC)
+
+#define BUTTRESS_CSE_BOOTLOAD_TIMEOUT_US	(5 * USEC_PER_SEC)
+#define BUTTRESS_CSE_AUTHENTICATE_TIMEOUT_US	(10 * USEC_PER_SEC)
+#define BUTTRESS_CSE_FWRESET_TIMEOUT_US		(100 * USEC_PER_MSEC)
+
+#define BUTTRESS_IPC_TX_TIMEOUT_MS		MSEC_PER_SEC
+#define BUTTRESS_IPC_RX_TIMEOUT_MS		MSEC_PER_SEC
+#define BUTTRESS_IPC_VALIDITY_TIMEOUT_US	(1 * USEC_PER_SEC)
+#define BUTTRESS_TSC_SYNC_TIMEOUT_US		(5 * USEC_PER_MSEC)
+
+#define BUTTRESS_IPC_RESET_RETRY		2000
+#define BUTTRESS_CSE_IPC_RESET_RETRY	4
+#define BUTTRESS_IPC_CMD_SEND_RETRY	1
+
+#define BUTTRESS_MAX_CONSECUTIVE_IRQS	100
+
+static const u32 ipu6_adev_irq_mask[2] = {
+	BUTTRESS_ISR_IS_IRQ,
+	BUTTRESS_ISR_PS_IRQ
+};
+
+int ipu6_buttress_ipc_reset(struct ipu6_device *isp,
+			    struct ipu6_buttress_ipc *ipc)
+{
+	unsigned int retries = BUTTRESS_IPC_RESET_RETRY;
+	struct ipu6_buttress *b = &isp->buttress;
+	u32 val = 0, csr_in_clr;
+
+	if (!isp->secure_mode) {
+		dev_dbg(&isp->pdev->dev, "Skip IPC reset for non-secure mode");
+		return 0;
+	}
+
+	mutex_lock(&b->ipc_mutex);
+
+	/* Clear-by-1 CSR (all bits), corresponding internal states. */
+	val = readl(isp->base + ipc->csr_in);
+	writel(val, isp->base + ipc->csr_in);
+
+	/* Set peer CSR bit IPC_PEER_COMP_ACTIONS_RST_PHASE1 */
+	writel(ENTRY, isp->base + ipc->csr_out);
+	/*
+	 * Clear-by-1 all CSR bits EXCEPT following
+	 * bits:
+	 * A. IPC_PEER_COMP_ACTIONS_RST_PHASE1.
+	 * B. IPC_PEER_COMP_ACTIONS_RST_PHASE2.
+	 * C. Possibly custom bits, depending on
+	 * their role.
+	 */
+	csr_in_clr = BUTTRESS_IU2CSECSR_IPC_PEER_DEASSERTED_REG_VALID_REQ |
+		BUTTRESS_IU2CSECSR_IPC_PEER_ACKED_REG_VALID |
+		BUTTRESS_IU2CSECSR_IPC_PEER_ASSERTED_REG_VALID_REQ | QUERY;
+
+	do {
+		usleep_range(400, 500);
+		val = readl(isp->base + ipc->csr_in);
+		switch (val) {
+		case ENTRY | EXIT:
+		case ENTRY | EXIT | QUERY:
+			/*
+			 * 1) Clear-by-1 CSR bits
+			 * (IPC_PEER_COMP_ACTIONS_RST_PHASE1,
+			 * IPC_PEER_COMP_ACTIONS_RST_PHASE2).
+			 * 2) Set peer CSR bit
+			 * IPC_PEER_QUERIED_IP_COMP_ACTIONS_RST_PHASE.
+			 */
+			writel(ENTRY | EXIT, isp->base + ipc->csr_in);
+			writel(QUERY, isp->base + ipc->csr_out);
+			break;
+		case ENTRY:
+		case ENTRY | QUERY:
+			/*
+			 * 1) Clear-by-1 CSR bits
+			 * (IPC_PEER_COMP_ACTIONS_RST_PHASE1,
+			 * IPC_PEER_QUERIED_IP_COMP_ACTIONS_RST_PHASE).
+			 * 2) Set peer CSR bit
+			 * IPC_PEER_COMP_ACTIONS_RST_PHASE1.
+			 */
+			writel(ENTRY | QUERY, isp->base + ipc->csr_in);
+			writel(ENTRY, isp->base + ipc->csr_out);
+			break;
+		case EXIT:
+		case EXIT | QUERY:
+			/*
+			 * Clear-by-1 CSR bit
+			 * IPC_PEER_COMP_ACTIONS_RST_PHASE2.
+			 * 1) Clear incoming doorbell.
+			 * 2) Clear-by-1 all CSR bits EXCEPT following
+			 * bits:
+			 * A. IPC_PEER_COMP_ACTIONS_RST_PHASE1.
+			 * B. IPC_PEER_COMP_ACTIONS_RST_PHASE2.
+			 * C. Possibly custom bits, depending on
+			 * their role.
+			 * 3) Set peer CSR bit
+			 * IPC_PEER_COMP_ACTIONS_RST_PHASE2.
+			 */
+			writel(EXIT, isp->base + ipc->csr_in);
+			writel(0, isp->base + ipc->db0_in);
+			writel(csr_in_clr, isp->base + ipc->csr_in);
+			writel(EXIT, isp->base + ipc->csr_out);
+
+			/*
+			 * Read csr_in again to make sure if RST_PHASE2 is done.
+			 * If csr_in is QUERY, it should be handled again.
+			 */
+			usleep_range(200, 300);
+			val = readl(isp->base + ipc->csr_in);
+			if (val & QUERY) {
+				dev_dbg(&isp->pdev->dev,
+					"RST_PHASE2 retry csr_in = %x\n", val);
+				break;
+			}
+			mutex_unlock(&b->ipc_mutex);
+			return 0;
+		case QUERY:
+			/*
+			 * 1) Clear-by-1 CSR bit
+			 * IPC_PEER_QUERIED_IP_COMP_ACTIONS_RST_PHASE.
+			 * 2) Set peer CSR bit
+			 * IPC_PEER_COMP_ACTIONS_RST_PHASE1
+			 */
+			writel(QUERY, isp->base + ipc->csr_in);
+			writel(ENTRY, isp->base + ipc->csr_out);
+			break;
+		default:
+			dev_dbg_ratelimited(&isp->pdev->dev,
+					    "Unexpected CSR 0x%x\n", val);
+			break;
+		}
+	} while (retries--);
+
+	mutex_unlock(&b->ipc_mutex);
+	dev_err(&isp->pdev->dev, "Timed out while waiting for CSE\n");
+
+	return -ETIMEDOUT;
+}
+
+static void ipu6_buttress_ipc_validity_close(struct ipu6_device *isp,
+					     struct ipu6_buttress_ipc *ipc)
+{
+	writel(BUTTRESS_IU2CSECSR_IPC_PEER_DEASSERTED_REG_VALID_REQ,
+	       isp->base + ipc->csr_out);
+}
+
+static int
+ipu6_buttress_ipc_validity_open(struct ipu6_device *isp,
+				struct ipu6_buttress_ipc *ipc)
+{
+	unsigned int mask = BUTTRESS_IU2CSECSR_IPC_PEER_ACKED_REG_VALID;
+	void __iomem *addr;
+	int ret;
+	u32 val;
+
+	writel(BUTTRESS_IU2CSECSR_IPC_PEER_ASSERTED_REG_VALID_REQ,
+	       isp->base + ipc->csr_out);
+
+	addr = isp->base + ipc->csr_in;
+	ret = readl_poll_timeout(addr, val, val & mask, 200,
+				 BUTTRESS_IPC_VALIDITY_TIMEOUT_US);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "CSE validity timeout 0x%x\n", val);
+		ipu6_buttress_ipc_validity_close(isp, ipc);
+	}
+
+	return ret;
+}
+
+static void ipu6_buttress_ipc_recv(struct ipu6_device *isp,
+				   struct ipu6_buttress_ipc *ipc, u32 *ipc_msg)
+{
+	if (ipc_msg)
+		*ipc_msg = readl(isp->base + ipc->data0_in);
+	writel(0, isp->base + ipc->db0_in);
+}
+
+static int ipu6_buttress_ipc_send_bulk(struct ipu6_device *isp,
+				       struct ipu6_ipc_buttress_bulk_msg *msgs,
+				       u32 size)
+{
+	unsigned long tx_timeout_jiffies, rx_timeout_jiffies;
+	unsigned int i, retry = BUTTRESS_IPC_CMD_SEND_RETRY;
+	struct ipu6_buttress *b = &isp->buttress;
+	struct ipu6_buttress_ipc *ipc = &b->cse;
+	u32 val;
+	int ret;
+	int tout;
+
+	mutex_lock(&b->ipc_mutex);
+
+	ret = ipu6_buttress_ipc_validity_open(isp, ipc);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "IPC validity open failed\n");
+		goto out;
+	}
+
+	tx_timeout_jiffies = msecs_to_jiffies(BUTTRESS_IPC_TX_TIMEOUT_MS);
+	rx_timeout_jiffies = msecs_to_jiffies(BUTTRESS_IPC_RX_TIMEOUT_MS);
+
+	for (i = 0; i < size; i++) {
+		reinit_completion(&ipc->send_complete);
+		if (msgs[i].require_resp)
+			reinit_completion(&ipc->recv_complete);
+
+		dev_dbg(&isp->pdev->dev, "bulk IPC command: 0x%x\n",
+			msgs[i].cmd);
+		writel(msgs[i].cmd, isp->base + ipc->data0_out);
+		val = BUTTRESS_IU2CSEDB0_BUSY | msgs[i].cmd_size;
+		writel(val, isp->base + ipc->db0_out);
+
+		tout = wait_for_completion_timeout(&ipc->send_complete,
+						   tx_timeout_jiffies);
+		if (!tout) {
+			dev_err(&isp->pdev->dev, "send IPC response timeout\n");
+			if (!retry--) {
+				ret = -ETIMEDOUT;
+				goto out;
+			}
+
+			/* Try again if CSE is not responding on first try */
+			writel(0, isp->base + ipc->db0_out);
+			i--;
+			continue;
+		}
+
+		retry = BUTTRESS_IPC_CMD_SEND_RETRY;
+
+		if (!msgs[i].require_resp)
+			continue;
+
+		tout = wait_for_completion_timeout(&ipc->recv_complete,
+						   rx_timeout_jiffies);
+		if (!tout) {
+			dev_err(&isp->pdev->dev, "recv IPC response timeout\n");
+			ret = -ETIMEDOUT;
+			goto out;
+		}
+
+		if (ipc->nack_mask &&
+		    (ipc->recv_data & ipc->nack_mask) == ipc->nack) {
+			dev_err(&isp->pdev->dev,
+				"IPC NACK for cmd 0x%x\n", msgs[i].cmd);
+			ret = -EIO;
+			goto out;
+		}
+
+		if (ipc->recv_data != msgs[i].expected_resp) {
+			dev_err(&isp->pdev->dev,
+				"expected resp: 0x%x, IPC response: 0x%x ",
+				msgs[i].expected_resp, ipc->recv_data);
+			ret = -EIO;
+			goto out;
+		}
+	}
+
+	dev_dbg(&isp->pdev->dev, "bulk IPC commands done\n");
+
+out:
+	ipu6_buttress_ipc_validity_close(isp, ipc);
+	mutex_unlock(&b->ipc_mutex);
+	return ret;
+}
+
+static int
+ipu6_buttress_ipc_send(struct ipu6_device *isp,
+		       u32 ipc_msg, u32 size, bool require_resp,
+		       u32 expected_resp)
+{
+	struct ipu6_ipc_buttress_bulk_msg msg = {
+		.cmd = ipc_msg,
+		.cmd_size = size,
+		.require_resp = require_resp,
+		.expected_resp = expected_resp,
+	};
+
+	return ipu6_buttress_ipc_send_bulk(isp, &msg, 1);
+}
+
+static irqreturn_t ipu6_buttress_call_isr(struct ipu6_bus_device *adev)
+{
+	irqreturn_t ret = IRQ_WAKE_THREAD;
+
+	if (!adev || !adev->auxdrv || !adev->auxdrv_data)
+		return IRQ_NONE;
+
+	if (adev->auxdrv_data->isr)
+		ret = adev->auxdrv_data->isr(adev);
+
+	if (ret == IRQ_WAKE_THREAD && !adev->auxdrv_data->isr_threaded)
+		ret = IRQ_NONE;
+
+	return ret;
+}
+
+irqreturn_t ipu6_buttress_isr(int irq, void *isp_ptr)
+{
+	struct ipu6_device *isp = isp_ptr;
+	struct ipu6_bus_device *adev[] = { isp->isys, isp->psys };
+	struct ipu6_buttress *b = &isp->buttress;
+	u32 reg_irq_sts = BUTTRESS_REG_ISR_STATUS;
+	irqreturn_t ret = IRQ_NONE;
+	u32 disable_irqs = 0;
+	u32 irq_status;
+	u32 i, count = 0;
+	int active;
+
+	active = pm_runtime_get_if_active(&isp->pdev->dev);
+	if (!active)
+		return IRQ_NONE;
+
+	irq_status = readl(isp->base + reg_irq_sts);
+	if (irq_status == 0 || WARN_ON_ONCE(irq_status == 0xffffffffu)) {
+		if (active > 0)
+			pm_runtime_put_noidle(&isp->pdev->dev);
+		return IRQ_NONE;
+	}
+
+	do {
+		writel(irq_status, isp->base + BUTTRESS_REG_ISR_CLEAR);
+
+		for (i = 0; i < ARRAY_SIZE(ipu6_adev_irq_mask); i++) {
+			irqreturn_t r = ipu6_buttress_call_isr(adev[i]);
+
+			if (!(irq_status & ipu6_adev_irq_mask[i]))
+				continue;
+
+			if (r == IRQ_WAKE_THREAD) {
+				ret = IRQ_WAKE_THREAD;
+				disable_irqs |= ipu6_adev_irq_mask[i];
+			} else if (ret == IRQ_NONE && r == IRQ_HANDLED) {
+				ret = IRQ_HANDLED;
+			}
+		}
+
+		if ((irq_status & BUTTRESS_EVENT) && ret == IRQ_NONE)
+			ret = IRQ_HANDLED;
+
+		if (irq_status & BUTTRESS_ISR_IPC_FROM_CSE_IS_WAITING) {
+			dev_dbg(&isp->pdev->dev,
+				"BUTTRESS_ISR_IPC_FROM_CSE_IS_WAITING\n");
+			ipu6_buttress_ipc_recv(isp, &b->cse, &b->cse.recv_data);
+			complete(&b->cse.recv_complete);
+		}
+
+		if (irq_status & BUTTRESS_ISR_IPC_EXEC_DONE_BY_CSE) {
+			dev_dbg(&isp->pdev->dev,
+				"BUTTRESS_ISR_IPC_EXEC_DONE_BY_CSE\n");
+			complete(&b->cse.send_complete);
+		}
+
+		if (irq_status & BUTTRESS_ISR_SAI_VIOLATION &&
+		    ipu6_buttress_get_secure_mode(isp))
+			dev_err(&isp->pdev->dev,
+				"BUTTRESS_ISR_SAI_VIOLATION\n");
+
+		if (irq_status & (BUTTRESS_ISR_IS_FATAL_MEM_ERR |
+				  BUTTRESS_ISR_PS_FATAL_MEM_ERR))
+			dev_err(&isp->pdev->dev,
+				"BUTTRESS_ISR_FATAL_MEM_ERR\n");
+
+		if (irq_status & BUTTRESS_ISR_UFI_ERROR)
+			dev_err(&isp->pdev->dev, "BUTTRESS_ISR_UFI_ERROR\n");
+
+		if (++count == BUTTRESS_MAX_CONSECUTIVE_IRQS) {
+			dev_err(&isp->pdev->dev, "too many consecutive IRQs\n");
+			ret = IRQ_NONE;
+			break;
+		}
+
+		irq_status = readl(isp->base + reg_irq_sts);
+	} while (irq_status);
+
+	if (disable_irqs)
+		writel(BUTTRESS_IRQS & ~disable_irqs,
+		       isp->base + BUTTRESS_REG_ISR_ENABLE);
+
+	if (active > 0)
+		pm_runtime_put(&isp->pdev->dev);
+
+	return ret;
+}
+
+irqreturn_t ipu6_buttress_isr_threaded(int irq, void *isp_ptr)
+{
+	struct ipu6_device *isp = isp_ptr;
+	struct ipu6_bus_device *adev[] = { isp->isys, isp->psys };
+	const struct ipu6_auxdrv_data *drv_data = NULL;
+	irqreturn_t ret = IRQ_NONE;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ipu6_adev_irq_mask) && adev[i]; i++) {
+		drv_data = adev[i]->auxdrv_data;
+		if (!drv_data)
+			continue;
+
+		if (drv_data->wake_isr_thread &&
+		    drv_data->isr_threaded(adev[i]) == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	writel(BUTTRESS_IRQS, isp->base + BUTTRESS_REG_ISR_ENABLE);
+
+	return ret;
+}
+
+int ipu6_buttress_power(struct device *dev,
+			const struct ipu6_buttress_ctrl *ctrl, bool on)
+{
+	struct ipu6_device *isp = to_ipu6_bus_device(dev)->isp;
+	u32 pwr_sts, val;
+	int ret;
+
+	if (!ctrl)
+		return 0;
+
+	mutex_lock(&isp->buttress.power_mutex);
+
+	if (!on) {
+		val = 0;
+		pwr_sts = ctrl->pwr_sts_off << ctrl->pwr_sts_shift;
+	} else {
+		val = BUTTRESS_FREQ_CTL_START |
+			FIELD_PREP(BUTTRESS_FREQ_CTL_RATIO_MASK,
+				   ctrl->ratio) |
+			FIELD_PREP(BUTTRESS_FREQ_CTL_QOS_FLOOR_MASK,
+				   ctrl->qos_floor) |
+			BUTTRESS_FREQ_CTL_ICCMAX_LEVEL;
+
+		pwr_sts = ctrl->pwr_sts_on << ctrl->pwr_sts_shift;
+	}
+
+	writel(val, isp->base + ctrl->freq_ctl);
+
+	ret = readl_poll_timeout(isp->base + BUTTRESS_REG_PWR_STATE,
+				 val, (val & ctrl->pwr_sts_mask) == pwr_sts,
+				 100, BUTTRESS_POWER_TIMEOUT_US);
+	if (ret)
+		dev_err(&isp->pdev->dev,
+			"Change power status timeout with 0x%x\n", val);
+
+	mutex_unlock(&isp->buttress.power_mutex);
+
+	return ret;
+}
+
+bool ipu6_buttress_get_secure_mode(struct ipu6_device *isp)
+{
+	u32 val;
+
+	val = readl(isp->base + BUTTRESS_REG_SECURITY_CTL);
+
+	return val & BUTTRESS_SECURITY_CTL_FW_SECURE_MODE;
+}
+
+bool ipu6_buttress_auth_done(struct ipu6_device *isp)
+{
+	u32 val;
+
+	if (!isp->secure_mode)
+		return true;
+
+	val = readl(isp->base + BUTTRESS_REG_SECURITY_CTL);
+	val = FIELD_GET(BUTTRESS_SECURITY_CTL_FW_SETUP_MASK, val);
+
+	return val == BUTTRESS_SECURITY_CTL_AUTH_DONE;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_auth_done, "INTEL_IPU6");
+
+int ipu6_buttress_reset_authentication(struct ipu6_device *isp)
+{
+	int ret;
+	u32 val;
+
+	if (!isp->secure_mode) {
+		dev_dbg(&isp->pdev->dev, "Skip auth for non-secure mode\n");
+		return 0;
+	}
+
+	writel(BUTTRESS_FW_RESET_CTL_START, isp->base +
+	       BUTTRESS_REG_FW_RESET_CTL);
+
+	ret = readl_poll_timeout(isp->base + BUTTRESS_REG_FW_RESET_CTL, val,
+				 val & BUTTRESS_FW_RESET_CTL_DONE, 500,
+				 BUTTRESS_CSE_FWRESET_TIMEOUT_US);
+	if (ret) {
+		dev_err(&isp->pdev->dev,
+			"Time out while resetting authentication state\n");
+		return ret;
+	}
+
+	dev_dbg(&isp->pdev->dev, "FW reset for authentication done\n");
+	writel(0, isp->base + BUTTRESS_REG_FW_RESET_CTL);
+	/* leave some time for HW restore */
+	usleep_range(800, 1000);
+
+	return 0;
+}
+
+int ipu6_buttress_map_fw_image(struct ipu6_bus_device *sys,
+			       const struct firmware *fw, struct sg_table *sgt)
+{
+	bool is_vmalloc = is_vmalloc_addr(fw->data);
+	struct pci_dev *pdev = sys->isp->pdev;
+	struct page **pages;
+	const void *addr;
+	unsigned long n_pages;
+	unsigned int i;
+	int ret;
+
+	if (!is_vmalloc && !virt_addr_valid(fw->data))
+		return -EDOM;
+
+	n_pages = PFN_UP(fw->size);
+
+	pages = kmalloc_array(n_pages, sizeof(*pages), GFP_KERNEL);
+	if (!pages)
+		return -ENOMEM;
+
+	addr = fw->data;
+	for (i = 0; i < n_pages; i++) {
+		struct page *p = is_vmalloc ?
+			vmalloc_to_page(addr) : virt_to_page(addr);
+
+		if (!p) {
+			ret = -ENOMEM;
+			goto out;
+		}
+		pages[i] = p;
+		addr += PAGE_SIZE;
+	}
+
+	ret = sg_alloc_table_from_pages(sgt, pages, n_pages, 0, fw->size,
+					GFP_KERNEL);
+	if (ret) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = dma_map_sgtable(&pdev->dev, sgt, DMA_TO_DEVICE, 0);
+	if (ret) {
+		sg_free_table(sgt);
+		goto out;
+	}
+
+	ret = ipu6_dma_map_sgtable(sys, sgt, DMA_TO_DEVICE, 0);
+	if (ret) {
+		dma_unmap_sgtable(&pdev->dev, sgt, DMA_TO_DEVICE, 0);
+		sg_free_table(sgt);
+		goto out;
+	}
+
+	ipu6_dma_sync_sgtable(sys, sgt);
+
+out:
+	kfree(pages);
+
+	return ret;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_map_fw_image, "INTEL_IPU6");
+
+void ipu6_buttress_unmap_fw_image(struct ipu6_bus_device *sys,
+				  struct sg_table *sgt)
+{
+	struct pci_dev *pdev = sys->isp->pdev;
+
+	ipu6_dma_unmap_sgtable(sys, sgt, DMA_TO_DEVICE, 0);
+	dma_unmap_sgtable(&pdev->dev, sgt, DMA_TO_DEVICE, 0);
+	sg_free_table(sgt);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_unmap_fw_image, "INTEL_IPU6");
+
+int ipu6_buttress_authenticate(struct ipu6_device *isp)
+{
+	struct ipu6_buttress *b = &isp->buttress;
+	struct ipu6_psys_pdata *psys_pdata;
+	u32 data, mask, done, fail;
+	int ret;
+
+	if (!isp->secure_mode) {
+		dev_dbg(&isp->pdev->dev, "Skip auth for non-secure mode\n");
+		return 0;
+	}
+
+	psys_pdata = isp->psys->pdata;
+
+	mutex_lock(&b->auth_mutex);
+
+	if (ipu6_buttress_auth_done(isp)) {
+		ret = 0;
+		goto out_unlock;
+	}
+
+	/*
+	 * Write address of FIT table to FW_SOURCE register
+	 * Let's use fw address. I.e. not using FIT table yet
+	 */
+	data = lower_32_bits(isp->psys->pkg_dir_dma_addr);
+	writel(data, isp->base + BUTTRESS_REG_FW_SOURCE_BASE_LO);
+
+	data = upper_32_bits(isp->psys->pkg_dir_dma_addr);
+	writel(data, isp->base + BUTTRESS_REG_FW_SOURCE_BASE_HI);
+
+	/*
+	 * Write boot_load into IU2CSEDATA0
+	 * Write sizeof(boot_load) | 0x2 << CLIENT_ID to
+	 * IU2CSEDB.IU2CSECMD and set IU2CSEDB.IU2CSEBUSY as
+	 */
+	dev_info(&isp->pdev->dev, "Sending BOOT_LOAD to CSE\n");
+
+	ret = ipu6_buttress_ipc_send(isp,
+				     BUTTRESS_IU2CSEDATA0_IPC_BOOT_LOAD,
+				     1, true,
+				     BUTTRESS_CSE2IUDATA0_IPC_BOOT_LOAD_DONE);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "CSE boot_load failed\n");
+		goto out_unlock;
+	}
+
+	mask = BUTTRESS_SECURITY_CTL_FW_SETUP_MASK;
+	done = BUTTRESS_SECURITY_CTL_FW_SETUP_DONE;
+	fail = BUTTRESS_SECURITY_CTL_AUTH_FAILED;
+	ret = readl_poll_timeout(isp->base + BUTTRESS_REG_SECURITY_CTL, data,
+				 ((data & mask) == done ||
+				  (data & mask) == fail), 500,
+				 BUTTRESS_CSE_BOOTLOAD_TIMEOUT_US);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "CSE boot_load timeout\n");
+		goto out_unlock;
+	}
+
+	if ((data & mask) == fail) {
+		dev_err(&isp->pdev->dev, "CSE auth failed\n");
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	ret = readl_poll_timeout(psys_pdata->base + BOOTLOADER_STATUS_OFFSET,
+				 data, data == BOOTLOADER_MAGIC_KEY, 500,
+				 BUTTRESS_CSE_BOOTLOAD_TIMEOUT_US);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "Unexpected magic number 0x%x\n",
+			data);
+		goto out_unlock;
+	}
+
+	/*
+	 * Write authenticate_run into IU2CSEDATA0
+	 * Write sizeof(boot_load) | 0x2 << CLIENT_ID to
+	 * IU2CSEDB.IU2CSECMD and set IU2CSEDB.IU2CSEBUSY as
+	 */
+	dev_info(&isp->pdev->dev, "Sending AUTHENTICATE_RUN to CSE\n");
+	ret = ipu6_buttress_ipc_send(isp,
+				     BUTTRESS_IU2CSEDATA0_IPC_AUTH_RUN,
+				     1, true,
+				     BUTTRESS_CSE2IUDATA0_IPC_AUTH_RUN_DONE);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "CSE authenticate_run failed\n");
+		goto out_unlock;
+	}
+
+	done = BUTTRESS_SECURITY_CTL_AUTH_DONE;
+	ret = readl_poll_timeout(isp->base + BUTTRESS_REG_SECURITY_CTL, data,
+				 ((data & mask) == done ||
+				  (data & mask) == fail), 500,
+				 BUTTRESS_CSE_AUTHENTICATE_TIMEOUT_US);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "CSE authenticate timeout\n");
+		goto out_unlock;
+	}
+
+	if ((data & mask) == fail) {
+		dev_err(&isp->pdev->dev, "CSE boot_load failed\n");
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	dev_info(&isp->pdev->dev, "CSE authenticate_run done\n");
+
+out_unlock:
+	mutex_unlock(&b->auth_mutex);
+
+	return ret;
+}
+
+static int ipu6_buttress_send_tsc_request(struct ipu6_device *isp)
+{
+	u32 val, mask, done;
+	int ret;
+
+	mask = BUTTRESS_PWR_STATE_HH_STATUS_MASK;
+
+	writel(BUTTRESS_FABRIC_CMD_START_TSC_SYNC,
+	       isp->base + BUTTRESS_REG_FABRIC_CMD);
+
+	val = readl(isp->base + BUTTRESS_REG_PWR_STATE);
+	val = FIELD_GET(mask, val);
+	if (val == BUTTRESS_PWR_STATE_HH_STATE_ERR) {
+		dev_err(&isp->pdev->dev, "Start tsc sync failed\n");
+		return -EINVAL;
+	}
+
+	done = BUTTRESS_PWR_STATE_HH_STATE_DONE;
+	ret = readl_poll_timeout(isp->base + BUTTRESS_REG_PWR_STATE, val,
+				 FIELD_GET(mask, val) == done, 500,
+				 BUTTRESS_TSC_SYNC_TIMEOUT_US);
+	if (ret)
+		dev_err(&isp->pdev->dev, "Start tsc sync timeout\n");
+
+	return ret;
+}
+
+int ipu6_buttress_start_tsc_sync(struct ipu6_device *isp)
+{
+	unsigned int i;
+
+	for (i = 0; i < BUTTRESS_TSC_SYNC_RESET_TRIAL_MAX; i++) {
+		u32 val;
+		int ret;
+
+		ret = ipu6_buttress_send_tsc_request(isp);
+		if (ret != -ETIMEDOUT)
+			return ret;
+
+		val = readl(isp->base + BUTTRESS_REG_TSW_CTL);
+		val = val | BUTTRESS_TSW_CTL_SOFT_RESET;
+		writel(val, isp->base + BUTTRESS_REG_TSW_CTL);
+		val = val & ~BUTTRESS_TSW_CTL_SOFT_RESET;
+		writel(val, isp->base + BUTTRESS_REG_TSW_CTL);
+	}
+
+	dev_err(&isp->pdev->dev, "TSC sync failed (timeout)\n");
+
+	return -ETIMEDOUT;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_start_tsc_sync, "INTEL_IPU6");
+
+void ipu6_buttress_tsc_read(struct ipu6_device *isp, u64 *val)
+{
+	u32 tsc_hi_1, tsc_hi_2, tsc_lo;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	tsc_hi_1 = readl(isp->base + BUTTRESS_REG_TSC_HI);
+	tsc_lo = readl(isp->base + BUTTRESS_REG_TSC_LO);
+	tsc_hi_2 = readl(isp->base + BUTTRESS_REG_TSC_HI);
+	if (tsc_hi_1 == tsc_hi_2) {
+		*val = (u64)tsc_hi_1 << 32 | tsc_lo;
+	} else {
+		/* Check if TSC has rolled over */
+		if (tsc_lo & BIT(31))
+			*val = (u64)tsc_hi_1 << 32 | tsc_lo;
+		else
+			*val = (u64)tsc_hi_2 << 32 | tsc_lo;
+	}
+	local_irq_restore(flags);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_tsc_read, "INTEL_IPU6");
+
+u64 ipu6_buttress_tsc_ticks_to_ns(u64 ticks, const struct ipu6_device *isp)
+{
+	u64 ns = ticks * 10000;
+
+	/*
+	 * converting TSC tick count to ns is calculated by:
+	 * Example (TSC clock frequency is 19.2MHz):
+	 * ns = ticks * 1000 000 000 / 19.2Mhz
+	 *    = ticks * 1000 000 000 / 19200000Hz
+	 *    = ticks * 10000 / 192 ns
+	 */
+	return div_u64(ns, isp->buttress.ref_clk);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_buttress_tsc_ticks_to_ns, "INTEL_IPU6");
+
+void ipu6_buttress_restore(struct ipu6_device *isp)
+{
+	struct ipu6_buttress *b = &isp->buttress;
+
+	writel(BUTTRESS_IRQS, isp->base + BUTTRESS_REG_ISR_CLEAR);
+	writel(BUTTRESS_IRQS, isp->base + BUTTRESS_REG_ISR_ENABLE);
+	writel(b->wdt_cached_value, isp->base + BUTTRESS_REG_WDT);
+}
+
+int ipu6_buttress_init(struct ipu6_device *isp)
+{
+	int ret, ipc_reset_retry = BUTTRESS_CSE_IPC_RESET_RETRY;
+	struct ipu6_buttress *b = &isp->buttress;
+	u32 val;
+
+	mutex_init(&b->power_mutex);
+	mutex_init(&b->auth_mutex);
+	mutex_init(&b->cons_mutex);
+	mutex_init(&b->ipc_mutex);
+	init_completion(&b->cse.send_complete);
+	init_completion(&b->cse.recv_complete);
+
+	b->cse.nack = BUTTRESS_CSE2IUDATA0_IPC_NACK;
+	b->cse.nack_mask = BUTTRESS_CSE2IUDATA0_IPC_NACK_MASK;
+	b->cse.csr_in = BUTTRESS_REG_CSE2IUCSR;
+	b->cse.csr_out = BUTTRESS_REG_IU2CSECSR;
+	b->cse.db0_in = BUTTRESS_REG_CSE2IUDB0;
+	b->cse.db0_out = BUTTRESS_REG_IU2CSEDB0;
+	b->cse.data0_in = BUTTRESS_REG_CSE2IUDATA0;
+	b->cse.data0_out = BUTTRESS_REG_IU2CSEDATA0;
+
+	INIT_LIST_HEAD(&b->constraints);
+
+	isp->secure_mode = ipu6_buttress_get_secure_mode(isp);
+	dev_dbg(&isp->pdev->dev, "IPU6 in %s mode touch 0x%x mask 0x%x\n",
+		isp->secure_mode ? "secure" : "non-secure",
+		readl(isp->base + BUTTRESS_REG_SECURITY_TOUCH),
+		readl(isp->base + BUTTRESS_REG_CAMERA_MASK));
+
+	b->wdt_cached_value = readl(isp->base + BUTTRESS_REG_WDT);
+	writel(BUTTRESS_IRQS, isp->base + BUTTRESS_REG_ISR_CLEAR);
+	writel(BUTTRESS_IRQS, isp->base + BUTTRESS_REG_ISR_ENABLE);
+
+	/* get ref_clk frequency by reading the indication in btrs control */
+	val = readl(isp->base + BUTTRESS_REG_BTRS_CTRL);
+	val = FIELD_GET(BUTTRESS_REG_BTRS_CTRL_REF_CLK_IND, val);
+
+	switch (val) {
+	case 0x0:
+		b->ref_clk = 240;
+		break;
+	case 0x1:
+		b->ref_clk = 192;
+		break;
+	case 0x2:
+		b->ref_clk = 384;
+		break;
+	default:
+		dev_warn(&isp->pdev->dev,
+			 "Unsupported ref clock, use 19.2Mhz by default.\n");
+		b->ref_clk = 192;
+		break;
+	}
+
+	/* Retry couple of times in case of CSE initialization is delayed */
+	do {
+		ret = ipu6_buttress_ipc_reset(isp, &b->cse);
+		if (ret) {
+			dev_warn(&isp->pdev->dev,
+				 "IPC reset protocol failed, retrying\n");
+		} else {
+			dev_dbg(&isp->pdev->dev, "IPC reset done\n");
+			return 0;
+		}
+	} while (ipc_reset_retry--);
+
+	dev_err(&isp->pdev->dev, "IPC reset protocol failed\n");
+
+	mutex_destroy(&b->power_mutex);
+	mutex_destroy(&b->auth_mutex);
+	mutex_destroy(&b->cons_mutex);
+	mutex_destroy(&b->ipc_mutex);
+
+	return ret;
+}
+
+void ipu6_buttress_exit(struct ipu6_device *isp)
+{
+	struct ipu6_buttress *b = &isp->buttress;
+
+	writel(0, isp->base + BUTTRESS_REG_ISR_ENABLE);
+
+	mutex_destroy(&b->power_mutex);
+	mutex_destroy(&b->auth_mutex);
+	mutex_destroy(&b->cons_mutex);
+	mutex_destroy(&b->ipc_mutex);
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-buttress.h b/drivers/media/pci/intel/ipu6/ipu6-buttress.h
new file mode 100644
index 000000000000..51e5ad48db82
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-buttress.h
@@ -0,0 +1,85 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_BUTTRESS_H
+#define IPU6_BUTTRESS_H
+
+#include <linux/completion.h>
+#include <linux/irqreturn.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+
+struct device;
+struct firmware;
+struct ipu6_device;
+struct ipu6_bus_device;
+
+#define BUTTRESS_PS_FREQ_STEP		25U
+#define BUTTRESS_MIN_FORCE_PS_FREQ	(BUTTRESS_PS_FREQ_STEP * 8)
+#define BUTTRESS_MAX_FORCE_PS_FREQ	(BUTTRESS_PS_FREQ_STEP * 32)
+
+#define BUTTRESS_IS_FREQ_STEP		25U
+#define BUTTRESS_MIN_FORCE_IS_FREQ	(BUTTRESS_IS_FREQ_STEP * 8)
+#define BUTTRESS_MAX_FORCE_IS_FREQ	(BUTTRESS_IS_FREQ_STEP * 22)
+
+struct ipu6_buttress_ctrl {
+	u32 freq_ctl, pwr_sts_shift, pwr_sts_mask, pwr_sts_on, pwr_sts_off;
+	unsigned int ratio;
+	unsigned int qos_floor;
+};
+
+struct ipu6_buttress_ipc {
+	struct completion send_complete;
+	struct completion recv_complete;
+	u32 nack;
+	u32 nack_mask;
+	u32 recv_data;
+	u32 csr_out;
+	u32 csr_in;
+	u32 db0_in;
+	u32 db0_out;
+	u32 data0_out;
+	u32 data0_in;
+};
+
+struct ipu6_buttress {
+	struct mutex power_mutex, auth_mutex, cons_mutex, ipc_mutex;
+	struct ipu6_buttress_ipc cse;
+	struct list_head constraints;
+	u32 wdt_cached_value;
+	bool force_suspend;
+	u32 ref_clk;
+};
+
+struct ipu6_ipc_buttress_bulk_msg {
+	u32 cmd;
+	u32 expected_resp;
+	bool require_resp;
+	u8 cmd_size;
+};
+
+int ipu6_buttress_ipc_reset(struct ipu6_device *isp,
+			    struct ipu6_buttress_ipc *ipc);
+int ipu6_buttress_map_fw_image(struct ipu6_bus_device *sys,
+			       const struct firmware *fw,
+			       struct sg_table *sgt);
+void ipu6_buttress_unmap_fw_image(struct ipu6_bus_device *sys,
+				  struct sg_table *sgt);
+int ipu6_buttress_power(struct device *dev,
+			const struct ipu6_buttress_ctrl *ctrl, bool on);
+bool ipu6_buttress_get_secure_mode(struct ipu6_device *isp);
+int ipu6_buttress_authenticate(struct ipu6_device *isp);
+int ipu6_buttress_reset_authentication(struct ipu6_device *isp);
+bool ipu6_buttress_auth_done(struct ipu6_device *isp);
+int ipu6_buttress_start_tsc_sync(struct ipu6_device *isp);
+void ipu6_buttress_tsc_read(struct ipu6_device *isp, u64 *val);
+u64 ipu6_buttress_tsc_ticks_to_ns(u64 ticks, const struct ipu6_device *isp);
+
+irqreturn_t ipu6_buttress_isr(int irq, void *isp_ptr);
+irqreturn_t ipu6_buttress_isr_threaded(int irq, void *isp_ptr);
+int ipu6_buttress_init(struct ipu6_device *isp);
+void ipu6_buttress_exit(struct ipu6_device *isp);
+void ipu6_buttress_csi_port_config(struct ipu6_device *isp,
+				   u32 legacy, u32 combo);
+void ipu6_buttress_restore(struct ipu6_device *isp);
+#endif /* IPU6_BUTTRESS_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-cpd.c b/drivers/media/pci/intel/ipu6/ipu6-cpd.c
new file mode 100644
index 000000000000..b7013f6524ec
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-cpd.c
@@ -0,0 +1,362 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/err.h>
+#include <linux/dma-mapping.h>
+#include <linux/gfp_types.h>
+#include <linux/math64.h>
+#include <linux/sizes.h>
+#include <linux/types.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-cpd.h"
+#include "ipu6-dma.h"
+
+/* 15 entries + header*/
+#define MAX_PKG_DIR_ENT_CNT		16
+/* 2 qword per entry/header */
+#define PKG_DIR_ENT_LEN			2
+/* PKG_DIR size in bytes */
+#define PKG_DIR_SIZE			((MAX_PKG_DIR_ENT_CNT) *	\
+					 (PKG_DIR_ENT_LEN) * sizeof(u64))
+/* _IUPKDR_ */
+#define PKG_DIR_HDR_MARK		0x5f4955504b44525fULL
+
+/* $CPD */
+#define CPD_HDR_MARK			0x44504324
+
+#define MAX_MANIFEST_SIZE		(SZ_2K * sizeof(u32))
+#define MAX_METADATA_SIZE		SZ_64K
+
+#define MAX_COMPONENT_ID		127
+#define MAX_COMPONENT_VERSION		0xffff
+
+#define MANIFEST_IDX	0
+#define METADATA_IDX	1
+#define MODULEDATA_IDX	2
+/*
+ * PKG_DIR Entry (type == id)
+ * 63:56        55      54:48   47:32   31:24   23:0
+ * Rsvd         Rsvd    Type    Version Rsvd    Size
+ */
+#define PKG_DIR_SIZE_MASK	GENMASK_ULL(23, 0)
+#define PKG_DIR_VERSION_MASK	GENMASK_ULL(47, 32)
+#define PKG_DIR_TYPE_MASK	GENMASK_ULL(54, 48)
+
+static inline const struct ipu6_cpd_ent *ipu6_cpd_get_entry(const void *cpd,
+							    u8 idx)
+{
+	const struct ipu6_cpd_hdr *cpd_hdr = cpd;
+	const struct ipu6_cpd_ent *ent;
+
+	ent = (const struct ipu6_cpd_ent *)((const u8 *)cpd + cpd_hdr->hdr_len);
+	return ent + idx;
+}
+
+#define ipu6_cpd_get_manifest(cpd) ipu6_cpd_get_entry(cpd, MANIFEST_IDX)
+#define ipu6_cpd_get_metadata(cpd) ipu6_cpd_get_entry(cpd, METADATA_IDX)
+#define ipu6_cpd_get_moduledata(cpd) ipu6_cpd_get_entry(cpd, MODULEDATA_IDX)
+
+static const struct ipu6_cpd_metadata_cmpnt_hdr *
+ipu6_cpd_metadata_get_cmpnt(struct ipu6_device *isp, const void *metadata,
+			    unsigned int metadata_size, u8 idx)
+{
+	size_t extn_size = sizeof(struct ipu6_cpd_metadata_extn);
+	size_t cmpnt_count = metadata_size - extn_size;
+
+	cmpnt_count = div_u64(cmpnt_count, isp->cpd_metadata_cmpnt_size);
+
+	if (idx > MAX_COMPONENT_ID || idx >= cmpnt_count) {
+		dev_err(&isp->pdev->dev, "Component index out of range (%d)\n",
+			idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	return metadata + extn_size + idx * isp->cpd_metadata_cmpnt_size;
+}
+
+static u32 ipu6_cpd_metadata_cmpnt_version(struct ipu6_device *isp,
+					   const void *metadata,
+					   unsigned int metadata_size, u8 idx)
+{
+	const struct ipu6_cpd_metadata_cmpnt_hdr *cmpnt;
+
+	cmpnt = ipu6_cpd_metadata_get_cmpnt(isp, metadata, metadata_size, idx);
+	if (IS_ERR(cmpnt))
+		return PTR_ERR(cmpnt);
+
+	return cmpnt->ver;
+}
+
+static int ipu6_cpd_metadata_get_cmpnt_id(struct ipu6_device *isp,
+					  const void *metadata,
+					  unsigned int metadata_size, u8 idx)
+{
+	const struct ipu6_cpd_metadata_cmpnt_hdr *cmpnt;
+
+	cmpnt = ipu6_cpd_metadata_get_cmpnt(isp, metadata, metadata_size, idx);
+	if (IS_ERR(cmpnt))
+		return PTR_ERR(cmpnt);
+
+	return cmpnt->id;
+}
+
+static int ipu6_cpd_parse_module_data(struct ipu6_device *isp,
+				      const void *module_data,
+				      unsigned int module_data_size,
+				      dma_addr_t dma_addr_module_data,
+				      u64 *pkg_dir, const void *metadata,
+				      unsigned int metadata_size)
+{
+	const struct ipu6_cpd_module_data_hdr *module_data_hdr;
+	const struct ipu6_cpd_hdr *dir_hdr;
+	const struct ipu6_cpd_ent *dir_ent;
+	unsigned int i;
+	u8 len;
+
+	if (!module_data)
+		return -EINVAL;
+
+	module_data_hdr = module_data;
+	dir_hdr = module_data + module_data_hdr->hdr_len;
+	len = dir_hdr->hdr_len;
+	dir_ent = (const struct ipu6_cpd_ent *)(((u8 *)dir_hdr) + len);
+
+	pkg_dir[0] = PKG_DIR_HDR_MARK;
+	/* pkg_dir entry count = component count + pkg_dir header */
+	pkg_dir[1] = dir_hdr->ent_cnt + 1;
+
+	for (i = 0; i < dir_hdr->ent_cnt; i++, dir_ent++) {
+		u64 *p = &pkg_dir[PKG_DIR_ENT_LEN *  (1 + i)];
+		int ver, id;
+
+		*p++ = dma_addr_module_data + dir_ent->offset;
+		id = ipu6_cpd_metadata_get_cmpnt_id(isp, metadata,
+						    metadata_size, i);
+		if (id < 0 || id > MAX_COMPONENT_ID) {
+			dev_err(&isp->pdev->dev, "Invalid CPD component id\n");
+			return -EINVAL;
+		}
+
+		ver = ipu6_cpd_metadata_cmpnt_version(isp, metadata,
+						      metadata_size, i);
+		if (ver < 0 || ver > MAX_COMPONENT_VERSION) {
+			dev_err(&isp->pdev->dev,
+				"Invalid CPD component version\n");
+			return -EINVAL;
+		}
+
+		*p = FIELD_PREP(PKG_DIR_SIZE_MASK, dir_ent->len) |
+			FIELD_PREP(PKG_DIR_TYPE_MASK, id) |
+			FIELD_PREP(PKG_DIR_VERSION_MASK, ver);
+	}
+
+	return 0;
+}
+
+int ipu6_cpd_create_pkg_dir(struct ipu6_bus_device *adev, const void *src)
+{
+	dma_addr_t dma_addr_src = sg_dma_address(adev->fw_sgt.sgl);
+	const struct ipu6_cpd_ent *ent, *man_ent, *met_ent;
+	struct ipu6_device *isp = adev->isp;
+	unsigned int man_sz, met_sz;
+	void *pkg_dir_pos;
+	int ret;
+
+	man_ent = ipu6_cpd_get_manifest(src);
+	man_sz = man_ent->len;
+
+	met_ent = ipu6_cpd_get_metadata(src);
+	met_sz = met_ent->len;
+
+	adev->pkg_dir_size = PKG_DIR_SIZE + man_sz + met_sz;
+	adev->pkg_dir = ipu6_dma_alloc(adev, adev->pkg_dir_size,
+				       &adev->pkg_dir_dma_addr, GFP_KERNEL, 0);
+	if (!adev->pkg_dir)
+		return -ENOMEM;
+
+	/*
+	 * pkg_dir entry/header:
+	 * qword | 63:56 | 55   | 54:48 | 47:32 | 31:24 | 23:0
+	 * N         Address/Offset/"_IUPKDR_"
+	 * N + 1 | rsvd  | rsvd | type  | ver   | rsvd  | size
+	 *
+	 * We can ignore other fields that size in N + 1 qword as they
+	 * are 0 anyway. Just setting size for now.
+	 */
+
+	ent = ipu6_cpd_get_moduledata(src);
+
+	ret = ipu6_cpd_parse_module_data(isp, src + ent->offset,
+					 ent->len, dma_addr_src + ent->offset,
+					 adev->pkg_dir, src + met_ent->offset,
+					 met_ent->len);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "Failed to parse module data\n");
+		ipu6_dma_free(adev, adev->pkg_dir_size,
+			      adev->pkg_dir, adev->pkg_dir_dma_addr, 0);
+		return ret;
+	}
+
+	/* Copy manifest after pkg_dir */
+	pkg_dir_pos = adev->pkg_dir + PKG_DIR_ENT_LEN * MAX_PKG_DIR_ENT_CNT;
+	memcpy(pkg_dir_pos, src + man_ent->offset, man_sz);
+
+	/* Copy metadata after manifest */
+	pkg_dir_pos += man_sz;
+	memcpy(pkg_dir_pos, src + met_ent->offset, met_sz);
+
+	ipu6_dma_sync_single(adev, adev->pkg_dir_dma_addr,
+			     adev->pkg_dir_size);
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_cpd_create_pkg_dir, "INTEL_IPU6");
+
+void ipu6_cpd_free_pkg_dir(struct ipu6_bus_device *adev)
+{
+	ipu6_dma_free(adev, adev->pkg_dir_size, adev->pkg_dir,
+		      adev->pkg_dir_dma_addr, 0);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_cpd_free_pkg_dir, "INTEL_IPU6");
+
+static int ipu6_cpd_validate_cpd(struct ipu6_device *isp, const void *cpd,
+				 unsigned long cpd_size,
+				 unsigned long data_size)
+{
+	const struct ipu6_cpd_hdr *cpd_hdr = cpd;
+	const struct ipu6_cpd_ent *ent;
+	unsigned int i;
+	u8 len;
+
+	len = cpd_hdr->hdr_len;
+
+	/* Ensure cpd hdr is within moduledata */
+	if (cpd_size < len) {
+		dev_err(&isp->pdev->dev, "Invalid CPD moduledata size\n");
+		return -EINVAL;
+	}
+
+	/* Sanity check for CPD header */
+	if ((cpd_size - len) / sizeof(*ent) < cpd_hdr->ent_cnt) {
+		dev_err(&isp->pdev->dev, "Invalid CPD header\n");
+		return -EINVAL;
+	}
+
+	/* Ensure that all entries are within moduledata */
+	ent = (const struct ipu6_cpd_ent *)(((const u8 *)cpd_hdr) + len);
+	for (i = 0; i < cpd_hdr->ent_cnt; i++, ent++) {
+		if (data_size < ent->offset ||
+		    data_size - ent->offset < ent->len) {
+			dev_err(&isp->pdev->dev, "Invalid CPD entry (%d)\n", i);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int ipu6_cpd_validate_moduledata(struct ipu6_device *isp,
+					const void *moduledata,
+					u32 moduledata_size)
+{
+	const struct ipu6_cpd_module_data_hdr *mod_hdr = moduledata;
+	int ret;
+
+	/* Ensure moduledata hdr is within moduledata */
+	if (moduledata_size < sizeof(*mod_hdr) ||
+	    moduledata_size < mod_hdr->hdr_len) {
+		dev_err(&isp->pdev->dev, "Invalid CPD moduledata size\n");
+		return -EINVAL;
+	}
+
+	dev_dbg(&isp->pdev->dev, "FW version: %x\n", mod_hdr->fw_pkg_date);
+	ret = ipu6_cpd_validate_cpd(isp, moduledata + mod_hdr->hdr_len,
+				    moduledata_size - mod_hdr->hdr_len,
+				    moduledata_size);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "Invalid CPD in moduledata\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ipu6_cpd_validate_metadata(struct ipu6_device *isp,
+				      const void *metadata, u32 meta_size)
+{
+	const struct ipu6_cpd_metadata_extn *extn = metadata;
+
+	/* Sanity check for metadata size */
+	if (meta_size < sizeof(*extn) || meta_size > MAX_METADATA_SIZE) {
+		dev_err(&isp->pdev->dev, "Invalid CPD metadata\n");
+		return -EINVAL;
+	}
+
+	/* Validate extension and image types */
+	if (extn->extn_type != IPU6_CPD_METADATA_EXTN_TYPE_IUNIT ||
+	    extn->img_type != IPU6_CPD_METADATA_IMAGE_TYPE_MAIN_FIRMWARE) {
+		dev_err(&isp->pdev->dev,
+			"Invalid CPD metadata descriptor img_type (%d)\n",
+			extn->img_type);
+		return -EINVAL;
+	}
+
+	/* Validate metadata size multiple of metadata components */
+	if ((meta_size - sizeof(*extn)) % isp->cpd_metadata_cmpnt_size) {
+		dev_err(&isp->pdev->dev, "Invalid CPD metadata size\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int ipu6_cpd_validate_cpd_file(struct ipu6_device *isp, const void *cpd_file,
+			       unsigned long cpd_file_size)
+{
+	const struct ipu6_cpd_hdr *hdr = cpd_file;
+	const struct ipu6_cpd_ent *ent;
+	int ret;
+
+	ret = ipu6_cpd_validate_cpd(isp, cpd_file, cpd_file_size,
+				    cpd_file_size);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "Invalid CPD in file\n");
+		return ret;
+	}
+
+	/* Check for CPD file marker */
+	if (hdr->hdr_mark != CPD_HDR_MARK) {
+		dev_err(&isp->pdev->dev, "Invalid CPD header\n");
+		return -EINVAL;
+	}
+
+	/* Sanity check for manifest size */
+	ent = ipu6_cpd_get_manifest(cpd_file);
+	if (ent->len > MAX_MANIFEST_SIZE) {
+		dev_err(&isp->pdev->dev, "Invalid CPD manifest size\n");
+		return -EINVAL;
+	}
+
+	/* Validate metadata */
+	ent = ipu6_cpd_get_metadata(cpd_file);
+	ret = ipu6_cpd_validate_metadata(isp, cpd_file + ent->offset, ent->len);
+	if (ret) {
+		dev_err(&isp->pdev->dev, "Invalid CPD metadata\n");
+		return ret;
+	}
+
+	/* Validate moduledata */
+	ent = ipu6_cpd_get_moduledata(cpd_file);
+	ret = ipu6_cpd_validate_moduledata(isp, cpd_file + ent->offset,
+					   ent->len);
+	if (ret)
+		dev_err(&isp->pdev->dev, "Invalid CPD moduledata\n");
+
+	return ret;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-cpd.h b/drivers/media/pci/intel/ipu6/ipu6-cpd.h
new file mode 100644
index 000000000000..e0e4fdeca902
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-cpd.h
@@ -0,0 +1,105 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2015--2024 Intel Corporation */
+
+#ifndef IPU6_CPD_H
+#define IPU6_CPD_H
+
+struct ipu6_device;
+struct ipu6_bus_device;
+
+#define IPU6_CPD_SIZE_OF_FW_ARCH_VERSION	7
+#define IPU6_CPD_SIZE_OF_SYSTEM_VERSION		11
+#define IPU6_CPD_SIZE_OF_COMPONENT_NAME		12
+
+#define IPU6_CPD_METADATA_EXTN_TYPE_IUNIT	0x10
+
+#define IPU6_CPD_METADATA_IMAGE_TYPE_RESERVED		0
+#define IPU6_CPD_METADATA_IMAGE_TYPE_BOOTLOADER		1
+#define IPU6_CPD_METADATA_IMAGE_TYPE_MAIN_FIRMWARE	2
+
+#define IPU6_CPD_PKG_DIR_PSYS_SERVER_IDX	0
+#define IPU6_CPD_PKG_DIR_ISYS_SERVER_IDX	1
+
+#define IPU6_CPD_PKG_DIR_CLIENT_PG_TYPE		3
+
+#define IPU6_CPD_METADATA_HASH_KEY_SIZE		48
+#define IPU6SE_CPD_METADATA_HASH_KEY_SIZE	32
+
+struct ipu6_cpd_module_data_hdr {
+	u32 hdr_len;
+	u32 endian;
+	u32 fw_pkg_date;
+	u32 hive_sdk_date;
+	u32 compiler_date;
+	u32 target_platform_type;
+	u8 sys_ver[IPU6_CPD_SIZE_OF_SYSTEM_VERSION];
+	u8 fw_arch_ver[IPU6_CPD_SIZE_OF_FW_ARCH_VERSION];
+	u8 rsvd[2];
+} __packed;
+
+/*
+ * ipu6_cpd_hdr structure updated as the chksum and
+ * sub_partition_name is unused on host side
+ * CSE layout version 1.6 for IPU6SE (hdr_len = 0x10)
+ * CSE layout version 1.7 for IPU6 (hdr_len = 0x14)
+ */
+struct ipu6_cpd_hdr {
+	u32 hdr_mark;
+	u32 ent_cnt;
+	u8 hdr_ver;
+	u8 ent_ver;
+	u8 hdr_len;
+} __packed;
+
+struct ipu6_cpd_ent {
+	u8 name[IPU6_CPD_SIZE_OF_COMPONENT_NAME];
+	u32 offset;
+	u32 len;
+	u8 rsvd[4];
+} __packed;
+
+struct ipu6_cpd_metadata_cmpnt_hdr {
+	u32 id;
+	u32 size;
+	u32 ver;
+} __packed;
+
+struct ipu6_cpd_metadata_cmpnt {
+	struct ipu6_cpd_metadata_cmpnt_hdr hdr;
+	u8 sha2_hash[IPU6_CPD_METADATA_HASH_KEY_SIZE];
+	u32 entry_point;
+	u32 icache_base_offs;
+	u8 attrs[16];
+} __packed;
+
+struct ipu6se_cpd_metadata_cmpnt {
+	struct ipu6_cpd_metadata_cmpnt_hdr hdr;
+	u8 sha2_hash[IPU6SE_CPD_METADATA_HASH_KEY_SIZE];
+	u32 entry_point;
+	u32 icache_base_offs;
+	u8 attrs[16];
+} __packed;
+
+struct ipu6_cpd_metadata_extn {
+	u32 extn_type;
+	u32 len;
+	u32 img_type;
+	u8 rsvd[16];
+} __packed;
+
+struct ipu6_cpd_client_pkg_hdr {
+	u32 prog_list_offs;
+	u32 prog_list_size;
+	u32 prog_desc_offs;
+	u32 prog_desc_size;
+	u32 pg_manifest_offs;
+	u32 pg_manifest_size;
+	u32 prog_bin_offs;
+	u32 prog_bin_size;
+} __packed;
+
+int ipu6_cpd_create_pkg_dir(struct ipu6_bus_device *adev, const void *src);
+void ipu6_cpd_free_pkg_dir(struct ipu6_bus_device *adev);
+int ipu6_cpd_validate_cpd_file(struct ipu6_device *isp, const void *cpd_file,
+			       unsigned long cpd_file_size);
+#endif /* IPU6_CPD_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-dma.c b/drivers/media/pci/intel/ipu6/ipu6-dma.c
new file mode 100644
index 000000000000..7296373d36b0
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-dma.c
@@ -0,0 +1,459 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/cacheflush.h>
+#include <linux/dma-mapping.h>
+#include <linux/iova.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/vmalloc.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-dma.h"
+#include "ipu6-mmu.h"
+
+struct vm_info {
+	struct list_head list;
+	struct page **pages;
+	dma_addr_t ipu6_iova;
+	void *vaddr;
+	unsigned long size;
+};
+
+static struct vm_info *get_vm_info(struct ipu6_mmu *mmu, dma_addr_t iova)
+{
+	struct vm_info *info, *save;
+
+	list_for_each_entry_safe(info, save, &mmu->vma_list, list) {
+		if (iova >= info->ipu6_iova &&
+		    iova < (info->ipu6_iova + info->size))
+			return info;
+	}
+
+	return NULL;
+}
+
+static void __clear_buffer(struct page *page, size_t size, unsigned long attrs)
+{
+	void *ptr;
+
+	if (!page)
+		return;
+	/*
+	 * Ensure that the allocated pages are zeroed, and that any data
+	 * lurking in the kernel direct-mapped region is invalidated.
+	 */
+	ptr = page_address(page);
+	memset(ptr, 0, size);
+	if ((attrs & DMA_ATTR_SKIP_CPU_SYNC) == 0)
+		clflush_cache_range(ptr, size);
+}
+
+static struct page **__alloc_buffer(size_t size, gfp_t gfp, unsigned long attrs)
+{
+	int count = PHYS_PFN(size);
+	int array_size = count * sizeof(struct page *);
+	struct page **pages;
+	int i = 0;
+
+	pages = kvzalloc(array_size, GFP_KERNEL);
+	if (!pages)
+		return NULL;
+
+	gfp |= __GFP_NOWARN;
+
+	while (count) {
+		int j, order = __fls(count);
+
+		pages[i] = alloc_pages(gfp, order);
+		while (!pages[i] && order)
+			pages[i] = alloc_pages(gfp, --order);
+		if (!pages[i])
+			goto error;
+
+		if (order) {
+			split_page(pages[i], order);
+			j = 1 << order;
+			while (j--)
+				pages[i + j] = pages[i] + j;
+		}
+
+		__clear_buffer(pages[i], PAGE_SIZE << order, attrs);
+		i += 1 << order;
+		count -= 1 << order;
+	}
+
+	return pages;
+error:
+	while (i--)
+		if (pages[i])
+			__free_pages(pages[i], 0);
+	kvfree(pages);
+	return NULL;
+}
+
+static void __free_buffer(struct page **pages, size_t size, unsigned long attrs)
+{
+	int count = PHYS_PFN(size);
+	unsigned int i;
+
+	for (i = 0; i < count && pages[i]; i++) {
+		__clear_buffer(pages[i], PAGE_SIZE, attrs);
+		__free_pages(pages[i], 0);
+	}
+
+	kvfree(pages);
+}
+
+void ipu6_dma_sync_single(struct ipu6_bus_device *sys, dma_addr_t dma_handle,
+			  size_t size)
+{
+	void *vaddr;
+	u32 offset;
+	struct vm_info *info;
+	struct ipu6_mmu *mmu = sys->mmu;
+
+	info = get_vm_info(mmu, dma_handle);
+	if (WARN_ON(!info))
+		return;
+
+	offset = dma_handle - info->ipu6_iova;
+	if (WARN_ON(size > (info->size - offset)))
+		return;
+
+	vaddr = info->vaddr + offset;
+	clflush_cache_range(vaddr, size);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_sync_single, "INTEL_IPU6");
+
+void ipu6_dma_sync_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		      int nents)
+{
+	struct scatterlist *sg;
+	int i;
+
+	for_each_sg(sglist, sg, nents, i)
+		clflush_cache_range(sg_virt(sg), sg->length);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_sync_sg, "INTEL_IPU6");
+
+void ipu6_dma_sync_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt)
+{
+	ipu6_dma_sync_sg(sys, sgt->sgl, sgt->orig_nents);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_sync_sgtable, "INTEL_IPU6");
+
+void *ipu6_dma_alloc(struct ipu6_bus_device *sys, size_t size,
+		     dma_addr_t *dma_handle, gfp_t gfp,
+		     unsigned long attrs)
+{
+	struct device *dev = &sys->auxdev.dev;
+	struct pci_dev *pdev = sys->isp->pdev;
+	dma_addr_t pci_dma_addr, ipu6_iova;
+	struct ipu6_mmu *mmu = sys->mmu;
+	struct vm_info *info;
+	unsigned long count;
+	struct page **pages;
+	struct iova *iova;
+	unsigned int i;
+	int ret;
+
+	info = kzalloc(sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return NULL;
+
+	size = PAGE_ALIGN(size);
+	count = PHYS_PFN(size);
+
+	iova = alloc_iova(&mmu->dmap->iovad, count,
+			  PHYS_PFN(mmu->dmap->mmu_info->aperture_end), 0);
+	if (!iova)
+		goto out_kfree;
+
+	pages = __alloc_buffer(size, gfp, attrs);
+	if (!pages)
+		goto out_free_iova;
+
+	dev_dbg(dev, "dma_alloc: size %zu iova low pfn %lu, high pfn %lu\n",
+		size, iova->pfn_lo, iova->pfn_hi);
+	for (i = 0; iova->pfn_lo + i <= iova->pfn_hi; i++) {
+		pci_dma_addr = dma_map_page_attrs(&pdev->dev, pages[i], 0,
+						  PAGE_SIZE, DMA_BIDIRECTIONAL,
+						  attrs);
+		dev_dbg(dev, "dma_alloc: mapped pci_dma_addr %pad\n",
+			&pci_dma_addr);
+		if (dma_mapping_error(&pdev->dev, pci_dma_addr)) {
+			dev_err(dev, "pci_dma_mapping for page[%d] failed", i);
+			goto out_unmap;
+		}
+
+		ret = ipu6_mmu_map(mmu->dmap->mmu_info,
+				   PFN_PHYS(iova->pfn_lo + i), pci_dma_addr,
+				   PAGE_SIZE);
+		if (ret) {
+			dev_err(dev, "ipu6_mmu_map for pci_dma[%d] %pad failed",
+				i, &pci_dma_addr);
+			dma_unmap_page_attrs(&pdev->dev, pci_dma_addr,
+					     PAGE_SIZE, DMA_BIDIRECTIONAL,
+					     attrs);
+			goto out_unmap;
+		}
+	}
+
+	info->vaddr = vmap(pages, count, VM_USERMAP, PAGE_KERNEL);
+	if (!info->vaddr)
+		goto out_unmap;
+
+	*dma_handle = PFN_PHYS(iova->pfn_lo);
+
+	info->pages = pages;
+	info->ipu6_iova = *dma_handle;
+	info->size = size;
+	list_add(&info->list, &mmu->vma_list);
+
+	return info->vaddr;
+
+out_unmap:
+	while (i--) {
+		ipu6_iova = PFN_PHYS(iova->pfn_lo + i);
+		pci_dma_addr = ipu6_mmu_iova_to_phys(mmu->dmap->mmu_info,
+						     ipu6_iova);
+		dma_unmap_page_attrs(&pdev->dev, pci_dma_addr, PAGE_SIZE,
+				     DMA_BIDIRECTIONAL, attrs);
+
+		ipu6_mmu_unmap(mmu->dmap->mmu_info, ipu6_iova, PAGE_SIZE);
+	}
+
+	__free_buffer(pages, size, attrs);
+
+out_free_iova:
+	__free_iova(&mmu->dmap->iovad, iova);
+out_kfree:
+	kfree(info);
+
+	return NULL;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_alloc, "INTEL_IPU6");
+
+void ipu6_dma_free(struct ipu6_bus_device *sys, size_t size, void *vaddr,
+		   dma_addr_t dma_handle, unsigned long attrs)
+{
+	struct ipu6_mmu *mmu = sys->mmu;
+	struct pci_dev *pdev = sys->isp->pdev;
+	struct iova *iova = find_iova(&mmu->dmap->iovad, PHYS_PFN(dma_handle));
+	dma_addr_t pci_dma_addr, ipu6_iova;
+	struct vm_info *info;
+	struct page **pages;
+	unsigned int i;
+
+	if (WARN_ON(!iova))
+		return;
+
+	info = get_vm_info(mmu, dma_handle);
+	if (WARN_ON(!info))
+		return;
+
+	if (WARN_ON(!info->vaddr))
+		return;
+
+	if (WARN_ON(!info->pages))
+		return;
+
+	list_del(&info->list);
+
+	size = PAGE_ALIGN(size);
+
+	pages = info->pages;
+
+	vunmap(vaddr);
+
+	for (i = 0; i < PHYS_PFN(size); i++) {
+		ipu6_iova = PFN_PHYS(iova->pfn_lo + i);
+		pci_dma_addr = ipu6_mmu_iova_to_phys(mmu->dmap->mmu_info,
+						     ipu6_iova);
+		dma_unmap_page_attrs(&pdev->dev, pci_dma_addr, PAGE_SIZE,
+				     DMA_BIDIRECTIONAL, attrs);
+	}
+
+	ipu6_mmu_unmap(mmu->dmap->mmu_info, PFN_PHYS(iova->pfn_lo),
+		       PFN_PHYS(iova_size(iova)));
+
+	__free_buffer(pages, size, attrs);
+
+	mmu->tlb_invalidate(mmu);
+
+	__free_iova(&mmu->dmap->iovad, iova);
+
+	kfree(info);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_free, "INTEL_IPU6");
+
+int ipu6_dma_mmap(struct ipu6_bus_device *sys, struct vm_area_struct *vma,
+		  void *addr, dma_addr_t iova, size_t size,
+		  unsigned long attrs)
+{
+	struct ipu6_mmu *mmu = sys->mmu;
+	size_t count = PFN_UP(size);
+	struct vm_info *info;
+	size_t i;
+	int ret;
+
+	info = get_vm_info(mmu, iova);
+	if (!info)
+		return -EFAULT;
+
+	if (!info->vaddr)
+		return -EFAULT;
+
+	if (vma->vm_start & ~PAGE_MASK)
+		return -EINVAL;
+
+	if (size > info->size)
+		return -EFAULT;
+
+	for (i = 0; i < count; i++) {
+		ret = vm_insert_page(vma, vma->vm_start + PFN_PHYS(i),
+				     info->pages[i]);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+void ipu6_dma_unmap_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		       int nents, enum dma_data_direction dir,
+		       unsigned long attrs)
+{
+	struct device *dev = &sys->auxdev.dev;
+	struct ipu6_mmu *mmu = sys->mmu;
+	struct iova *iova = find_iova(&mmu->dmap->iovad,
+				      PHYS_PFN(sg_dma_address(sglist)));
+	struct scatterlist *sg;
+	dma_addr_t pci_dma_addr;
+	unsigned int i;
+
+	if (!nents)
+		return;
+
+	if (WARN_ON(!iova))
+		return;
+
+	/*
+	 * Before IPU6 mmu unmap, return the pci dma address back to sg
+	 * assume the nents is less than orig_nents as the least granule
+	 * is 1 SZ_4K page
+	 */
+	dev_dbg(dev, "trying to unmap concatenated %u ents\n", nents);
+	for_each_sg(sglist, sg, nents, i) {
+		dev_dbg(dev, "unmap sg[%d] %pad size %u\n", i,
+			&sg_dma_address(sg), sg_dma_len(sg));
+		pci_dma_addr = ipu6_mmu_iova_to_phys(mmu->dmap->mmu_info,
+						     sg_dma_address(sg));
+		dev_dbg(dev, "return pci_dma_addr %pad back to sg[%d]\n",
+			&pci_dma_addr, i);
+		sg_dma_address(sg) = pci_dma_addr;
+	}
+
+	dev_dbg(dev, "ipu6_mmu_unmap low pfn %lu high pfn %lu\n",
+		iova->pfn_lo, iova->pfn_hi);
+	ipu6_mmu_unmap(mmu->dmap->mmu_info, PFN_PHYS(iova->pfn_lo),
+		       PFN_PHYS(iova_size(iova)));
+
+	mmu->tlb_invalidate(mmu);
+	__free_iova(&mmu->dmap->iovad, iova);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_unmap_sg, "INTEL_IPU6");
+
+int ipu6_dma_map_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		    int nents, enum dma_data_direction dir,
+		    unsigned long attrs)
+{
+	struct device *dev = &sys->auxdev.dev;
+	struct ipu6_mmu *mmu = sys->mmu;
+	struct scatterlist *sg;
+	struct iova *iova;
+	size_t npages = 0;
+	unsigned long iova_addr;
+	int i;
+
+	for_each_sg(sglist, sg, nents, i) {
+		if (sg->offset) {
+			dev_err(dev, "Unsupported non-zero sg[%d].offset %x\n",
+				i, sg->offset);
+			return -EFAULT;
+		}
+	}
+
+	for_each_sg(sglist, sg, nents, i)
+		npages += PFN_UP(sg_dma_len(sg));
+
+	dev_dbg(dev, "dmamap trying to map %d ents %zu pages\n",
+		nents, npages);
+
+	iova = alloc_iova(&mmu->dmap->iovad, npages,
+			  PHYS_PFN(mmu->dmap->mmu_info->aperture_end), 0);
+	if (!iova)
+		return 0;
+
+	dev_dbg(dev, "dmamap: iova low pfn %lu, high pfn %lu\n", iova->pfn_lo,
+		iova->pfn_hi);
+
+	iova_addr = iova->pfn_lo;
+	for_each_sg(sglist, sg, nents, i) {
+		phys_addr_t iova_pa;
+		int ret;
+
+		iova_pa = PFN_PHYS(iova_addr);
+		dev_dbg(dev, "mapping entry %d: iova %pap phy %pap size %d\n",
+			i, &iova_pa, &sg_dma_address(sg), sg_dma_len(sg));
+
+		ret = ipu6_mmu_map(mmu->dmap->mmu_info, PFN_PHYS(iova_addr),
+				   sg_dma_address(sg),
+				   PAGE_ALIGN(sg_dma_len(sg)));
+		if (ret)
+			goto out_fail;
+
+		sg_dma_address(sg) = PFN_PHYS(iova_addr);
+
+		iova_addr += PFN_UP(sg_dma_len(sg));
+	}
+
+	dev_dbg(dev, "dmamap %d ents %zu pages mapped\n", nents, npages);
+
+	return nents;
+
+out_fail:
+	ipu6_dma_unmap_sg(sys, sglist, i, dir, attrs);
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_map_sg, "INTEL_IPU6");
+
+int ipu6_dma_map_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt,
+			 enum dma_data_direction dir, unsigned long attrs)
+{
+	int nents;
+
+	nents = ipu6_dma_map_sg(sys, sgt->sgl, sgt->nents, dir, attrs);
+	if (nents < 0)
+		return nents;
+
+	sgt->nents = nents;
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_map_sgtable, "INTEL_IPU6");
+
+void ipu6_dma_unmap_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt,
+			    enum dma_data_direction dir, unsigned long attrs)
+{
+	ipu6_dma_unmap_sg(sys, sgt->sgl, sgt->nents, dir, attrs);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_dma_unmap_sgtable, "INTEL_IPU6");
diff --git a/drivers/media/pci/intel/ipu6/ipu6-dma.h b/drivers/media/pci/intel/ipu6/ipu6-dma.h
new file mode 100644
index 000000000000..ae9b9a5df57f
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-dma.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_DMA_H
+#define IPU6_DMA_H
+
+#include <linux/iova.h>
+#include <linux/scatterlist.h>
+#include <linux/types.h>
+
+#include "ipu6-bus.h"
+
+struct ipu6_mmu_info;
+
+struct ipu6_dma_mapping {
+	struct ipu6_mmu_info *mmu_info;
+	struct iova_domain iovad;
+};
+
+void ipu6_dma_sync_single(struct ipu6_bus_device *sys, dma_addr_t dma_handle,
+			  size_t size);
+void ipu6_dma_sync_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		      int nents);
+void ipu6_dma_sync_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt);
+void *ipu6_dma_alloc(struct ipu6_bus_device *sys, size_t size,
+		     dma_addr_t *dma_handle, gfp_t gfp,
+		     unsigned long attrs);
+void ipu6_dma_free(struct ipu6_bus_device *sys, size_t size, void *vaddr,
+		   dma_addr_t dma_handle, unsigned long attrs);
+int ipu6_dma_mmap(struct ipu6_bus_device *sys, struct vm_area_struct *vma,
+		  void *addr, dma_addr_t iova, size_t size,
+		  unsigned long attrs);
+int ipu6_dma_map_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		    int nents, enum dma_data_direction dir,
+		    unsigned long attrs);
+void ipu6_dma_unmap_sg(struct ipu6_bus_device *sys, struct scatterlist *sglist,
+		       int nents, enum dma_data_direction dir,
+		       unsigned long attrs);
+int ipu6_dma_map_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt,
+			 enum dma_data_direction dir, unsigned long attrs);
+void ipu6_dma_unmap_sgtable(struct ipu6_bus_device *sys, struct sg_table *sgt,
+			    enum dma_data_direction dir, unsigned long attrs);
+#endif /* IPU6_DMA_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-fw-com.c b/drivers/media/pci/intel/ipu6/ipu6-fw-com.c
new file mode 100644
index 000000000000..40d8ce138a67
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-fw-com.c
@@ -0,0 +1,413 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/math.h>
+#include <linux/overflow.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-dma.h"
+#include "ipu6-fw-com.h"
+
+/*
+ * FWCOM layer is a shared resource between FW and driver. It consist
+ * of token queues to both send and receive directions. Queue is simply
+ * an array of structures with read and write indexes to the queue.
+ * There are 1...n queues to both directions. Queues locates in
+ * system RAM and are mapped to ISP MMU so that both CPU and ISP can
+ * see the same buffer. Indexes are located in ISP DMEM so that FW code
+ * can poll those with very low latency and cost. CPU access to indexes is
+ * more costly but that happens only at message sending time and
+ * interrupt triggered message handling. CPU doesn't need to poll indexes.
+ * wr_reg / rd_reg are offsets to those dmem location. They are not
+ * the indexes itself.
+ */
+
+/* Shared structure between driver and FW - do not modify */
+struct ipu6_fw_sys_queue {
+	u64 host_address;
+	u32 vied_address;
+	u32 size;
+	u32 token_size;
+	u32 wr_reg;	/* reg number in subsystem's regmem */
+	u32 rd_reg;
+	u32 _align;
+} __packed;
+
+struct ipu6_fw_sys_queue_res {
+	u64 host_address;
+	u32 vied_address;
+	u32 reg;
+} __packed;
+
+enum syscom_state {
+	/* Program load or explicit host setting should init to this */
+	SYSCOM_STATE_UNINIT = 0x57a7e000,
+	/* SP Syscom sets this when it is ready for use */
+	SYSCOM_STATE_READY = 0x57a7e001,
+	/* SP Syscom sets this when no more syscom accesses will happen */
+	SYSCOM_STATE_INACTIVE = 0x57a7e002,
+};
+
+enum syscom_cmd {
+	/* Program load or explicit host setting should init to this */
+	SYSCOM_COMMAND_UNINIT = 0x57a7f000,
+	/* Host Syscom requests syscom to become inactive */
+	SYSCOM_COMMAND_INACTIVE = 0x57a7f001,
+};
+
+/* firmware config: data that sent from the host to SP via DDR */
+/* Cell copies data into a context */
+
+struct ipu6_fw_syscom_config {
+	u32 firmware_address;
+
+	u32 num_input_queues;
+	u32 num_output_queues;
+
+	/* ISP pointers to an array of ipu6_fw_sys_queue structures */
+	u32 input_queue;
+	u32 output_queue;
+
+	/* ISYS / PSYS private data */
+	u32 specific_addr;
+	u32 specific_size;
+};
+
+struct ipu6_fw_com_context {
+	struct ipu6_bus_device *adev;
+	void __iomem *dmem_addr;
+	int (*cell_ready)(struct ipu6_bus_device *adev);
+	void (*cell_start)(struct ipu6_bus_device *adev);
+
+	void *dma_buffer;
+	dma_addr_t dma_addr;
+	unsigned int dma_size;
+
+	struct ipu6_fw_sys_queue *input_queue;	/* array of host to SP queues */
+	struct ipu6_fw_sys_queue *output_queue;	/* array of SP to host */
+
+	u32 config_vied_addr;
+
+	unsigned int buttress_boot_offset;
+	void __iomem *base_addr;
+};
+
+#define FW_COM_WR_REG 0
+#define FW_COM_RD_REG 4
+
+#define REGMEM_OFFSET 0
+#define TUNIT_MAGIC_PATTERN 0x5a5a5a5a
+
+enum regmem_id {
+	/* pass pkg_dir address to SPC in non-secure mode */
+	PKG_DIR_ADDR_REG = 0,
+	/* Tunit CFG blob for secure - provided by host.*/
+	TUNIT_CFG_DWR_REG = 1,
+	/* syscom commands - modified by the host */
+	SYSCOM_COMMAND_REG = 2,
+	/* Store interrupt status - updated by SP */
+	SYSCOM_IRQ_REG = 3,
+	/* first syscom queue pointer register */
+	SYSCOM_QPR_BASE_REG = 4
+};
+
+#define BUTTRESS_FW_BOOT_PARAMS_0 0x4000
+#define BUTTRESS_FW_BOOT_PARAM_REG(base, offset, id)			\
+	((base) + BUTTRESS_FW_BOOT_PARAMS_0 + ((offset) + (id)) * 4)
+
+enum buttress_syscom_id {
+	/* pass syscom configuration to SPC */
+	SYSCOM_CONFIG_ID		= 0,
+	/* syscom state - modified by SP */
+	SYSCOM_STATE_ID			= 1,
+	/* syscom vtl0 addr mask */
+	SYSCOM_VTL0_ADDR_MASK_ID	= 2,
+	SYSCOM_ID_MAX
+};
+
+static void ipu6_sys_queue_init(struct ipu6_fw_sys_queue *q, unsigned int size,
+				unsigned int token_size,
+				struct ipu6_fw_sys_queue_res *res)
+{
+	unsigned int buf_size = (size + 1) * token_size;
+
+	q->size = size + 1;
+	q->token_size = token_size;
+
+	/* acquire the shared buffer space */
+	q->host_address = res->host_address;
+	res->host_address += buf_size;
+	q->vied_address = res->vied_address;
+	res->vied_address += buf_size;
+
+	/* acquire the shared read and writer pointers */
+	q->wr_reg = res->reg;
+	res->reg++;
+	q->rd_reg = res->reg;
+	res->reg++;
+}
+
+void *ipu6_fw_com_prepare(struct ipu6_fw_com_cfg *cfg,
+			  struct ipu6_bus_device *adev, void __iomem *base)
+{
+	size_t conf_size, inq_size, outq_size, specific_size;
+	struct ipu6_fw_syscom_config *config_host_addr;
+	unsigned int sizeinput = 0, sizeoutput = 0;
+	struct ipu6_fw_sys_queue_res res;
+	struct ipu6_fw_com_context *ctx;
+	struct device *dev = &adev->auxdev.dev;
+	size_t sizeall, offset;
+	void *specific_host_addr;
+	unsigned int i;
+
+	if (!cfg || !cfg->cell_start || !cfg->cell_ready)
+		return NULL;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return NULL;
+	ctx->dmem_addr = base + cfg->dmem_addr + REGMEM_OFFSET;
+	ctx->adev = adev;
+	ctx->cell_start = cfg->cell_start;
+	ctx->cell_ready = cfg->cell_ready;
+	ctx->buttress_boot_offset = cfg->buttress_boot_offset;
+	ctx->base_addr  = base;
+
+	/*
+	 * Allocate DMA mapped memory. Allocate one big chunk.
+	 */
+	/* Base cfg for FW */
+	conf_size = roundup(sizeof(struct ipu6_fw_syscom_config), 8);
+	/* Descriptions of the queues */
+	inq_size = size_mul(cfg->num_input_queues,
+			    sizeof(struct ipu6_fw_sys_queue));
+	outq_size = size_mul(cfg->num_output_queues,
+			     sizeof(struct ipu6_fw_sys_queue));
+	/* FW specific information structure */
+	specific_size = roundup(cfg->specific_size, 8);
+
+	sizeall = conf_size + inq_size + outq_size + specific_size;
+
+	for (i = 0; i < cfg->num_input_queues; i++)
+		sizeinput += size_mul(cfg->input[i].queue_size + 1,
+				      cfg->input[i].token_size);
+
+	for (i = 0; i < cfg->num_output_queues; i++)
+		sizeoutput += size_mul(cfg->output[i].queue_size + 1,
+				       cfg->output[i].token_size);
+
+	sizeall += sizeinput + sizeoutput;
+
+	ctx->dma_buffer = ipu6_dma_alloc(adev, sizeall, &ctx->dma_addr,
+					 GFP_KERNEL, 0);
+	if (!ctx->dma_buffer) {
+		dev_err(dev, "failed to allocate dma memory\n");
+		kfree(ctx);
+		return NULL;
+	}
+
+	ctx->dma_size = sizeall;
+
+	config_host_addr = ctx->dma_buffer;
+	ctx->config_vied_addr = ctx->dma_addr;
+
+	offset = conf_size;
+	ctx->input_queue = ctx->dma_buffer + offset;
+	config_host_addr->input_queue = ctx->dma_addr + offset;
+	config_host_addr->num_input_queues = cfg->num_input_queues;
+
+	offset += inq_size;
+	ctx->output_queue = ctx->dma_buffer + offset;
+	config_host_addr->output_queue = ctx->dma_addr + offset;
+	config_host_addr->num_output_queues = cfg->num_output_queues;
+
+	/* copy firmware specific data */
+	offset += outq_size;
+	specific_host_addr = ctx->dma_buffer + offset;
+	config_host_addr->specific_addr = ctx->dma_addr + offset;
+	config_host_addr->specific_size = cfg->specific_size;
+	if (cfg->specific_addr && cfg->specific_size)
+		memcpy(specific_host_addr, cfg->specific_addr,
+		       cfg->specific_size);
+
+	ipu6_dma_sync_single(adev, ctx->config_vied_addr, sizeall);
+
+	/* initialize input queues */
+	offset += specific_size;
+	res.reg = SYSCOM_QPR_BASE_REG;
+	res.host_address = (uintptr_t)(ctx->dma_buffer + offset);
+	res.vied_address = ctx->dma_addr + offset;
+	for (i = 0; i < cfg->num_input_queues; i++)
+		ipu6_sys_queue_init(ctx->input_queue + i,
+				    cfg->input[i].queue_size,
+				    cfg->input[i].token_size, &res);
+
+	/* initialize output queues */
+	offset += sizeinput;
+	res.host_address = (uintptr_t)(ctx->dma_buffer + offset);
+	res.vied_address = ctx->dma_addr + offset;
+	for (i = 0; i < cfg->num_output_queues; i++) {
+		ipu6_sys_queue_init(ctx->output_queue + i,
+				    cfg->output[i].queue_size,
+				    cfg->output[i].token_size, &res);
+	}
+
+	return ctx;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_fw_com_prepare, "INTEL_IPU6");
+
+int ipu6_fw_com_open(struct ipu6_fw_com_context *ctx)
+{
+	/* write magic pattern to disable the tunit trace */
+	writel(TUNIT_MAGIC_PATTERN, ctx->dmem_addr + TUNIT_CFG_DWR_REG * 4);
+	/* Check if SP is in valid state */
+	if (!ctx->cell_ready(ctx->adev))
+		return -EIO;
+
+	/* store syscom uninitialized command */
+	writel(SYSCOM_COMMAND_UNINIT, ctx->dmem_addr + SYSCOM_COMMAND_REG * 4);
+
+	/* store syscom uninitialized state */
+	writel(SYSCOM_STATE_UNINIT,
+	       BUTTRESS_FW_BOOT_PARAM_REG(ctx->base_addr,
+					  ctx->buttress_boot_offset,
+					  SYSCOM_STATE_ID));
+
+	/* store firmware configuration address */
+	writel(ctx->config_vied_addr,
+	       BUTTRESS_FW_BOOT_PARAM_REG(ctx->base_addr,
+					  ctx->buttress_boot_offset,
+					  SYSCOM_CONFIG_ID));
+	ctx->cell_start(ctx->adev);
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_fw_com_open, "INTEL_IPU6");
+
+int ipu6_fw_com_close(struct ipu6_fw_com_context *ctx)
+{
+	int state;
+
+	state = readl(BUTTRESS_FW_BOOT_PARAM_REG(ctx->base_addr,
+						 ctx->buttress_boot_offset,
+						 SYSCOM_STATE_ID));
+	if (state != SYSCOM_STATE_READY)
+		return -EBUSY;
+
+	/* set close request flag */
+	writel(SYSCOM_COMMAND_INACTIVE, ctx->dmem_addr +
+	       SYSCOM_COMMAND_REG * 4);
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_fw_com_close, "INTEL_IPU6");
+
+int ipu6_fw_com_release(struct ipu6_fw_com_context *ctx, unsigned int force)
+{
+	/* check if release is forced, an verify cell state if it is not */
+	if (!force && !ctx->cell_ready(ctx->adev))
+		return -EBUSY;
+
+	ipu6_dma_free(ctx->adev, ctx->dma_size,
+		      ctx->dma_buffer, ctx->dma_addr, 0);
+	kfree(ctx);
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_fw_com_release, "INTEL_IPU6");
+
+bool ipu6_fw_com_ready(struct ipu6_fw_com_context *ctx)
+{
+	int state;
+
+	state = readl(BUTTRESS_FW_BOOT_PARAM_REG(ctx->base_addr,
+						 ctx->buttress_boot_offset,
+						 SYSCOM_STATE_ID));
+
+	return state == SYSCOM_STATE_READY;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_fw_com_ready, "INTEL_IPU6");
+
+void *ipu6_send_get_token(struct ipu6_fw_com_context *ctx, int q_nbr)
+{
+	struct ipu6_fw_sys_queue *q = &ctx->input_queue[q_nbr];
+	void __iomem *q_dmem = ctx->dmem_addr + q->wr_reg * 4;
+	unsigned int wr, rd;
+	unsigned int packets;
+	unsigned int index;
+
+	wr = readl(q_dmem + FW_COM_WR_REG);
+	rd = readl(q_dmem + FW_COM_RD_REG);
+
+	if (WARN_ON_ONCE(wr >= q->size || rd >= q->size))
+		return NULL;
+
+	if (wr < rd)
+		packets = rd - wr - 1;
+	else
+		packets = q->size - (wr - rd + 1);
+
+	if (!packets)
+		return NULL;
+
+	index = readl(q_dmem + FW_COM_WR_REG);
+
+	return (void *)((uintptr_t)q->host_address + index * q->token_size);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_send_get_token, "INTEL_IPU6");
+
+void ipu6_send_put_token(struct ipu6_fw_com_context *ctx, int q_nbr)
+{
+	struct ipu6_fw_sys_queue *q = &ctx->input_queue[q_nbr];
+	void __iomem *q_dmem = ctx->dmem_addr + q->wr_reg * 4;
+	unsigned int wr = readl(q_dmem + FW_COM_WR_REG) + 1;
+
+	if (wr >= q->size)
+		wr = 0;
+
+	writel(wr, q_dmem + FW_COM_WR_REG);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_send_put_token, "INTEL_IPU6");
+
+void *ipu6_recv_get_token(struct ipu6_fw_com_context *ctx, int q_nbr)
+{
+	struct ipu6_fw_sys_queue *q = &ctx->output_queue[q_nbr];
+	void __iomem *q_dmem = ctx->dmem_addr + q->wr_reg * 4;
+	unsigned int wr, rd;
+	unsigned int packets;
+
+	wr = readl(q_dmem + FW_COM_WR_REG);
+	rd = readl(q_dmem + FW_COM_RD_REG);
+
+	if (WARN_ON_ONCE(wr >= q->size || rd >= q->size))
+		return NULL;
+
+	if (wr < rd)
+		wr += q->size;
+
+	packets = wr - rd;
+	if (!packets)
+		return NULL;
+
+	return (void *)((uintptr_t)q->host_address + rd * q->token_size);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_recv_get_token, "INTEL_IPU6");
+
+void ipu6_recv_put_token(struct ipu6_fw_com_context *ctx, int q_nbr)
+{
+	struct ipu6_fw_sys_queue *q = &ctx->output_queue[q_nbr];
+	void __iomem *q_dmem = ctx->dmem_addr + q->wr_reg * 4;
+	unsigned int rd = readl(q_dmem + FW_COM_RD_REG) + 1;
+
+	if (rd >= q->size)
+		rd = 0;
+
+	writel(rd, q_dmem + FW_COM_RD_REG);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_recv_put_token, "INTEL_IPU6");
diff --git a/drivers/media/pci/intel/ipu6/ipu6-fw-com.h b/drivers/media/pci/intel/ipu6/ipu6-fw-com.h
new file mode 100644
index 000000000000..b02285a3e43e
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-fw-com.h
@@ -0,0 +1,47 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_FW_COM_H
+#define IPU6_FW_COM_H
+
+struct ipu6_fw_com_context;
+struct ipu6_bus_device;
+
+struct ipu6_fw_syscom_queue_config {
+	unsigned int queue_size;	/* tokens per queue */
+	unsigned int token_size;	/* bytes per token */
+};
+
+#define SYSCOM_BUTTRESS_FW_PARAMS_ISYS_OFFSET	0
+
+struct ipu6_fw_com_cfg {
+	unsigned int num_input_queues;
+	unsigned int num_output_queues;
+	struct ipu6_fw_syscom_queue_config *input;
+	struct ipu6_fw_syscom_queue_config *output;
+
+	unsigned int dmem_addr;
+
+	/* firmware-specific configuration data */
+	void *specific_addr;
+	unsigned int specific_size;
+	int (*cell_ready)(struct ipu6_bus_device *adev);
+	void (*cell_start)(struct ipu6_bus_device *adev);
+
+	unsigned int buttress_boot_offset;
+};
+
+void *ipu6_fw_com_prepare(struct ipu6_fw_com_cfg *cfg,
+			  struct ipu6_bus_device *adev, void __iomem *base);
+
+int ipu6_fw_com_open(struct ipu6_fw_com_context *ctx);
+bool ipu6_fw_com_ready(struct ipu6_fw_com_context *ctx);
+int ipu6_fw_com_close(struct ipu6_fw_com_context *ctx);
+int ipu6_fw_com_release(struct ipu6_fw_com_context *ctx, unsigned int force);
+
+void *ipu6_recv_get_token(struct ipu6_fw_com_context *ctx, int q_nbr);
+void ipu6_recv_put_token(struct ipu6_fw_com_context *ctx, int q_nbr);
+void *ipu6_send_get_token(struct ipu6_fw_com_context *ctx, int q_nbr);
+void ipu6_send_put_token(struct ipu6_fw_com_context *ctx, int q_nbr);
+
+#endif
diff --git a/drivers/media/pci/intel/ipu6/ipu6-fw-isys.c b/drivers/media/pci/intel/ipu6/ipu6-fw-isys.c
new file mode 100644
index 000000000000..62ed92ff1d30
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-fw-isys.c
@@ -0,0 +1,487 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/cacheflush.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-fw-com.h"
+#include "ipu6-isys.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+#include "ipu6-platform-regs.h"
+
+static const char send_msg_types[N_IPU6_FW_ISYS_SEND_TYPE][32] = {
+	"STREAM_OPEN",
+	"STREAM_START",
+	"STREAM_START_AND_CAPTURE",
+	"STREAM_CAPTURE",
+	"STREAM_STOP",
+	"STREAM_FLUSH",
+	"STREAM_CLOSE"
+};
+
+static int handle_proxy_response(struct ipu6_isys *isys, unsigned int req_id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_fw_isys_proxy_resp_info_abi *resp;
+	int ret;
+
+	resp = ipu6_recv_get_token(isys->fwcom, IPU6_BASE_PROXY_RECV_QUEUES);
+	if (!resp)
+		return 1;
+
+	dev_dbg(dev, "Proxy response: id %u, error %u, details %u\n",
+		resp->request_id, resp->error_info.error,
+		resp->error_info.error_details);
+
+	ret = req_id == resp->request_id ? 0 : -EIO;
+
+	ipu6_recv_put_token(isys->fwcom, IPU6_BASE_PROXY_RECV_QUEUES);
+
+	return ret;
+}
+
+int ipu6_fw_isys_send_proxy_token(struct ipu6_isys *isys,
+				  unsigned int req_id,
+				  unsigned int index,
+				  unsigned int offset, u32 value)
+{
+	struct ipu6_fw_com_context *ctx = isys->fwcom;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_fw_proxy_send_queue_token *token;
+	unsigned int timeout = 1000;
+	int ret;
+
+	dev_dbg(dev,
+		"proxy send: req_id 0x%x, index %d, offset 0x%x, value 0x%x\n",
+		req_id, index, offset, value);
+
+	token = ipu6_send_get_token(ctx, IPU6_BASE_PROXY_SEND_QUEUES);
+	if (!token)
+		return -EBUSY;
+
+	token->request_id = req_id;
+	token->region_index = index;
+	token->offset = offset;
+	token->value = value;
+	ipu6_send_put_token(ctx, IPU6_BASE_PROXY_SEND_QUEUES);
+
+	do {
+		usleep_range(100, 110);
+		ret = handle_proxy_response(isys, req_id);
+		if (!ret)
+			break;
+		if (ret == -EIO) {
+			dev_err(dev, "Proxy respond with unexpected id\n");
+			break;
+		}
+		timeout--;
+	} while (ret && timeout);
+
+	if (!timeout)
+		dev_err(dev, "Proxy response timed out\n");
+
+	return ret;
+}
+
+int ipu6_fw_isys_complex_cmd(struct ipu6_isys *isys,
+			     const unsigned int stream_handle,
+			     void *cpu_mapped_buf,
+			     dma_addr_t dma_mapped_buf,
+			     size_t size, u16 send_type)
+{
+	struct ipu6_fw_com_context *ctx = isys->fwcom;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_fw_send_queue_token *token;
+
+	if (send_type >= N_IPU6_FW_ISYS_SEND_TYPE)
+		return -EINVAL;
+
+	dev_dbg(dev, "send_token: %s\n", send_msg_types[send_type]);
+
+	/*
+	 * Time to flush cache in case we have some payload. Not all messages
+	 * have that
+	 */
+	if (cpu_mapped_buf)
+		clflush_cache_range(cpu_mapped_buf, size);
+
+	token = ipu6_send_get_token(ctx,
+				    stream_handle + IPU6_BASE_MSG_SEND_QUEUES);
+	if (!token)
+		return -EBUSY;
+
+	token->payload = dma_mapped_buf;
+	token->buf_handle = (unsigned long)cpu_mapped_buf;
+	token->send_type = send_type;
+
+	ipu6_send_put_token(ctx, stream_handle + IPU6_BASE_MSG_SEND_QUEUES);
+
+	return 0;
+}
+
+int ipu6_fw_isys_simple_cmd(struct ipu6_isys *isys,
+			    const unsigned int stream_handle, u16 send_type)
+{
+	return ipu6_fw_isys_complex_cmd(isys, stream_handle, NULL, 0, 0,
+					send_type);
+}
+
+int ipu6_fw_isys_close(struct ipu6_isys *isys)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	int retry = IPU6_ISYS_CLOSE_RETRY;
+	unsigned long flags;
+	void *fwcom;
+	int ret;
+
+	/*
+	 * Stop the isys fw. Actual close takes
+	 * some time as the FW must stop its actions including code fetch
+	 * to SP icache.
+	 * spinlock to wait the interrupt handler to be finished
+	 */
+	spin_lock_irqsave(&isys->power_lock, flags);
+	ret = ipu6_fw_com_close(isys->fwcom);
+	fwcom = isys->fwcom;
+	isys->fwcom = NULL;
+	spin_unlock_irqrestore(&isys->power_lock, flags);
+	if (ret)
+		dev_err(dev, "Device close failure: %d\n", ret);
+
+	/* release probably fails if the close failed. Let's try still */
+	do {
+		usleep_range(400, 500);
+		ret = ipu6_fw_com_release(fwcom, 0);
+		retry--;
+	} while (ret && retry);
+
+	if (ret) {
+		dev_err(dev, "Device release time out %d\n", ret);
+		spin_lock_irqsave(&isys->power_lock, flags);
+		isys->fwcom = fwcom;
+		spin_unlock_irqrestore(&isys->power_lock, flags);
+	}
+
+	return ret;
+}
+
+void ipu6_fw_isys_cleanup(struct ipu6_isys *isys)
+{
+	int ret;
+
+	ret = ipu6_fw_com_release(isys->fwcom, 1);
+	if (ret < 0)
+		dev_warn(&isys->adev->auxdev.dev,
+			 "Device busy, fw_com release failed.");
+	isys->fwcom = NULL;
+}
+
+static void start_sp(struct ipu6_bus_device *adev)
+{
+	struct ipu6_isys *isys = ipu6_bus_get_drvdata(adev);
+	void __iomem *spc_regs_base = isys->pdata->base +
+		isys->pdata->ipdata->hw_variant.spc_offset;
+	u32 val = IPU6_ISYS_SPC_STATUS_START |
+		IPU6_ISYS_SPC_STATUS_RUN |
+		IPU6_ISYS_SPC_STATUS_CTRL_ICACHE_INVALIDATE;
+
+	val |= isys->icache_prefetch ? IPU6_ISYS_SPC_STATUS_ICACHE_PREFETCH : 0;
+
+	writel(val, spc_regs_base + IPU6_ISYS_REG_SPC_STATUS_CTRL);
+}
+
+static int query_sp(struct ipu6_bus_device *adev)
+{
+	struct ipu6_isys *isys = ipu6_bus_get_drvdata(adev);
+	void __iomem *spc_regs_base = isys->pdata->base +
+		isys->pdata->ipdata->hw_variant.spc_offset;
+	u32 val;
+
+	val = readl(spc_regs_base + IPU6_ISYS_REG_SPC_STATUS_CTRL);
+	/* return true when READY == 1, START == 0 */
+	val &= IPU6_ISYS_SPC_STATUS_READY | IPU6_ISYS_SPC_STATUS_START;
+
+	return val == IPU6_ISYS_SPC_STATUS_READY;
+}
+
+static int ipu6_isys_fwcom_cfg_init(struct ipu6_isys *isys,
+				    struct ipu6_fw_com_cfg *fwcom,
+				    unsigned int num_streams)
+{
+	unsigned int max_send_queues, max_sram_blocks, max_devq_size;
+	struct ipu6_fw_syscom_queue_config *input_queue_cfg;
+	struct ipu6_fw_syscom_queue_config *output_queue_cfg;
+	struct device *dev = &isys->adev->auxdev.dev;
+	int type_proxy = IPU6_FW_ISYS_QUEUE_TYPE_PROXY;
+	int type_dev = IPU6_FW_ISYS_QUEUE_TYPE_DEV;
+	int type_msg = IPU6_FW_ISYS_QUEUE_TYPE_MSG;
+	int base_dev_send = IPU6_BASE_DEV_SEND_QUEUES;
+	int base_msg_send = IPU6_BASE_MSG_SEND_QUEUES;
+	int base_msg_recv = IPU6_BASE_MSG_RECV_QUEUES;
+	struct ipu6_fw_isys_fw_config *isys_fw_cfg;
+	u32 num_in_message_queues;
+	unsigned int max_streams;
+	unsigned int size;
+	unsigned int i;
+
+	max_streams = isys->pdata->ipdata->max_streams;
+	max_send_queues = isys->pdata->ipdata->max_send_queues;
+	max_sram_blocks = isys->pdata->ipdata->max_sram_blocks;
+	max_devq_size = isys->pdata->ipdata->max_devq_size;
+	num_in_message_queues = clamp(num_streams, 1U, max_streams);
+	isys_fw_cfg = devm_kzalloc(dev, sizeof(*isys_fw_cfg), GFP_KERNEL);
+	if (!isys_fw_cfg)
+		return -ENOMEM;
+
+	isys_fw_cfg->num_send_queues[type_proxy] = IPU6_N_MAX_PROXY_SEND_QUEUES;
+	isys_fw_cfg->num_send_queues[type_dev] = IPU6_N_MAX_DEV_SEND_QUEUES;
+	isys_fw_cfg->num_send_queues[type_msg] = num_in_message_queues;
+	isys_fw_cfg->num_recv_queues[type_proxy] = IPU6_N_MAX_PROXY_RECV_QUEUES;
+	/* Common msg/dev return queue */
+	isys_fw_cfg->num_recv_queues[type_dev] = 0;
+	isys_fw_cfg->num_recv_queues[type_msg] = 1;
+
+	size = sizeof(*input_queue_cfg) * max_send_queues;
+	input_queue_cfg = devm_kzalloc(dev, size, GFP_KERNEL);
+	if (!input_queue_cfg)
+		return -ENOMEM;
+
+	size = sizeof(*output_queue_cfg) * IPU6_N_MAX_RECV_QUEUES;
+	output_queue_cfg = devm_kzalloc(dev, size, GFP_KERNEL);
+	if (!output_queue_cfg)
+		return -ENOMEM;
+
+	fwcom->input = input_queue_cfg;
+	fwcom->output = output_queue_cfg;
+
+	fwcom->num_input_queues = isys_fw_cfg->num_send_queues[type_proxy] +
+		isys_fw_cfg->num_send_queues[type_dev] +
+		isys_fw_cfg->num_send_queues[type_msg];
+
+	fwcom->num_output_queues = isys_fw_cfg->num_recv_queues[type_proxy] +
+		isys_fw_cfg->num_recv_queues[type_dev] +
+		isys_fw_cfg->num_recv_queues[type_msg];
+
+	/* SRAM partitioning. Equal partitioning is set. */
+	for (i = 0; i < max_sram_blocks; i++) {
+		if (i < num_in_message_queues)
+			isys_fw_cfg->buffer_partition.num_gda_pages[i] =
+				(IPU6_DEVICE_GDA_NR_PAGES *
+				 IPU6_DEVICE_GDA_VIRT_FACTOR) /
+				num_in_message_queues;
+		else
+			isys_fw_cfg->buffer_partition.num_gda_pages[i] = 0;
+	}
+
+	/* FW assumes proxy interface at fwcom queue 0 */
+	for (i = 0; i < isys_fw_cfg->num_send_queues[type_proxy]; i++) {
+		input_queue_cfg[i].token_size =
+			sizeof(struct ipu6_fw_proxy_send_queue_token);
+		input_queue_cfg[i].queue_size = IPU6_ISYS_SIZE_PROXY_SEND_QUEUE;
+	}
+
+	for (i = 0; i < isys_fw_cfg->num_send_queues[type_dev]; i++) {
+		input_queue_cfg[base_dev_send + i].token_size =
+			sizeof(struct ipu6_fw_send_queue_token);
+		input_queue_cfg[base_dev_send + i].queue_size = max_devq_size;
+	}
+
+	for (i = 0; i < isys_fw_cfg->num_send_queues[type_msg]; i++) {
+		input_queue_cfg[base_msg_send + i].token_size =
+			sizeof(struct ipu6_fw_send_queue_token);
+		input_queue_cfg[base_msg_send + i].queue_size =
+			IPU6_ISYS_SIZE_SEND_QUEUE;
+	}
+
+	for (i = 0; i < isys_fw_cfg->num_recv_queues[type_proxy]; i++) {
+		output_queue_cfg[i].token_size =
+			sizeof(struct ipu6_fw_proxy_resp_queue_token);
+		output_queue_cfg[i].queue_size =
+			IPU6_ISYS_SIZE_PROXY_RECV_QUEUE;
+	}
+	/* There is no recv DEV queue */
+	for (i = 0; i < isys_fw_cfg->num_recv_queues[type_msg]; i++) {
+		output_queue_cfg[base_msg_recv + i].token_size =
+			sizeof(struct ipu6_fw_resp_queue_token);
+		output_queue_cfg[base_msg_recv + i].queue_size =
+			IPU6_ISYS_SIZE_RECV_QUEUE;
+	}
+
+	fwcom->dmem_addr = isys->pdata->ipdata->hw_variant.dmem_offset;
+	fwcom->specific_addr = isys_fw_cfg;
+	fwcom->specific_size = sizeof(*isys_fw_cfg);
+
+	return 0;
+}
+
+int ipu6_fw_isys_init(struct ipu6_isys *isys, unsigned int num_streams)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	int retry = IPU6_ISYS_OPEN_RETRY;
+	struct ipu6_fw_com_cfg fwcom = {
+		.cell_start = start_sp,
+		.cell_ready = query_sp,
+		.buttress_boot_offset = SYSCOM_BUTTRESS_FW_PARAMS_ISYS_OFFSET,
+	};
+	int ret;
+
+	ipu6_isys_fwcom_cfg_init(isys, &fwcom, num_streams);
+
+	isys->fwcom = ipu6_fw_com_prepare(&fwcom, isys->adev,
+					  isys->pdata->base);
+	if (!isys->fwcom) {
+		dev_err(dev, "isys fw com prepare failed\n");
+		return -EIO;
+	}
+
+	ret = ipu6_fw_com_open(isys->fwcom);
+	if (ret) {
+		dev_err(dev, "isys fw com open failed %d\n", ret);
+		return ret;
+	}
+
+	do {
+		usleep_range(400, 500);
+		if (ipu6_fw_com_ready(isys->fwcom))
+			break;
+		retry--;
+	} while (retry > 0);
+
+	if (!retry) {
+		dev_err(dev, "isys port open ready failed %d\n", ret);
+		ipu6_fw_isys_close(isys);
+		ret = -EIO;
+	}
+
+	return ret;
+}
+
+struct ipu6_fw_isys_resp_info_abi *
+ipu6_fw_isys_get_resp(void *context, unsigned int queue)
+{
+	return ipu6_recv_get_token(context, queue);
+}
+
+void ipu6_fw_isys_put_resp(void *context, unsigned int queue)
+{
+	ipu6_recv_put_token(context, queue);
+}
+
+void ipu6_fw_isys_dump_stream_cfg(struct device *dev,
+				  struct ipu6_fw_isys_stream_cfg_data_abi *cfg)
+{
+	unsigned int i;
+
+	dev_dbg(dev, "-----------------------------------------------------\n");
+	dev_dbg(dev, "IPU6_FW_ISYS_STREAM_CFG_DATA\n");
+
+	dev_dbg(dev, "compfmt = %d\n", cfg->vc);
+	dev_dbg(dev, "src = %d\n", cfg->src);
+	dev_dbg(dev, "vc = %d\n", cfg->vc);
+	dev_dbg(dev, "isl_use = %d\n", cfg->isl_use);
+	dev_dbg(dev, "sensor_type = %d\n", cfg->sensor_type);
+
+	dev_dbg(dev, "send_irq_sof_discarded = %d\n",
+		cfg->send_irq_sof_discarded);
+	dev_dbg(dev, "send_irq_eof_discarded = %d\n",
+		cfg->send_irq_eof_discarded);
+	dev_dbg(dev, "send_resp_sof_discarded = %d\n",
+		cfg->send_resp_sof_discarded);
+	dev_dbg(dev, "send_resp_eof_discarded = %d\n",
+		cfg->send_resp_eof_discarded);
+
+	dev_dbg(dev, "crop:\n");
+	dev_dbg(dev, "\t.left_top = [%d, %d]\n", cfg->crop.left_offset,
+		cfg->crop.top_offset);
+	dev_dbg(dev, "\t.right_bottom = [%d, %d]\n", cfg->crop.right_offset,
+		cfg->crop.bottom_offset);
+
+	dev_dbg(dev, "nof_input_pins = %d\n", cfg->nof_input_pins);
+	for (i = 0; i < cfg->nof_input_pins; i++) {
+		dev_dbg(dev, "input pin[%d]:\n", i);
+		dev_dbg(dev, "\t.dt = 0x%0x\n", cfg->input_pins[i].dt);
+		dev_dbg(dev, "\t.mipi_store_mode = %d\n",
+			cfg->input_pins[i].mipi_store_mode);
+		dev_dbg(dev, "\t.bits_per_pix = %d\n",
+			cfg->input_pins[i].bits_per_pix);
+		dev_dbg(dev, "\t.mapped_dt = 0x%0x\n",
+			cfg->input_pins[i].mapped_dt);
+		dev_dbg(dev, "\t.input_res = %dx%d\n",
+			cfg->input_pins[i].input_res.width,
+			cfg->input_pins[i].input_res.height);
+		dev_dbg(dev, "\t.mipi_decompression = %d\n",
+			cfg->input_pins[i].mipi_decompression);
+		dev_dbg(dev, "\t.capture_mode = %d\n",
+			cfg->input_pins[i].capture_mode);
+	}
+
+	dev_dbg(dev, "nof_output_pins = %d\n", cfg->nof_output_pins);
+	for (i = 0; i < cfg->nof_output_pins; i++) {
+		dev_dbg(dev, "output_pin[%d]:\n", i);
+		dev_dbg(dev, "\t.input_pin_id = %d\n",
+			cfg->output_pins[i].input_pin_id);
+		dev_dbg(dev, "\t.output_res = %dx%d\n",
+			cfg->output_pins[i].output_res.width,
+			cfg->output_pins[i].output_res.height);
+		dev_dbg(dev, "\t.stride = %d\n", cfg->output_pins[i].stride);
+		dev_dbg(dev, "\t.pt = %d\n", cfg->output_pins[i].pt);
+		dev_dbg(dev, "\t.payload_buf_size = %d\n",
+			cfg->output_pins[i].payload_buf_size);
+		dev_dbg(dev, "\t.ft = %d\n", cfg->output_pins[i].ft);
+		dev_dbg(dev, "\t.watermark_in_lines = %d\n",
+			cfg->output_pins[i].watermark_in_lines);
+		dev_dbg(dev, "\t.send_irq = %d\n",
+			cfg->output_pins[i].send_irq);
+		dev_dbg(dev, "\t.reserve_compression = %d\n",
+			cfg->output_pins[i].reserve_compression);
+		dev_dbg(dev, "\t.snoopable = %d\n",
+			cfg->output_pins[i].snoopable);
+		dev_dbg(dev, "\t.error_handling_enable = %d\n",
+			cfg->output_pins[i].error_handling_enable);
+		dev_dbg(dev, "\t.sensor_type = %d\n",
+			cfg->output_pins[i].sensor_type);
+	}
+	dev_dbg(dev, "-----------------------------------------------------\n");
+}
+
+void
+ipu6_fw_isys_dump_frame_buff_set(struct device *dev,
+				 struct ipu6_fw_isys_frame_buff_set_abi *buf,
+				 unsigned int outputs)
+{
+	unsigned int i;
+
+	dev_dbg(dev, "-----------------------------------------------------\n");
+	dev_dbg(dev, "IPU6_FW_ISYS_FRAME_BUFF_SET\n");
+
+	for (i = 0; i < outputs; i++) {
+		dev_dbg(dev, "output_pin[%d]:\n", i);
+		dev_dbg(dev, "\t.out_buf_id = %llu\n",
+			buf->output_pins[i].out_buf_id);
+		dev_dbg(dev, "\t.addr = 0x%x\n", buf->output_pins[i].addr);
+		dev_dbg(dev, "\t.compress = %d\n",
+			buf->output_pins[i].compress);
+	}
+
+	dev_dbg(dev, "send_irq_sof = 0x%x\n", buf->send_irq_sof);
+	dev_dbg(dev, "send_irq_eof = 0x%x\n", buf->send_irq_eof);
+	dev_dbg(dev, "send_resp_sof = 0x%x\n", buf->send_resp_sof);
+	dev_dbg(dev, "send_resp_eof = 0x%x\n", buf->send_resp_eof);
+	dev_dbg(dev, "send_irq_capture_ack = 0x%x\n",
+		buf->send_irq_capture_ack);
+	dev_dbg(dev, "send_irq_capture_done = 0x%x\n",
+		buf->send_irq_capture_done);
+	dev_dbg(dev, "send_resp_capture_ack = 0x%x\n",
+		buf->send_resp_capture_ack);
+	dev_dbg(dev, "send_resp_capture_done = 0x%x\n",
+		buf->send_resp_capture_done);
+
+	dev_dbg(dev, "-----------------------------------------------------\n");
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-fw-isys.h b/drivers/media/pci/intel/ipu6/ipu6-fw-isys.h
new file mode 100644
index 000000000000..b60f02076d8a
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-fw-isys.h
@@ -0,0 +1,596 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_FW_ISYS_H
+#define IPU6_FW_ISYS_H
+
+#include <linux/types.h>
+
+struct device;
+struct ipu6_isys;
+
+/* Max number of Input/Output Pins */
+#define IPU6_MAX_IPINS 4
+
+#define IPU6_MAX_OPINS ((IPU6_MAX_IPINS) + 1)
+
+#define IPU6_STREAM_ID_MAX 16
+#define IPU6_NONSECURE_STREAM_ID_MAX 12
+#define IPU6_DEV_SEND_QUEUE_SIZE (IPU6_STREAM_ID_MAX)
+#define IPU6_NOF_SRAM_BLOCKS_MAX (IPU6_STREAM_ID_MAX)
+#define IPU6_N_MAX_MSG_SEND_QUEUES (IPU6_STREAM_ID_MAX)
+#define IPU6SE_STREAM_ID_MAX 8
+#define IPU6SE_NONSECURE_STREAM_ID_MAX 4
+#define IPU6SE_DEV_SEND_QUEUE_SIZE (IPU6SE_STREAM_ID_MAX)
+#define IPU6SE_NOF_SRAM_BLOCKS_MAX (IPU6SE_STREAM_ID_MAX)
+#define IPU6SE_N_MAX_MSG_SEND_QUEUES (IPU6SE_STREAM_ID_MAX)
+
+/* Single return queue for all streams/commands type */
+#define IPU6_N_MAX_MSG_RECV_QUEUES 1
+/* Single device queue for high priority commands (bypass in-order queue) */
+#define IPU6_N_MAX_DEV_SEND_QUEUES 1
+/* Single dedicated send queue for proxy interface */
+#define IPU6_N_MAX_PROXY_SEND_QUEUES 1
+/* Single dedicated recv queue for proxy interface */
+#define IPU6_N_MAX_PROXY_RECV_QUEUES 1
+/* Send queues layout */
+#define IPU6_BASE_PROXY_SEND_QUEUES 0
+#define IPU6_BASE_DEV_SEND_QUEUES \
+	(IPU6_BASE_PROXY_SEND_QUEUES + IPU6_N_MAX_PROXY_SEND_QUEUES)
+#define IPU6_BASE_MSG_SEND_QUEUES \
+	(IPU6_BASE_DEV_SEND_QUEUES + IPU6_N_MAX_DEV_SEND_QUEUES)
+/* Recv queues layout */
+#define IPU6_BASE_PROXY_RECV_QUEUES 0
+#define IPU6_BASE_MSG_RECV_QUEUES \
+	(IPU6_BASE_PROXY_RECV_QUEUES + IPU6_N_MAX_PROXY_RECV_QUEUES)
+#define IPU6_N_MAX_RECV_QUEUES \
+	(IPU6_BASE_MSG_RECV_QUEUES + IPU6_N_MAX_MSG_RECV_QUEUES)
+
+#define IPU6_N_MAX_SEND_QUEUES \
+	(IPU6_BASE_MSG_SEND_QUEUES + IPU6_N_MAX_MSG_SEND_QUEUES)
+#define IPU6SE_N_MAX_SEND_QUEUES \
+	(IPU6_BASE_MSG_SEND_QUEUES + IPU6SE_N_MAX_MSG_SEND_QUEUES)
+
+/* Max number of planes for frame formats supported by the FW */
+#define IPU6_PIN_PLANES_MAX 4
+
+#define IPU6_FW_ISYS_SENSOR_TYPE_START 14
+#define IPU6_FW_ISYS_SENSOR_TYPE_END 19
+#define IPU6SE_FW_ISYS_SENSOR_TYPE_START 6
+#define IPU6SE_FW_ISYS_SENSOR_TYPE_END 11
+/*
+ * Device close takes some time from last ack message to actual stopping
+ * of the SP processor. As long as the SP processor runs we can't proceed with
+ * clean up of resources.
+ */
+#define IPU6_ISYS_OPEN_RETRY			2000
+#define IPU6_ISYS_CLOSE_RETRY			2000
+#define IPU6_FW_CALL_TIMEOUT_JIFFIES		msecs_to_jiffies(2000)
+
+enum ipu6_fw_isys_resp_type {
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_OPEN_DONE = 0,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_START_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_STOP_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_FLUSH_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_CLOSE_ACK,
+	IPU6_FW_ISYS_RESP_TYPE_PIN_DATA_READY,
+	IPU6_FW_ISYS_RESP_TYPE_PIN_DATA_WATERMARK,
+	IPU6_FW_ISYS_RESP_TYPE_FRAME_SOF,
+	IPU6_FW_ISYS_RESP_TYPE_FRAME_EOF,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_DONE,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_DONE,
+	IPU6_FW_ISYS_RESP_TYPE_PIN_DATA_SKIPPED,
+	IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_SKIPPED,
+	IPU6_FW_ISYS_RESP_TYPE_FRAME_SOF_DISCARDED,
+	IPU6_FW_ISYS_RESP_TYPE_FRAME_EOF_DISCARDED,
+	IPU6_FW_ISYS_RESP_TYPE_STATS_DATA_READY,
+	N_IPU6_FW_ISYS_RESP_TYPE
+};
+
+enum ipu6_fw_isys_send_type {
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_OPEN = 0,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_START,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_START_AND_CAPTURE,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_STOP,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_FLUSH,
+	IPU6_FW_ISYS_SEND_TYPE_STREAM_CLOSE,
+	N_IPU6_FW_ISYS_SEND_TYPE
+};
+
+enum ipu6_fw_isys_queue_type {
+	IPU6_FW_ISYS_QUEUE_TYPE_PROXY = 0,
+	IPU6_FW_ISYS_QUEUE_TYPE_DEV,
+	IPU6_FW_ISYS_QUEUE_TYPE_MSG,
+	N_IPU6_FW_ISYS_QUEUE_TYPE
+};
+
+enum ipu6_fw_isys_stream_source {
+	IPU6_FW_ISYS_STREAM_SRC_PORT_0 = 0,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_1,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_2,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_3,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_4,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_5,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_6,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_7,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_8,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_9,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_10,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_11,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_12,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_13,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_14,
+	IPU6_FW_ISYS_STREAM_SRC_PORT_15,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_0,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_1,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_2,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_3,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_4,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_5,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_6,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_7,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_8,
+	IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_9,
+	N_IPU6_FW_ISYS_STREAM_SRC
+};
+
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT0 IPU6_FW_ISYS_STREAM_SRC_PORT_0
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT1 IPU6_FW_ISYS_STREAM_SRC_PORT_1
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT2 IPU6_FW_ISYS_STREAM_SRC_PORT_2
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT3 IPU6_FW_ISYS_STREAM_SRC_PORT_3
+
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_PORTA IPU6_FW_ISYS_STREAM_SRC_PORT_4
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_PORTB IPU6_FW_ISYS_STREAM_SRC_PORT_5
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_CPHY_PORT0 \
+	IPU6_FW_ISYS_STREAM_SRC_PORT_6
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_CPHY_PORT1 \
+	IPU6_FW_ISYS_STREAM_SRC_PORT_7
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_CPHY_PORT2 \
+	IPU6_FW_ISYS_STREAM_SRC_PORT_8
+#define IPU6_FW_ISYS_STREAM_SRC_CSI2_3PH_CPHY_PORT3 \
+	IPU6_FW_ISYS_STREAM_SRC_PORT_9
+
+#define IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_PORT0 IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_0
+#define IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_PORT1 IPU6_FW_ISYS_STREAM_SRC_MIPIGEN_1
+
+/*
+ * enum ipu6_fw_isys_mipi_vc: MIPI csi2 spec
+ * supports up to 4 virtual per physical channel
+ */
+enum ipu6_fw_isys_mipi_vc {
+	IPU6_FW_ISYS_MIPI_VC_0 = 0,
+	IPU6_FW_ISYS_MIPI_VC_1,
+	IPU6_FW_ISYS_MIPI_VC_2,
+	IPU6_FW_ISYS_MIPI_VC_3,
+	N_IPU6_FW_ISYS_MIPI_VC
+};
+
+enum ipu6_fw_isys_frame_format_type {
+	IPU6_FW_ISYS_FRAME_FORMAT_NV11 = 0, /* 12 bit YUV 411, Y, UV plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_NV12,	/* 12 bit YUV 420, Y, UV plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_NV12_16, /* 16 bit YUV 420, Y, UV plane */
+	/* 12 bit YUV 420, Intel proprietary tiled format */
+	IPU6_FW_ISYS_FRAME_FORMAT_NV12_TILEY,
+
+	IPU6_FW_ISYS_FRAME_FORMAT_NV16,	/* 16 bit YUV 422, Y, UV plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_NV21,	/* 12 bit YUV 420, Y, VU plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_NV61,	/* 16 bit YUV 422, Y, VU plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_YV12,	/* 12 bit YUV 420, Y, V, U plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_YV16,	/* 16 bit YUV 422, Y, V, U plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV420, /* 12 bit YUV 420, Y, U, V plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV420_10, /* yuv420, 10 bits per subpixel */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV420_12, /* yuv420, 12 bits per subpixel */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV420_14, /* yuv420, 14 bits per subpixel */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV420_16, /* yuv420, 16 bits per subpixel */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV422, /* 16 bit YUV 422, Y, U, V plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV422_16, /* yuv422, 16 bits per subpixel */
+	IPU6_FW_ISYS_FRAME_FORMAT_UYVY,	/* 16 bit YUV 422, UYVY interleaved */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUYV,	/* 16 bit YUV 422, YUYV interleaved */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV444, /* 24 bit YUV 444, Y, U, V plane */
+	/* Internal format, 2 y lines followed by a uvinterleaved line */
+	IPU6_FW_ISYS_FRAME_FORMAT_YUV_LINE,
+	IPU6_FW_ISYS_FRAME_FORMAT_RAW8,	/* RAW8, 1 plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_RAW10, /* RAW10, 1 plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_RAW12, /* RAW12, 1 plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_RAW14, /* RAW14, 1 plane */
+	IPU6_FW_ISYS_FRAME_FORMAT_RAW16, /* RAW16, 1 plane */
+	/**
+	 * 16 bit RGB, 1 plane. Each 3 sub pixels are packed into one 16 bit
+	 * value, 5 bits for R, 6 bits for G and 5 bits for B.
+	 */
+	IPU6_FW_ISYS_FRAME_FORMAT_RGB565,
+	IPU6_FW_ISYS_FRAME_FORMAT_PLANAR_RGB888, /* 24 bit RGB, 3 planes */
+	IPU6_FW_ISYS_FRAME_FORMAT_RGBA888, /* 32 bit RGBA, 1 plane, A=Alpha */
+	IPU6_FW_ISYS_FRAME_FORMAT_QPLANE6, /* Internal, for advanced ISP */
+	IPU6_FW_ISYS_FRAME_FORMAT_BINARY_8, /* byte stream, used for jpeg. */
+	N_IPU6_FW_ISYS_FRAME_FORMAT
+};
+
+enum ipu6_fw_isys_pin_type {
+	/* captured as MIPI packets */
+	IPU6_FW_ISYS_PIN_TYPE_MIPI = 0,
+	/* captured through the SoC path */
+	IPU6_FW_ISYS_PIN_TYPE_RAW_SOC = 3,
+};
+
+/*
+ * enum ipu6_fw_isys_mipi_store_mode. Describes if long MIPI packets reach
+ * MIPI SRAM with the long packet header or
+ * if not, then only option is to capture it with pin type MIPI.
+ */
+enum ipu6_fw_isys_mipi_store_mode {
+	IPU6_FW_ISYS_MIPI_STORE_MODE_NORMAL = 0,
+	IPU6_FW_ISYS_MIPI_STORE_MODE_DISCARD_LONG_HEADER,
+	N_IPU6_FW_ISYS_MIPI_STORE_MODE
+};
+
+enum ipu6_fw_isys_capture_mode {
+	IPU6_FW_ISYS_CAPTURE_MODE_REGULAR = 0,
+	IPU6_FW_ISYS_CAPTURE_MODE_BURST,
+	N_IPU6_FW_ISYS_CAPTURE_MODE,
+};
+
+enum ipu6_fw_isys_sensor_mode {
+	IPU6_FW_ISYS_SENSOR_MODE_NORMAL = 0,
+	IPU6_FW_ISYS_SENSOR_MODE_TOBII,
+	N_IPU6_FW_ISYS_SENSOR_MODE,
+};
+
+enum ipu6_fw_isys_error {
+	IPU6_FW_ISYS_ERROR_NONE = 0,
+	IPU6_FW_ISYS_ERROR_FW_INTERNAL_CONSISTENCY,
+	IPU6_FW_ISYS_ERROR_HW_CONSISTENCY,
+	IPU6_FW_ISYS_ERROR_DRIVER_INVALID_COMMAND_SEQUENCE,
+	IPU6_FW_ISYS_ERROR_DRIVER_INVALID_DEVICE_CONFIGURATION,
+	IPU6_FW_ISYS_ERROR_DRIVER_INVALID_STREAM_CONFIGURATION,
+	IPU6_FW_ISYS_ERROR_DRIVER_INVALID_FRAME_CONFIGURATION,
+	IPU6_FW_ISYS_ERROR_INSUFFICIENT_RESOURCES,
+	IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO,
+	IPU6_FW_ISYS_ERROR_HW_REPORTED_SIG2CIO,
+	IPU6_FW_ISYS_ERROR_SENSOR_FW_SYNC,
+	IPU6_FW_ISYS_ERROR_STREAM_IN_SUSPENSION,
+	IPU6_FW_ISYS_ERROR_RESPONSE_QUEUE_FULL,
+	N_IPU6_FW_ISYS_ERROR
+};
+
+enum ipu6_fw_proxy_error {
+	IPU6_FW_PROXY_ERROR_NONE = 0,
+	IPU6_FW_PROXY_ERROR_INVALID_WRITE_REGION,
+	IPU6_FW_PROXY_ERROR_INVALID_WRITE_OFFSET,
+	N_IPU6_FW_PROXY_ERROR
+};
+
+/* firmware ABI structure below are aligned in firmware, no need pack */
+struct ipu6_fw_isys_buffer_partition_abi {
+	u32 num_gda_pages[IPU6_STREAM_ID_MAX];
+};
+
+struct ipu6_fw_isys_fw_config {
+	struct ipu6_fw_isys_buffer_partition_abi buffer_partition;
+	u32 num_send_queues[N_IPU6_FW_ISYS_QUEUE_TYPE];
+	u32 num_recv_queues[N_IPU6_FW_ISYS_QUEUE_TYPE];
+};
+
+/*
+ * struct ipu6_fw_isys_resolution_abi: Generic resolution structure.
+ */
+struct ipu6_fw_isys_resolution_abi {
+	u32 width;
+	u32 height;
+};
+
+/**
+ * struct ipu6_fw_isys_output_pin_payload_abi - ISYS output pin buffer
+ * @out_buf_id: Points to output pin buffer - buffer identifier
+ * @addr: Points to output pin buffer - CSS Virtual Address
+ * @compress: Request frame compression (1), or  not (0)
+ */
+struct ipu6_fw_isys_output_pin_payload_abi {
+	u64 out_buf_id;
+	u32 addr;
+	u32 compress;
+};
+
+/**
+ * struct ipu6_fw_isys_output_pin_info_abi - ISYS output pin info
+ * @output_res: output pin resolution
+ * @stride: output stride in Bytes (not valid for statistics)
+ * @watermark_in_lines: pin watermark level in lines
+ * @payload_buf_size: minimum size in Bytes of all buffers that will be
+ *			supplied for capture on this pin
+ * @ts_offsets: ts_offsets
+ * @s2m_pixel_soc_pixel_remapping: pixel soc remapping (see the definition of
+ *				   S2M_PIXEL_SOC_PIXEL_REMAPPING_FLAG_NO_REMAPPING)
+ * @csi_be_soc_pixel_remapping: see s2m_pixel_soc_pixel_remapping
+ * @send_irq: assert if pin event should trigger irq
+ * @input_pin_id: related input pin id
+ * @pt: pin type -real format "enum ipu6_fw_isys_pin_type"
+ * @ft: frame format type -real format "enum ipu6_fw_isys_frame_format_type"
+ * @reserved: a reserved field
+ * @reserve_compression: reserve compression resources for pin
+ * @snoopable: snoopable
+ * @error_handling_enable: enable error handling
+ * @sensor_type: sensor_type
+ */
+struct ipu6_fw_isys_output_pin_info_abi {
+	struct ipu6_fw_isys_resolution_abi output_res;
+	u32 stride;
+	u32 watermark_in_lines;
+	u32 payload_buf_size;
+	u32 ts_offsets[IPU6_PIN_PLANES_MAX];
+	u32 s2m_pixel_soc_pixel_remapping;
+	u32 csi_be_soc_pixel_remapping;
+	u8 send_irq;
+	u8 input_pin_id;
+	u8 pt;
+	u8 ft;
+	u8 reserved;
+	u8 reserve_compression;
+	u8 snoopable;
+	u8 error_handling_enable;
+	u32 sensor_type;
+};
+
+/**
+ * struct ipu6_fw_isys_input_pin_info_abi - ISYS input pin info
+ * @input_res: input resolution
+ * @dt: mipi data type ((enum ipu6_fw_isys_mipi_data_type)
+ * @mipi_store_mode: defines if legacy long packet header will be stored or
+ *		     discarded if discarded, output pin type for this
+ *		     input pin can only be MIPI
+ *		     (enum ipu6_fw_isys_mipi_store_mode)
+ * @bits_per_pix: native bits per pixel
+ * @mapped_dt: actual data type from sensor
+ * @mipi_decompression: defines which compression will be in mipi backend
+ * @crop_first_and_last_lines: Control whether to crop the first and last line
+ *			       of the input image. Crop done by HW device.
+ * @capture_mode: mode of capture, regular or burst, default value is regular
+ * @reserved: a reserved field
+ */
+struct ipu6_fw_isys_input_pin_info_abi {
+	struct ipu6_fw_isys_resolution_abi input_res;
+	u8 dt;
+	u8 mipi_store_mode;
+	u8 bits_per_pix;
+	u8 mapped_dt;
+	u8 mipi_decompression;
+	u8 crop_first_and_last_lines;
+	u8 capture_mode;
+	u8 reserved;
+};
+
+/**
+ * struct ipu6_fw_isys_cropping_abi - ISYS cropping coordinates
+ * @top_offset: Top offset
+ * @left_offset: Left offset
+ * @bottom_offset: Bottom offset
+ * @right_offset: Right offset
+ */
+struct ipu6_fw_isys_cropping_abi {
+	s32 top_offset;
+	s32 left_offset;
+	s32 bottom_offset;
+	s32 right_offset;
+};
+
+/**
+ * struct ipu6_fw_isys_stream_cfg_data_abi - ISYS stream configuration data
+ * ISYS stream configuration data structure
+ * @crop: for extended use and is not used in FW currently
+ * @input_pins: input pin descriptors
+ * @output_pins: output pin descriptors
+ * @compfmt: de-compression setting for User Defined Data
+ * @nof_input_pins: number of input pins
+ * @nof_output_pins: number of output pins
+ * @send_irq_sof_discarded: send irq on discarded frame sof response
+ *		- if '1' it will override the send_resp_sof_discarded
+ *		  and send the response
+ *		- if '0' the send_resp_sof_discarded will determine
+ *		  whether to send the response
+ * @send_irq_eof_discarded: send irq on discarded frame eof response
+ *		- if '1' it will override the send_resp_eof_discarded
+ *		  and send the response
+ *		- if '0' the send_resp_eof_discarded will determine
+ *		  whether to send the response
+ * @send_resp_sof_discarded: send response for discarded frame sof detected,
+ *			     used only when send_irq_sof_discarded is '0'
+ * @send_resp_eof_discarded: send response for discarded frame eof detected,
+ *			     used only when send_irq_eof_discarded is '0'
+ * @src: Stream source index e.g. MIPI_generator_0, CSI2-rx_1
+ * @vc: MIPI Virtual Channel (up to 4 virtual per physical channel)
+ * @isl_use: indicates whether stream requires ISL and how
+ * @sensor_type: type of connected sensor, tobii or others, default is 0
+ * @reserved: a reserved field
+ * @reserved2: a reserved field
+ */
+struct ipu6_fw_isys_stream_cfg_data_abi {
+	struct ipu6_fw_isys_cropping_abi crop;
+	struct ipu6_fw_isys_input_pin_info_abi input_pins[IPU6_MAX_IPINS];
+	struct ipu6_fw_isys_output_pin_info_abi output_pins[IPU6_MAX_OPINS];
+	u32 compfmt;
+	u8 nof_input_pins;
+	u8 nof_output_pins;
+	u8 send_irq_sof_discarded;
+	u8 send_irq_eof_discarded;
+	u8 send_resp_sof_discarded;
+	u8 send_resp_eof_discarded;
+	u8 src;
+	u8 vc;
+	u8 isl_use;
+	u8 sensor_type;
+	u8 reserved;
+	u8 reserved2;
+};
+
+/**
+ * struct ipu6_fw_isys_frame_buff_set_abi - ISYS frame buffer set (request)
+ * @output_pins: output pin addresses
+ * @send_irq_sof: send irq on frame sof response
+ *		- if '1' it will override the send_resp_sof and
+ *		  send the response
+ *		- if '0' the send_resp_sof will determine whether to
+ *		  send the response
+ * @send_irq_eof: send irq on frame eof response
+ *		- if '1' it will override the send_resp_eof and
+ *		  send the response
+ *		- if '0' the send_resp_eof will determine whether to
+ *		  send the response
+ * @send_irq_capture_ack: send irq on capture ack
+ * @send_irq_capture_done: send irq on capture done
+ * @send_resp_sof: send response for frame sof detected,
+ *		   used only when send_irq_sof is '0'
+ * @send_resp_eof: send response for frame eof detected,
+ *		   used only when send_irq_eof is '0'
+ * @send_resp_capture_ack: send response for capture ack event
+ * @send_resp_capture_done: send response for capture done event
+ * @reserved: a reserved field
+ */
+struct ipu6_fw_isys_frame_buff_set_abi {
+	struct ipu6_fw_isys_output_pin_payload_abi output_pins[IPU6_MAX_OPINS];
+	u8 send_irq_sof;
+	u8 send_irq_eof;
+	u8 send_irq_capture_ack;
+	u8 send_irq_capture_done;
+	u8 send_resp_sof;
+	u8 send_resp_eof;
+	u8 send_resp_capture_ack;
+	u8 send_resp_capture_done;
+	u8 reserved[8];
+};
+
+/**
+ * struct ipu6_fw_isys_error_info_abi - ISYS error information
+ * @error: error code if something went wrong
+ * @error_details: depending on error code, it may contain additional error info
+ */
+struct ipu6_fw_isys_error_info_abi {
+	u32 error;
+	u32 error_details;
+};
+
+/**
+ * struct ipu6_fw_isys_resp_info_abi - ISYS firmware response
+ * @buf_id: buffer ID
+ * @pin: this var is only valid for pin event related responses,
+ *     contains pin addresses
+ * @error_info: error information from the FW
+ * @timestamp: Time information for event if available
+ * @stream_handle: stream id the response corresponds to
+ * @type: response type (enum ipu6_fw_isys_resp_type)
+ * @pin_id: pin id that the pin payload corresponds to
+ * @reserved: a reserved field
+ * @reserved2: a reserved field
+ */
+struct ipu6_fw_isys_resp_info_abi {
+	u64 buf_id;
+	struct ipu6_fw_isys_output_pin_payload_abi pin;
+	struct ipu6_fw_isys_error_info_abi error_info;
+	u32 timestamp[2];
+	u8 stream_handle;
+	u8 type;
+	u8 pin_id;
+	u8 reserved;
+	u32 reserved2;
+};
+
+/**
+ * struct ipu6_fw_isys_proxy_error_info_abi - ISYS proxy error
+ * @error: error code if something went wrong
+ * @error_details: depending on error code, it may contain additional error info
+ */
+struct ipu6_fw_isys_proxy_error_info_abi {
+	u32 error;
+	u32 error_details;
+};
+
+struct ipu6_fw_isys_proxy_resp_info_abi {
+	u32 request_id;
+	struct ipu6_fw_isys_proxy_error_info_abi error_info;
+};
+
+/**
+ * struct ipu6_fw_proxy_write_queue_token - ISYS proxy write queue token
+ * @request_id: update id for the specific proxy write request
+ * @region_index: Region id for the proxy write request
+ * @offset: Offset of the write request according to the base address
+ *	    of the region
+ * @value: Value that is requested to be written with the proxy write request
+ */
+struct ipu6_fw_proxy_write_queue_token {
+	u32 request_id;
+	u32 region_index;
+	u32 offset;
+	u32 value;
+};
+
+/**
+ * struct ipu6_fw_resp_queue_token - ISYS response queue token
+ * @resp_info: response info
+ */
+struct ipu6_fw_resp_queue_token {
+	struct ipu6_fw_isys_resp_info_abi resp_info;
+};
+
+/**
+ * struct ipu6_fw_send_queue_token - ISYS send queue token
+ * @buf_handle: buffer handle
+ * @payload: payload
+ * @send_type: send_type
+ * @stream_id: stream_id
+ */
+struct ipu6_fw_send_queue_token {
+	u64 buf_handle;
+	u32 payload;
+	u16 send_type;
+	u16 stream_id;
+};
+
+/**
+ * struct ipu6_fw_proxy_resp_queue_token - ISYS proxy response queue token
+ * @proxy_resp_info: proxy response info
+ */
+struct ipu6_fw_proxy_resp_queue_token {
+	struct ipu6_fw_isys_proxy_resp_info_abi proxy_resp_info;
+};
+
+/**
+ * struct ipu6_fw_proxy_send_queue_token - SYS proxy send queue token
+ * @request_id: request_id
+ * @region_index: region_index
+ * @offset: offset
+ * @value: value
+ */
+struct ipu6_fw_proxy_send_queue_token {
+	u32 request_id;
+	u32 region_index;
+	u32 offset;
+	u32 value;
+};
+
+void
+ipu6_fw_isys_dump_stream_cfg(struct device *dev,
+			     struct ipu6_fw_isys_stream_cfg_data_abi *cfg);
+void
+ipu6_fw_isys_dump_frame_buff_set(struct device *dev,
+				 struct ipu6_fw_isys_frame_buff_set_abi *buf,
+				 unsigned int outputs);
+int ipu6_fw_isys_init(struct ipu6_isys *isys, unsigned int num_streams);
+int ipu6_fw_isys_close(struct ipu6_isys *isys);
+int ipu6_fw_isys_simple_cmd(struct ipu6_isys *isys,
+			    const unsigned int stream_handle, u16 send_type);
+int ipu6_fw_isys_complex_cmd(struct ipu6_isys *isys,
+			     const unsigned int stream_handle,
+			     void *cpu_mapped_buf, dma_addr_t dma_mapped_buf,
+			     size_t size, u16 send_type);
+int ipu6_fw_isys_send_proxy_token(struct ipu6_isys *isys,
+				  unsigned int req_id,
+				  unsigned int index,
+				  unsigned int offset, u32 value);
+void ipu6_fw_isys_cleanup(struct ipu6_isys *isys);
+struct ipu6_fw_isys_resp_info_abi *
+ipu6_fw_isys_get_resp(void *context, unsigned int queue);
+void ipu6_fw_isys_put_resp(void *context, unsigned int queue);
+#endif
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c
new file mode 100644
index 000000000000..da8581a37e22
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.c
@@ -0,0 +1,643 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/atomic.h>
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/minmax.h>
+#include <linux/sprintf.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-isys-subdev.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+
+static const u32 csi2_supported_codes[] = {
+	MEDIA_BUS_FMT_RGB565_1X16,
+	MEDIA_BUS_FMT_RGB888_1X24,
+	MEDIA_BUS_FMT_UYVY8_1X16,
+	MEDIA_BUS_FMT_YUYV8_1X16,
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+	MEDIA_BUS_FMT_SBGGR8_1X8,
+	MEDIA_BUS_FMT_SGBRG8_1X8,
+	MEDIA_BUS_FMT_SGRBG8_1X8,
+	MEDIA_BUS_FMT_SRGGB8_1X8,
+	MEDIA_BUS_FMT_META_8,
+	MEDIA_BUS_FMT_META_10,
+	MEDIA_BUS_FMT_META_12,
+	MEDIA_BUS_FMT_META_16,
+	MEDIA_BUS_FMT_META_24,
+	0
+};
+
+/*
+ * Strings corresponding to CSI-2 receiver errors are here.
+ * Corresponding macros are defined in the header file.
+ */
+static const struct ipu6_csi2_error dphy_rx_errors[] = {
+	{ "Single packet header error corrected", true },
+	{ "Multiple packet header errors detected", true },
+	{ "Payload checksum (CRC) error", true },
+	{ "Transfer FIFO overflow", false },
+	{ "Reserved short packet data type detected", true },
+	{ "Reserved long packet data type detected", true },
+	{ "Incomplete long packet detected", false },
+	{ "Frame sync error", false },
+	{ "Line sync error", false },
+	{ "DPHY recoverable synchronization error", true },
+	{ "DPHY fatal error", false },
+	{ "DPHY elastic FIFO overflow", false },
+	{ "Inter-frame short packet discarded", true },
+	{ "Inter-frame long packet discarded", true },
+	{ "MIPI pktgen overflow", false },
+	{ "MIPI pktgen data loss", false },
+	{ "FIFO overflow", false },
+	{ "Lane deskew", false },
+	{ "SOT sync error", false },
+	{ "HSIDLE detected", false }
+};
+
+s64 ipu6_isys_csi2_get_link_freq(struct ipu6_isys_csi2 *csi2)
+{
+	struct media_pad *src_pad;
+
+	if (!csi2)
+		return -EINVAL;
+
+	src_pad = media_entity_remote_source_pad_unique(&csi2->asd.sd.entity);
+	if (IS_ERR(src_pad)) {
+		dev_err(&csi2->isys->adev->auxdev.dev,
+			"can't get source pad of %s (%ld)\n",
+			csi2->asd.sd.name, PTR_ERR(src_pad));
+		return PTR_ERR(src_pad);
+	}
+
+	return v4l2_get_link_freq(src_pad, 0, 0);
+}
+
+static int csi2_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				struct v4l2_event_subscription *sub)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
+	struct device *dev = &csi2->isys->adev->auxdev.dev;
+
+	dev_dbg(dev, "csi2 subscribe event(type %u id %u)\n",
+		sub->type, sub->id);
+
+	switch (sub->type) {
+	case V4L2_EVENT_FRAME_SYNC:
+		return v4l2_event_subscribe(fh, sub, 10, NULL);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_subdev_core_ops csi2_sd_core_ops = {
+	.subscribe_event = csi2_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+/*
+ * The input system CSI2+ receiver has several
+ * parameters affecting the receiver timings. These depend
+ * on the MIPI bus frequency F in Hz (sensor transmitter rate)
+ * as follows:
+ *	register value = (A/1e9 + B * UI) / COUNT_ACC
+ * where
+ *	UI = 1 / (2 * F) in seconds
+ *	COUNT_ACC = counter accuracy in seconds
+ *	COUNT_ACC = 0.125 ns = 1 / 8 ns, ACCINV = 8.
+ *
+ * A and B are coefficients from the table below,
+ * depending whether the register minimum or maximum value is
+ * calculated.
+ *				       Minimum     Maximum
+ * Clock lane			       A     B     A     B
+ * reg_rx_csi_dly_cnt_termen_clane     0     0    38     0
+ * reg_rx_csi_dly_cnt_settle_clane    95    -8   300   -16
+ * Data lanes
+ * reg_rx_csi_dly_cnt_termen_dlane0    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane0   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane1    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane1   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane2    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane2   85    -2   145    -6
+ * reg_rx_csi_dly_cnt_termen_dlane3    0     0    35     4
+ * reg_rx_csi_dly_cnt_settle_dlane3   85    -2   145    -6
+ *
+ * We use the minimum values of both A and B.
+ */
+
+#define DIV_SHIFT	8
+#define CSI2_ACCINV	8
+
+static u32 calc_timing(s32 a, s32 b, s64 link_freq, s32 accinv)
+{
+	return accinv * a + (accinv * b * (500000000 >> DIV_SHIFT)
+			     / (s32)(link_freq >> DIV_SHIFT));
+}
+
+static int
+ipu6_isys_csi2_calc_timing(struct ipu6_isys_csi2 *csi2,
+			   struct ipu6_isys_csi2_timing *timing, s32 accinv)
+{
+	struct device *dev = &csi2->isys->adev->auxdev.dev;
+	s64 link_freq;
+
+	link_freq = ipu6_isys_csi2_get_link_freq(csi2);
+	if (link_freq < 0)
+		return link_freq;
+
+	timing->ctermen = calc_timing(CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_A,
+				      CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_B,
+				      link_freq, accinv);
+	timing->csettle = calc_timing(CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_A,
+				      CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_B,
+				      link_freq, accinv);
+	timing->dtermen = calc_timing(CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_A,
+				      CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_B,
+				      link_freq, accinv);
+	timing->dsettle = calc_timing(CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_A,
+				      CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_B,
+				      link_freq, accinv);
+
+	dev_dbg(dev, "ctermen %u csettle %u dtermen %u dsettle %u\n",
+		timing->ctermen, timing->csettle,
+		timing->dtermen, timing->dsettle);
+
+	return 0;
+}
+
+void ipu6_isys_register_errors(struct ipu6_isys_csi2 *csi2)
+{
+	u32 irq = readl(csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+			CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
+	struct ipu6_isys *isys = csi2->isys;
+	u32 mask;
+
+	mask = isys->pdata->ipdata->csi2.irq_mask;
+	writel(irq & mask, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+	csi2->receiver_errors |= irq & mask;
+}
+
+void ipu6_isys_csi2_error(struct ipu6_isys_csi2 *csi2)
+{
+	struct device *dev = &csi2->isys->adev->auxdev.dev;
+	const struct ipu6_csi2_error *errors;
+	u32 status;
+	u32 i;
+
+	/* register errors once more in case of interrupts are disabled */
+	ipu6_isys_register_errors(csi2);
+	status = csi2->receiver_errors;
+	csi2->receiver_errors = 0;
+	errors = dphy_rx_errors;
+
+	for (i = 0; i < CSI_RX_NUM_ERRORS_IN_IRQ; i++) {
+		if (status & BIT(i))
+			dev_err_ratelimited(dev, "csi2-%i error: %s\n",
+					    csi2->port, errors[i].error_string);
+	}
+}
+
+static int ipu6_isys_csi2_set_stream(struct v4l2_subdev *sd,
+				     const struct ipu6_isys_csi2_timing *timing,
+				     unsigned int nlanes, int enable)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
+	struct ipu6_isys *isys = csi2->isys;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_isys_csi2_config cfg;
+	unsigned int nports;
+	int ret = 0;
+	u32 mask = 0;
+	u32 i;
+
+	dev_dbg(dev, "stream %s CSI2-%u with %u lanes\n", enable ? "on" : "off",
+		csi2->port, nlanes);
+
+	cfg.port = csi2->port;
+	cfg.nlanes = nlanes;
+
+	mask = isys->pdata->ipdata->csi2.irq_mask;
+	nports = isys->pdata->ipdata->csi2.nports;
+
+	if (!enable) {
+		writel(0, csi2->base + CSI_REG_CSI_FE_ENABLE);
+		writel(0, csi2->base + CSI_REG_PPI2CSI_ENABLE);
+
+		writel(0,
+		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+		       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
+		writel(mask,
+		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+		       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+		writel(0,
+		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+		       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
+		writel(0xffffffff,
+		       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+		       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+
+		isys->phy_set_power(isys, &cfg, timing, false);
+
+		writel(0, isys->pdata->base + CSI_REG_HUB_FW_ACCESS_PORT
+		       (isys->pdata->ipdata->csi2.fw_access_port_ofs,
+			csi2->port));
+		writel(0, isys->pdata->base +
+		       CSI_REG_HUB_DRV_ACCESS_PORT(csi2->port));
+
+		return ret;
+	}
+
+	/* reset port reset */
+	writel(0x1, csi2->base + CSI_REG_PORT_GPREG_SRST);
+	usleep_range(100, 200);
+	writel(0x0, csi2->base + CSI_REG_PORT_GPREG_SRST);
+
+	/* enable port clock */
+	for (i = 0; i < nports; i++) {
+		writel(1, isys->pdata->base + CSI_REG_HUB_DRV_ACCESS_PORT(i));
+		writel(1, isys->pdata->base + CSI_REG_HUB_FW_ACCESS_PORT
+		       (isys->pdata->ipdata->csi2.fw_access_port_ofs, i));
+	}
+
+	/* enable all error related irq */
+	writel(mask,
+	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
+	writel(mask,
+	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_MASK_OFFSET);
+	writel(mask,
+	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+	writel(mask,
+	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_LEVEL_NOT_PULSE_OFFSET);
+	writel(mask,
+	       csi2->base + CSI_PORT_REG_BASE_IRQ_CSI +
+	       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
+
+	/*
+	 * Using event from firmware instead of irq to handle CSI2 sync event
+	 * which can reduce system wakeups. If CSI2 sync irq enabled, we need
+	 * disable the firmware CSI2 sync event to avoid duplicate handling.
+	 */
+	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
+	writel(0, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_MASK_OFFSET);
+	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+	writel(0, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_LEVEL_NOT_PULSE_OFFSET);
+	writel(0xffffffff, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET);
+
+	/* configure to enable FE and PPI2CSI */
+	writel(0, csi2->base + CSI_REG_CSI_FE_MODE);
+	writel(CSI_SENSOR_INPUT, csi2->base + CSI_REG_CSI_FE_MUX_CTRL);
+	writel(CSI_CNTR_SENSOR_LINE_ID | CSI_CNTR_SENSOR_FRAME_ID,
+	       csi2->base + CSI_REG_CSI_FE_SYNC_CNTR_SEL);
+	writel(FIELD_PREP(PPI_INTF_CONFIG_NOF_ENABLED_DLANES_MASK, nlanes - 1),
+	       csi2->base + CSI_REG_PPI2CSI_CONFIG_PPI_INTF);
+
+	writel(1, csi2->base + CSI_REG_PPI2CSI_ENABLE);
+	writel(1, csi2->base + CSI_REG_CSI_FE_ENABLE);
+
+	ret = isys->phy_set_power(isys, &cfg, timing, true);
+	if (ret)
+		dev_err(dev, "csi-%d phy power up failed %d\n", csi2->port,
+			ret);
+
+	return ret;
+}
+
+static int ipu6_isys_csi2_enable_streams(struct v4l2_subdev *sd,
+					 struct v4l2_subdev_state *state,
+					 u32 pad, u64 streams_mask)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	struct ipu6_isys_csi2 *csi2 = to_ipu6_isys_csi2(asd);
+	struct ipu6_isys_csi2_timing timing = { };
+	struct v4l2_subdev *remote_sd;
+	struct media_pad *remote_pad;
+	u64 sink_streams;
+	int ret;
+
+	remote_pad = media_pad_remote_pad_first(&sd->entity.pads[CSI2_PAD_SINK]);
+	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, CSI2_PAD_SRC,
+						       CSI2_PAD_SINK,
+						       &streams_mask);
+
+	ret = ipu6_isys_csi2_calc_timing(csi2, &timing, CSI2_ACCINV);
+	if (ret)
+		return ret;
+
+	ret = ipu6_isys_csi2_set_stream(sd, &timing, csi2->nlanes, true);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_enable_streams(remote_sd, remote_pad->index,
+					 sink_streams);
+	if (ret) {
+		ipu6_isys_csi2_set_stream(sd, NULL, 0, false);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int ipu6_isys_csi2_disable_streams(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_state *state,
+					  u32 pad, u64 streams_mask)
+{
+	struct v4l2_subdev *remote_sd;
+	struct media_pad *remote_pad;
+	u64 sink_streams;
+
+	sink_streams = v4l2_subdev_state_xlate_streams(state, CSI2_PAD_SRC,
+						       CSI2_PAD_SINK,
+						       &streams_mask);
+
+	remote_pad = media_pad_remote_pad_first(&sd->entity.pads[CSI2_PAD_SINK]);
+	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
+
+	ipu6_isys_csi2_set_stream(sd, NULL, 0, false);
+
+	v4l2_subdev_disable_streams(remote_sd, remote_pad->index, sink_streams);
+
+	return 0;
+}
+
+static int ipu6_isys_csi2_set_sel(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	struct device *dev = &asd->isys->adev->auxdev.dev;
+	struct v4l2_mbus_framefmt *sink_ffmt;
+	struct v4l2_mbus_framefmt *src_ffmt;
+	struct v4l2_rect *crop;
+
+	if (sel->pad == CSI2_PAD_SINK || sel->target != V4L2_SEL_TGT_CROP)
+		return -EINVAL;
+
+	sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state,
+								 sel->pad,
+								 sel->stream);
+	if (!sink_ffmt)
+		return -EINVAL;
+
+	src_ffmt = v4l2_subdev_state_get_format(state, sel->pad, sel->stream);
+	if (!src_ffmt)
+		return -EINVAL;
+
+	crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream);
+	if (!crop)
+		return -EINVAL;
+
+	/* Only vertical cropping is supported */
+	sel->r.left = 0;
+	sel->r.width = sink_ffmt->width;
+	/* Non-bayer formats can't be single line cropped */
+	if (!ipu6_isys_is_bayer_format(sink_ffmt->code))
+		sel->r.top &= ~1;
+	sel->r.height = clamp(sel->r.height & ~1, IPU6_ISYS_MIN_HEIGHT,
+			      sink_ffmt->height - sel->r.top);
+	*crop = sel->r;
+
+	/* update source pad format */
+	src_ffmt->width = sel->r.width;
+	src_ffmt->height = sel->r.height;
+	if (ipu6_isys_is_bayer_format(sink_ffmt->code))
+		src_ffmt->code = ipu6_isys_convert_bayer_order(sink_ffmt->code,
+							       sel->r.left,
+							       sel->r.top);
+	dev_dbg(dev, "set crop for %s sel: %d,%d,%d,%d code: 0x%x\n",
+		sd->name, sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		src_ffmt->code);
+
+	return 0;
+}
+
+static int ipu6_isys_csi2_get_sel(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state,
+				  struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_mbus_framefmt *sink_ffmt;
+	struct v4l2_rect *crop;
+	int ret = 0;
+
+	if (sd->entity.pads[sel->pad].flags & MEDIA_PAD_FL_SINK)
+		return -EINVAL;
+
+	sink_ffmt = v4l2_subdev_state_get_opposite_stream_format(state,
+								 sel->pad,
+								 sel->stream);
+	if (!sink_ffmt)
+		return -EINVAL;
+
+	crop = v4l2_subdev_state_get_crop(state, sel->pad, sel->stream);
+	if (!crop)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = sink_ffmt->width;
+		sel->r.height = sink_ffmt->height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		sel->r = *crop;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops csi2_sd_pad_ops = {
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = ipu6_isys_subdev_set_fmt,
+	.get_selection = ipu6_isys_csi2_get_sel,
+	.set_selection = ipu6_isys_csi2_set_sel,
+	.enum_mbus_code = ipu6_isys_subdev_enum_mbus_code,
+	.set_routing = ipu6_isys_subdev_set_routing,
+	.enable_streams = ipu6_isys_csi2_enable_streams,
+	.disable_streams = ipu6_isys_csi2_disable_streams,
+};
+
+static const struct v4l2_subdev_ops csi2_sd_ops = {
+	.core = &csi2_sd_core_ops,
+	.pad = &csi2_sd_pad_ops,
+};
+
+static const struct media_entity_operations csi2_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
+};
+
+void ipu6_isys_csi2_cleanup(struct ipu6_isys_csi2 *csi2)
+{
+	if (!csi2->isys)
+		return;
+
+	v4l2_device_unregister_subdev(&csi2->asd.sd);
+	v4l2_subdev_cleanup(&csi2->asd.sd);
+	ipu6_isys_subdev_cleanup(&csi2->asd);
+	csi2->isys = NULL;
+}
+
+int ipu6_isys_csi2_init(struct ipu6_isys_csi2 *csi2,
+			struct ipu6_isys *isys,
+			void __iomem *base, unsigned int index)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	int ret;
+
+	csi2->isys = isys;
+	csi2->base = base;
+	csi2->port = index;
+
+	csi2->asd.sd.entity.ops = &csi2_entity_ops;
+	csi2->asd.isys = isys;
+	ret = ipu6_isys_subdev_init(&csi2->asd, &csi2_sd_ops, 0,
+				    NR_OF_CSI2_SINK_PADS, NR_OF_CSI2_SRC_PADS);
+	if (ret)
+		goto fail;
+
+	csi2->asd.source = IPU6_FW_ISYS_STREAM_SRC_CSI2_PORT0 + index;
+	csi2->asd.supported_codes = csi2_supported_codes;
+	snprintf(csi2->asd.sd.name, sizeof(csi2->asd.sd.name),
+		 IPU6_ISYS_ENTITY_PREFIX " CSI2 %u", index);
+	v4l2_set_subdevdata(&csi2->asd.sd, &csi2->asd);
+	ret = v4l2_subdev_init_finalize(&csi2->asd.sd);
+	if (ret) {
+		dev_err(dev, "failed to init v4l2 subdev\n");
+		goto fail;
+	}
+
+	ret = v4l2_device_register_subdev(&isys->v4l2_dev, &csi2->asd.sd);
+	if (ret) {
+		dev_err(dev, "failed to register v4l2 subdev\n");
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	ipu6_isys_csi2_cleanup(csi2);
+
+	return ret;
+}
+
+void ipu6_isys_csi2_sof_event_by_stream(struct ipu6_isys_stream *stream)
+{
+	struct video_device *vdev = stream->asd->sd.devnode;
+	struct device *dev = &stream->isys->adev->auxdev.dev;
+	struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
+	struct v4l2_event ev = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+	};
+
+	ev.u.frame_sync.frame_sequence = atomic_fetch_inc(&stream->sequence);
+	v4l2_event_queue(vdev, &ev);
+
+	dev_dbg(dev, "sof_event::csi2-%i sequence: %i, vc: %d\n",
+		csi2->port, ev.u.frame_sync.frame_sequence, stream->vc);
+}
+
+void ipu6_isys_csi2_eof_event_by_stream(struct ipu6_isys_stream *stream)
+{
+	struct device *dev = &stream->isys->adev->auxdev.dev;
+	struct ipu6_isys_csi2 *csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
+	u32 frame_sequence = atomic_read(&stream->sequence);
+
+	dev_dbg(dev, "eof_event::csi2-%i sequence: %i\n",
+		csi2->port, frame_sequence);
+}
+
+int ipu6_isys_csi2_get_remote_desc(u32 source_stream,
+				   struct ipu6_isys_csi2 *csi2,
+				   struct media_entity *source_entity,
+				   struct v4l2_mbus_frame_desc_entry *entry)
+{
+	struct v4l2_mbus_frame_desc_entry *desc_entry = NULL;
+	struct device *dev = &csi2->isys->adev->auxdev.dev;
+	struct v4l2_mbus_frame_desc desc;
+	struct v4l2_subdev *source;
+	struct media_pad *pad;
+	unsigned int i;
+	int ret;
+
+	source = media_entity_to_v4l2_subdev(source_entity);
+	if (!source)
+		return -EPIPE;
+
+	pad = media_pad_remote_pad_first(&csi2->asd.pad[CSI2_PAD_SINK]);
+	if (!pad)
+		return -EPIPE;
+
+	ret = v4l2_subdev_call(source, pad, get_frame_desc, pad->index, &desc);
+	if (ret)
+		return ret;
+
+	if (desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		dev_err(dev, "Unsupported frame descriptor type\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < desc.num_entries; i++) {
+		if (source_stream == desc.entry[i].stream) {
+			desc_entry = &desc.entry[i];
+			break;
+		}
+	}
+
+	if (!desc_entry) {
+		dev_err(dev, "Failed to find stream %u from remote subdev\n",
+			source_stream);
+		return -EINVAL;
+	}
+
+	if (desc_entry->bus.csi2.vc >= NR_OF_CSI2_VC) {
+		dev_err(dev, "invalid vc %d\n", desc_entry->bus.csi2.vc);
+		return -EINVAL;
+	}
+
+	*entry = *desc_entry;
+
+	return 0;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.h b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.h
new file mode 100644
index 000000000000..ce8eed91065c
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-csi2.h
@@ -0,0 +1,78 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_ISYS_CSI2_H
+#define IPU6_ISYS_CSI2_H
+
+#include <linux/container_of.h>
+
+#include "ipu6-isys-subdev.h"
+#include "ipu6-isys-video.h"
+
+struct media_entity;
+struct v4l2_mbus_frame_desc_entry;
+
+struct ipu6_isys_video;
+struct ipu6_isys;
+struct ipu6_isys_stream;
+
+#define NR_OF_CSI2_VC		16
+#define INVALID_VC_ID		-1
+#define NR_OF_CSI2_SINK_PADS	1
+#define CSI2_PAD_SINK		0
+#define NR_OF_CSI2_SRC_PADS	8
+#define CSI2_PAD_SRC		1
+#define NR_OF_CSI2_PADS		(NR_OF_CSI2_SINK_PADS + NR_OF_CSI2_SRC_PADS)
+
+#define CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_A		0
+#define CSI2_CSI_RX_DLY_CNT_TERMEN_CLANE_B		0
+#define CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_A		95
+#define CSI2_CSI_RX_DLY_CNT_SETTLE_CLANE_B		-8
+
+#define CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_A		0
+#define CSI2_CSI_RX_DLY_CNT_TERMEN_DLANE_B		0
+#define CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_A		85
+#define CSI2_CSI_RX_DLY_CNT_SETTLE_DLANE_B		-2
+
+struct ipu6_isys_csi2 {
+	struct ipu6_isys_subdev asd;
+	struct ipu6_isys *isys;
+	struct ipu6_isys_video av[NR_OF_CSI2_SRC_PADS];
+
+	void __iomem *base;
+	u32 receiver_errors;
+	unsigned int nlanes;
+	unsigned int port;
+};
+
+struct ipu6_isys_csi2_timing {
+	u32 ctermen;
+	u32 csettle;
+	u32 dtermen;
+	u32 dsettle;
+};
+
+struct ipu6_csi2_error {
+	const char *error_string;
+	bool is_info_only;
+};
+
+#define ipu6_isys_subdev_to_csi2(__sd) \
+	container_of(__sd, struct ipu6_isys_csi2, asd)
+
+#define to_ipu6_isys_csi2(__asd) container_of(__asd, struct ipu6_isys_csi2, asd)
+
+s64 ipu6_isys_csi2_get_link_freq(struct ipu6_isys_csi2 *csi2);
+int ipu6_isys_csi2_init(struct ipu6_isys_csi2 *csi2, struct ipu6_isys *isys,
+			void __iomem *base, unsigned int index);
+void ipu6_isys_csi2_cleanup(struct ipu6_isys_csi2 *csi2);
+void ipu6_isys_csi2_sof_event_by_stream(struct ipu6_isys_stream *stream);
+void ipu6_isys_csi2_eof_event_by_stream(struct ipu6_isys_stream *stream);
+void ipu6_isys_register_errors(struct ipu6_isys_csi2 *csi2);
+void ipu6_isys_csi2_error(struct ipu6_isys_csi2 *csi2);
+int ipu6_isys_csi2_get_remote_desc(u32 source_stream,
+				   struct ipu6_isys_csi2 *csi2,
+				   struct media_entity *source_entity,
+				   struct v4l2_mbus_frame_desc_entry *entry);
+
+#endif /* IPU6_ISYS_CSI2_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-dwc-phy.c b/drivers/media/pci/intel/ipu6/ipu6-isys-dwc-phy.c
new file mode 100644
index 000000000000..db2874843453
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-dwc-phy.c
@@ -0,0 +1,536 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/iopoll.h>
+#include <linux/math64.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-isys.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+
+#define IPU6_DWC_DPHY_BASE(i)			(0x238038 + 0x34 * (i))
+#define IPU6_DWC_DPHY_RSTZ			0x00
+#define IPU6_DWC_DPHY_SHUTDOWNZ			0x04
+#define IPU6_DWC_DPHY_HSFREQRANGE		0x08
+#define IPU6_DWC_DPHY_CFGCLKFREQRANGE		0x0c
+#define IPU6_DWC_DPHY_TEST_IFC_ACCESS_MODE	0x10
+#define IPU6_DWC_DPHY_TEST_IFC_REQ		0x14
+#define IPU6_DWC_DPHY_TEST_IFC_REQ_COMPLETION	0x18
+#define IPU6_DWC_DPHY_DFT_CTRL0			0x28
+#define IPU6_DWC_DPHY_DFT_CTRL1			0x2c
+#define IPU6_DWC_DPHY_DFT_CTRL2			0x30
+
+/*
+ * test IFC request definition:
+ * - req: 0 for read, 1 for write
+ * - 12 bits address
+ * - 8bits data (will ignore for read)
+ * --24----16------4-----0
+ * --|-data-|-addr-|-req-|
+ */
+#define IFC_REQ(req, addr, data) (FIELD_PREP(GENMASK(23, 16), data) | \
+				  FIELD_PREP(GENMASK(15, 4), addr) | \
+				  FIELD_PREP(GENMASK(1, 0), req))
+
+#define TEST_IFC_REQ_READ	0
+#define TEST_IFC_REQ_WRITE	1
+#define TEST_IFC_REQ_RESET	2
+
+#define TEST_IFC_ACCESS_MODE_FSM	0
+#define TEST_IFC_ACCESS_MODE_IFC_CTL	1
+
+enum phy_fsm_state {
+	PHY_FSM_STATE_POWERON = 0,
+	PHY_FSM_STATE_BGPON = 1,
+	PHY_FSM_STATE_CAL_TYPE = 2,
+	PHY_FSM_STATE_BURNIN_CAL = 3,
+	PHY_FSM_STATE_TERMCAL = 4,
+	PHY_FSM_STATE_OFFSETCAL = 5,
+	PHY_FSM_STATE_OFFSET_LANE = 6,
+	PHY_FSM_STATE_IDLE = 7,
+	PHY_FSM_STATE_ULP = 8,
+	PHY_FSM_STATE_DDLTUNNING = 9,
+	PHY_FSM_STATE_SKEW_BACKWARD = 10,
+	PHY_FSM_STATE_INVALID,
+};
+
+static void dwc_dphy_write(struct ipu6_isys *isys, u32 phy_id, u32 addr,
+			   u32 data)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	void __iomem *base = isys_base + IPU6_DWC_DPHY_BASE(phy_id);
+
+	dev_dbg(dev, "write: reg 0x%zx = data 0x%x", base + addr - isys_base,
+		data);
+	writel(data, base + addr);
+}
+
+static u32 dwc_dphy_read(struct ipu6_isys *isys, u32 phy_id, u32 addr)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	void __iomem *base = isys_base + IPU6_DWC_DPHY_BASE(phy_id);
+	u32 data;
+
+	data = readl(base + addr);
+	dev_dbg(dev, "read: reg 0x%zx = data 0x%x", base + addr - isys_base,
+		data);
+
+	return data;
+}
+
+static void dwc_dphy_write_mask(struct ipu6_isys *isys, u32 phy_id, u32 addr,
+				u32 data, u8 shift, u8 width)
+{
+	u32 temp;
+	u32 mask;
+
+	mask = (1 << width) - 1;
+	temp = dwc_dphy_read(isys, phy_id, addr);
+	temp &= ~(mask << shift);
+	temp |= (data & mask) << shift;
+	dwc_dphy_write(isys, phy_id, addr, temp);
+}
+
+static u32 __maybe_unused dwc_dphy_read_mask(struct ipu6_isys *isys, u32 phy_id,
+					     u32 addr, u8 shift,  u8 width)
+{
+	u32 val;
+
+	val = dwc_dphy_read(isys, phy_id, addr) >> shift;
+	return val & ((1 << width) - 1);
+}
+
+#define DWC_DPHY_TIMEOUT (5 * USEC_PER_SEC)
+static int dwc_dphy_ifc_read(struct ipu6_isys *isys, u32 phy_id, u32 addr,
+			     u32 *val)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	void __iomem *base = isys_base + IPU6_DWC_DPHY_BASE(phy_id);
+	void __iomem *reg;
+	u32 completion;
+	int ret;
+
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_TEST_IFC_REQ,
+		       IFC_REQ(TEST_IFC_REQ_READ, addr, 0));
+	reg = base + IPU6_DWC_DPHY_TEST_IFC_REQ_COMPLETION;
+	ret = readl_poll_timeout(reg, completion, !(completion & BIT(0)),
+				 10, DWC_DPHY_TIMEOUT);
+	if (ret) {
+		dev_err(dev, "DWC ifc request read timeout\n");
+		return ret;
+	}
+
+	*val = completion >> 8 & 0xff;
+	*val = FIELD_GET(GENMASK(15, 8), completion);
+	dev_dbg(dev, "DWC ifc read 0x%x = 0x%x", addr, *val);
+
+	return 0;
+}
+
+static int dwc_dphy_ifc_write(struct ipu6_isys *isys, u32 phy_id, u32 addr,
+			      u32 data)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	void __iomem *base = isys_base + IPU6_DWC_DPHY_BASE(phy_id);
+	void __iomem *reg;
+	u32 completion;
+	int ret;
+
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_TEST_IFC_REQ,
+		       IFC_REQ(TEST_IFC_REQ_WRITE, addr, data));
+	completion = readl(base + IPU6_DWC_DPHY_TEST_IFC_REQ_COMPLETION);
+	reg = base + IPU6_DWC_DPHY_TEST_IFC_REQ_COMPLETION;
+	ret = readl_poll_timeout(reg, completion, !(completion & BIT(0)),
+				 10, DWC_DPHY_TIMEOUT);
+	if (ret)
+		dev_err(dev, "DWC ifc request write timeout\n");
+
+	return ret;
+}
+
+static void dwc_dphy_ifc_write_mask(struct ipu6_isys *isys, u32 phy_id,
+				    u32 addr, u32 data, u8 shift, u8 width)
+{
+	u32 temp, mask;
+	int ret;
+
+	ret = dwc_dphy_ifc_read(isys, phy_id, addr, &temp);
+	if (ret)
+		return;
+
+	mask = (1 << width) - 1;
+	temp &= ~(mask << shift);
+	temp |= (data & mask) << shift;
+	dwc_dphy_ifc_write(isys, phy_id, addr, temp);
+}
+
+static u32 dwc_dphy_ifc_read_mask(struct ipu6_isys *isys, u32 phy_id, u32 addr,
+				  u8 shift, u8 width)
+{
+	int ret;
+	u32 val;
+
+	ret = dwc_dphy_ifc_read(isys, phy_id, addr, &val);
+	if (ret)
+		return 0;
+
+	return ((val >> shift) & ((1 << width) - 1));
+}
+
+static int dwc_dphy_pwr_up(struct ipu6_isys *isys, u32 phy_id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	u32 fsm_state;
+	int ret;
+
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_RSTZ, 1);
+	usleep_range(10, 20);
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_SHUTDOWNZ, 1);
+
+	ret = read_poll_timeout(dwc_dphy_ifc_read_mask, fsm_state,
+				(fsm_state == PHY_FSM_STATE_IDLE ||
+				 fsm_state == PHY_FSM_STATE_ULP),
+				100, DWC_DPHY_TIMEOUT, false, isys,
+				phy_id, 0x1e, 0, 4);
+
+	if (ret)
+		dev_err(dev, "Dphy %d power up failed, state 0x%x", phy_id,
+			fsm_state);
+
+	return ret;
+}
+
+struct dwc_dphy_freq_range {
+	u8 hsfreq;
+	u16 min;
+	u16 max;
+	u16 default_mbps;
+	u16 osc_freq_target;
+};
+
+#define DPHY_FREQ_RANGE_NUM		(63)
+#define DPHY_FREQ_RANGE_INVALID_INDEX	(0xff)
+static const struct dwc_dphy_freq_range freqranges[DPHY_FREQ_RANGE_NUM] = {
+	{0x00,	80,	97,	80,	335},
+	{0x10,	80,	107,	90,	335},
+	{0x20,	84,	118,	100,	335},
+	{0x30,	93,	128,	110,	335},
+	{0x01,	103,	139,	120,	335},
+	{0x11,	112,	149,	130,	335},
+	{0x21,	122,	160,	140,	335},
+	{0x31,	131,	170,	150,	335},
+	{0x02,	141,	181,	160,	335},
+	{0x12,	150,	191,	170,	335},
+	{0x22,	160,	202,	180,	335},
+	{0x32,	169,	212,	190,	335},
+	{0x03,	183,	228,	205,	335},
+	{0x13,	198,	244,	220,	335},
+	{0x23,	212,	259,	235,	335},
+	{0x33,	226,	275,	250,	335},
+	{0x04,	250,	301,	275,	335},
+	{0x14,	274,	328,	300,	335},
+	{0x25,	297,	354,	325,	335},
+	{0x35,	321,	380,	350,	335},
+	{0x05,	369,	433,	400,	335},
+	{0x16,	416,	485,	450,	335},
+	{0x26,	464,	538,	500,	335},
+	{0x37,	511,	590,	550,	335},
+	{0x07,	559,	643,	600,	335},
+	{0x18,	606,	695,	650,	335},
+	{0x28,	654,	748,	700,	335},
+	{0x39,	701,	800,	750,	335},
+	{0x09,	749,	853,	800,	335},
+	{0x19,	796,	905,	850,	335},
+	{0x29,	844,	958,	900,	335},
+	{0x3a,	891,	1010,	950,	335},
+	{0x0a,	939,	1063,	1000,	335},
+	{0x1a,	986,	1115,	1050,	335},
+	{0x2a,	1034,	1168,	1100,	335},
+	{0x3b,	1081,	1220,	1150,	335},
+	{0x0b,	1129,	1273,	1200,	335},
+	{0x1b,	1176,	1325,	1250,	335},
+	{0x2b,	1224,	1378,	1300,	335},
+	{0x3c,	1271,	1430,	1350,	335},
+	{0x0c,	1319,	1483,	1400,	335},
+	{0x1c,	1366,	1535,	1450,	335},
+	{0x2c,	1414,	1588,	1500,	335},
+	{0x3d,	1461,	1640,	1550,	208},
+	{0x0d,	1509,	1693,	1600,	214},
+	{0x1d,	1556,	1745,	1650,	221},
+	{0x2e,	1604,	1798,	1700,	228},
+	{0x3e,	1651,	1850,	1750,	234},
+	{0x0e,	1699,	1903,	1800,	241},
+	{0x1e,	1746,	1955,	1850,	248},
+	{0x2f,	1794,	2008,	1900,	255},
+	{0x3f,	1841,	2060,	1950,	261},
+	{0x0f,	1889,	2113,	2000,	268},
+	{0x40,	1936,	2165,	2050,	275},
+	{0x41,	1984,	2218,	2100,	281},
+	{0x42,	2031,	2270,	2150,	288},
+	{0x43,	2079,	2323,	2200,	294},
+	{0x44,	2126,	2375,	2250,	302},
+	{0x45,	2174,	2428,	2300,	308},
+	{0x46,	2221,	2480,	2350,	315},
+	{0x47,	2269,	2500,	2400,	321},
+	{0x48,	2316,	2500,	2450,	328},
+	{0x49,	2364,	2500,	2500,	335}
+};
+
+static u16 get_hsfreq_by_mbps(u32 mbps)
+{
+	unsigned int i = DPHY_FREQ_RANGE_NUM;
+
+	while (i--) {
+		if (freqranges[i].default_mbps == mbps ||
+		    (mbps >= freqranges[i].min && mbps <= freqranges[i].max))
+			return i;
+	}
+
+	return DPHY_FREQ_RANGE_INVALID_INDEX;
+}
+
+static int ipu6_isys_dwc_phy_config(struct ipu6_isys *isys,
+				    u32 phy_id, u32 mbps)
+{
+	struct ipu6_bus_device *adev = isys->adev;
+	struct device *dev = &adev->auxdev.dev;
+	struct ipu6_device *isp = adev->isp;
+	u32 cfg_clk_freqrange;
+	u32 osc_freq_target;
+	u32 index;
+
+	dev_dbg(dev, "config Dphy %u with %u mbps", phy_id, mbps);
+
+	index = get_hsfreq_by_mbps(mbps);
+	if (index == DPHY_FREQ_RANGE_INVALID_INDEX) {
+		dev_err(dev, "link freq not found for mbps %u", mbps);
+		return -EINVAL;
+	}
+
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_HSFREQRANGE,
+			    freqranges[index].hsfreq, 0, 7);
+
+	/* Force termination Calibration */
+	if (isys->phy_termcal_val) {
+		dwc_dphy_ifc_write_mask(isys, phy_id, 0x20a, 0x1, 0, 1);
+		dwc_dphy_ifc_write_mask(isys, phy_id, 0x209, 0x3, 0, 2);
+		dwc_dphy_ifc_write_mask(isys, phy_id, 0x209,
+					isys->phy_termcal_val, 4, 4);
+	}
+
+	/*
+	 * Enable override to configure the DDL target oscillation
+	 * frequency on bit 0 of register 0xe4
+	 */
+	dwc_dphy_ifc_write_mask(isys, phy_id, 0xe4, 0x1, 0, 1);
+	/*
+	 * configure registers 0xe2, 0xe3 with the
+	 * appropriate DDL target oscillation frequency
+	 * 0x1cc(460)
+	 */
+	osc_freq_target = freqranges[index].osc_freq_target;
+	dwc_dphy_ifc_write_mask(isys, phy_id, 0xe2,
+				osc_freq_target & 0xff, 0, 8);
+	dwc_dphy_ifc_write_mask(isys, phy_id, 0xe3,
+				(osc_freq_target >> 8) & 0xf, 0, 4);
+
+	if (mbps < 1500) {
+		/* deskew_polarity_rw, for < 1.5Gbps */
+		dwc_dphy_ifc_write_mask(isys, phy_id, 0x8, 0x1, 5, 1);
+	}
+
+	/*
+	 * Set cfgclkfreqrange[5:0] = round[(Fcfg_clk(MHz)-17)*4]
+	 * (38.4 - 17) * 4 = ~85 (0x55)
+	 */
+	cfg_clk_freqrange = (isp->buttress.ref_clk - 170) * 4 / 10;
+	dev_dbg(dev, "ref_clk = %u clk_freqrange = %u",
+		isp->buttress.ref_clk, cfg_clk_freqrange);
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_CFGCLKFREQRANGE,
+			    cfg_clk_freqrange, 0, 8);
+
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_DFT_CTRL2, 0x1, 4, 1);
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_DFT_CTRL2, 0x1, 8, 1);
+
+	return 0;
+}
+
+static void ipu6_isys_dwc_phy_aggr_setup(struct ipu6_isys *isys, u32 master,
+					 u32 slave, u32 mbps)
+{
+	/* Config mastermacro */
+	dwc_dphy_ifc_write_mask(isys, master, 0x133, 0x1, 0, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x133, 0x0, 0, 1);
+
+	/* Config master PHY clk lane to drive long channel clk */
+	dwc_dphy_ifc_write_mask(isys, master, 0x307, 0x1, 2, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x307, 0x0, 2, 1);
+
+	/* Config both PHYs data lanes to get clk from long channel */
+	dwc_dphy_ifc_write_mask(isys, master, 0x508, 0x1, 5, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x508, 0x1, 5, 1);
+	dwc_dphy_ifc_write_mask(isys, master, 0x708, 0x1, 5, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x708, 0x1, 5, 1);
+
+	/* Config slave PHY clk lane to bypass long channel clk to DDR clk */
+	dwc_dphy_ifc_write_mask(isys, master, 0x308, 0x0, 3, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x308, 0x1, 3, 1);
+
+	/* Override slave PHY clk lane enable (DPHYRXCLK_CLL_demux module) */
+	dwc_dphy_ifc_write_mask(isys, slave, 0xe0, 0x3, 0, 2);
+
+	/* Override slave PHY DDR clk lane enable (DPHYHSRX_div124 module) */
+	dwc_dphy_ifc_write_mask(isys, slave, 0xe1, 0x1, 1, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x307, 0x1, 3, 1);
+
+	/* Turn off slave PHY LP-RX clk lane */
+	dwc_dphy_ifc_write_mask(isys, slave, 0x304, 0x1, 7, 1);
+	dwc_dphy_ifc_write_mask(isys, slave, 0x305, 0xa, 0, 5);
+}
+
+#define PHY_E	4
+static int ipu6_isys_dwc_phy_powerup_ack(struct ipu6_isys *isys, u32 phy_id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	u32 rescal_done;
+	int ret;
+
+	ret = dwc_dphy_pwr_up(isys, phy_id);
+	if (ret != 0) {
+		dev_err(dev, "Dphy %u power up failed(%d)", phy_id, ret);
+		return ret;
+	}
+
+	/* reset forcerxmode */
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_DFT_CTRL2, 0, 4, 1);
+	dwc_dphy_write_mask(isys, phy_id, IPU6_DWC_DPHY_DFT_CTRL2, 0, 8, 1);
+
+	dev_dbg(dev, "Dphy %u is ready!", phy_id);
+
+	if (phy_id != PHY_E || isys->phy_termcal_val)
+		return 0;
+
+	usleep_range(100, 200);
+	rescal_done = dwc_dphy_ifc_read_mask(isys, phy_id, 0x221, 7, 1);
+	if (rescal_done) {
+		isys->phy_termcal_val = dwc_dphy_ifc_read_mask(isys, phy_id,
+							       0x220, 2, 4);
+		dev_dbg(dev, "termcal done with value = %u",
+			isys->phy_termcal_val);
+	}
+
+	return 0;
+}
+
+static void ipu6_isys_dwc_phy_reset(struct ipu6_isys *isys, u32 phy_id)
+{
+	dev_dbg(&isys->adev->auxdev.dev, "Reset phy %u", phy_id);
+
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_SHUTDOWNZ, 0);
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_RSTZ, 0);
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_TEST_IFC_ACCESS_MODE,
+		       TEST_IFC_ACCESS_MODE_FSM);
+	dwc_dphy_write(isys, phy_id, IPU6_DWC_DPHY_TEST_IFC_REQ,
+		       TEST_IFC_REQ_RESET);
+}
+
+int ipu6_isys_dwc_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	u32 phy_id, primary, secondary;
+	u32 nlanes, port, mbps;
+	s64 link_freq;
+	int ret;
+
+	port = cfg->port;
+
+	if (!isys_base || port >= isys->pdata->ipdata->csi2.nports) {
+		dev_warn(dev, "invalid port ID %d\n", port);
+		return -EINVAL;
+	}
+
+	nlanes = cfg->nlanes;
+	/* only port 0, 2 and 4 support 4 lanes */
+	if (nlanes == 4 && port % 2) {
+		dev_err(dev, "invalid csi-port %u with %u lanes\n", port,
+			nlanes);
+		return -EINVAL;
+	}
+
+	link_freq = ipu6_isys_csi2_get_link_freq(&isys->csi2[port]);
+	if (link_freq < 0) {
+		dev_err(dev, "get link freq failed(%lld).\n", link_freq);
+		return link_freq;
+	}
+
+	mbps = div_u64(link_freq, 500000);
+
+	phy_id = port;
+	primary = port & ~1;
+	secondary = primary + 1;
+	if (on) {
+		if (nlanes == 4) {
+			dev_dbg(dev, "config phy %u and %u in aggr mode\n",
+				primary, secondary);
+
+			ipu6_isys_dwc_phy_reset(isys, primary);
+			ipu6_isys_dwc_phy_reset(isys, secondary);
+			ipu6_isys_dwc_phy_aggr_setup(isys, primary,
+						     secondary, mbps);
+
+			ret = ipu6_isys_dwc_phy_config(isys, primary, mbps);
+			if (ret)
+				return ret;
+			ret = ipu6_isys_dwc_phy_config(isys, secondary, mbps);
+			if (ret)
+				return ret;
+
+			ret = ipu6_isys_dwc_phy_powerup_ack(isys, primary);
+			if (ret)
+				return ret;
+
+			ret = ipu6_isys_dwc_phy_powerup_ack(isys, secondary);
+			return ret;
+		}
+
+		dev_dbg(dev, "config phy %u with %u lanes in non-aggr mode\n",
+			phy_id, nlanes);
+
+		ipu6_isys_dwc_phy_reset(isys, phy_id);
+		ret = ipu6_isys_dwc_phy_config(isys, phy_id, mbps);
+		if (ret)
+			return ret;
+
+		ret = ipu6_isys_dwc_phy_powerup_ack(isys, phy_id);
+		return ret;
+	}
+
+	if (nlanes == 4) {
+		dev_dbg(dev, "Power down phy %u and phy %u for port %u\n",
+			primary, secondary, port);
+		ipu6_isys_dwc_phy_reset(isys, secondary);
+		ipu6_isys_dwc_phy_reset(isys, primary);
+
+		return 0;
+	}
+
+	dev_dbg(dev, "Powerdown phy %u with %u lanes\n", phy_id, nlanes);
+
+	ipu6_isys_dwc_phy_reset(isys, phy_id);
+
+	return 0;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-jsl-phy.c b/drivers/media/pci/intel/ipu6/ipu6-isys-jsl-phy.c
new file mode 100644
index 000000000000..c804291cfae9
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-jsl-phy.c
@@ -0,0 +1,242 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/device.h>
+#include <linux/io.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+
+/* only use BB0, BB2, BB4, and BB6 on PHY0 */
+#define IPU6SE_ISYS_PHY_BB_NUM		4
+#define IPU6SE_ISYS_PHY_0_BASE		0x10000
+
+#define PHY_CPHY_DLL_OVRD(x)		(0x100 + 0x100 * (x))
+#define PHY_CPHY_RX_CONTROL1(x)		(0x110 + 0x100 * (x))
+#define PHY_DPHY_CFG(x)			(0x148 + 0x100 * (x))
+#define PHY_BB_AFE_CONFIG(x)		(0x174 + 0x100 * (x))
+
+/*
+ * use port_cfg to configure that which data lanes used
+ * +---------+     +------+ +-----+
+ * | port0 x4<-----|      | |     |
+ * |         |     | port | |     |
+ * | port1 x2<-----|      | |     |
+ * |         |     |      <-| PHY |
+ * | port2 x4<-----|      | |     |
+ * |         |     |config| |     |
+ * | port3 x2<-----|      | |     |
+ * +---------+     +------+ +-----+
+ */
+static const unsigned int csi2_port_cfg[][3] = {
+	{0, 0, 0x1f}, /* no link */
+	{4, 0, 0x10}, /* x4 + x4 config */
+	{2, 0, 0x12}, /* x2 + x2 config */
+	{1, 0, 0x13}, /* x1 + x1 config */
+	{2, 1, 0x15}, /* x2x1 + x2x1 config */
+	{1, 1, 0x16}, /* x1x1 + x1x1 config */
+	{2, 2, 0x18}, /* x2x2 + x2x2 config */
+	{1, 2, 0x19} /* x1x2 + x1x2 config */
+};
+
+/* port, nlanes, bbindex, portcfg */
+static const unsigned int phy_port_cfg[][4] = {
+	/* sip0 */
+	{0, 1, 0, 0x15},
+	{0, 2, 0, 0x15},
+	{0, 4, 0, 0x15},
+	{0, 4, 2, 0x22},
+	/* sip1 */
+	{2, 1, 4, 0x15},
+	{2, 2, 4, 0x15},
+	{2, 4, 4, 0x15},
+	{2, 4, 6, 0x22}
+};
+
+static void ipu6_isys_csi2_phy_config_by_port(struct ipu6_isys *isys,
+					      unsigned int port,
+					      unsigned int nlanes)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *base = isys->adev->isp->base;
+	unsigned int bbnum;
+	u32 val, reg, i;
+
+	dev_dbg(dev, "port %u with %u lanes", port, nlanes);
+
+	/* only support <1.5Gbps */
+	for (i = 0; i < IPU6SE_ISYS_PHY_BB_NUM; i++) {
+		/* cphy_dll_ovrd.crcdc_fsm_dlane0 = 13 */
+		reg = IPU6SE_ISYS_PHY_0_BASE + PHY_CPHY_DLL_OVRD(i);
+		val = readl(base + reg);
+		val |= FIELD_PREP(GENMASK(6, 1), 13);
+		writel(val, base + reg);
+
+		/* cphy_rx_control1.en_crc1 = 1 */
+		reg = IPU6SE_ISYS_PHY_0_BASE + PHY_CPHY_RX_CONTROL1(i);
+		val = readl(base + reg);
+		val |= BIT(31);
+		writel(val, base + reg);
+
+		/* dphy_cfg.reserved = 1, .lden_from_dll_ovrd_0 = 1 */
+		reg = IPU6SE_ISYS_PHY_0_BASE + PHY_DPHY_CFG(i);
+		val = readl(base + reg);
+		val |= BIT(25) | BIT(26);
+		writel(val, base + reg);
+
+		/* cphy_dll_ovrd.lden_crcdc_fsm_dlane0 = 1 */
+		reg = IPU6SE_ISYS_PHY_0_BASE + PHY_CPHY_DLL_OVRD(i);
+		val = readl(base + reg);
+		val |= BIT(0);
+		writel(val, base + reg);
+	}
+
+	/* Front end config, use minimal channel loss */
+	for (i = 0; i < ARRAY_SIZE(phy_port_cfg); i++) {
+		if (phy_port_cfg[i][0] == port &&
+		    phy_port_cfg[i][1] == nlanes) {
+			bbnum = phy_port_cfg[i][2] / 2;
+			reg = IPU6SE_ISYS_PHY_0_BASE + PHY_BB_AFE_CONFIG(bbnum);
+			val = readl(base + reg);
+			val |= phy_port_cfg[i][3];
+			writel(val, base + reg);
+		}
+	}
+}
+
+static void ipu6_isys_csi2_rx_control(struct ipu6_isys *isys)
+{
+	void __iomem *base = isys->adev->isp->base;
+	u32 val, reg;
+
+	reg = CSI2_HUB_GPREG_SIP0_CSI_RX_A_CONTROL;
+	val = readl(base + reg);
+	val |= BIT(0);
+	writel(val, base + CSI2_HUB_GPREG_SIP0_CSI_RX_A_CONTROL);
+
+	reg = CSI2_HUB_GPREG_SIP0_CSI_RX_B_CONTROL;
+	val = readl(base + reg);
+	val |= BIT(0);
+	writel(val, base + CSI2_HUB_GPREG_SIP0_CSI_RX_B_CONTROL);
+
+	reg = CSI2_HUB_GPREG_SIP1_CSI_RX_A_CONTROL;
+	val = readl(base + reg);
+	val |= BIT(0);
+	writel(val, base + CSI2_HUB_GPREG_SIP1_CSI_RX_A_CONTROL);
+
+	reg = CSI2_HUB_GPREG_SIP1_CSI_RX_B_CONTROL;
+	val = readl(base + reg);
+	val |= BIT(0);
+	writel(val, base + CSI2_HUB_GPREG_SIP1_CSI_RX_B_CONTROL);
+}
+
+static int ipu6_isys_csi2_set_port_cfg(struct ipu6_isys *isys,
+				       unsigned int port, unsigned int nlanes)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	unsigned int sip = port / 2;
+	unsigned int index;
+
+	switch (nlanes) {
+	case 1:
+		index = 5;
+		break;
+	case 2:
+		index = 6;
+		break;
+	case 4:
+		index = 1;
+		break;
+	default:
+		dev_err(dev, "lanes nr %u is unsupported\n", nlanes);
+		return -EINVAL;
+	}
+
+	dev_dbg(dev, "port config for port %u with %u lanes\n",	port, nlanes);
+
+	writel(csi2_port_cfg[index][2],
+	       isys->pdata->base + CSI2_HUB_GPREG_SIP_FB_PORT_CFG(sip));
+
+	return 0;
+}
+
+static void
+ipu6_isys_csi2_set_timing(struct ipu6_isys *isys,
+			  const struct ipu6_isys_csi2_timing *timing,
+			  unsigned int port, unsigned int nlanes)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *reg;
+	u32 port_base;
+	u32 i;
+
+	port_base = (port % 2) ? CSI2_SIP_TOP_CSI_RX_PORT_BASE_1(port) :
+		CSI2_SIP_TOP_CSI_RX_PORT_BASE_0(port);
+
+	dev_dbg(dev, "set timing for port %u with %u lanes\n", port, nlanes);
+
+	reg = isys->pdata->base + port_base;
+	reg += CSI2_SIP_TOP_CSI_RX_DLY_CNT_TERMEN_CLANE;
+
+	writel(timing->ctermen, reg);
+
+	reg = isys->pdata->base + port_base;
+	reg += CSI2_SIP_TOP_CSI_RX_DLY_CNT_SETTLE_CLANE;
+	writel(timing->csettle, reg);
+
+	for (i = 0; i < nlanes; i++) {
+		reg = isys->pdata->base + port_base;
+		reg += CSI2_SIP_TOP_CSI_RX_DLY_CNT_TERMEN_DLANE(i);
+		writel(timing->dtermen, reg);
+
+		reg = isys->pdata->base + port_base;
+		reg += CSI2_SIP_TOP_CSI_RX_DLY_CNT_SETTLE_DLANE(i);
+		writel(timing->dsettle, reg);
+	}
+}
+
+#define DPHY_TIMER_INCR	0x28
+int ipu6_isys_jsl_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	int ret = 0;
+	u32 nlanes;
+	u32 port;
+
+	if (!on)
+		return 0;
+
+	port = cfg->port;
+	nlanes = cfg->nlanes;
+
+	if (!isys_base || port >= isys->pdata->ipdata->csi2.nports) {
+		dev_warn(dev, "invalid port ID %d\n", port);
+		return -EINVAL;
+	}
+
+	ipu6_isys_csi2_phy_config_by_port(isys, port, nlanes);
+
+	writel(DPHY_TIMER_INCR,
+	       isys->pdata->base + CSI2_HUB_GPREG_DPHY_TIMER_INCR);
+
+	/* set port cfg and rx timing */
+	ipu6_isys_csi2_set_timing(isys, timing, port, nlanes);
+
+	ret = ipu6_isys_csi2_set_port_cfg(isys, port, nlanes);
+	if (ret)
+		return ret;
+
+	ipu6_isys_csi2_rx_control(isys);
+
+	return 0;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-mcd-phy.c b/drivers/media/pci/intel/ipu6/ipu6-isys-mcd-phy.c
new file mode 100644
index 000000000000..71aa5009512a
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-mcd-phy.c
@@ -0,0 +1,720 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bits.h>
+#include <linux/container_of.h>
+#include <linux/device.h>
+#include <linux/iopoll.h>
+#include <linux/list.h>
+#include <linux/refcount.h>
+#include <linux/time64.h>
+
+#include <media/v4l2-async.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+
+#define CSI_REG_HUB_GPREG_PHY_CTL(id) (CSI_REG_BASE + 0x18008 + (id) * 0x8)
+#define CSI_REG_HUB_GPREG_PHY_CTL_RESET			BIT(4)
+#define CSI_REG_HUB_GPREG_PHY_CTL_PWR_EN		BIT(0)
+#define CSI_REG_HUB_GPREG_PHY_STATUS(id) (CSI_REG_BASE + 0x1800c + (id) * 0x8)
+#define CSI_REG_HUB_GPREG_PHY_POWER_ACK			BIT(0)
+#define CSI_REG_HUB_GPREG_PHY_READY			BIT(4)
+
+#define MCD_PHY_POWER_STATUS_TIMEOUT			(200 * USEC_PER_MSEC)
+
+/*
+ * bridge to phy in buttress reg map, each phy has 16 kbytes
+ * only 2 phys for TGL U and Y
+ */
+#define IPU6_ISYS_MCD_PHY_BASE(i)			(0x10000 + (i) * 0x4000)
+
+/*
+ *  There are 2 MCD DPHY instances on TGL and 1 MCD DPHY instance on ADL.
+ *  Each MCD PHY has 12-lanes which has 8 data lanes and 4 clock lanes.
+ *  CSI port 1, 3 (5, 7) can support max 2 data lanes.
+ *  CSI port 0, 2 (4, 6) can support max 4 data lanes.
+ *  PHY configurations are PPI based instead of port.
+ *  Left:
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | PPI     | PPI5    | PPI4    | PPI3    | PPI2   | PPI1    | PPI0     |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x4      | unused  | D3      | D2      | C0     | D0      | D1       |
+ *  |---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x2x2    | C1      | D0      | D1      | C0     | D0      | D1       |
+ *  ----------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x2x1    | C1      | D0      | unused  | C0     | D0      | D1       |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x1x1    | C1      | D0      | unused  | C0     | D0      | unused   |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x1x2    | C1      | D0      | D1      | C0     | D0      | unused   |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *
+ *  Right:
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | PPI     | PPI6    | PPI7    | PPI8    | PPI9   | PPI10   | PPI11    |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x4      | D1      | D0      | C2      | D2     | D3      | unused   |
+ *  |---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x2x2    | D1      | D0      | C2      | D1     | D0      | C3       |
+ *  ----------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x2x1    | D1      | D0      | C2      | unused | D0      | C3       |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x1x1    | unused  | D0      | C2      | unused | D0      | C3       |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *  |         |         |         |         |        |         |          |
+ *  | x1x2    | unused  | D0      | C2      | D1     | D0      | C3       |
+ *  +---------+---------+---------+---------+--------+---------+----------+
+ *
+ * ppi mapping per phy :
+ *
+ * x4 + x4:
+ * Left : port0 - PPI range {0, 1, 2, 3, 4}
+ * Right: port2 - PPI range {6, 7, 8, 9, 10}
+ *
+ * x4 + x2x2:
+ * Left: port0 - PPI range {0, 1, 2, 3, 4}
+ * Right: port2 - PPI range {6, 7, 8}, port3 - PPI range {9, 10, 11}
+ *
+ * x2x2 + x4:
+ * Left: port0 - PPI range {0, 1, 2}, port1 - PPI range {3, 4, 5}
+ * Right: port2 - PPI range {6, 7, 8, 9, 10}
+ *
+ * x2x2 + x2x2:
+ * Left : port0 - PPI range {0, 1, 2}, port1 - PPI range {3, 4, 5}
+ * Right: port2 - PPI range {6, 7, 8}, port3 - PPI range {9, 10, 11}
+ */
+
+struct phy_reg {
+	u32 reg;
+	u32 val;
+};
+
+static const struct phy_reg common_init_regs[] = {
+	/* for TGL-U, use 0x80000000 */
+	{0x00000040, 0x80000000},
+	{0x00000044, 0x00a80880},
+	{0x00000044, 0x00b80880},
+	{0x00000010, 0x0000078c},
+	{0x00000344, 0x2f4401e2},
+	{0x00000544, 0x924401e2},
+	{0x00000744, 0x594401e2},
+	{0x00000944, 0x624401e2},
+	{0x00000b44, 0xfc4401e2},
+	{0x00000d44, 0xc54401e2},
+	{0x00000f44, 0x034401e2},
+	{0x00001144, 0x8f4401e2},
+	{0x00001344, 0x754401e2},
+	{0x00001544, 0xe94401e2},
+	{0x00001744, 0xcb4401e2},
+	{0x00001944, 0xfa4401e2}
+};
+
+static const struct phy_reg x1_port0_config_regs[] = {
+	{0x00000694, 0xc80060fa},
+	{0x00000680, 0x3d4f78ea},
+	{0x00000690, 0x10a0140b},
+	{0x000006a8, 0xdf04010a},
+	{0x00000700, 0x57050060},
+	{0x00000710, 0x0030001c},
+	{0x00000738, 0x5f004444},
+	{0x0000073c, 0x78464204},
+	{0x00000748, 0x7821f940},
+	{0x0000074c, 0xb2000433},
+	{0x00000494, 0xfe6030fa},
+	{0x00000480, 0x29ef5ed0},
+	{0x00000490, 0x10a0540b},
+	{0x000004a8, 0x7a01010a},
+	{0x00000500, 0xef053460},
+	{0x00000510, 0xe030101c},
+	{0x00000538, 0xdf808444},
+	{0x0000053c, 0xc8422204},
+	{0x00000540, 0x0180088c},
+	{0x00000574, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x1_port1_config_regs[] = {
+	{0x00000c94, 0xc80060fa},
+	{0x00000c80, 0xcf47abea},
+	{0x00000c90, 0x10a0840b},
+	{0x00000ca8, 0xdf04010a},
+	{0x00000d00, 0x57050060},
+	{0x00000d10, 0x0030001c},
+	{0x00000d38, 0x5f004444},
+	{0x00000d3c, 0x78464204},
+	{0x00000d48, 0x7821f940},
+	{0x00000d4c, 0xb2000433},
+	{0x00000a94, 0xc91030fa},
+	{0x00000a80, 0x5a166ed0},
+	{0x00000a90, 0x10a0540b},
+	{0x00000aa8, 0x5d060100},
+	{0x00000b00, 0xef053460},
+	{0x00000b10, 0xa030101c},
+	{0x00000b38, 0xdf808444},
+	{0x00000b3c, 0xc8422204},
+	{0x00000b40, 0x0180088c},
+	{0x00000b74, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x1_port2_config_regs[] = {
+	{0x00001294, 0x28f000fa},
+	{0x00001280, 0x08130cea},
+	{0x00001290, 0x10a0140b},
+	{0x000012a8, 0xd704010a},
+	{0x00001300, 0x8d050060},
+	{0x00001310, 0x0030001c},
+	{0x00001338, 0xdf008444},
+	{0x0000133c, 0x78422204},
+	{0x00001348, 0x7821f940},
+	{0x0000134c, 0x5a000433},
+	{0x00001094, 0x2d20b0fa},
+	{0x00001080, 0xade75dd0},
+	{0x00001090, 0x10a0540b},
+	{0x000010a8, 0xb101010a},
+	{0x00001100, 0x33053460},
+	{0x00001110, 0x0030101c},
+	{0x00001138, 0xdf808444},
+	{0x0000113c, 0xc8422204},
+	{0x00001140, 0x8180088c},
+	{0x00001174, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x1_port3_config_regs[] = {
+	{0x00001894, 0xc80060fa},
+	{0x00001880, 0x0f90fd6a},
+	{0x00001890, 0x10a0840b},
+	{0x000018a8, 0xdf04010a},
+	{0x00001900, 0x57050060},
+	{0x00001910, 0x0030001c},
+	{0x00001938, 0x5f004444},
+	{0x0000193c, 0x78464204},
+	{0x00001948, 0x7821f940},
+	{0x0000194c, 0xb2000433},
+	{0x00001694, 0x3050d0fa},
+	{0x00001680, 0x0ef6d050},
+	{0x00001690, 0x10a0540b},
+	{0x000016a8, 0xe301010a},
+	{0x00001700, 0x69053460},
+	{0x00001710, 0xa030101c},
+	{0x00001738, 0xdf808444},
+	{0x0000173c, 0xc8422204},
+	{0x00001740, 0x0180088c},
+	{0x00001774, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x2_port0_config_regs[] = {
+	{0x00000694, 0xc80060fa},
+	{0x00000680, 0x3d4f78ea},
+	{0x00000690, 0x10a0140b},
+	{0x000006a8, 0xdf04010a},
+	{0x00000700, 0x57050060},
+	{0x00000710, 0x0030001c},
+	{0x00000738, 0x5f004444},
+	{0x0000073c, 0x78464204},
+	{0x00000748, 0x7821f940},
+	{0x0000074c, 0xb2000433},
+	{0x00000494, 0xc80060fa},
+	{0x00000480, 0x29ef5ed8},
+	{0x00000490, 0x10a0540b},
+	{0x000004a8, 0x7a01010a},
+	{0x00000500, 0xef053460},
+	{0x00000510, 0xe030101c},
+	{0x00000538, 0xdf808444},
+	{0x0000053c, 0xc8422204},
+	{0x00000540, 0x0180088c},
+	{0x00000574, 0x00000000},
+	{0x00000294, 0xc80060fa},
+	{0x00000280, 0xcb45b950},
+	{0x00000290, 0x10a0540b},
+	{0x000002a8, 0x8c01010a},
+	{0x00000300, 0xef053460},
+	{0x00000310, 0x8030101c},
+	{0x00000338, 0x41808444},
+	{0x0000033c, 0x32422204},
+	{0x00000340, 0x0180088c},
+	{0x00000374, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x2_port1_config_regs[] = {
+	{0x00000c94, 0xc80060fa},
+	{0x00000c80, 0xcf47abea},
+	{0x00000c90, 0x10a0840b},
+	{0x00000ca8, 0xdf04010a},
+	{0x00000d00, 0x57050060},
+	{0x00000d10, 0x0030001c},
+	{0x00000d38, 0x5f004444},
+	{0x00000d3c, 0x78464204},
+	{0x00000d48, 0x7821f940},
+	{0x00000d4c, 0xb2000433},
+	{0x00000a94, 0xc80060fa},
+	{0x00000a80, 0x5a166ed8},
+	{0x00000a90, 0x10a0540b},
+	{0x00000aa8, 0x7a01010a},
+	{0x00000b00, 0xef053460},
+	{0x00000b10, 0xa030101c},
+	{0x00000b38, 0xdf808444},
+	{0x00000b3c, 0xc8422204},
+	{0x00000b40, 0x0180088c},
+	{0x00000b74, 0x00000000},
+	{0x00000894, 0xc80060fa},
+	{0x00000880, 0x4d4f21d0},
+	{0x00000890, 0x10a0540b},
+	{0x000008a8, 0x5601010a},
+	{0x00000900, 0xef053460},
+	{0x00000910, 0x8030101c},
+	{0x00000938, 0xdf808444},
+	{0x0000093c, 0xc8422204},
+	{0x00000940, 0x0180088c},
+	{0x00000974, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x2_port2_config_regs[] = {
+	{0x00001294, 0xc80060fa},
+	{0x00001280, 0x08130cea},
+	{0x00001290, 0x10a0140b},
+	{0x000012a8, 0xd704010a},
+	{0x00001300, 0x8d050060},
+	{0x00001310, 0x0030001c},
+	{0x00001338, 0xdf008444},
+	{0x0000133c, 0x78422204},
+	{0x00001348, 0x7821f940},
+	{0x0000134c, 0x5a000433},
+	{0x00001094, 0xc80060fa},
+	{0x00001080, 0xade75dd8},
+	{0x00001090, 0x10a0540b},
+	{0x000010a8, 0xb101010a},
+	{0x00001100, 0x33053460},
+	{0x00001110, 0x0030101c},
+	{0x00001138, 0xdf808444},
+	{0x0000113c, 0xc8422204},
+	{0x00001140, 0x8180088c},
+	{0x00001174, 0x00000000},
+	{0x00000e94, 0xc80060fa},
+	{0x00000e80, 0x0fbf16d0},
+	{0x00000e90, 0x10a0540b},
+	{0x00000ea8, 0x7a01010a},
+	{0x00000f00, 0xf5053460},
+	{0x00000f10, 0xc030101c},
+	{0x00000f38, 0xdf808444},
+	{0x00000f3c, 0xc8422204},
+	{0x00000f40, 0x8180088c},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x2_port3_config_regs[] = {
+	{0x00001894, 0xc80060fa},
+	{0x00001880, 0x0f90fd6a},
+	{0x00001890, 0x10a0840b},
+	{0x000018a8, 0xdf04010a},
+	{0x00001900, 0x57050060},
+	{0x00001910, 0x0030001c},
+	{0x00001938, 0x5f004444},
+	{0x0000193c, 0x78464204},
+	{0x00001948, 0x7821f940},
+	{0x0000194c, 0xb2000433},
+	{0x00001694, 0xc80060fa},
+	{0x00001680, 0x0ef6d058},
+	{0x00001690, 0x10a0540b},
+	{0x000016a8, 0x7a01010a},
+	{0x00001700, 0x69053460},
+	{0x00001710, 0xa030101c},
+	{0x00001738, 0xdf808444},
+	{0x0000173c, 0xc8422204},
+	{0x00001740, 0x0180088c},
+	{0x00001774, 0x00000000},
+	{0x00001494, 0xc80060fa},
+	{0x00001480, 0xf9d34bd0},
+	{0x00001490, 0x10a0540b},
+	{0x000014a8, 0x7a01010a},
+	{0x00001500, 0x1b053460},
+	{0x00001510, 0x0030101c},
+	{0x00001538, 0xdf808444},
+	{0x0000153c, 0xc8422204},
+	{0x00001540, 0x8180088c},
+	{0x00001574, 0x00000000},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x4_port0_config_regs[] = {
+	{0x00000694, 0xc80060fa},
+	{0x00000680, 0x3d4f78fa},
+	{0x00000690, 0x10a0140b},
+	{0x000006a8, 0xdf04010a},
+	{0x00000700, 0x57050060},
+	{0x00000710, 0x0030001c},
+	{0x00000738, 0x5f004444},
+	{0x0000073c, 0x78464204},
+	{0x00000748, 0x7821f940},
+	{0x0000074c, 0xb2000433},
+	{0x00000494, 0xfe6030fa},
+	{0x00000480, 0x29ef5ed8},
+	{0x00000490, 0x10a0540b},
+	{0x000004a8, 0x7a01010a},
+	{0x00000500, 0xef053460},
+	{0x00000510, 0xe030101c},
+	{0x00000538, 0xdf808444},
+	{0x0000053c, 0xc8422204},
+	{0x00000540, 0x0180088c},
+	{0x00000574, 0x00000004},
+	{0x00000294, 0x23e030fa},
+	{0x00000280, 0xcb45b950},
+	{0x00000290, 0x10a0540b},
+	{0x000002a8, 0x8c01010a},
+	{0x00000300, 0xef053460},
+	{0x00000310, 0x8030101c},
+	{0x00000338, 0x41808444},
+	{0x0000033c, 0x32422204},
+	{0x00000340, 0x0180088c},
+	{0x00000374, 0x00000004},
+	{0x00000894, 0x5620b0fa},
+	{0x00000880, 0x4d4f21dc},
+	{0x00000890, 0x10a0540b},
+	{0x000008a8, 0x5601010a},
+	{0x00000900, 0xef053460},
+	{0x00000910, 0x8030101c},
+	{0x00000938, 0xdf808444},
+	{0x0000093c, 0xc8422204},
+	{0x00000940, 0x0180088c},
+	{0x00000974, 0x00000004},
+	{0x00000a94, 0xc91030fa},
+	{0x00000a80, 0x5a166ecc},
+	{0x00000a90, 0x10a0540b},
+	{0x00000aa8, 0x5d01010a},
+	{0x00000b00, 0xef053460},
+	{0x00000b10, 0xa030101c},
+	{0x00000b38, 0xdf808444},
+	{0x00000b3c, 0xc8422204},
+	{0x00000b40, 0x0180088c},
+	{0x00000b74, 0x00000004},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x4_port1_config_regs[] = {
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x4_port2_config_regs[] = {
+	{0x00001294, 0x28f000fa},
+	{0x00001280, 0x08130cfa},
+	{0x00001290, 0x10c0140b},
+	{0x000012a8, 0xd704010a},
+	{0x00001300, 0x8d050060},
+	{0x00001310, 0x0030001c},
+	{0x00001338, 0xdf008444},
+	{0x0000133c, 0x78422204},
+	{0x00001348, 0x7821f940},
+	{0x0000134c, 0x5a000433},
+	{0x00001094, 0x2d20b0fa},
+	{0x00001080, 0xade75dd8},
+	{0x00001090, 0x10a0540b},
+	{0x000010a8, 0xb101010a},
+	{0x00001100, 0x33053460},
+	{0x00001110, 0x0030101c},
+	{0x00001138, 0xdf808444},
+	{0x0000113c, 0xc8422204},
+	{0x00001140, 0x8180088c},
+	{0x00001174, 0x00000004},
+	{0x00000e94, 0xd308d0fa},
+	{0x00000e80, 0x0fbf16d0},
+	{0x00000e90, 0x10a0540b},
+	{0x00000ea8, 0x2c01010a},
+	{0x00000f00, 0xf5053460},
+	{0x00000f10, 0xc030101c},
+	{0x00000f38, 0xdf808444},
+	{0x00000f3c, 0xc8422204},
+	{0x00000f40, 0x8180088c},
+	{0x00000f74, 0x00000004},
+	{0x00001494, 0x136850fa},
+	{0x00001480, 0xf9d34bdc},
+	{0x00001490, 0x10a0540b},
+	{0x000014a8, 0x5a01010a},
+	{0x00001500, 0x1b053460},
+	{0x00001510, 0x0030101c},
+	{0x00001538, 0xdf808444},
+	{0x0000153c, 0xc8422204},
+	{0x00001540, 0x8180088c},
+	{0x00001574, 0x00000004},
+	{0x00001694, 0x3050d0fa},
+	{0x00001680, 0x0ef6d04c},
+	{0x00001690, 0x10a0540b},
+	{0x000016a8, 0xe301010a},
+	{0x00001700, 0x69053460},
+	{0x00001710, 0xa030101c},
+	{0x00001738, 0xdf808444},
+	{0x0000173c, 0xc8422204},
+	{0x00001740, 0x0180088c},
+	{0x00001774, 0x00000004},
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg x4_port3_config_regs[] = {
+	{0x00000000, 0x00000000}
+};
+
+static const struct phy_reg *x1_config_regs[4] = {
+	x1_port0_config_regs,
+	x1_port1_config_regs,
+	x1_port2_config_regs,
+	x1_port3_config_regs
+};
+
+static const struct phy_reg *x2_config_regs[4] = {
+	x2_port0_config_regs,
+	x2_port1_config_regs,
+	x2_port2_config_regs,
+	x2_port3_config_regs
+};
+
+static const struct phy_reg *x4_config_regs[4] = {
+	x4_port0_config_regs,
+	x4_port1_config_regs,
+	x4_port2_config_regs,
+	x4_port3_config_regs
+};
+
+static const struct phy_reg **config_regs[3] = {
+	x1_config_regs,
+	x2_config_regs,
+	x4_config_regs
+};
+
+static int ipu6_isys_mcd_phy_powerup_ack(struct ipu6_isys *isys, u8 id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	u32 val;
+	int ret;
+
+	val = readl(isys_base + CSI_REG_HUB_GPREG_PHY_CTL(id));
+	val |= CSI_REG_HUB_GPREG_PHY_CTL_PWR_EN;
+	writel(val, isys_base + CSI_REG_HUB_GPREG_PHY_CTL(id));
+
+	ret = readl_poll_timeout(isys_base + CSI_REG_HUB_GPREG_PHY_STATUS(id),
+				 val, val & CSI_REG_HUB_GPREG_PHY_POWER_ACK,
+				 200, MCD_PHY_POWER_STATUS_TIMEOUT);
+	if (ret)
+		dev_err(dev, "PHY%d powerup ack timeout", id);
+
+	return ret;
+}
+
+static int ipu6_isys_mcd_phy_powerdown_ack(struct ipu6_isys *isys, u8 id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	u32 val;
+	int ret;
+
+	writel(0, isys_base + CSI_REG_HUB_GPREG_PHY_CTL(id));
+	ret = readl_poll_timeout(isys_base + CSI_REG_HUB_GPREG_PHY_STATUS(id),
+				 val, !(val & CSI_REG_HUB_GPREG_PHY_POWER_ACK),
+				 200, MCD_PHY_POWER_STATUS_TIMEOUT);
+	if (ret)
+		dev_err(dev, "PHY%d powerdown ack timeout", id);
+
+	return ret;
+}
+
+static void ipu6_isys_mcd_phy_reset(struct ipu6_isys *isys, u8 id, bool assert)
+{
+	void __iomem *isys_base = isys->pdata->base;
+	u32 val;
+
+	val = readl(isys_base + CSI_REG_HUB_GPREG_PHY_CTL(id));
+	if (assert)
+		val |= CSI_REG_HUB_GPREG_PHY_CTL_RESET;
+	else
+		val &= ~(CSI_REG_HUB_GPREG_PHY_CTL_RESET);
+
+	writel(val, isys_base + CSI_REG_HUB_GPREG_PHY_CTL(id));
+}
+
+static int ipu6_isys_mcd_phy_ready(struct ipu6_isys *isys, u8 id)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	u32 val;
+	int ret;
+
+	ret = readl_poll_timeout(isys_base + CSI_REG_HUB_GPREG_PHY_STATUS(id),
+				 val, val & CSI_REG_HUB_GPREG_PHY_READY,
+				 200, MCD_PHY_POWER_STATUS_TIMEOUT);
+	if (ret)
+		dev_err(dev, "PHY%d ready ack timeout", id);
+
+	return ret;
+}
+
+static void ipu6_isys_mcd_phy_common_init(struct ipu6_isys *isys)
+{
+	struct ipu6_bus_device *adev = isys->adev;
+	struct ipu6_device *isp = adev->isp;
+	void __iomem *isp_base = isp->base;
+	struct sensor_async_sd *s_asd;
+	struct v4l2_async_connection *asc;
+	void __iomem *phy_base;
+	unsigned int i;
+	u8 phy_id;
+
+	list_for_each_entry(asc, &isys->notifier.done_list, asc_entry) {
+		s_asd = container_of(asc, struct sensor_async_sd, asc);
+		phy_id = s_asd->csi2.port / 4;
+		phy_base = isp_base + IPU6_ISYS_MCD_PHY_BASE(phy_id);
+
+		for (i = 0; i < ARRAY_SIZE(common_init_regs); i++)
+			writel(common_init_regs[i].val,
+			       phy_base + common_init_regs[i].reg);
+	}
+}
+
+static int ipu6_isys_driver_port_to_phy_port(struct ipu6_isys_csi2_config *cfg)
+{
+	int phy_port;
+	int ret;
+
+	if (!(cfg->nlanes == 4 || cfg->nlanes == 2 || cfg->nlanes == 1))
+		return -EINVAL;
+
+	/* B,F -> C0 A,E -> C1 C,G -> C2 D,H -> C4 */
+	/* normalize driver port number */
+	phy_port = cfg->port % 4;
+
+	/* swap port number only for A and B */
+	if (phy_port == 0)
+		phy_port = 1;
+	else if (phy_port == 1)
+		phy_port = 0;
+
+	ret = phy_port;
+
+	/* check validity per lane configuration */
+	if (cfg->nlanes == 4 && !(phy_port == 0 || phy_port == 2))
+		ret = -EINVAL;
+	else if ((cfg->nlanes == 2 || cfg->nlanes == 1) &&
+		 !(phy_port >= 0 && phy_port <= 3))
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int ipu6_isys_mcd_phy_config(struct ipu6_isys *isys)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_bus_device *adev = isys->adev;
+	const struct phy_reg **phy_config_regs;
+	struct ipu6_device *isp = adev->isp;
+	void __iomem *isp_base = isp->base;
+	struct sensor_async_sd *s_asd;
+	struct ipu6_isys_csi2_config cfg;
+	struct v4l2_async_connection *asc;
+	int phy_port, phy_id;
+	unsigned int i;
+	void __iomem *phy_base;
+
+	list_for_each_entry(asc, &isys->notifier.done_list, asc_entry) {
+		s_asd = container_of(asc, struct sensor_async_sd, asc);
+		cfg.port = s_asd->csi2.port;
+		cfg.nlanes = s_asd->csi2.nlanes;
+		phy_port = ipu6_isys_driver_port_to_phy_port(&cfg);
+		if (phy_port < 0) {
+			dev_err(dev, "invalid port %d for lane %d", cfg.port,
+				cfg.nlanes);
+			return -ENXIO;
+		}
+
+		phy_id = cfg.port / 4;
+		phy_base = isp_base + IPU6_ISYS_MCD_PHY_BASE(phy_id);
+		dev_dbg(dev, "port%d PHY%u lanes %u\n", cfg.port, phy_id,
+			cfg.nlanes);
+
+		phy_config_regs = config_regs[cfg.nlanes / 2];
+		cfg.port = phy_port;
+		for (i = 0; phy_config_regs[cfg.port][i].reg; i++)
+			writel(phy_config_regs[cfg.port][i].val,
+			       phy_base + phy_config_regs[cfg.port][i].reg);
+	}
+
+	return 0;
+}
+
+#define CSI_MCD_PHY_NUM		2
+static refcount_t phy_power_ref_count[CSI_MCD_PHY_NUM];
+
+int ipu6_isys_mcd_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	void __iomem *isys_base = isys->pdata->base;
+	u8 port, phy_id;
+	refcount_t *ref;
+	int ret;
+
+	port = cfg->port;
+	phy_id = port / 4;
+
+	ref = &phy_power_ref_count[phy_id];
+
+	dev_dbg(dev, "for phy %d port %d, lanes: %d\n", phy_id, port,
+		cfg->nlanes);
+
+	if (!isys_base || port >= isys->pdata->ipdata->csi2.nports) {
+		dev_warn(dev, "invalid port ID %d\n", port);
+		return -EINVAL;
+	}
+
+	if (on) {
+		if (refcount_read(ref)) {
+			dev_dbg(dev, "for phy %d is already UP", phy_id);
+			refcount_inc(ref);
+			return 0;
+		}
+
+		ret = ipu6_isys_mcd_phy_powerup_ack(isys, phy_id);
+		if (ret)
+			return ret;
+
+		ipu6_isys_mcd_phy_reset(isys, phy_id, 0);
+		ipu6_isys_mcd_phy_common_init(isys);
+
+		ret = ipu6_isys_mcd_phy_config(isys);
+		if (ret)
+			return ret;
+
+		ipu6_isys_mcd_phy_reset(isys, phy_id, 1);
+		ret = ipu6_isys_mcd_phy_ready(isys, phy_id);
+		if (ret)
+			return ret;
+
+		refcount_set(ref, 1);
+		return 0;
+	}
+
+	if (!refcount_dec_and_test(ref))
+		return 0;
+
+	return ipu6_isys_mcd_phy_powerdown_ack(isys, phy_id);
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-queue.c b/drivers/media/pci/intel/ipu6/ipu6-isys-queue.c
new file mode 100644
index 000000000000..aa2cf7287477
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-queue.c
@@ -0,0 +1,850 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+#include <linux/atomic.h>
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/lockdep.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-dma.h"
+#include "ipu6-fw-isys.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-video.h"
+
+static int ipu6_isys_buf_init(struct vb2_buffer *vb)
+{
+	struct ipu6_isys *isys = vb2_get_drv_priv(vb->vb2_queue);
+	struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
+	struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
+	struct ipu6_isys_video_buffer *ivb =
+		vb2_buffer_to_ipu6_isys_video_buffer(vvb);
+	int ret;
+
+	ret = ipu6_dma_map_sgtable(isys->adev, sg, DMA_TO_DEVICE, 0);
+	if (ret)
+		return ret;
+
+	ivb->dma_addr = sg_dma_address(sg->sgl);
+
+	return 0;
+}
+
+static void ipu6_isys_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct ipu6_isys *isys = vb2_get_drv_priv(vb->vb2_queue);
+	struct sg_table *sg = vb2_dma_sg_plane_desc(vb, 0);
+	struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
+	struct ipu6_isys_video_buffer *ivb =
+		vb2_buffer_to_ipu6_isys_video_buffer(vvb);
+
+	ivb->dma_addr = 0;
+	ipu6_dma_unmap_sgtable(isys->adev, sg, DMA_TO_DEVICE, 0);
+}
+
+static int ipu6_isys_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
+				 unsigned int *num_planes, unsigned int sizes[],
+				 struct device *alloc_devs[])
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	u32 size = ipu6_isys_get_data_size(av);
+
+	/* num_planes == 0: we're being called through VIDIOC_REQBUFS */
+	if (!*num_planes) {
+		sizes[0] = size;
+	} else if (sizes[0] < size) {
+		dev_dbg(dev, "%s: queue setup: size %u < %u\n",
+			av->vdev.name, sizes[0], size);
+		return -EINVAL;
+	}
+
+	*num_planes = 1;
+
+	return 0;
+}
+
+static int ipu6_isys_buf_prepare(struct vb2_buffer *vb)
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	u32 bytesperline = ipu6_isys_get_bytes_per_line(av);
+	u32 height = ipu6_isys_get_frame_height(av);
+	u32 size = ipu6_isys_get_data_size(av);
+
+	dev_dbg(dev, "buffer: %s: configured size %u, buffer size %lu\n",
+		av->vdev.name, size, vb2_plane_size(vb, 0));
+
+	if (size > vb2_plane_size(vb, 0))
+		return -EINVAL;
+
+	vb2_set_plane_payload(vb, 0, bytesperline * height);
+
+	return 0;
+}
+
+/*
+ * Queue a buffer list back to incoming or active queues. The buffers
+ * are removed from the buffer list.
+ */
+void ipu6_isys_buffer_list_queue(struct ipu6_isys_buffer_list *bl,
+				 unsigned long op_flags,
+				 enum vb2_buffer_state state)
+{
+	struct ipu6_isys_buffer *ib, *ib_safe;
+	unsigned long flags;
+	bool first = true;
+
+	if (!bl)
+		return;
+
+	WARN_ON_ONCE(!bl->nbufs);
+	WARN_ON_ONCE(op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE &&
+		     op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING);
+
+	list_for_each_entry_safe(ib, ib_safe, &bl->head, head) {
+		struct ipu6_isys_video *av;
+		struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+		struct ipu6_isys_queue *aq =
+			vb2_queue_to_isys_queue(vb->vb2_queue);
+		struct device *dev;
+
+		av = ipu6_isys_queue_to_video(aq);
+		dev = &av->isys->adev->auxdev.dev;
+		spin_lock_irqsave(&aq->lock, flags);
+		list_del(&ib->head);
+		if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_ACTIVE)
+			list_add(&ib->head, &aq->active);
+		else if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_INCOMING)
+			list_add_tail(&ib->head, &aq->incoming);
+		spin_unlock_irqrestore(&aq->lock, flags);
+
+		if (op_flags & IPU6_ISYS_BUFFER_LIST_FL_SET_STATE)
+			vb2_buffer_done(vb, state);
+
+		if (first) {
+			dev_dbg(dev,
+				"queue buf list %p flags %lx, s %d, %d bufs\n",
+				bl, op_flags, state, bl->nbufs);
+			first = false;
+		}
+
+		bl->nbufs--;
+	}
+
+	WARN_ON(bl->nbufs);
+}
+
+/*
+ * flush_firmware_streamon_fail() - Flush in cases where requests may
+ * have been queued to firmware and the *firmware streamon fails for a
+ * reason or another.
+ */
+static void flush_firmware_streamon_fail(struct ipu6_isys_stream *stream)
+{
+	struct device *dev = &stream->isys->adev->auxdev.dev;
+	struct ipu6_isys_queue *aq;
+	unsigned long flags;
+
+	lockdep_assert_held(&stream->mutex);
+
+	list_for_each_entry(aq, &stream->queues, node) {
+		struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+		struct ipu6_isys_buffer *ib, *ib_safe;
+
+		spin_lock_irqsave(&aq->lock, flags);
+		list_for_each_entry_safe(ib, ib_safe, &aq->active, head) {
+			struct vb2_buffer *vb =
+				ipu6_isys_buffer_to_vb2_buffer(ib);
+
+			list_del(&ib->head);
+			if (av->streaming) {
+				dev_dbg(dev,
+					"%s: queue buffer %u back to incoming\n",
+					av->vdev.name, vb->index);
+				/* Queue already streaming, return to driver. */
+				list_add(&ib->head, &aq->incoming);
+				continue;
+			}
+			/* Queue not yet streaming, return to user. */
+			dev_dbg(dev, "%s: return %u back to videobuf2\n",
+				av->vdev.name, vb->index);
+			vb2_buffer_done(ipu6_isys_buffer_to_vb2_buffer(ib),
+					VB2_BUF_STATE_QUEUED);
+		}
+		spin_unlock_irqrestore(&aq->lock, flags);
+	}
+}
+
+/*
+ * Attempt obtaining a buffer list from the incoming queues, a list of buffers
+ * that contains one entry from each video buffer queue. If a buffer can't be
+ * obtained from every queue, the buffers are returned back to the queue.
+ */
+static int buffer_list_get(struct ipu6_isys_stream *stream,
+			   struct ipu6_isys_buffer_list *bl)
+{
+	struct device *dev = &stream->isys->adev->auxdev.dev;
+	struct ipu6_isys_queue *aq;
+	unsigned long flags;
+	unsigned long buf_flag = IPU6_ISYS_BUFFER_LIST_FL_INCOMING;
+
+	bl->nbufs = 0;
+	INIT_LIST_HEAD(&bl->head);
+
+	list_for_each_entry(aq, &stream->queues, node) {
+		struct ipu6_isys_buffer *ib;
+
+		spin_lock_irqsave(&aq->lock, flags);
+		if (list_empty(&aq->incoming)) {
+			spin_unlock_irqrestore(&aq->lock, flags);
+			if (!list_empty(&bl->head))
+				ipu6_isys_buffer_list_queue(bl, buf_flag, 0);
+			return -ENODATA;
+		}
+
+		ib = list_last_entry(&aq->incoming,
+				     struct ipu6_isys_buffer, head);
+
+		dev_dbg(dev, "buffer: %s: buffer %u\n",
+			ipu6_isys_queue_to_video(aq)->vdev.name,
+			ipu6_isys_buffer_to_vb2_buffer(ib)->index);
+		list_del(&ib->head);
+		list_add(&ib->head, &bl->head);
+		spin_unlock_irqrestore(&aq->lock, flags);
+
+		bl->nbufs++;
+	}
+
+	dev_dbg(dev, "get buffer list %p, %u buffers\n", bl, bl->nbufs);
+
+	return 0;
+}
+
+static void
+ipu6_isys_buf_to_fw_frame_buf_pin(struct vb2_buffer *vb,
+				  struct ipu6_fw_isys_frame_buff_set_abi *set)
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
+	struct ipu6_isys_video_buffer *ivb =
+		vb2_buffer_to_ipu6_isys_video_buffer(vvb);
+
+	set->output_pins[aq->fw_output].addr = ivb->dma_addr;
+	set->output_pins[aq->fw_output].out_buf_id = vb->index + 1;
+}
+
+/*
+ * Convert a buffer list to a isys fw ABI framebuffer set. The
+ * buffer list is not modified.
+ */
+#define IPU6_ISYS_FRAME_NUM_THRESHOLD  (30)
+void
+ipu6_isys_buf_to_fw_frame_buf(struct ipu6_fw_isys_frame_buff_set_abi *set,
+			      struct ipu6_isys_stream *stream,
+			      struct ipu6_isys_buffer_list *bl)
+{
+	struct ipu6_isys_buffer *ib;
+
+	WARN_ON(!bl->nbufs);
+
+	set->send_irq_sof = 1;
+	set->send_resp_sof = 1;
+	set->send_irq_eof = 0;
+	set->send_resp_eof = 0;
+
+	if (stream->streaming)
+		set->send_irq_capture_ack = 0;
+	else
+		set->send_irq_capture_ack = 1;
+	set->send_irq_capture_done = 0;
+
+	set->send_resp_capture_ack = 1;
+	set->send_resp_capture_done = 1;
+	if (atomic_read(&stream->sequence) >= IPU6_ISYS_FRAME_NUM_THRESHOLD) {
+		set->send_resp_capture_ack = 0;
+		set->send_resp_capture_done = 0;
+	}
+
+	list_for_each_entry(ib, &bl->head, head) {
+		struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+
+		ipu6_isys_buf_to_fw_frame_buf_pin(vb, set);
+	}
+}
+
+/* Start streaming for real. The buffer list must be available. */
+static int ipu6_isys_stream_start(struct ipu6_isys_video *av,
+				  struct ipu6_isys_buffer_list *bl, bool error)
+{
+	struct ipu6_isys_stream *stream = av->stream;
+	struct device *dev = &stream->isys->adev->auxdev.dev;
+	struct ipu6_isys_buffer_list __bl;
+	int ret;
+
+	mutex_lock(&stream->isys->stream_mutex);
+	ret = ipu6_isys_video_set_streaming(av, 1, bl);
+	mutex_unlock(&stream->isys->stream_mutex);
+	if (ret)
+		goto out_requeue;
+
+	stream->streaming = 1;
+
+	bl = &__bl;
+
+	do {
+		struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL;
+		struct isys_fw_msgs *msg;
+		u16 send_type = IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE;
+
+		ret = buffer_list_get(stream, bl);
+		if (ret < 0)
+			break;
+
+		msg = ipu6_get_fw_msg_buf(stream);
+		if (!msg)
+			return -ENOMEM;
+
+		buf = &msg->fw_msg.frame;
+		ipu6_isys_buf_to_fw_frame_buf(buf, stream, bl);
+		ipu6_fw_isys_dump_frame_buff_set(dev, buf,
+						 stream->nr_output_pins);
+		ipu6_isys_buffer_list_queue(bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE,
+					    0);
+		ret = ipu6_fw_isys_complex_cmd(stream->isys,
+					       stream->stream_handle, buf,
+					       msg->dma_addr, sizeof(*buf),
+					       send_type);
+	} while (!WARN_ON(ret));
+
+	return 0;
+
+out_requeue:
+	if (bl && bl->nbufs)
+		ipu6_isys_buffer_list_queue(bl,
+					    IPU6_ISYS_BUFFER_LIST_FL_INCOMING |
+					    (error ?
+					    IPU6_ISYS_BUFFER_LIST_FL_SET_STATE :
+					     0), error ? VB2_BUF_STATE_ERROR :
+					    VB2_BUF_STATE_QUEUED);
+	flush_firmware_streamon_fail(stream);
+
+	return ret;
+}
+
+static void buf_queue(struct vb2_buffer *vb)
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct vb2_v4l2_buffer *vvb = to_vb2_v4l2_buffer(vb);
+	struct ipu6_isys_video_buffer *ivb =
+		vb2_buffer_to_ipu6_isys_video_buffer(vvb);
+	struct ipu6_isys_buffer *ib = &ivb->ib;
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct media_pipeline *media_pipe =
+		media_entity_pipeline(&av->vdev.entity);
+	struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL;
+	struct ipu6_isys_stream *stream = av->stream;
+	struct ipu6_isys_buffer_list bl;
+	struct isys_fw_msgs *msg;
+	unsigned long flags;
+	dma_addr_t dma;
+	int ret;
+
+	dev_dbg(dev, "queue buffer %u for %s\n", vb->index, av->vdev.name);
+
+	dma = ivb->dma_addr;
+	dev_dbg(dev, "iova: iova %pad\n", &dma);
+
+	spin_lock_irqsave(&aq->lock, flags);
+	list_add(&ib->head, &aq->incoming);
+	spin_unlock_irqrestore(&aq->lock, flags);
+
+	if (!media_pipe || !vb->vb2_queue->start_streaming_called) {
+		dev_dbg(dev, "media pipeline is not ready for %s\n",
+			av->vdev.name);
+		return;
+	}
+
+	mutex_lock(&stream->mutex);
+
+	if (stream->nr_streaming != stream->nr_queues) {
+		dev_dbg(dev, "not streaming yet, adding to incoming\n");
+		goto out;
+	}
+
+	/*
+	 * We just put one buffer to the incoming list of this queue
+	 * (above). Let's see whether all queues in the pipeline would
+	 * have a buffer.
+	 */
+	ret = buffer_list_get(stream, &bl);
+	if (ret < 0) {
+		dev_dbg(dev, "No buffers available\n");
+		goto out;
+	}
+
+	msg = ipu6_get_fw_msg_buf(stream);
+	if (!msg) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	buf = &msg->fw_msg.frame;
+	ipu6_isys_buf_to_fw_frame_buf(buf, stream, &bl);
+	ipu6_fw_isys_dump_frame_buff_set(dev, buf, stream->nr_output_pins);
+
+	if (!stream->streaming) {
+		ret = ipu6_isys_stream_start(av, &bl, true);
+		if (ret)
+			dev_err(dev, "stream start failed.\n");
+		goto out;
+	}
+
+	/*
+	 * We must queue the buffers in the buffer list to the
+	 * appropriate video buffer queues BEFORE passing them to the
+	 * firmware since we could get a buffer event back before we
+	 * have queued them ourselves to the active queue.
+	 */
+	ipu6_isys_buffer_list_queue(&bl, IPU6_ISYS_BUFFER_LIST_FL_ACTIVE, 0);
+
+	ret = ipu6_fw_isys_complex_cmd(stream->isys, stream->stream_handle,
+				       buf, msg->dma_addr, sizeof(*buf),
+				       IPU6_FW_ISYS_SEND_TYPE_STREAM_CAPTURE);
+	if (ret < 0)
+		dev_err(dev, "send stream capture failed\n");
+
+out:
+	mutex_unlock(&stream->mutex);
+}
+
+static int ipu6_isys_link_fmt_validate(struct ipu6_isys_queue *aq)
+{
+	struct v4l2_mbus_framefmt format;
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct media_pad *remote_pad =
+		media_pad_remote_pad_first(av->vdev.entity.pads);
+	struct v4l2_subdev *sd;
+	u32 r_stream, code;
+	int ret;
+
+	if (!remote_pad)
+		return -ENOTCONN;
+
+	sd = media_entity_to_v4l2_subdev(remote_pad->entity);
+	r_stream = ipu6_isys_get_src_stream_by_src_pad(sd, remote_pad->index);
+
+	ret = ipu6_isys_get_stream_pad_fmt(sd, remote_pad->index, r_stream,
+					   &format);
+
+	if (ret) {
+		dev_dbg(dev, "failed to get %s: pad %d, stream:%d format\n",
+			sd->entity.name, remote_pad->index, r_stream);
+		return ret;
+	}
+
+	if (format.width != ipu6_isys_get_frame_width(av) ||
+	    format.height != ipu6_isys_get_frame_height(av)) {
+		dev_dbg(dev, "wrong width or height %ux%u (%ux%u expected)\n",
+			ipu6_isys_get_frame_width(av),
+			ipu6_isys_get_frame_height(av), format.width,
+			format.height);
+		return -EINVAL;
+	}
+
+	code = ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0)->code;
+	if (format.code != code) {
+		dev_dbg(dev, "wrong mbus code 0x%8.8x (0x%8.8x expected)\n",
+			code, format.code);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void return_buffers(struct ipu6_isys_queue *aq,
+			   enum vb2_buffer_state state)
+{
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct ipu6_isys_buffer *ib;
+	bool need_reset = false;
+	unsigned long flags;
+
+	spin_lock_irqsave(&aq->lock, flags);
+	while (!list_empty(&aq->incoming)) {
+		struct vb2_buffer *vb;
+
+		ib = list_first_entry(&aq->incoming, struct ipu6_isys_buffer,
+				      head);
+		vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+		list_del(&ib->head);
+		spin_unlock_irqrestore(&aq->lock, flags);
+
+		vb2_buffer_done(vb, state);
+
+		spin_lock_irqsave(&aq->lock, flags);
+	}
+
+	/*
+	 * Something went wrong (FW crash / HW hang / not all buffers
+	 * returned from isys) if there are still buffers queued in active
+	 * queue. We have to clean up places a bit.
+	 */
+	while (!list_empty(&aq->active)) {
+		struct vb2_buffer *vb;
+
+		ib = list_first_entry(&aq->active, struct ipu6_isys_buffer,
+				      head);
+		vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+
+		list_del(&ib->head);
+		spin_unlock_irqrestore(&aq->lock, flags);
+
+		vb2_buffer_done(vb, state);
+
+		spin_lock_irqsave(&aq->lock, flags);
+		need_reset = true;
+	}
+
+	spin_unlock_irqrestore(&aq->lock, flags);
+
+	if (need_reset) {
+		mutex_lock(&av->isys->mutex);
+		av->isys->need_reset = true;
+		mutex_unlock(&av->isys->mutex);
+	}
+}
+
+static void ipu6_isys_stream_cleanup(struct ipu6_isys_video *av)
+{
+	video_device_pipeline_stop(&av->vdev);
+	ipu6_isys_put_stream(av->stream);
+	av->stream = NULL;
+}
+
+static int start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	const struct ipu6_isys_pixelformat *pfmt =
+		ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0);
+	struct ipu6_isys_buffer_list __bl, *bl = NULL;
+	struct ipu6_isys_stream *stream;
+	struct media_entity *source_entity = NULL;
+	int nr_queues, ret;
+
+	dev_dbg(dev, "stream: %s: width %u, height %u, css pixelformat %u\n",
+		av->vdev.name, ipu6_isys_get_frame_width(av),
+		ipu6_isys_get_frame_height(av), pfmt->css_pixelformat);
+
+	ret = ipu6_isys_setup_video(av, &source_entity, &nr_queues);
+	if (ret < 0) {
+		dev_dbg(dev, "failed to setup video\n");
+		goto out_return_buffers;
+	}
+
+	ret = ipu6_isys_link_fmt_validate(aq);
+	if (ret) {
+		dev_dbg(dev,
+			"%s: link format validation failed (%d)\n",
+			av->vdev.name, ret);
+		goto out_pipeline_stop;
+	}
+
+	ret = ipu6_isys_fw_open(av->isys);
+	if (ret)
+		goto out_pipeline_stop;
+
+	stream = av->stream;
+	mutex_lock(&stream->mutex);
+	if (!stream->nr_streaming) {
+		ret = ipu6_isys_video_prepare_stream(av, source_entity,
+						     nr_queues);
+		if (ret)
+			goto out_fw_close;
+	}
+
+	stream->nr_streaming++;
+	dev_dbg(dev, "queue %u of %u\n", stream->nr_streaming,
+		stream->nr_queues);
+
+	list_add(&aq->node, &stream->queues);
+	ipu6_isys_configure_stream_watermark(av, true);
+	ipu6_isys_update_stream_watermark(av, true);
+
+	if (stream->nr_streaming != stream->nr_queues)
+		goto out;
+
+	bl = &__bl;
+	ret = buffer_list_get(stream, bl);
+	if (ret < 0) {
+		dev_warn(dev, "no buffer available, DRIVER BUG?\n");
+		goto out;
+	}
+
+	ret = ipu6_isys_stream_start(av, bl, false);
+	if (ret)
+		goto out_stream_start;
+
+out:
+	mutex_unlock(&stream->mutex);
+
+	return 0;
+
+out_stream_start:
+	ipu6_isys_update_stream_watermark(av, false);
+	list_del(&aq->node);
+	stream->nr_streaming--;
+
+out_fw_close:
+	mutex_unlock(&stream->mutex);
+	ipu6_isys_fw_close(av->isys);
+
+out_pipeline_stop:
+	ipu6_isys_stream_cleanup(av);
+
+out_return_buffers:
+	return_buffers(aq, VB2_BUF_STATE_QUEUED);
+
+	return ret;
+}
+
+static void stop_streaming(struct vb2_queue *q)
+{
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(q);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct ipu6_isys_stream *stream = av->stream;
+
+	mutex_lock(&stream->mutex);
+
+	ipu6_isys_update_stream_watermark(av, false);
+
+	mutex_lock(&av->isys->stream_mutex);
+	if (stream->nr_streaming == stream->nr_queues && stream->streaming)
+		ipu6_isys_video_set_streaming(av, 0, NULL);
+	mutex_unlock(&av->isys->stream_mutex);
+
+	stream->nr_streaming--;
+	list_del(&aq->node);
+	stream->streaming = 0;
+	mutex_unlock(&stream->mutex);
+
+	ipu6_isys_stream_cleanup(av);
+
+	return_buffers(aq, VB2_BUF_STATE_ERROR);
+
+	ipu6_isys_fw_close(av->isys);
+}
+
+static unsigned int
+get_sof_sequence_by_timestamp(struct ipu6_isys_stream *stream, u64 time)
+{
+	struct ipu6_isys *isys = stream->isys;
+	struct device *dev = &isys->adev->auxdev.dev;
+	unsigned int i;
+
+	/*
+	 * The timestamp is invalid as no TSC in some FPGA platform,
+	 * so get the sequence from pipeline directly in this case.
+	 */
+	if (time == 0)
+		return atomic_read(&stream->sequence) - 1;
+
+	for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++)
+		if (time == stream->seq[i].timestamp) {
+			dev_dbg(dev, "sof: using seq nr %u for ts %llu\n",
+				stream->seq[i].sequence, time);
+			return stream->seq[i].sequence;
+		}
+
+	for (i = 0; i < IPU6_ISYS_MAX_PARALLEL_SOF; i++)
+		dev_dbg(dev, "sof: sequence %u, timestamp value %llu\n",
+			stream->seq[i].sequence, stream->seq[i].timestamp);
+
+	return 0;
+}
+
+static u64 get_sof_ns_delta(struct ipu6_isys_video *av, u64 timestamp)
+{
+	struct ipu6_bus_device *adev = av->isys->adev;
+	struct ipu6_device *isp = adev->isp;
+	u64 delta, tsc_now;
+
+	ipu6_buttress_tsc_read(isp, &tsc_now);
+	if (!tsc_now)
+		return 0;
+
+	delta = tsc_now - timestamp;
+
+	return ipu6_buttress_tsc_ticks_to_ns(delta, isp);
+}
+
+static void
+ipu6_isys_buf_calc_sequence_time(struct ipu6_isys_buffer *ib, u64 time)
+{
+	struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct ipu6_isys_queue *aq = vb2_queue_to_isys_queue(vb->vb2_queue);
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct ipu6_isys_stream *stream = av->stream;
+	u64 ns;
+	u32 sequence;
+
+	ns = ktime_get_ns() - get_sof_ns_delta(av, time);
+	sequence = get_sof_sequence_by_timestamp(stream, time);
+
+	vbuf->vb2_buf.timestamp = ns;
+	vbuf->sequence = sequence;
+
+	dev_dbg(dev, "buf: %s: buffer done, CPU-timestamp:%lld, sequence:%d\n",
+		av->vdev.name, ktime_get_ns(), sequence);
+	dev_dbg(dev, "index:%d, vbuf timestamp:%lld\n", vb->index,
+		vbuf->vb2_buf.timestamp);
+}
+
+static void ipu6_isys_queue_buf_done(struct ipu6_isys_buffer *ib)
+{
+	struct vb2_buffer *vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+
+	if (atomic_read(&ib->str2mmio_flag)) {
+		vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
+		/*
+		 * Operation on buffer is ended with error and will be reported
+		 * to the userspace when it is de-queued
+		 */
+		atomic_set(&ib->str2mmio_flag, 0);
+	} else {
+		vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
+	}
+}
+
+static void
+ipu6_stream_buf_ready(struct ipu6_isys_stream *stream, u8 pin_id, u32 pin_addr,
+		      u64 time, bool error_check)
+{
+	struct ipu6_isys_queue *aq = stream->output_pins_queue[pin_id];
+	struct ipu6_isys *isys = stream->isys;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct ipu6_isys_buffer *ib;
+	struct vb2_buffer *vb;
+	unsigned long flags;
+	bool first = true;
+	struct vb2_v4l2_buffer *buf;
+
+	spin_lock_irqsave(&aq->lock, flags);
+	if (list_empty(&aq->active)) {
+		spin_unlock_irqrestore(&aq->lock, flags);
+		dev_err(dev, "active queue empty\n");
+		return;
+	}
+
+	list_for_each_entry_reverse(ib, &aq->active, head) {
+		struct ipu6_isys_video_buffer *ivb;
+		struct vb2_v4l2_buffer *vvb;
+		dma_addr_t addr;
+
+		vb = ipu6_isys_buffer_to_vb2_buffer(ib);
+		vvb = to_vb2_v4l2_buffer(vb);
+		ivb = vb2_buffer_to_ipu6_isys_video_buffer(vvb);
+		addr = ivb->dma_addr;
+
+		if (pin_addr != addr) {
+			if (first)
+				dev_err(dev, "Unexpected buffer address %pad\n",
+					&addr);
+			first = false;
+			continue;
+		}
+
+		if (error_check) {
+			/*
+			 * Check for error message:
+			 * 'IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO'
+			 */
+			atomic_set(&ib->str2mmio_flag, 1);
+		}
+		dev_dbg(dev, "buffer: found buffer %pad\n", &addr);
+
+		buf = to_vb2_v4l2_buffer(vb);
+		buf->field = V4L2_FIELD_NONE;
+
+		list_del(&ib->head);
+		spin_unlock_irqrestore(&aq->lock, flags);
+
+		ipu6_isys_buf_calc_sequence_time(ib, time);
+
+		ipu6_isys_queue_buf_done(ib);
+
+		return;
+	}
+
+	dev_err(dev, "Failed to find a matching video buffer\n");
+
+	spin_unlock_irqrestore(&aq->lock, flags);
+}
+
+void ipu6_isys_queue_buf_ready(struct ipu6_isys_stream *stream,
+			       struct ipu6_fw_isys_resp_info_abi *info)
+{
+	u64 time = (u64)info->timestamp[1] << 32 | info->timestamp[0];
+	bool err = info->error_info.error == IPU6_FW_ISYS_ERROR_HW_REPORTED_STR2MMIO;
+
+	ipu6_stream_buf_ready(stream, info->pin_id, info->pin.addr, time, err);
+}
+
+static const struct vb2_ops ipu6_isys_queue_ops = {
+	.queue_setup = ipu6_isys_queue_setup,
+	.buf_init = ipu6_isys_buf_init,
+	.buf_prepare = ipu6_isys_buf_prepare,
+	.buf_cleanup = ipu6_isys_buf_cleanup,
+	.start_streaming = start_streaming,
+	.stop_streaming = stop_streaming,
+	.buf_queue = buf_queue,
+};
+
+int ipu6_isys_queue_init(struct ipu6_isys_queue *aq)
+{
+	struct ipu6_isys *isys = ipu6_isys_queue_to_video(aq)->isys;
+	struct ipu6_isys_video *av = ipu6_isys_queue_to_video(aq);
+	struct ipu6_bus_device *adev = isys->adev;
+	int ret;
+
+	/* no support for userptr */
+	if (!aq->vbq.io_modes)
+		aq->vbq.io_modes = VB2_MMAP | VB2_DMABUF;
+
+	aq->vbq.drv_priv = isys;
+	aq->vbq.ops = &ipu6_isys_queue_ops;
+	aq->vbq.lock = &av->mutex;
+	aq->vbq.mem_ops = &vb2_dma_sg_memops;
+	aq->vbq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	aq->vbq.min_queued_buffers = 1;
+	aq->vbq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+
+	ret = vb2_queue_init(&aq->vbq);
+	if (ret)
+		return ret;
+
+	aq->vbq.dev = &adev->isp->pdev->dev;
+	spin_lock_init(&aq->lock);
+	INIT_LIST_HEAD(&aq->active);
+	INIT_LIST_HEAD(&aq->incoming);
+
+	return 0;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-queue.h b/drivers/media/pci/intel/ipu6/ipu6-isys-queue.h
new file mode 100644
index 000000000000..844dfda15ab6
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-queue.h
@@ -0,0 +1,71 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_ISYS_QUEUE_H
+#define IPU6_ISYS_QUEUE_H
+
+#include <linux/container_of.h>
+#include <linux/atomic.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/spinlock_types.h>
+
+#include <media/videobuf2-v4l2.h>
+
+#include "ipu6-fw-isys.h"
+#include "ipu6-isys-video.h"
+
+struct ipu6_isys_stream;
+
+struct ipu6_isys_queue {
+	struct vb2_queue vbq;
+	struct list_head node;
+	spinlock_t lock; /* Protects active and incoming lists */
+	struct list_head active;
+	struct list_head incoming;
+	unsigned int fw_output;
+};
+
+struct ipu6_isys_buffer {
+	struct list_head head;
+	atomic_t str2mmio_flag;
+};
+
+struct ipu6_isys_video_buffer {
+	struct vb2_v4l2_buffer vb_v4l2;
+	struct ipu6_isys_buffer ib;
+	dma_addr_t dma_addr;
+};
+
+#define IPU6_ISYS_BUFFER_LIST_FL_INCOMING	BIT(0)
+#define IPU6_ISYS_BUFFER_LIST_FL_ACTIVE	BIT(1)
+#define IPU6_ISYS_BUFFER_LIST_FL_SET_STATE	BIT(2)
+
+struct ipu6_isys_buffer_list {
+	struct list_head head;
+	unsigned int nbufs;
+};
+
+#define vb2_queue_to_isys_queue(__vb2) \
+	container_of(__vb2, struct ipu6_isys_queue, vbq)
+
+#define ipu6_isys_to_isys_video_buffer(__ib) \
+	container_of(__ib, struct ipu6_isys_video_buffer, ib)
+
+#define vb2_buffer_to_ipu6_isys_video_buffer(__vvb) \
+	container_of(__vvb, struct ipu6_isys_video_buffer, vb_v4l2)
+
+#define ipu6_isys_buffer_to_vb2_buffer(__ib) \
+	(&ipu6_isys_to_isys_video_buffer(__ib)->vb_v4l2.vb2_buf)
+
+void ipu6_isys_buffer_list_queue(struct ipu6_isys_buffer_list *bl,
+				 unsigned long op_flags,
+				 enum vb2_buffer_state state);
+void
+ipu6_isys_buf_to_fw_frame_buf(struct ipu6_fw_isys_frame_buff_set_abi *set,
+			      struct ipu6_isys_stream *stream,
+			      struct ipu6_isys_buffer_list *bl);
+void ipu6_isys_queue_buf_ready(struct ipu6_isys_stream *stream,
+			       struct ipu6_fw_isys_resp_info_abi *info);
+int ipu6_isys_queue_init(struct ipu6_isys_queue *aq);
+#endif /* IPU6_ISYS_QUEUE_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.c b/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.c
new file mode 100644
index 000000000000..0a06de5c739c
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.c
@@ -0,0 +1,403 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/minmax.h>
+
+#include <media/media-entity.h>
+#include <media/mipi-csi2.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-subdev.h"
+
+unsigned int ipu6_isys_mbus_code_to_bpp(u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_RGB888_1X24:
+	case MEDIA_BUS_FMT_META_24:
+		return 24;
+	case MEDIA_BUS_FMT_RGB565_1X16:
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+	case MEDIA_BUS_FMT_META_16:
+		return 16;
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+	case MEDIA_BUS_FMT_SGRBG12_1X12:
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+	case MEDIA_BUS_FMT_META_12:
+		return 12;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+	case MEDIA_BUS_FMT_META_10:
+		return 10;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+	case MEDIA_BUS_FMT_META_8:
+		return 8;
+	default:
+		WARN_ON(1);
+		return 8;
+	}
+}
+
+unsigned int ipu6_isys_mbus_code_to_mipi(u32 code)
+{
+	switch (code) {
+	case MEDIA_BUS_FMT_RGB565_1X16:
+		return MIPI_CSI2_DT_RGB565;
+	case MEDIA_BUS_FMT_RGB888_1X24:
+		return MIPI_CSI2_DT_RGB888;
+	case MEDIA_BUS_FMT_UYVY8_1X16:
+	case MEDIA_BUS_FMT_YUYV8_1X16:
+		return MIPI_CSI2_DT_YUV422_8B;
+	case MEDIA_BUS_FMT_SBGGR16_1X16:
+	case MEDIA_BUS_FMT_SGBRG16_1X16:
+	case MEDIA_BUS_FMT_SGRBG16_1X16:
+	case MEDIA_BUS_FMT_SRGGB16_1X16:
+		return MIPI_CSI2_DT_RAW16;
+	case MEDIA_BUS_FMT_SBGGR12_1X12:
+	case MEDIA_BUS_FMT_SGBRG12_1X12:
+	case MEDIA_BUS_FMT_SGRBG12_1X12:
+	case MEDIA_BUS_FMT_SRGGB12_1X12:
+		return MIPI_CSI2_DT_RAW12;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		return MIPI_CSI2_DT_RAW10;
+	case MEDIA_BUS_FMT_SBGGR8_1X8:
+	case MEDIA_BUS_FMT_SGBRG8_1X8:
+	case MEDIA_BUS_FMT_SGRBG8_1X8:
+	case MEDIA_BUS_FMT_SRGGB8_1X8:
+		return MIPI_CSI2_DT_RAW8;
+	default:
+		/* return unavailable MIPI data type - 0x3f */
+		WARN_ON(1);
+		return 0x3f;
+	}
+}
+
+bool ipu6_isys_is_bayer_format(u32 code)
+{
+	switch (ipu6_isys_mbus_code_to_mipi(code)) {
+	case MIPI_CSI2_DT_RAW8:
+	case MIPI_CSI2_DT_RAW10:
+	case MIPI_CSI2_DT_RAW12:
+	case MIPI_CSI2_DT_RAW14:
+	case MIPI_CSI2_DT_RAW16:
+	case MIPI_CSI2_DT_RAW20:
+	case MIPI_CSI2_DT_RAW24:
+	case MIPI_CSI2_DT_RAW28:
+		return true;
+	default:
+		return false;
+	}
+}
+
+u32 ipu6_isys_convert_bayer_order(u32 code, int x, int y)
+{
+	static const u32 code_map[] = {
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MEDIA_BUS_FMT_SRGGB16_1X16,
+		MEDIA_BUS_FMT_SGRBG16_1X16,
+		MEDIA_BUS_FMT_SGBRG16_1X16,
+		MEDIA_BUS_FMT_SBGGR16_1X16,
+	};
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(code_map); i++)
+		if (code_map[i] == code)
+			break;
+
+	if (WARN_ON(i == ARRAY_SIZE(code_map)))
+		return code;
+
+	return code_map[i ^ (((y & 1) << 1) | (x & 1))];
+}
+
+int ipu6_isys_subdev_set_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *format)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_rect *crop;
+	u32 code = asd->supported_codes[0];
+	u32 other_pad, other_stream;
+	unsigned int i;
+	int ret;
+
+	/* No transcoding, source and sink formats must match. */
+	if ((sd->entity.pads[format->pad].flags & MEDIA_PAD_FL_SOURCE) &&
+	    sd->entity.num_pads > 1)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	format->format.width = clamp(format->format.width, IPU6_ISYS_MIN_WIDTH,
+				     IPU6_ISYS_MAX_WIDTH);
+	format->format.height = clamp(format->format.height,
+				      IPU6_ISYS_MIN_HEIGHT,
+				      IPU6_ISYS_MAX_HEIGHT);
+
+	for (i = 0; asd->supported_codes[i]; i++) {
+		if (asd->supported_codes[i] == format->format.code) {
+			code = asd->supported_codes[i];
+			break;
+		}
+	}
+	format->format.code = code;
+	format->format.field = V4L2_FIELD_NONE;
+
+	/* Store the format and propagate it to the source pad. */
+	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	if (!(sd->entity.pads[format->pad].flags & MEDIA_PAD_FL_SINK))
+		return 0;
+
+	/* propagate format to following source pad */
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
+	*fmt = format->format;
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+						    format->pad,
+						    format->stream,
+						    &other_pad,
+						    &other_stream);
+	if (ret)
+		return -EINVAL;
+
+	crop = v4l2_subdev_state_get_crop(state, other_pad, other_stream);
+	/* reset crop */
+	crop->left = 0;
+	crop->top = 0;
+	crop->width = fmt->width;
+	crop->height = fmt->height;
+
+	return 0;
+}
+
+int ipu6_isys_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct ipu6_isys_subdev *asd = to_ipu6_isys_subdev(sd);
+	const u32 *supported_codes = asd->supported_codes;
+	u32 index;
+
+	for (index = 0; supported_codes[index]; index++) {
+		if (index == code->index) {
+			code->code = supported_codes[index];
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int subdev_set_routing(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 4096,
+		.height = 3072,
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.field = V4L2_FIELD_NONE,
+	};
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	return v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+}
+
+int ipu6_isys_get_stream_pad_fmt(struct v4l2_subdev *sd, u32 pad, u32 stream,
+				 struct v4l2_mbus_framefmt *format)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_subdev_state *state;
+
+	if (!sd || !format)
+		return -EINVAL;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	fmt = v4l2_subdev_state_get_format(state, pad, stream);
+	if (fmt)
+		*format = *fmt;
+	v4l2_subdev_unlock_state(state);
+
+	return fmt ? 0 : -EINVAL;
+}
+
+int ipu6_isys_get_stream_pad_crop(struct v4l2_subdev *sd, u32 pad, u32 stream,
+				  struct v4l2_rect *crop)
+{
+	struct v4l2_subdev_state *state;
+	struct v4l2_rect *rect;
+
+	if (!sd || !crop)
+		return -EINVAL;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	rect = v4l2_subdev_state_get_crop(state, pad, stream);
+	if (rect)
+		*crop = *rect;
+	v4l2_subdev_unlock_state(state);
+
+	return rect ? 0 : -EINVAL;
+}
+
+u32 ipu6_isys_get_src_stream_by_src_pad(struct v4l2_subdev *sd, u32 pad)
+{
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev_route *routes;
+	unsigned int i;
+	u32 source_stream = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (!state)
+		return 0;
+
+	routes = state->routing.routes;
+	for (i = 0; i < state->routing.num_routes; i++) {
+		if (routes[i].source_pad == pad) {
+			source_stream = routes[i].source_stream;
+			break;
+		}
+	}
+
+	v4l2_subdev_unlock_state(state);
+
+	return source_stream;
+}
+
+static int ipu6_isys_subdev_init_state(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route route = {
+		.sink_pad = 0,
+		.sink_stream = 0,
+		.source_pad = 1,
+		.source_stream = 0,
+		.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+	};
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = 1,
+		.routes = &route,
+	};
+
+	return subdev_set_routing(sd, state, &routing);
+}
+
+int ipu6_isys_subdev_set_routing(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 enum v4l2_subdev_format_whence which,
+				 struct v4l2_subdev_krouting *routing)
+{
+	return subdev_set_routing(sd, state, routing);
+}
+
+static const struct v4l2_subdev_internal_ops ipu6_isys_subdev_internal_ops = {
+	.init_state = ipu6_isys_subdev_init_state,
+};
+
+int ipu6_isys_subdev_init(struct ipu6_isys_subdev *asd,
+			  const struct v4l2_subdev_ops *ops,
+			  unsigned int nr_ctrls,
+			  unsigned int num_sink_pads,
+			  unsigned int num_source_pads)
+{
+	unsigned int num_pads = num_sink_pads + num_source_pads;
+	unsigned int i;
+	int ret;
+
+	v4l2_subdev_init(&asd->sd, ops);
+
+	asd->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
+			 V4L2_SUBDEV_FL_HAS_EVENTS |
+			 V4L2_SUBDEV_FL_STREAMS;
+	asd->sd.owner = THIS_MODULE;
+	asd->sd.dev = &asd->isys->adev->auxdev.dev;
+	asd->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	asd->sd.internal_ops = &ipu6_isys_subdev_internal_ops;
+
+	asd->pad = devm_kcalloc(&asd->isys->adev->auxdev.dev, num_pads,
+				sizeof(*asd->pad), GFP_KERNEL);
+	if (!asd->pad)
+		return -ENOMEM;
+
+	for (i = 0; i < num_sink_pads; i++)
+		asd->pad[i].flags = MEDIA_PAD_FL_SINK |
+				    MEDIA_PAD_FL_MUST_CONNECT;
+
+	for (i = num_sink_pads; i < num_pads; i++)
+		asd->pad[i].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&asd->sd.entity, num_pads, asd->pad);
+	if (ret)
+		return ret;
+
+	if (asd->ctrl_init) {
+		ret = v4l2_ctrl_handler_init(&asd->ctrl_handler, nr_ctrls);
+		if (ret)
+			goto out_media_entity_cleanup;
+
+		asd->ctrl_init(&asd->sd);
+		if (asd->ctrl_handler.error) {
+			ret = asd->ctrl_handler.error;
+			goto out_v4l2_ctrl_handler_free;
+		}
+
+		asd->sd.ctrl_handler = &asd->ctrl_handler;
+	}
+
+	asd->source = -1;
+
+	return 0;
+
+out_v4l2_ctrl_handler_free:
+	v4l2_ctrl_handler_free(&asd->ctrl_handler);
+
+out_media_entity_cleanup:
+	media_entity_cleanup(&asd->sd.entity);
+
+	return ret;
+}
+
+void ipu6_isys_subdev_cleanup(struct ipu6_isys_subdev *asd)
+{
+	media_entity_cleanup(&asd->sd.entity);
+	v4l2_ctrl_handler_free(&asd->ctrl_handler);
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.h b/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.h
new file mode 100644
index 000000000000..268dfa01e903
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-subdev.h
@@ -0,0 +1,55 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_ISYS_SUBDEV_H
+#define IPU6_ISYS_SUBDEV_H
+
+#include <linux/container_of.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-subdev.h>
+
+struct ipu6_isys;
+
+struct ipu6_isys_subdev {
+	struct v4l2_subdev sd;
+	struct ipu6_isys *isys;
+	u32 const *supported_codes;
+	struct media_pad *pad;
+	struct v4l2_ctrl_handler ctrl_handler;
+	void (*ctrl_init)(struct v4l2_subdev *sd);
+	int source;	/* SSI stream source; -1 if unset */
+};
+
+#define to_ipu6_isys_subdev(__sd) \
+	container_of(__sd, struct ipu6_isys_subdev, sd)
+
+unsigned int ipu6_isys_mbus_code_to_bpp(u32 code);
+unsigned int ipu6_isys_mbus_code_to_mipi(u32 code);
+bool ipu6_isys_is_bayer_format(u32 code);
+u32 ipu6_isys_convert_bayer_order(u32 code, int x, int y);
+
+int ipu6_isys_subdev_set_fmt(struct v4l2_subdev *sd,
+			     struct v4l2_subdev_state *state,
+			     struct v4l2_subdev_format *fmt);
+int ipu6_isys_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_mbus_code_enum
+				    *code);
+u32 ipu6_isys_get_src_stream_by_src_pad(struct v4l2_subdev *sd, u32 pad);
+int ipu6_isys_get_stream_pad_fmt(struct v4l2_subdev *sd, u32 pad, u32 stream,
+				 struct v4l2_mbus_framefmt *format);
+int ipu6_isys_get_stream_pad_crop(struct v4l2_subdev *sd, u32 pad, u32 stream,
+				  struct v4l2_rect *crop);
+int ipu6_isys_subdev_set_routing(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 enum v4l2_subdev_format_whence which,
+				 struct v4l2_subdev_krouting *routing);
+int ipu6_isys_subdev_init(struct ipu6_isys_subdev *asd,
+			  const struct v4l2_subdev_ops *ops,
+			  unsigned int nr_ctrls,
+			  unsigned int num_sink_pads,
+			  unsigned int num_source_pads);
+void ipu6_isys_subdev_cleanup(struct ipu6_isys_subdev *asd);
+#endif /* IPU6_ISYS_SUBDEV_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-video.c b/drivers/media/pci/intel/ipu6/ipu6-isys-video.c
new file mode 100644
index 000000000000..24a2ef93474c
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-video.c
@@ -0,0 +1,1391 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/align.h>
+#include <linux/bits.h>
+#include <linux/bug.h>
+#include <linux/completion.h>
+#include <linux/container_of.h>
+#include <linux/device.h>
+#include <linux/list.h>
+#include <linux/math64.h>
+#include <linux/minmax.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pm_runtime.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-cpd.h"
+#include "ipu6-fw-isys.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-isys-queue.h"
+#include "ipu6-isys-video.h"
+#include "ipu6-platform-regs.h"
+
+const struct ipu6_isys_pixelformat ipu6_isys_pfmts[] = {
+	{ V4L2_PIX_FMT_SBGGR12, 16, 12, MEDIA_BUS_FMT_SBGGR12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SGBRG12, 16, 12, MEDIA_BUS_FMT_SGBRG12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SGRBG12, 16, 12, MEDIA_BUS_FMT_SGRBG12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SRGGB12, 16, 12, MEDIA_BUS_FMT_SRGGB12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SBGGR10, 16, 10, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SGBRG10, 16, 10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SGRBG10, 16, 10, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SRGGB10, 16, 10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16 },
+	{ V4L2_PIX_FMT_SBGGR8, 8, 8, MEDIA_BUS_FMT_SBGGR8_1X8,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW8 },
+	{ V4L2_PIX_FMT_SGBRG8, 8, 8, MEDIA_BUS_FMT_SGBRG8_1X8,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW8 },
+	{ V4L2_PIX_FMT_SGRBG8, 8, 8, MEDIA_BUS_FMT_SGRBG8_1X8,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW8 },
+	{ V4L2_PIX_FMT_SRGGB8, 8, 8, MEDIA_BUS_FMT_SRGGB8_1X8,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW8 },
+	{ V4L2_PIX_FMT_SBGGR12P, 12, 12, MEDIA_BUS_FMT_SBGGR12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW12 },
+	{ V4L2_PIX_FMT_SGBRG12P, 12, 12, MEDIA_BUS_FMT_SGBRG12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW12 },
+	{ V4L2_PIX_FMT_SGRBG12P, 12, 12, MEDIA_BUS_FMT_SGRBG12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW12 },
+	{ V4L2_PIX_FMT_SRGGB12P, 12, 12, MEDIA_BUS_FMT_SRGGB12_1X12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW12 },
+	{ V4L2_PIX_FMT_SBGGR10P, 10, 10, MEDIA_BUS_FMT_SBGGR10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW10 },
+	{ V4L2_PIX_FMT_SGBRG10P, 10, 10, MEDIA_BUS_FMT_SGBRG10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW10 },
+	{ V4L2_PIX_FMT_SGRBG10P, 10, 10, MEDIA_BUS_FMT_SGRBG10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW10 },
+	{ V4L2_PIX_FMT_SRGGB10P, 10, 10, MEDIA_BUS_FMT_SRGGB10_1X10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW10 },
+	{ V4L2_PIX_FMT_UYVY, 16, 16, MEDIA_BUS_FMT_UYVY8_1X16,
+	  IPU6_FW_ISYS_FRAME_FORMAT_UYVY},
+	{ V4L2_PIX_FMT_YUYV, 16, 16, MEDIA_BUS_FMT_YUYV8_1X16,
+	  IPU6_FW_ISYS_FRAME_FORMAT_YUYV},
+	{ V4L2_PIX_FMT_RGB565, 16, 16, MEDIA_BUS_FMT_RGB565_1X16,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RGB565 },
+	{ V4L2_PIX_FMT_BGR24, 24, 24, MEDIA_BUS_FMT_RGB888_1X24,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RGBA888 },
+	{ V4L2_META_FMT_GENERIC_8, 8, 8, MEDIA_BUS_FMT_META_8,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW8, true },
+	{ V4L2_META_FMT_GENERIC_CSI2_10, 10, 10, MEDIA_BUS_FMT_META_10,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW10, true },
+	{ V4L2_META_FMT_GENERIC_CSI2_12, 12, 12, MEDIA_BUS_FMT_META_12,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW12, true },
+	{ V4L2_META_FMT_GENERIC_CSI2_16, 16, 16, MEDIA_BUS_FMT_META_16,
+	  IPU6_FW_ISYS_FRAME_FORMAT_RAW16, true },
+};
+
+static int video_open(struct file *file)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+	struct ipu6_isys *isys = av->isys;
+	struct ipu6_bus_device *adev = isys->adev;
+
+	mutex_lock(&isys->mutex);
+	if (isys->need_reset) {
+		mutex_unlock(&isys->mutex);
+		dev_warn(&adev->auxdev.dev, "isys power cycle required\n");
+		return -EIO;
+	}
+	mutex_unlock(&isys->mutex);
+
+	return v4l2_fh_open(file);
+}
+
+const struct ipu6_isys_pixelformat *
+ipu6_isys_get_isys_format(u32 pixelformat, u32 type)
+{
+	const struct ipu6_isys_pixelformat *default_pfmt = NULL;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(ipu6_isys_pfmts); i++) {
+		const struct ipu6_isys_pixelformat *pfmt = &ipu6_isys_pfmts[i];
+
+		if (type && ((!pfmt->is_meta &&
+			      type != V4L2_BUF_TYPE_VIDEO_CAPTURE) ||
+			     (pfmt->is_meta &&
+			      type != V4L2_BUF_TYPE_META_CAPTURE)))
+			continue;
+
+		if (!default_pfmt)
+			default_pfmt = pfmt;
+
+		if (pfmt->pixelformat != pixelformat)
+			continue;
+
+		return pfmt;
+	}
+
+	return default_pfmt;
+}
+
+static int ipu6_isys_vidioc_querycap(struct file *file, void *fh,
+				     struct v4l2_capability *cap)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	strscpy(cap->driver, IPU6_ISYS_NAME, sizeof(cap->driver));
+	strscpy(cap->card, av->isys->media_dev.model, sizeof(cap->card));
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_enum_fmt(struct file *file, void *fh,
+				     struct v4l2_fmtdesc *f)
+{
+	unsigned int i, num_found;
+
+	for (i = 0, num_found = 0; i < ARRAY_SIZE(ipu6_isys_pfmts); i++) {
+		if ((ipu6_isys_pfmts[i].is_meta &&
+		     f->type != V4L2_BUF_TYPE_META_CAPTURE) ||
+		    (!ipu6_isys_pfmts[i].is_meta &&
+		     f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE))
+			continue;
+
+		if (f->mbus_code && f->mbus_code != ipu6_isys_pfmts[i].code)
+			continue;
+
+		if (num_found < f->index) {
+			num_found++;
+			continue;
+		}
+
+		f->flags = 0;
+		f->pixelformat = ipu6_isys_pfmts[i].pixelformat;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ipu6_isys_vidioc_enum_framesizes(struct file *file, void *fh,
+					    struct v4l2_frmsizeenum *fsize)
+{
+	unsigned int i;
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	for (i = 0; i < ARRAY_SIZE(ipu6_isys_pfmts); i++) {
+		if (fsize->pixel_format != ipu6_isys_pfmts[i].pixelformat)
+			continue;
+
+		fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+		fsize->stepwise.min_width = IPU6_ISYS_MIN_WIDTH;
+		fsize->stepwise.max_width = IPU6_ISYS_MAX_WIDTH;
+		fsize->stepwise.min_height = IPU6_ISYS_MIN_HEIGHT;
+		fsize->stepwise.max_height = IPU6_ISYS_MAX_HEIGHT;
+		fsize->stepwise.step_width = 2;
+		fsize->stepwise.step_height = 2;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ipu6_isys_vidioc_g_fmt_vid_cap(struct file *file, void *fh,
+				      struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	f->fmt.pix = av->pix_fmt;
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_g_fmt_meta_cap(struct file *file, void *fh,
+					   struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	f->fmt.meta = av->meta_fmt;
+
+	return 0;
+}
+
+static void ipu6_isys_try_fmt_cap(struct ipu6_isys_video *av, u32 type,
+				  u32 *format, u32 *width, u32 *height,
+				  u32 *bytesperline, u32 *sizeimage)
+{
+	const struct ipu6_isys_pixelformat *pfmt =
+		ipu6_isys_get_isys_format(*format, type);
+
+	*format = pfmt->pixelformat;
+	*width = clamp(*width, IPU6_ISYS_MIN_WIDTH, IPU6_ISYS_MAX_WIDTH);
+	*height = clamp(*height, IPU6_ISYS_MIN_HEIGHT, IPU6_ISYS_MAX_HEIGHT);
+
+	if (pfmt->bpp != pfmt->bpp_packed)
+		*bytesperline = *width * DIV_ROUND_UP(pfmt->bpp, BITS_PER_BYTE);
+	else
+		*bytesperline = DIV_ROUND_UP(*width * pfmt->bpp, BITS_PER_BYTE);
+
+	*bytesperline = ALIGN(*bytesperline, 64);
+
+	/*
+	 * (height + 1) * bytesperline due to a hardware issue: the DMA unit
+	 * is a power of two, and a line should be transferred as few units
+	 * as possible. The result is that up to line length more data than
+	 * the image size may be transferred to memory after the image.
+	 * Another limitation is the GDA allocation unit size. For low
+	 * resolution it gives a bigger number. Use larger one to avoid
+	 * memory corruption.
+	 */
+	*sizeimage = *bytesperline * *height +
+		max(*bytesperline,
+		    av->isys->pdata->ipdata->isys_dma_overshoot);
+}
+
+static void __ipu6_isys_vidioc_try_fmt_vid_cap(struct ipu6_isys_video *av,
+					       struct v4l2_format *f)
+{
+	ipu6_isys_try_fmt_cap(av, f->type, &f->fmt.pix.pixelformat,
+			      &f->fmt.pix.width, &f->fmt.pix.height,
+			      &f->fmt.pix.bytesperline, &f->fmt.pix.sizeimage);
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
+	f->fmt.pix.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	f->fmt.pix.quantization = V4L2_QUANTIZATION_DEFAULT;
+	f->fmt.pix.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+}
+
+static int ipu6_isys_vidioc_try_fmt_vid_cap(struct file *file, void *fh,
+					    struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	if (vb2_is_busy(&av->aq.vbq))
+		return -EBUSY;
+
+	__ipu6_isys_vidioc_try_fmt_vid_cap(av, f);
+
+	return 0;
+}
+
+static int __ipu6_isys_vidioc_try_fmt_meta_cap(struct ipu6_isys_video *av,
+					       struct v4l2_format *f)
+{
+	ipu6_isys_try_fmt_cap(av, f->type, &f->fmt.meta.dataformat,
+			      &f->fmt.meta.width, &f->fmt.meta.height,
+			      &f->fmt.meta.bytesperline,
+			      &f->fmt.meta.buffersize);
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_try_fmt_meta_cap(struct file *file, void *fh,
+					     struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	__ipu6_isys_vidioc_try_fmt_meta_cap(av, f);
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_s_fmt_vid_cap(struct file *file, void *fh,
+				      struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	ipu6_isys_vidioc_try_fmt_vid_cap(file, fh, f);
+	av->pix_fmt = f->fmt.pix;
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_s_fmt_meta_cap(struct file *file, void *fh,
+					   struct v4l2_format *f)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+
+	if (vb2_is_busy(&av->aq.vbq))
+		return -EBUSY;
+
+	ipu6_isys_vidioc_try_fmt_meta_cap(file, fh, f);
+	av->meta_fmt = f->fmt.meta;
+
+	return 0;
+}
+
+static int ipu6_isys_vidioc_reqbufs(struct file *file, void *priv,
+				    struct v4l2_requestbuffers *p)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+	int ret;
+
+	av->aq.vbq.is_multiplanar = V4L2_TYPE_IS_MULTIPLANAR(p->type);
+	av->aq.vbq.is_output = V4L2_TYPE_IS_OUTPUT(p->type);
+
+	ret = vb2_queue_change_type(&av->aq.vbq, p->type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_reqbufs(file, priv, p);
+}
+
+static int ipu6_isys_vidioc_create_bufs(struct file *file, void *priv,
+					struct v4l2_create_buffers *p)
+{
+	struct ipu6_isys_video *av = video_drvdata(file);
+	int ret;
+
+	av->aq.vbq.is_multiplanar = V4L2_TYPE_IS_MULTIPLANAR(p->format.type);
+	av->aq.vbq.is_output = V4L2_TYPE_IS_OUTPUT(p->format.type);
+
+	ret = vb2_queue_change_type(&av->aq.vbq, p->format.type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_create_bufs(file, priv, p);
+}
+
+static int link_validate(struct media_link *link)
+{
+	struct ipu6_isys_video *av =
+		container_of(link->sink, struct ipu6_isys_video, pad);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct v4l2_subdev_state *s_state;
+	struct v4l2_subdev *s_sd;
+	struct v4l2_mbus_framefmt *s_fmt;
+	struct media_pad *s_pad;
+	u32 s_stream, code;
+	int ret = -EPIPE;
+
+	if (!link->source->entity)
+		return ret;
+
+	s_sd = media_entity_to_v4l2_subdev(link->source->entity);
+	s_state = v4l2_subdev_get_unlocked_active_state(s_sd);
+	if (!s_state)
+		return ret;
+
+	dev_dbg(dev, "validating link \"%s\":%u -> \"%s\"\n",
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name);
+
+	s_pad = media_pad_remote_pad_first(&av->pad);
+	s_stream = ipu6_isys_get_src_stream_by_src_pad(s_sd, s_pad->index);
+
+	v4l2_subdev_lock_state(s_state);
+
+	s_fmt = v4l2_subdev_state_get_format(s_state, s_pad->index, s_stream);
+	if (!s_fmt) {
+		dev_err(dev, "failed to get source pad format\n");
+		goto unlock;
+	}
+
+	code = ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0)->code;
+
+	if (s_fmt->width != ipu6_isys_get_frame_width(av) ||
+	    s_fmt->height != ipu6_isys_get_frame_height(av) ||
+	    s_fmt->code != code) {
+		dev_dbg(dev, "format mismatch %dx%d,%x != %dx%d,%x\n",
+			s_fmt->width, s_fmt->height, s_fmt->code,
+			ipu6_isys_get_frame_width(av),
+			ipu6_isys_get_frame_height(av), code);
+		goto unlock;
+	}
+
+	v4l2_subdev_unlock_state(s_state);
+
+	return 0;
+unlock:
+	v4l2_subdev_unlock_state(s_state);
+
+	return ret;
+}
+
+static void get_stream_opened(struct ipu6_isys_video *av)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&av->isys->streams_lock, flags);
+	av->isys->stream_opened++;
+	spin_unlock_irqrestore(&av->isys->streams_lock, flags);
+}
+
+static void put_stream_opened(struct ipu6_isys_video *av)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&av->isys->streams_lock, flags);
+	av->isys->stream_opened--;
+	spin_unlock_irqrestore(&av->isys->streams_lock, flags);
+}
+
+static int ipu6_isys_fw_pin_cfg(struct ipu6_isys_video *av,
+				struct ipu6_fw_isys_stream_cfg_data_abi *cfg)
+{
+	struct media_pad *src_pad = media_pad_remote_pad_first(&av->pad);
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(src_pad->entity);
+	struct ipu6_fw_isys_input_pin_info_abi *input_pin;
+	struct ipu6_fw_isys_output_pin_info_abi *output_pin;
+	struct ipu6_isys_stream *stream = av->stream;
+	struct ipu6_isys_queue *aq = &av->aq;
+	struct v4l2_mbus_framefmt fmt;
+	const struct ipu6_isys_pixelformat *pfmt =
+		ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0);
+	struct v4l2_rect v4l2_crop;
+	struct ipu6_isys *isys = av->isys;
+	struct device *dev = &isys->adev->auxdev.dev;
+	int input_pins = cfg->nof_input_pins++;
+	int output_pins;
+	u32 src_stream;
+	int ret;
+
+	src_stream = ipu6_isys_get_src_stream_by_src_pad(sd, src_pad->index);
+	ret = ipu6_isys_get_stream_pad_fmt(sd, src_pad->index, src_stream,
+					   &fmt);
+	if (ret < 0) {
+		dev_err(dev, "can't get stream format (%d)\n", ret);
+		return ret;
+	}
+
+	ret = ipu6_isys_get_stream_pad_crop(sd, src_pad->index, src_stream,
+					    &v4l2_crop);
+	if (ret < 0) {
+		dev_err(dev, "can't get stream crop (%d)\n", ret);
+		return ret;
+	}
+
+	input_pin = &cfg->input_pins[input_pins];
+	input_pin->input_res.width = fmt.width;
+	input_pin->input_res.height = fmt.height;
+	input_pin->dt = av->dt;
+	input_pin->bits_per_pix = pfmt->bpp_packed;
+	input_pin->mapped_dt = 0x40; /* invalid mipi data type */
+	input_pin->mipi_decompression = 0;
+	input_pin->capture_mode = IPU6_FW_ISYS_CAPTURE_MODE_REGULAR;
+	input_pin->mipi_store_mode = pfmt->bpp == pfmt->bpp_packed ?
+		IPU6_FW_ISYS_MIPI_STORE_MODE_DISCARD_LONG_HEADER :
+		IPU6_FW_ISYS_MIPI_STORE_MODE_NORMAL;
+	input_pin->crop_first_and_last_lines = v4l2_crop.top & 1;
+
+	output_pins = cfg->nof_output_pins++;
+	aq->fw_output = output_pins;
+	stream->output_pins_queue[output_pins] = aq;
+
+	output_pin = &cfg->output_pins[output_pins];
+	output_pin->input_pin_id = input_pins;
+	output_pin->output_res.width = ipu6_isys_get_frame_width(av);
+	output_pin->output_res.height = ipu6_isys_get_frame_height(av);
+
+	output_pin->stride = ipu6_isys_get_bytes_per_line(av);
+	if (pfmt->bpp != pfmt->bpp_packed)
+		output_pin->pt = IPU6_FW_ISYS_PIN_TYPE_RAW_SOC;
+	else
+		output_pin->pt = IPU6_FW_ISYS_PIN_TYPE_MIPI;
+	output_pin->ft = pfmt->css_pixelformat;
+	output_pin->send_irq = 1;
+	memset(output_pin->ts_offsets, 0, sizeof(output_pin->ts_offsets));
+	output_pin->s2m_pixel_soc_pixel_remapping =
+		S2M_PIXEL_SOC_PIXEL_REMAPPING_FLAG_NO_REMAPPING;
+	output_pin->csi_be_soc_pixel_remapping =
+		CSI_BE_SOC_PIXEL_REMAPPING_FLAG_NO_REMAPPING;
+
+	output_pin->snoopable = true;
+	output_pin->error_handling_enable = false;
+	output_pin->sensor_type = isys->sensor_type++;
+	if (isys->sensor_type > isys->pdata->ipdata->sensor_type_end)
+		isys->sensor_type = isys->pdata->ipdata->sensor_type_start;
+
+	return 0;
+}
+
+static int start_stream_firmware(struct ipu6_isys_video *av,
+				 struct ipu6_isys_buffer_list *bl)
+{
+	struct ipu6_fw_isys_stream_cfg_data_abi *stream_cfg;
+	struct ipu6_fw_isys_frame_buff_set_abi *buf = NULL;
+	struct ipu6_isys_stream *stream = av->stream;
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct isys_fw_msgs *msg = NULL;
+	struct ipu6_isys_queue *aq;
+	int ret, retout, tout;
+	u16 send_type;
+
+	msg = ipu6_get_fw_msg_buf(stream);
+	if (!msg)
+		return -ENOMEM;
+
+	stream_cfg = &msg->fw_msg.stream;
+	stream_cfg->src = stream->stream_source;
+	stream_cfg->vc = stream->vc;
+	stream_cfg->isl_use = 0;
+	stream_cfg->sensor_type = IPU6_FW_ISYS_SENSOR_MODE_NORMAL;
+
+	list_for_each_entry(aq, &stream->queues, node) {
+		struct ipu6_isys_video *__av = ipu6_isys_queue_to_video(aq);
+
+		ret = ipu6_isys_fw_pin_cfg(__av, stream_cfg);
+		if (ret < 0) {
+			ipu6_put_fw_msg_buf(av->isys, (uintptr_t)stream_cfg);
+			return ret;
+		}
+	}
+
+	ipu6_fw_isys_dump_stream_cfg(dev, stream_cfg);
+
+	stream->nr_output_pins = stream_cfg->nof_output_pins;
+
+	reinit_completion(&stream->stream_open_completion);
+
+	ret = ipu6_fw_isys_complex_cmd(av->isys, stream->stream_handle,
+				       stream_cfg, msg->dma_addr,
+				       sizeof(*stream_cfg),
+				       IPU6_FW_ISYS_SEND_TYPE_STREAM_OPEN);
+	if (ret < 0) {
+		dev_err(dev, "can't open stream (%d)\n", ret);
+		ipu6_put_fw_msg_buf(av->isys, (uintptr_t)stream_cfg);
+		return ret;
+	}
+
+	get_stream_opened(av);
+
+	tout = wait_for_completion_timeout(&stream->stream_open_completion,
+					   IPU6_FW_CALL_TIMEOUT_JIFFIES);
+
+	ipu6_put_fw_msg_buf(av->isys, (uintptr_t)stream_cfg);
+
+	if (!tout) {
+		dev_err(dev, "stream open time out\n");
+		ret = -ETIMEDOUT;
+		goto out_put_stream_opened;
+	}
+	if (stream->error) {
+		dev_err(dev, "stream open error: %d\n", stream->error);
+		ret = -EIO;
+		goto out_put_stream_opened;
+	}
+	dev_dbg(dev, "start stream: open complete\n");
+
+	if (bl) {
+		msg = ipu6_get_fw_msg_buf(stream);
+		if (!msg) {
+			ret = -ENOMEM;
+			goto out_put_stream_opened;
+		}
+		buf = &msg->fw_msg.frame;
+		ipu6_isys_buf_to_fw_frame_buf(buf, stream, bl);
+		ipu6_isys_buffer_list_queue(bl,
+					    IPU6_ISYS_BUFFER_LIST_FL_ACTIVE, 0);
+	}
+
+	reinit_completion(&stream->stream_start_completion);
+
+	if (bl) {
+		send_type = IPU6_FW_ISYS_SEND_TYPE_STREAM_START_AND_CAPTURE;
+		ipu6_fw_isys_dump_frame_buff_set(dev, buf,
+						 stream_cfg->nof_output_pins);
+		ret = ipu6_fw_isys_complex_cmd(av->isys, stream->stream_handle,
+					       buf, msg->dma_addr,
+					       sizeof(*buf), send_type);
+	} else {
+		send_type = IPU6_FW_ISYS_SEND_TYPE_STREAM_START;
+		ret = ipu6_fw_isys_simple_cmd(av->isys, stream->stream_handle,
+					      send_type);
+	}
+
+	if (ret < 0) {
+		dev_err(dev, "can't start streaming (%d)\n", ret);
+		goto out_stream_close;
+	}
+
+	tout = wait_for_completion_timeout(&stream->stream_start_completion,
+					   IPU6_FW_CALL_TIMEOUT_JIFFIES);
+	if (!tout) {
+		dev_err(dev, "stream start time out\n");
+		ret = -ETIMEDOUT;
+		goto out_stream_close;
+	}
+	if (stream->error) {
+		dev_err(dev, "stream start error: %d\n", stream->error);
+		ret = -EIO;
+		goto out_stream_close;
+	}
+	dev_dbg(dev, "start stream: complete\n");
+
+	return 0;
+
+out_stream_close:
+	reinit_completion(&stream->stream_close_completion);
+
+	retout = ipu6_fw_isys_simple_cmd(av->isys,
+					 stream->stream_handle,
+					 IPU6_FW_ISYS_SEND_TYPE_STREAM_CLOSE);
+	if (retout < 0) {
+		dev_dbg(dev, "can't close stream (%d)\n", retout);
+		goto out_put_stream_opened;
+	}
+
+	tout = wait_for_completion_timeout(&stream->stream_close_completion,
+					   IPU6_FW_CALL_TIMEOUT_JIFFIES);
+	if (!tout)
+		dev_err(dev, "stream close time out\n");
+	else if (stream->error)
+		dev_err(dev, "stream close error: %d\n", stream->error);
+	else
+		dev_dbg(dev, "stream close complete\n");
+
+out_put_stream_opened:
+	put_stream_opened(av);
+
+	return ret;
+}
+
+static void stop_streaming_firmware(struct ipu6_isys_video *av)
+{
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct ipu6_isys_stream *stream = av->stream;
+	int ret, tout;
+
+	reinit_completion(&stream->stream_stop_completion);
+
+	ret = ipu6_fw_isys_simple_cmd(av->isys, stream->stream_handle,
+				      IPU6_FW_ISYS_SEND_TYPE_STREAM_FLUSH);
+
+	if (ret < 0) {
+		dev_err(dev, "can't stop stream (%d)\n", ret);
+		return;
+	}
+
+	tout = wait_for_completion_timeout(&stream->stream_stop_completion,
+					   IPU6_FW_CALL_TIMEOUT_JIFFIES);
+	if (!tout)
+		dev_warn(dev, "stream stop time out\n");
+	else if (stream->error)
+		dev_warn(dev, "stream stop error: %d\n", stream->error);
+	else
+		dev_dbg(dev, "stop stream: complete\n");
+}
+
+static void close_streaming_firmware(struct ipu6_isys_video *av)
+{
+	struct ipu6_isys_stream *stream = av->stream;
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	int ret, tout;
+
+	reinit_completion(&stream->stream_close_completion);
+
+	ret = ipu6_fw_isys_simple_cmd(av->isys, stream->stream_handle,
+				      IPU6_FW_ISYS_SEND_TYPE_STREAM_CLOSE);
+	if (ret < 0) {
+		dev_err(dev, "can't close stream (%d)\n", ret);
+		return;
+	}
+
+	tout = wait_for_completion_timeout(&stream->stream_close_completion,
+					   IPU6_FW_CALL_TIMEOUT_JIFFIES);
+	if (!tout)
+		dev_warn(dev, "stream close time out\n");
+	else if (stream->error)
+		dev_warn(dev, "stream close error: %d\n", stream->error);
+	else
+		dev_dbg(dev, "close stream: complete\n");
+
+	put_stream_opened(av);
+}
+
+int ipu6_isys_video_prepare_stream(struct ipu6_isys_video *av,
+				   struct media_entity *source_entity,
+				   int nr_queues)
+{
+	struct ipu6_isys_stream *stream = av->stream;
+	struct ipu6_isys_csi2 *csi2;
+
+	if (WARN_ON(stream->nr_streaming))
+		return -EINVAL;
+
+	stream->nr_queues = nr_queues;
+	atomic_set(&stream->sequence, 0);
+
+	stream->seq_index = 0;
+	memset(stream->seq, 0, sizeof(stream->seq));
+
+	if (WARN_ON(!list_empty(&stream->queues)))
+		return -EINVAL;
+
+	stream->stream_source = stream->asd->source;
+	csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
+	csi2->receiver_errors = 0;
+	stream->source_entity = source_entity;
+
+	dev_dbg(&av->isys->adev->auxdev.dev,
+		"prepare stream: external entity %s\n",
+		stream->source_entity->name);
+
+	return 0;
+}
+
+void ipu6_isys_configure_stream_watermark(struct ipu6_isys_video *av,
+					  bool state)
+{
+	struct ipu6_isys *isys = av->isys;
+	struct ipu6_isys_csi2 *csi2 = NULL;
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct v4l2_mbus_framefmt format;
+	struct v4l2_subdev *esd;
+	struct v4l2_control hb = { .id = V4L2_CID_HBLANK, .value = 0 };
+	unsigned int bpp, lanes;
+	s64 link_freq = 0;
+	u64 pixel_rate = 0;
+	int ret;
+
+	if (!state)
+		return;
+
+	esd = media_entity_to_v4l2_subdev(av->stream->source_entity);
+
+	av->watermark.width = ipu6_isys_get_frame_width(av);
+	av->watermark.height = ipu6_isys_get_frame_height(av);
+	av->watermark.sram_gran_shift = isys->pdata->ipdata->sram_gran_shift;
+	av->watermark.sram_gran_size = isys->pdata->ipdata->sram_gran_size;
+
+	ret = v4l2_g_ctrl(esd->ctrl_handler, &hb);
+	if (!ret && hb.value >= 0)
+		av->watermark.hblank = hb.value;
+	else
+		av->watermark.hblank = 0;
+
+	csi2 = ipu6_isys_subdev_to_csi2(av->stream->asd);
+	link_freq = ipu6_isys_csi2_get_link_freq(csi2);
+	if (link_freq > 0) {
+		lanes = csi2->nlanes;
+		ret = ipu6_isys_get_stream_pad_fmt(&csi2->asd.sd, 0,
+						   av->source_stream, &format);
+		if (!ret) {
+			bpp = ipu6_isys_mbus_code_to_bpp(format.code);
+			pixel_rate = mul_u64_u32_div(link_freq, lanes * 2, bpp);
+		}
+	}
+
+	av->watermark.pixel_rate = pixel_rate;
+
+	if (!pixel_rate) {
+		mutex_lock(&iwake_watermark->mutex);
+		iwake_watermark->force_iwake_disable = true;
+		mutex_unlock(&iwake_watermark->mutex);
+		dev_warn(dev, "unexpected pixel_rate from %s, disable iwake.\n",
+			 av->stream->source_entity->name);
+	}
+}
+
+static void calculate_stream_datarate(struct ipu6_isys_video *av)
+{
+	struct video_stream_watermark *watermark = &av->watermark;
+	const struct ipu6_isys_pixelformat *pfmt =
+		ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0);
+	u32 pages_per_line, pb_bytes_per_line, pixels_per_line, bytes_per_line;
+	u64 line_time_ns, stream_data_rate;
+	u16 shift, size;
+
+	shift = watermark->sram_gran_shift;
+	size = watermark->sram_gran_size;
+
+	pixels_per_line = watermark->width + watermark->hblank;
+	line_time_ns =  div_u64(pixels_per_line * NSEC_PER_SEC,
+				watermark->pixel_rate);
+	bytes_per_line = watermark->width * pfmt->bpp / 8;
+	pages_per_line = DIV_ROUND_UP(bytes_per_line, size);
+	pb_bytes_per_line = pages_per_line << shift;
+	stream_data_rate = div64_u64(pb_bytes_per_line * 1000, line_time_ns);
+
+	watermark->stream_data_rate = stream_data_rate;
+}
+
+void ipu6_isys_update_stream_watermark(struct ipu6_isys_video *av, bool state)
+{
+	struct isys_iwake_watermark *iwake_watermark =
+		&av->isys->iwake_watermark;
+
+	if (!av->watermark.pixel_rate)
+		return;
+
+	if (state) {
+		calculate_stream_datarate(av);
+		mutex_lock(&iwake_watermark->mutex);
+		list_add(&av->watermark.stream_node,
+			 &iwake_watermark->video_list);
+		mutex_unlock(&iwake_watermark->mutex);
+	} else {
+		av->watermark.stream_data_rate = 0;
+		mutex_lock(&iwake_watermark->mutex);
+		list_del(&av->watermark.stream_node);
+		mutex_unlock(&iwake_watermark->mutex);
+	}
+
+	update_watermark_setting(av->isys);
+}
+
+void ipu6_isys_put_stream(struct ipu6_isys_stream *stream)
+{
+	struct device *dev;
+	unsigned int i;
+	unsigned long flags;
+
+	if (!stream) {
+		pr_err("ipu6-isys: no available stream\n");
+		return;
+	}
+
+	dev = &stream->isys->adev->auxdev.dev;
+
+	spin_lock_irqsave(&stream->isys->streams_lock, flags);
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++) {
+		if (&stream->isys->streams[i] == stream) {
+			if (stream->isys->streams_ref_count[i] > 0)
+				stream->isys->streams_ref_count[i]--;
+			else
+				dev_warn(dev, "invalid stream %d\n", i);
+
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&stream->isys->streams_lock, flags);
+}
+
+static struct ipu6_isys_stream *
+ipu6_isys_get_stream(struct ipu6_isys_video *av, struct ipu6_isys_subdev *asd)
+{
+	struct ipu6_isys_stream *stream = NULL;
+	struct ipu6_isys *isys = av->isys;
+	unsigned long flags;
+	unsigned int i;
+	u8 vc = av->vc;
+
+	if (!isys)
+		return NULL;
+
+	spin_lock_irqsave(&isys->streams_lock, flags);
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++) {
+		if (isys->streams_ref_count[i] && isys->streams[i].vc == vc &&
+		    isys->streams[i].asd == asd) {
+			isys->streams_ref_count[i]++;
+			stream = &isys->streams[i];
+			break;
+		}
+	}
+
+	if (!stream) {
+		for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++) {
+			if (!isys->streams_ref_count[i]) {
+				isys->streams_ref_count[i]++;
+				stream = &isys->streams[i];
+				stream->vc = vc;
+				stream->asd = asd;
+				break;
+			}
+		}
+	}
+	spin_unlock_irqrestore(&isys->streams_lock, flags);
+
+	return stream;
+}
+
+struct ipu6_isys_stream *
+ipu6_isys_query_stream_by_handle(struct ipu6_isys *isys, u8 stream_handle)
+{
+	unsigned long flags;
+	struct ipu6_isys_stream *stream = NULL;
+
+	if (!isys)
+		return NULL;
+
+	if (stream_handle >= IPU6_ISYS_MAX_STREAMS) {
+		dev_err(&isys->adev->auxdev.dev,
+			"stream_handle %d is invalid\n", stream_handle);
+		return NULL;
+	}
+
+	spin_lock_irqsave(&isys->streams_lock, flags);
+	if (isys->streams_ref_count[stream_handle] > 0) {
+		isys->streams_ref_count[stream_handle]++;
+		stream = &isys->streams[stream_handle];
+	}
+	spin_unlock_irqrestore(&isys->streams_lock, flags);
+
+	return stream;
+}
+
+struct ipu6_isys_stream *
+ipu6_isys_query_stream_by_source(struct ipu6_isys *isys, int source, u8 vc)
+{
+	struct ipu6_isys_stream *stream = NULL;
+	unsigned long flags;
+	unsigned int i;
+
+	if (!isys)
+		return NULL;
+
+	if (source < 0) {
+		dev_err(&isys->adev->auxdev.dev,
+			"query stream with invalid port number\n");
+		return NULL;
+	}
+
+	spin_lock_irqsave(&isys->streams_lock, flags);
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++) {
+		if (!isys->streams_ref_count[i])
+			continue;
+
+		if (isys->streams[i].stream_source == source &&
+		    isys->streams[i].vc == vc) {
+			stream = &isys->streams[i];
+			isys->streams_ref_count[i]++;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&isys->streams_lock, flags);
+
+	return stream;
+}
+
+static u64 get_stream_mask_by_pipeline(struct ipu6_isys_video *__av)
+{
+	struct media_pipeline *pipeline =
+		media_entity_pipeline(&__av->vdev.entity);
+	unsigned int i;
+	u64 stream_mask = 0;
+
+	for (i = 0; i < NR_OF_CSI2_SRC_PADS; i++) {
+		struct ipu6_isys_video *av = &__av->csi2->av[i];
+
+		if (pipeline == media_entity_pipeline(&av->vdev.entity))
+			stream_mask |= BIT_ULL(av->source_stream);
+	}
+
+	return stream_mask;
+}
+
+int ipu6_isys_video_set_streaming(struct ipu6_isys_video *av, int state,
+				  struct ipu6_isys_buffer_list *bl)
+{
+	struct v4l2_subdev_krouting *routing;
+	struct ipu6_isys_stream *stream = av->stream;
+	struct v4l2_subdev_state *subdev_state;
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct v4l2_subdev *sd;
+	struct media_pad *r_pad;
+	u32 sink_pad, sink_stream;
+	u64 r_stream;
+	u64 stream_mask = 0;
+	int ret = 0;
+
+	dev_dbg(dev, "set stream: %d\n", state);
+
+	if (WARN(!stream->source_entity, "No source entity for stream\n"))
+		return -ENODEV;
+
+	sd = &stream->asd->sd;
+	r_pad = media_pad_remote_pad_first(&av->pad);
+	r_stream = ipu6_isys_get_src_stream_by_src_pad(sd, r_pad->index);
+
+	subdev_state = v4l2_subdev_lock_and_get_active_state(sd);
+	routing = &subdev_state->routing;
+	ret = v4l2_subdev_routing_find_opposite_end(routing, r_pad->index,
+						    r_stream, &sink_pad,
+						    &sink_stream);
+	v4l2_subdev_unlock_state(subdev_state);
+	if (ret)
+		return ret;
+
+	stream_mask = get_stream_mask_by_pipeline(av);
+	if (!state) {
+		stop_streaming_firmware(av);
+
+		/* stop sub-device which connects with video */
+		dev_dbg(dev, "stream off entity %s pad:%d mask:0x%llx\n",
+			sd->name, r_pad->index, stream_mask);
+		ret = v4l2_subdev_disable_streams(sd, r_pad->index,
+						  stream_mask);
+		if (ret) {
+			dev_err(dev, "stream off %s failed with %d\n", sd->name,
+				ret);
+			return ret;
+		}
+		close_streaming_firmware(av);
+	} else {
+		ret = start_stream_firmware(av, bl);
+		if (ret) {
+			dev_err(dev, "start stream of firmware failed\n");
+			return ret;
+		}
+
+		/* start sub-device which connects with video */
+		dev_dbg(dev, "stream on %s pad %d mask 0x%llx\n", sd->name,
+			r_pad->index, stream_mask);
+		ret = v4l2_subdev_enable_streams(sd, r_pad->index, stream_mask);
+		if (ret) {
+			dev_err(dev, "stream on %s failed with %d\n", sd->name,
+				ret);
+			goto out_media_entity_stop_streaming_firmware;
+		}
+	}
+
+	av->streaming = state;
+
+	return 0;
+
+out_media_entity_stop_streaming_firmware:
+	stop_streaming_firmware(av);
+
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops ipu6_v4l2_ioctl_ops = {
+	.vidioc_querycap = ipu6_isys_vidioc_querycap,
+	.vidioc_enum_fmt_vid_cap = ipu6_isys_vidioc_enum_fmt,
+	.vidioc_enum_fmt_meta_cap = ipu6_isys_vidioc_enum_fmt,
+	.vidioc_enum_framesizes = ipu6_isys_vidioc_enum_framesizes,
+	.vidioc_g_fmt_vid_cap = ipu6_isys_vidioc_g_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap = ipu6_isys_vidioc_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = ipu6_isys_vidioc_try_fmt_vid_cap,
+	.vidioc_g_fmt_meta_cap = ipu6_isys_vidioc_g_fmt_meta_cap,
+	.vidioc_s_fmt_meta_cap = ipu6_isys_vidioc_s_fmt_meta_cap,
+	.vidioc_try_fmt_meta_cap = ipu6_isys_vidioc_try_fmt_meta_cap,
+	.vidioc_reqbufs = ipu6_isys_vidioc_reqbufs,
+	.vidioc_create_bufs = ipu6_isys_vidioc_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+};
+
+static const struct media_entity_operations entity_ops = {
+	.link_validate = link_validate,
+};
+
+static const struct v4l2_file_operations isys_fops = {
+	.owner = THIS_MODULE,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+	.open = video_open,
+	.release = vb2_fop_release,
+};
+
+int ipu6_isys_fw_open(struct ipu6_isys *isys)
+{
+	struct ipu6_bus_device *adev = isys->adev;
+	const struct ipu6_isys_internal_pdata *ipdata = isys->pdata->ipdata;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&adev->auxdev.dev);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&isys->mutex);
+
+	if (isys->ref_count++)
+		goto unlock;
+
+	ipu6_configure_spc(adev->isp, &ipdata->hw_variant,
+			   IPU6_CPD_PKG_DIR_ISYS_SERVER_IDX, isys->pdata->base,
+			   adev->pkg_dir, adev->pkg_dir_dma_addr);
+
+	/*
+	 * Buffers could have been left to wrong queue at last closure.
+	 * Move them now back to empty buffer queue.
+	 */
+	ipu6_cleanup_fw_msg_bufs(isys);
+
+	if (isys->fwcom) {
+		/*
+		 * Something went wrong in previous shutdown. As we are now
+		 * restarting isys we can safely delete old context.
+		 */
+		dev_warn(&adev->auxdev.dev, "clearing old context\n");
+		ipu6_fw_isys_cleanup(isys);
+	}
+
+	ret = ipu6_fw_isys_init(isys, ipdata->num_parallel_streams);
+	if (ret < 0)
+		goto out;
+
+unlock:
+	mutex_unlock(&isys->mutex);
+
+	return 0;
+
+out:
+	isys->ref_count--;
+	mutex_unlock(&isys->mutex);
+	pm_runtime_put(&adev->auxdev.dev);
+
+	return ret;
+}
+
+void ipu6_isys_fw_close(struct ipu6_isys *isys)
+{
+	mutex_lock(&isys->mutex);
+
+	isys->ref_count--;
+	if (!isys->ref_count) {
+		ipu6_fw_isys_close(isys);
+		if (isys->fwcom) {
+			isys->need_reset = true;
+			dev_warn(&isys->adev->auxdev.dev,
+				 "failed to close fw isys\n");
+		}
+	}
+
+	mutex_unlock(&isys->mutex);
+
+	if (isys->need_reset)
+		pm_runtime_put_sync(&isys->adev->auxdev.dev);
+	else
+		pm_runtime_put(&isys->adev->auxdev.dev);
+}
+
+int ipu6_isys_setup_video(struct ipu6_isys_video *av,
+			  struct media_entity **source_entity, int *nr_queues)
+{
+	const struct ipu6_isys_pixelformat *pfmt =
+		ipu6_isys_get_isys_format(ipu6_isys_get_format(av), 0);
+	struct device *dev = &av->isys->adev->auxdev.dev;
+	struct v4l2_mbus_frame_desc_entry entry;
+	struct v4l2_subdev_route *route = NULL;
+	struct v4l2_subdev_route *r;
+	struct v4l2_subdev_state *state;
+	struct ipu6_isys_subdev *asd;
+	struct v4l2_subdev *remote_sd;
+	struct media_pipeline *pipeline;
+	struct media_pad *source_pad, *remote_pad;
+	int ret = -EINVAL;
+
+	*nr_queues = 0;
+
+	remote_pad = media_pad_remote_pad_unique(&av->pad);
+	if (IS_ERR(remote_pad)) {
+		dev_dbg(dev, "failed to get remote pad\n");
+		return PTR_ERR(remote_pad);
+	}
+
+	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
+	asd = to_ipu6_isys_subdev(remote_sd);
+	source_pad = media_pad_remote_pad_first(&remote_pad->entity->pads[0]);
+	if (!source_pad) {
+		dev_dbg(dev, "No external source entity\n");
+		return -ENODEV;
+	}
+
+	*source_entity = source_pad->entity;
+
+	/* Find the root */
+	state = v4l2_subdev_lock_and_get_active_state(remote_sd);
+	for_each_active_route(&state->routing, r) {
+		(*nr_queues)++;
+
+		if (r->source_pad == remote_pad->index)
+			route = r;
+	}
+
+	if (!route) {
+		v4l2_subdev_unlock_state(state);
+		dev_dbg(dev, "Failed to find route\n");
+		return -ENODEV;
+	}
+	av->source_stream = route->sink_stream;
+	v4l2_subdev_unlock_state(state);
+
+	ret = ipu6_isys_csi2_get_remote_desc(av->source_stream,
+					     to_ipu6_isys_csi2(asd),
+					     *source_entity, &entry);
+	if (ret == -ENOIOCTLCMD) {
+		av->vc = 0;
+		av->dt = ipu6_isys_mbus_code_to_mipi(pfmt->code);
+	} else if (!ret) {
+		dev_dbg(dev, "Framedesc: stream %u, len %u, vc %u, dt %#x\n",
+			entry.stream, entry.length, entry.bus.csi2.vc,
+			entry.bus.csi2.dt);
+
+		av->vc = entry.bus.csi2.vc;
+		av->dt = entry.bus.csi2.dt;
+	} else {
+		dev_err(dev, "failed to get remote frame desc\n");
+		return ret;
+	}
+
+	pipeline = media_entity_pipeline(&av->vdev.entity);
+	if (!pipeline)
+		ret = video_device_pipeline_alloc_start(&av->vdev);
+	else
+		ret = video_device_pipeline_start(&av->vdev, pipeline);
+	if (ret < 0) {
+		dev_dbg(dev, "media pipeline start failed\n");
+		return ret;
+	}
+
+	av->stream = ipu6_isys_get_stream(av, asd);
+	if (!av->stream) {
+		video_device_pipeline_stop(&av->vdev);
+		dev_err(dev, "no available stream for firmware\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Do everything that's needed to initialise things related to video
+ * buffer queue, video node, and the related media entity. The caller
+ * is expected to assign isys field and set the name of the video
+ * device.
+ */
+int ipu6_isys_video_init(struct ipu6_isys_video *av)
+{
+	struct v4l2_format format = {
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
+		.fmt.pix = {
+			.width = 1920,
+			.height = 1080,
+		},
+	};
+	struct v4l2_format format_meta = {
+		.type = V4L2_BUF_TYPE_META_CAPTURE,
+		.fmt.meta = {
+			.width = 1920,
+			.height = 4,
+		},
+	};
+	int ret;
+
+	mutex_init(&av->mutex);
+	av->vdev.device_caps = V4L2_CAP_STREAMING | V4L2_CAP_IO_MC |
+			       V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE;
+	av->vdev.vfl_dir = VFL_DIR_RX;
+
+	ret = ipu6_isys_queue_init(&av->aq);
+	if (ret)
+		goto out_free_watermark;
+
+	av->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
+	ret = media_entity_pads_init(&av->vdev.entity, 1, &av->pad);
+	if (ret)
+		goto out_vb2_queue_release;
+
+	av->vdev.entity.ops = &entity_ops;
+	av->vdev.release = video_device_release_empty;
+	av->vdev.fops = &isys_fops;
+	av->vdev.v4l2_dev = &av->isys->v4l2_dev;
+	av->vdev.dev_parent = &av->isys->adev->isp->pdev->dev;
+	if (!av->vdev.ioctl_ops)
+		av->vdev.ioctl_ops = &ipu6_v4l2_ioctl_ops;
+	av->vdev.queue = &av->aq.vbq;
+	av->vdev.lock = &av->mutex;
+
+	__ipu6_isys_vidioc_try_fmt_vid_cap(av, &format);
+	av->pix_fmt = format.fmt.pix;
+	__ipu6_isys_vidioc_try_fmt_meta_cap(av, &format_meta);
+	av->meta_fmt = format_meta.fmt.meta;
+
+	set_bit(V4L2_FL_USES_V4L2_FH, &av->vdev.flags);
+	video_set_drvdata(&av->vdev, av);
+
+	ret = video_register_device(&av->vdev, VFL_TYPE_VIDEO, -1);
+	if (ret)
+		goto out_media_entity_cleanup;
+
+	return ret;
+
+out_media_entity_cleanup:
+	vb2_video_unregister_device(&av->vdev);
+	media_entity_cleanup(&av->vdev.entity);
+
+out_vb2_queue_release:
+	vb2_queue_release(&av->aq.vbq);
+
+out_free_watermark:
+	mutex_destroy(&av->mutex);
+
+	return ret;
+}
+
+void ipu6_isys_video_cleanup(struct ipu6_isys_video *av)
+{
+	vb2_video_unregister_device(&av->vdev);
+	media_entity_cleanup(&av->vdev.entity);
+	mutex_destroy(&av->mutex);
+}
+
+u32 ipu6_isys_get_format(struct ipu6_isys_video *av)
+{
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return av->pix_fmt.pixelformat;
+
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_META_CAPTURE)
+		return av->meta_fmt.dataformat;
+
+	return 0;
+}
+
+u32 ipu6_isys_get_data_size(struct ipu6_isys_video *av)
+{
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return av->pix_fmt.sizeimage;
+
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_META_CAPTURE)
+		return av->meta_fmt.buffersize;
+
+	return 0;
+}
+
+u32 ipu6_isys_get_bytes_per_line(struct ipu6_isys_video *av)
+{
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return av->pix_fmt.bytesperline;
+
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_META_CAPTURE)
+		return av->meta_fmt.bytesperline;
+
+	return 0;
+}
+
+u32 ipu6_isys_get_frame_width(struct ipu6_isys_video *av)
+{
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return av->pix_fmt.width;
+
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_META_CAPTURE)
+		return av->meta_fmt.width;
+
+	return 0;
+}
+
+u32 ipu6_isys_get_frame_height(struct ipu6_isys_video *av)
+{
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return av->pix_fmt.height;
+
+	if (av->aq.vbq.type == V4L2_BUF_TYPE_META_CAPTURE)
+		return av->meta_fmt.height;
+
+	return 0;
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys-video.h b/drivers/media/pci/intel/ipu6/ipu6-isys-video.h
new file mode 100644
index 000000000000..1dd36f2a077e
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys-video.h
@@ -0,0 +1,135 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_ISYS_VIDEO_H
+#define IPU6_ISYS_VIDEO_H
+
+#include <linux/atomic.h>
+#include <linux/completion.h>
+#include <linux/container_of.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+
+#include <media/media-entity.h>
+#include <media/v4l2-dev.h>
+
+#include "ipu6-isys-queue.h"
+
+#define IPU6_ISYS_OUTPUT_PINS 11
+#define IPU6_ISYS_MAX_PARALLEL_SOF 2
+
+struct file;
+struct ipu6_isys;
+struct ipu6_isys_csi2;
+struct ipu6_isys_subdev;
+
+struct ipu6_isys_pixelformat {
+	u32 pixelformat;
+	u32 bpp;
+	u32 bpp_packed;
+	u32 code;
+	u32 css_pixelformat;
+	bool is_meta;
+};
+
+struct sequence_info {
+	unsigned int sequence;
+	u64 timestamp;
+};
+
+/*
+ * Align with firmware stream. Each stream represents a CSI virtual channel.
+ * May map to multiple video devices
+ */
+struct ipu6_isys_stream {
+	struct mutex mutex;
+	struct media_entity *source_entity;
+	atomic_t sequence;
+	unsigned int seq_index;
+	struct sequence_info seq[IPU6_ISYS_MAX_PARALLEL_SOF];
+	int stream_source;
+	int stream_handle;
+	unsigned int nr_output_pins;
+	struct ipu6_isys_subdev *asd;
+
+	int nr_queues;	/* Number of capture queues */
+	int nr_streaming;
+	int streaming;	/* Has streaming been really started? */
+	struct list_head queues;
+	struct completion stream_open_completion;
+	struct completion stream_close_completion;
+	struct completion stream_start_completion;
+	struct completion stream_stop_completion;
+	struct ipu6_isys *isys;
+
+	struct ipu6_isys_queue *output_pins_queue[IPU6_ISYS_OUTPUT_PINS];
+	int error;
+	u8 vc;
+};
+
+struct video_stream_watermark {
+	u32 width;
+	u32 height;
+	u32 hblank;
+	u32 frame_rate;
+	u64 pixel_rate;
+	u64 stream_data_rate;
+	u16 sram_gran_shift;
+	u16 sram_gran_size;
+	struct list_head stream_node;
+};
+
+struct ipu6_isys_video {
+	struct ipu6_isys_queue aq;
+	/* Serialise access to other fields in the struct. */
+	struct mutex mutex;
+	struct media_pad pad;
+	struct video_device vdev;
+	struct v4l2_pix_format pix_fmt;
+	struct v4l2_meta_format meta_fmt;
+	struct ipu6_isys *isys;
+	struct ipu6_isys_csi2 *csi2;
+	struct ipu6_isys_stream *stream;
+	unsigned int streaming;
+	struct video_stream_watermark watermark;
+	u32 source_stream;
+	u8 vc;
+	u8 dt;
+};
+
+#define ipu6_isys_queue_to_video(__aq) \
+	container_of(__aq, struct ipu6_isys_video, aq)
+
+extern const struct ipu6_isys_pixelformat ipu6_isys_pfmts[];
+extern const struct ipu6_isys_pixelformat ipu6_isys_pfmts_packed[];
+
+const struct ipu6_isys_pixelformat *
+ipu6_isys_get_isys_format(u32 pixelformat, u32 code);
+int ipu6_isys_video_prepare_stream(struct ipu6_isys_video *av,
+				   struct media_entity *source_entity,
+				   int nr_queues);
+int ipu6_isys_video_set_streaming(struct ipu6_isys_video *av, int state,
+				  struct ipu6_isys_buffer_list *bl);
+int ipu6_isys_fw_open(struct ipu6_isys *isys);
+void ipu6_isys_fw_close(struct ipu6_isys *isys);
+int ipu6_isys_setup_video(struct ipu6_isys_video *av,
+			  struct media_entity **source_entity, int *nr_queues);
+int ipu6_isys_video_init(struct ipu6_isys_video *av);
+void ipu6_isys_video_cleanup(struct ipu6_isys_video *av);
+void ipu6_isys_put_stream(struct ipu6_isys_stream *stream);
+struct ipu6_isys_stream *
+ipu6_isys_query_stream_by_handle(struct ipu6_isys *isys, u8 stream_handle);
+struct ipu6_isys_stream *
+ipu6_isys_query_stream_by_source(struct ipu6_isys *isys, int source, u8 vc);
+
+void ipu6_isys_configure_stream_watermark(struct ipu6_isys_video *av,
+					  bool state);
+void ipu6_isys_update_stream_watermark(struct ipu6_isys_video *av, bool state);
+
+u32 ipu6_isys_get_format(struct ipu6_isys_video *av);
+u32 ipu6_isys_get_data_size(struct ipu6_isys_video *av);
+u32 ipu6_isys_get_bytes_per_line(struct ipu6_isys_video *av);
+u32 ipu6_isys_get_frame_width(struct ipu6_isys_video *av);
+u32 ipu6_isys_get_frame_height(struct ipu6_isys_video *av);
+
+#endif /* IPU6_ISYS_VIDEO_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys.c b/drivers/media/pci/intel/ipu6/ipu6-isys.c
new file mode 100644
index 000000000000..fc0ec0a4b8f5
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys.c
@@ -0,0 +1,1380 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/auxiliary_bus.h>
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/completion.h>
+#include <linux/container_of.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/io.h>
+#include <linux/irqreturn.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/pci.h>
+#include <linux/pm_runtime.h>
+#include <linux/pm_qos.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/string.h>
+
+#include <media/ipu-bridge.h>
+#include <media/media-device.h>
+#include <media/media-entity.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+
+#include "ipu6-bus.h"
+#include "ipu6-cpd.h"
+#include "ipu6-dma.h"
+#include "ipu6-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-mmu.h"
+#include "ipu6-platform-buttress-regs.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+#include "ipu6-platform-regs.h"
+
+#define IPU6_BUTTRESS_FABIC_CONTROL		0x68
+#define GDA_ENABLE_IWAKE_INDEX			2
+#define GDA_IWAKE_THRESHOLD_INDEX		1
+#define GDA_IRQ_CRITICAL_THRESHOLD_INDEX	0
+#define GDA_MEMOPEN_THRESHOLD_INDEX		3
+#define DEFAULT_DID_RATIO			90
+#define DEFAULT_IWAKE_THRESHOLD			0x42
+#define DEFAULT_MEM_OPEN_TIME			10
+#define ONE_THOUSAND_MICROSECOND		1000
+/* One page is 2KB, 8 x 16 x 16 = 2048B = 2KB */
+#define ISF_DMA_TOP_GDA_PROFERTY_PAGE_SIZE	0x800
+
+/* LTR & DID value are 10 bit at most */
+#define LTR_DID_VAL_MAX				1023
+#define LTR_DEFAULT_VALUE			0x70503c19
+#define FILL_TIME_DEFAULT_VALUE			0xfff0783c
+#define LTR_DID_PKGC_2R				20
+#define LTR_SCALE_DEFAULT			5
+#define LTR_SCALE_1024NS			2
+#define DID_SCALE_1US				2
+#define DID_SCALE_32US				3
+#define REG_PKGC_PMON_CFG			0xb00
+
+#define VAL_PKGC_PMON_CFG_RESET			0x38
+#define VAL_PKGC_PMON_CFG_START			0x7
+
+#define IS_PIXEL_BUFFER_PAGES			0x80
+/*
+ * when iwake mode is disabled, the critical threshold is statically set
+ * to 75% of the IS pixel buffer, criticalThreshold = (128 * 3) / 4
+ */
+#define CRITICAL_THRESHOLD_IWAKE_DISABLE	(IS_PIXEL_BUFFER_PAGES * 3 / 4)
+
+union fabric_ctrl {
+	struct {
+		u16 ltr_val   : 10;
+		u16 ltr_scale : 3;
+		u16 reserved  : 3;
+		u16 did_val   : 10;
+		u16 did_scale : 3;
+		u16 reserved2 : 1;
+		u16 keep_power_in_D0   : 1;
+		u16 keep_power_override : 1;
+	} bits;
+	u32 value;
+};
+
+enum ltr_did_type {
+	LTR_IWAKE_ON,
+	LTR_IWAKE_OFF,
+	LTR_ISYS_ON,
+	LTR_ISYS_OFF,
+	LTR_ENHANNCE_IWAKE,
+	LTR_TYPE_MAX
+};
+
+#define ISYS_PM_QOS_VALUE	300
+
+static int isys_isr_one(struct ipu6_bus_device *adev);
+
+static int
+isys_complete_ext_device_registration(struct ipu6_isys *isys,
+				      struct v4l2_subdev *sd,
+				      struct ipu6_isys_csi2_config *csi2)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < sd->entity.num_pads; i++) {
+		if (sd->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE)
+			break;
+	}
+
+	if (i == sd->entity.num_pads) {
+		dev_warn(dev, "no src pad in external entity\n");
+		ret = -ENOENT;
+		goto unregister_subdev;
+	}
+
+	ret = media_create_pad_link(&sd->entity, i,
+				    &isys->csi2[csi2->port].asd.sd.entity,
+				    0, MEDIA_LNK_FL_ENABLED |
+				       MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		dev_warn(dev, "can't create link\n");
+		goto unregister_subdev;
+	}
+
+	isys->csi2[csi2->port].nlanes = csi2->nlanes;
+
+	return 0;
+
+unregister_subdev:
+	v4l2_device_unregister_subdev(sd);
+
+	return ret;
+}
+
+static void isys_stream_init(struct ipu6_isys *isys)
+{
+	u32 i;
+
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++) {
+		mutex_init(&isys->streams[i].mutex);
+		init_completion(&isys->streams[i].stream_open_completion);
+		init_completion(&isys->streams[i].stream_close_completion);
+		init_completion(&isys->streams[i].stream_start_completion);
+		init_completion(&isys->streams[i].stream_stop_completion);
+		INIT_LIST_HEAD(&isys->streams[i].queues);
+		isys->streams[i].isys = isys;
+		isys->streams[i].stream_handle = i;
+		isys->streams[i].vc = INVALID_VC_ID;
+	}
+}
+
+static void isys_csi2_unregister_subdevices(struct ipu6_isys *isys)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2 =
+		&isys->pdata->ipdata->csi2;
+	unsigned int i;
+
+	for (i = 0; i < csi2->nports; i++)
+		ipu6_isys_csi2_cleanup(&isys->csi2[i]);
+}
+
+static int isys_csi2_register_subdevices(struct ipu6_isys *isys)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2_pdata =
+		&isys->pdata->ipdata->csi2;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < csi2_pdata->nports; i++) {
+		ret = ipu6_isys_csi2_init(&isys->csi2[i], isys,
+					  isys->pdata->base +
+					  CSI_REG_PORT_BASE(i), i);
+		if (ret)
+			goto fail;
+
+		isys->isr_csi2_bits |= IPU6_ISYS_UNISPART_IRQ_CSI2(i);
+	}
+
+	return 0;
+
+fail:
+	while (i--)
+		ipu6_isys_csi2_cleanup(&isys->csi2[i]);
+
+	return ret;
+}
+
+static int isys_csi2_create_media_links(struct ipu6_isys *isys)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2_pdata =
+		&isys->pdata->ipdata->csi2;
+	struct device *dev = &isys->adev->auxdev.dev;
+	unsigned int i, j;
+	int ret;
+
+	for (i = 0; i < csi2_pdata->nports; i++) {
+		struct media_entity *sd = &isys->csi2[i].asd.sd.entity;
+
+		for (j = 0; j < NR_OF_CSI2_SRC_PADS; j++) {
+			struct ipu6_isys_video *av = &isys->csi2[i].av[j];
+
+			ret = media_create_pad_link(sd, CSI2_PAD_SRC + j,
+						    &av->vdev.entity, 0, 0);
+			if (ret) {
+				dev_err(dev, "CSI2 can't create link\n");
+				return ret;
+			}
+
+			av->csi2 = &isys->csi2[i];
+		}
+	}
+
+	return 0;
+}
+
+static void isys_unregister_video_devices(struct ipu6_isys *isys)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2_pdata =
+		&isys->pdata->ipdata->csi2;
+	unsigned int i, j;
+
+	for (i = 0; i < csi2_pdata->nports; i++)
+		for (j = 0; j < NR_OF_CSI2_SRC_PADS; j++)
+			ipu6_isys_video_cleanup(&isys->csi2[i].av[j]);
+}
+
+static int isys_register_video_devices(struct ipu6_isys *isys)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2_pdata =
+		&isys->pdata->ipdata->csi2;
+	unsigned int i, j;
+	int ret;
+
+	for (i = 0; i < csi2_pdata->nports; i++) {
+		for (j = 0; j < NR_OF_CSI2_SRC_PADS; j++) {
+			struct ipu6_isys_video *av = &isys->csi2[i].av[j];
+
+			snprintf(av->vdev.name, sizeof(av->vdev.name),
+				 IPU6_ISYS_ENTITY_PREFIX " ISYS Capture %u",
+				 i * NR_OF_CSI2_SRC_PADS + j);
+			av->isys = isys;
+			av->aq.vbq.buf_struct_size =
+				sizeof(struct ipu6_isys_video_buffer);
+
+			ret = ipu6_isys_video_init(av);
+			if (ret)
+				goto fail;
+		}
+	}
+
+	return 0;
+
+fail:
+	while (i--) {
+		while (j--)
+			ipu6_isys_video_cleanup(&isys->csi2[i].av[j]);
+		j = NR_OF_CSI2_SRC_PADS;
+	}
+
+	return ret;
+}
+
+void isys_setup_hw(struct ipu6_isys *isys)
+{
+	void __iomem *base = isys->pdata->base;
+	const u8 *thd = isys->pdata->ipdata->hw_variant.cdc_fifo_threshold;
+	u32 irqs = 0;
+	unsigned int i, nports;
+
+	nports = isys->pdata->ipdata->csi2.nports;
+
+	/* Enable irqs for all MIPI ports */
+	for (i = 0; i < nports; i++)
+		irqs |= IPU6_ISYS_UNISPART_IRQ_CSI2(i);
+
+	writel(irqs, base + isys->pdata->ipdata->csi2.ctrl0_irq_edge);
+	writel(irqs, base + isys->pdata->ipdata->csi2.ctrl0_irq_lnp);
+	writel(irqs, base + isys->pdata->ipdata->csi2.ctrl0_irq_mask);
+	writel(irqs, base + isys->pdata->ipdata->csi2.ctrl0_irq_enable);
+	writel(GENMASK(19, 0),
+	       base + isys->pdata->ipdata->csi2.ctrl0_irq_clear);
+
+	irqs = ISYS_UNISPART_IRQS;
+	writel(irqs, base + IPU6_REG_ISYS_UNISPART_IRQ_EDGE);
+	writel(irqs, base + IPU6_REG_ISYS_UNISPART_IRQ_LEVEL_NOT_PULSE);
+	writel(GENMASK(28, 0), base + IPU6_REG_ISYS_UNISPART_IRQ_CLEAR);
+	writel(irqs, base + IPU6_REG_ISYS_UNISPART_IRQ_MASK);
+	writel(irqs, base + IPU6_REG_ISYS_UNISPART_IRQ_ENABLE);
+
+	writel(0, base + IPU6_REG_ISYS_UNISPART_SW_IRQ_REG);
+	writel(0, base + IPU6_REG_ISYS_UNISPART_SW_IRQ_MUX_REG);
+
+	/* Write CDC FIFO threshold values for isys */
+	for (i = 0; i < isys->pdata->ipdata->hw_variant.cdc_fifos; i++)
+		writel(thd[i], base + IPU6_REG_ISYS_CDC_THRESHOLD(i));
+}
+
+static void ipu6_isys_csi2_isr(struct ipu6_isys_csi2 *csi2)
+{
+	struct ipu6_isys_stream *stream;
+	unsigned int i;
+	u32 status;
+	int source;
+
+	ipu6_isys_register_errors(csi2);
+
+	status = readl(csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+		       CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET);
+
+	writel(status, csi2->base + CSI_PORT_REG_BASE_IRQ_CSI_SYNC +
+	       CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET);
+
+	source = csi2->asd.source;
+	for (i = 0; i < NR_OF_CSI2_VC; i++) {
+		if (status & IPU_CSI_RX_IRQ_FS_VC(i)) {
+			stream = ipu6_isys_query_stream_by_source(csi2->isys,
+								  source, i);
+			if (stream) {
+				ipu6_isys_csi2_sof_event_by_stream(stream);
+				ipu6_isys_put_stream(stream);
+			}
+		}
+
+		if (status & IPU_CSI_RX_IRQ_FE_VC(i)) {
+			stream = ipu6_isys_query_stream_by_source(csi2->isys,
+								  source, i);
+			if (stream) {
+				ipu6_isys_csi2_eof_event_by_stream(stream);
+				ipu6_isys_put_stream(stream);
+			}
+		}
+	}
+}
+
+irqreturn_t isys_isr(struct ipu6_bus_device *adev)
+{
+	struct ipu6_isys *isys = ipu6_bus_get_drvdata(adev);
+	void __iomem *base = isys->pdata->base;
+	u32 status_sw, status_csi;
+	u32 ctrl0_status, ctrl0_clear;
+
+	spin_lock(&isys->power_lock);
+	if (!isys->power) {
+		spin_unlock(&isys->power_lock);
+		return IRQ_NONE;
+	}
+
+	ctrl0_status = isys->pdata->ipdata->csi2.ctrl0_irq_status;
+	ctrl0_clear = isys->pdata->ipdata->csi2.ctrl0_irq_clear;
+
+	status_csi = readl(isys->pdata->base + ctrl0_status);
+	status_sw = readl(isys->pdata->base +
+			  IPU6_REG_ISYS_UNISPART_IRQ_STATUS);
+
+	writel(ISYS_UNISPART_IRQS & ~IPU6_ISYS_UNISPART_IRQ_SW,
+	       base + IPU6_REG_ISYS_UNISPART_IRQ_MASK);
+
+	do {
+		writel(status_csi, isys->pdata->base + ctrl0_clear);
+
+		writel(status_sw, isys->pdata->base +
+		       IPU6_REG_ISYS_UNISPART_IRQ_CLEAR);
+
+		if (isys->isr_csi2_bits & status_csi) {
+			unsigned int i;
+
+			for (i = 0; i < isys->pdata->ipdata->csi2.nports; i++) {
+				/* irq from not enabled port */
+				if (!isys->csi2[i].base)
+					continue;
+				if (status_csi & IPU6_ISYS_UNISPART_IRQ_CSI2(i))
+					ipu6_isys_csi2_isr(&isys->csi2[i]);
+			}
+		}
+
+		writel(0, base + IPU6_REG_ISYS_UNISPART_SW_IRQ_REG);
+
+		if (!isys_isr_one(adev))
+			status_sw = IPU6_ISYS_UNISPART_IRQ_SW;
+		else
+			status_sw = 0;
+
+		status_csi = readl(isys->pdata->base + ctrl0_status);
+		status_sw |= readl(isys->pdata->base +
+				   IPU6_REG_ISYS_UNISPART_IRQ_STATUS);
+	} while ((status_csi & isys->isr_csi2_bits) ||
+		 (status_sw & IPU6_ISYS_UNISPART_IRQ_SW));
+
+	writel(ISYS_UNISPART_IRQS, base + IPU6_REG_ISYS_UNISPART_IRQ_MASK);
+
+	spin_unlock(&isys->power_lock);
+
+	return IRQ_HANDLED;
+}
+
+static void get_lut_ltrdid(struct ipu6_isys *isys, struct ltr_did *pltr_did)
+{
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+	struct ltr_did ltrdid_default;
+
+	ltrdid_default.lut_ltr.value = LTR_DEFAULT_VALUE;
+	ltrdid_default.lut_fill_time.value = FILL_TIME_DEFAULT_VALUE;
+
+	if (iwake_watermark->ltrdid.lut_ltr.value)
+		*pltr_did = iwake_watermark->ltrdid;
+	else
+		*pltr_did = ltrdid_default;
+}
+
+static int set_iwake_register(struct ipu6_isys *isys, u32 index, u32 value)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	u32 req_id = index;
+	u32 offset = 0;
+	int ret;
+
+	ret = ipu6_fw_isys_send_proxy_token(isys, req_id, index, offset, value);
+	if (ret)
+		dev_err(dev, "write %d failed %d", index, ret);
+
+	return ret;
+}
+
+/*
+ * When input system is powered up and before enabling any new sensor capture,
+ * or after disabling any sensor capture the following values need to be set:
+ * LTR_value = LTR(usec) from calculation;
+ * LTR_scale = 2;
+ * DID_value = DID(usec) from calculation;
+ * DID_scale = 2;
+ *
+ * When input system is powered down, the LTR and DID values
+ * must be returned to the default values:
+ * LTR_value = 1023;
+ * LTR_scale = 5;
+ * DID_value = 1023;
+ * DID_scale = 2;
+ */
+static void set_iwake_ltrdid(struct ipu6_isys *isys, u16 ltr, u16 did,
+			     enum ltr_did_type use)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	u16 ltr_val, ltr_scale = LTR_SCALE_1024NS;
+	u16 did_val, did_scale = DID_SCALE_1US;
+	struct ipu6_device *isp = isys->adev->isp;
+	union fabric_ctrl fc;
+
+	switch (use) {
+	case LTR_IWAKE_ON:
+		ltr_val = min_t(u16, ltr, (u16)LTR_DID_VAL_MAX);
+		did_val = min_t(u16, did, (u16)LTR_DID_VAL_MAX);
+		ltr_scale = (ltr == LTR_DID_VAL_MAX &&
+			     did == LTR_DID_VAL_MAX) ?
+			LTR_SCALE_DEFAULT : LTR_SCALE_1024NS;
+		break;
+	case LTR_ISYS_ON:
+	case LTR_IWAKE_OFF:
+		ltr_val = LTR_DID_PKGC_2R;
+		did_val = LTR_DID_PKGC_2R;
+		break;
+	case LTR_ISYS_OFF:
+		ltr_val   = LTR_DID_VAL_MAX;
+		did_val   = LTR_DID_VAL_MAX;
+		ltr_scale = LTR_SCALE_DEFAULT;
+		break;
+	case LTR_ENHANNCE_IWAKE:
+		if (ltr == LTR_DID_VAL_MAX && did == LTR_DID_VAL_MAX) {
+			ltr_val = LTR_DID_VAL_MAX;
+			did_val = LTR_DID_VAL_MAX;
+			ltr_scale = LTR_SCALE_DEFAULT;
+		} else if (did < ONE_THOUSAND_MICROSECOND) {
+			ltr_val = ltr;
+			did_val = did;
+		} else {
+			ltr_val = ltr;
+			/* div 90% value by 32 to account for scale change */
+			did_val = did / 32;
+			did_scale = DID_SCALE_32US;
+		}
+		break;
+	default:
+		ltr_val   = LTR_DID_VAL_MAX;
+		did_val   = LTR_DID_VAL_MAX;
+		ltr_scale = LTR_SCALE_DEFAULT;
+		break;
+	}
+
+	fc.value = readl(isp->base + IPU6_BUTTRESS_FABIC_CONTROL);
+	fc.bits.ltr_val = ltr_val;
+	fc.bits.ltr_scale = ltr_scale;
+	fc.bits.did_val = did_val;
+	fc.bits.did_scale = did_scale;
+
+	dev_dbg(dev, "ltr: value %u scale %u, did: value %u scale %u\n",
+		ltr_val, ltr_scale, did_val, did_scale);
+	writel(fc.value, isp->base + IPU6_BUTTRESS_FABIC_CONTROL);
+}
+
+/*
+ * Driver may clear register GDA_ENABLE_IWAKE before FW configures the
+ * stream for debug purpose. Otherwise driver should not access this register.
+ */
+static void enable_iwake(struct ipu6_isys *isys, bool enable)
+{
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+	int ret;
+
+	mutex_lock(&iwake_watermark->mutex);
+
+	if (iwake_watermark->iwake_enabled == enable) {
+		mutex_unlock(&iwake_watermark->mutex);
+		return;
+	}
+
+	ret = set_iwake_register(isys, GDA_ENABLE_IWAKE_INDEX, enable);
+	if (!ret)
+		iwake_watermark->iwake_enabled = enable;
+
+	mutex_unlock(&iwake_watermark->mutex);
+}
+
+void update_watermark_setting(struct ipu6_isys *isys)
+{
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+	u32 iwake_threshold, iwake_critical_threshold, page_num;
+	struct device *dev = &isys->adev->auxdev.dev;
+	u32 calc_fill_time_us = 0, ltr = 0, did = 0;
+	struct video_stream_watermark *p_watermark;
+	enum ltr_did_type ltr_did_type;
+	struct list_head *stream_node;
+	u64 isys_pb_datarate_mbs = 0;
+	u32 mem_open_threshold = 0;
+	struct ltr_did ltrdid;
+	u64 threshold_bytes;
+	u32 max_sram_size;
+	u32 shift;
+
+	shift = isys->pdata->ipdata->sram_gran_shift;
+	max_sram_size = isys->pdata->ipdata->max_sram_size;
+
+	mutex_lock(&iwake_watermark->mutex);
+	if (iwake_watermark->force_iwake_disable) {
+		set_iwake_ltrdid(isys, 0, 0, LTR_IWAKE_OFF);
+		set_iwake_register(isys, GDA_IRQ_CRITICAL_THRESHOLD_INDEX,
+				   CRITICAL_THRESHOLD_IWAKE_DISABLE);
+		goto unlock_exit;
+	}
+
+	if (list_empty(&iwake_watermark->video_list)) {
+		isys_pb_datarate_mbs = 0;
+	} else {
+		list_for_each(stream_node, &iwake_watermark->video_list) {
+			p_watermark = list_entry(stream_node,
+						 struct video_stream_watermark,
+						 stream_node);
+			isys_pb_datarate_mbs += p_watermark->stream_data_rate;
+		}
+	}
+	mutex_unlock(&iwake_watermark->mutex);
+
+	if (!isys_pb_datarate_mbs) {
+		enable_iwake(isys, false);
+		set_iwake_ltrdid(isys, 0, 0, LTR_IWAKE_OFF);
+		mutex_lock(&iwake_watermark->mutex);
+		set_iwake_register(isys, GDA_IRQ_CRITICAL_THRESHOLD_INDEX,
+				   CRITICAL_THRESHOLD_IWAKE_DISABLE);
+		goto unlock_exit;
+	}
+
+	enable_iwake(isys, true);
+	calc_fill_time_us = div64_u64(max_sram_size, isys_pb_datarate_mbs);
+
+	if (isys->pdata->ipdata->enhanced_iwake) {
+		ltr = isys->pdata->ipdata->ltr;
+		did = calc_fill_time_us * DEFAULT_DID_RATIO / 100;
+		ltr_did_type = LTR_ENHANNCE_IWAKE;
+	} else {
+		get_lut_ltrdid(isys, &ltrdid);
+
+		if (calc_fill_time_us <= ltrdid.lut_fill_time.bits.th0)
+			ltr = 0;
+		else if (calc_fill_time_us <= ltrdid.lut_fill_time.bits.th1)
+			ltr = ltrdid.lut_ltr.bits.val0;
+		else if (calc_fill_time_us <= ltrdid.lut_fill_time.bits.th2)
+			ltr = ltrdid.lut_ltr.bits.val1;
+		else if (calc_fill_time_us <= ltrdid.lut_fill_time.bits.th3)
+			ltr = ltrdid.lut_ltr.bits.val2;
+		else
+			ltr = ltrdid.lut_ltr.bits.val3;
+
+		did = calc_fill_time_us - ltr;
+		ltr_did_type = LTR_IWAKE_ON;
+	}
+
+	set_iwake_ltrdid(isys, ltr, did, ltr_did_type);
+
+	/* calculate iwake threshold with 2KB granularity pages */
+	threshold_bytes = did * isys_pb_datarate_mbs;
+	iwake_threshold = max_t(u32, 1, threshold_bytes >> shift);
+	iwake_threshold = min_t(u32, iwake_threshold, max_sram_size);
+
+	mutex_lock(&iwake_watermark->mutex);
+	if (isys->pdata->ipdata->enhanced_iwake) {
+		set_iwake_register(isys, GDA_IWAKE_THRESHOLD_INDEX,
+				   DEFAULT_IWAKE_THRESHOLD);
+		/* calculate number of pages that will be filled in 10 usec */
+		page_num = (DEFAULT_MEM_OPEN_TIME * isys_pb_datarate_mbs) /
+			ISF_DMA_TOP_GDA_PROFERTY_PAGE_SIZE;
+		page_num += ((DEFAULT_MEM_OPEN_TIME * isys_pb_datarate_mbs) %
+			     ISF_DMA_TOP_GDA_PROFERTY_PAGE_SIZE) ? 1 : 0;
+		mem_open_threshold = isys->pdata->ipdata->memopen_threshold;
+		mem_open_threshold = max_t(u32, mem_open_threshold, page_num);
+		dev_dbg(dev, "mem_open_threshold: %u\n", mem_open_threshold);
+		set_iwake_register(isys, GDA_MEMOPEN_THRESHOLD_INDEX,
+				   mem_open_threshold);
+	} else {
+		set_iwake_register(isys, GDA_IWAKE_THRESHOLD_INDEX,
+				   iwake_threshold);
+	}
+
+	iwake_critical_threshold = iwake_threshold +
+		(IS_PIXEL_BUFFER_PAGES - iwake_threshold) / 2;
+
+	dev_dbg(dev, "threshold: %u critical: %u\n", iwake_threshold,
+		iwake_critical_threshold);
+
+	set_iwake_register(isys, GDA_IRQ_CRITICAL_THRESHOLD_INDEX,
+			   iwake_critical_threshold);
+
+	writel(VAL_PKGC_PMON_CFG_RESET,
+	       isys->adev->isp->base + REG_PKGC_PMON_CFG);
+	writel(VAL_PKGC_PMON_CFG_START,
+	       isys->adev->isp->base + REG_PKGC_PMON_CFG);
+unlock_exit:
+	mutex_unlock(&iwake_watermark->mutex);
+}
+
+static void isys_iwake_watermark_init(struct ipu6_isys *isys)
+{
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+
+	INIT_LIST_HEAD(&iwake_watermark->video_list);
+	mutex_init(&iwake_watermark->mutex);
+
+	iwake_watermark->ltrdid.lut_ltr.value = 0;
+	iwake_watermark->isys = isys;
+	iwake_watermark->iwake_enabled = false;
+	iwake_watermark->force_iwake_disable = false;
+}
+
+static void isys_iwake_watermark_cleanup(struct ipu6_isys *isys)
+{
+	struct isys_iwake_watermark *iwake_watermark = &isys->iwake_watermark;
+
+	mutex_lock(&iwake_watermark->mutex);
+	list_del(&iwake_watermark->video_list);
+	mutex_unlock(&iwake_watermark->mutex);
+
+	mutex_destroy(&iwake_watermark->mutex);
+}
+
+/* The .bound() notifier callback when a match is found */
+static int isys_notifier_bound(struct v4l2_async_notifier *notifier,
+			       struct v4l2_subdev *sd,
+			       struct v4l2_async_connection *asc)
+{
+	struct ipu6_isys *isys =
+		container_of(notifier, struct ipu6_isys, notifier);
+	struct sensor_async_sd *s_asd =
+		container_of(asc, struct sensor_async_sd, asc);
+	int ret;
+
+	if (s_asd->csi2.port >= isys->pdata->ipdata->csi2.nports) {
+		dev_err(&isys->adev->auxdev.dev, "invalid csi2 port %u\n",
+			s_asd->csi2.port);
+		return -EINVAL;
+	}
+
+	ret = ipu_bridge_instantiate_vcm(sd->dev);
+	if (ret) {
+		dev_err(&isys->adev->auxdev.dev, "instantiate vcm failed\n");
+		return ret;
+	}
+
+	dev_dbg(&isys->adev->auxdev.dev, "bind %s nlanes is %d port is %d\n",
+		sd->name, s_asd->csi2.nlanes, s_asd->csi2.port);
+	ret = isys_complete_ext_device_registration(isys, sd, &s_asd->csi2);
+	if (ret)
+		return ret;
+
+	return v4l2_device_register_subdev_nodes(&isys->v4l2_dev);
+}
+
+static int isys_notifier_complete(struct v4l2_async_notifier *notifier)
+{
+	struct ipu6_isys *isys =
+		container_of(notifier, struct ipu6_isys, notifier);
+
+	return v4l2_device_register_subdev_nodes(&isys->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations isys_async_ops = {
+	.bound = isys_notifier_bound,
+	.complete = isys_notifier_complete,
+};
+
+#define ISYS_MAX_PORTS 8
+static int isys_notifier_init(struct ipu6_isys *isys)
+{
+	struct ipu6_device *isp = isys->adev->isp;
+	struct device *dev = &isp->pdev->dev;
+	unsigned int i;
+	int ret;
+
+	v4l2_async_nf_init(&isys->notifier, &isys->v4l2_dev);
+
+	for (i = 0; i < ISYS_MAX_PORTS; i++) {
+		struct v4l2_fwnode_endpoint vep = {
+			.bus_type = V4L2_MBUS_CSI2_DPHY
+		};
+		struct sensor_async_sd *s_asd;
+		struct fwnode_handle *ep;
+
+		ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), i, 0,
+						FWNODE_GRAPH_ENDPOINT_NEXT);
+		if (!ep)
+			continue;
+
+		ret = v4l2_fwnode_endpoint_parse(ep, &vep);
+		if (ret) {
+			dev_err(dev, "fwnode endpoint parse failed: %d\n", ret);
+			goto err_parse;
+		}
+
+		s_asd = v4l2_async_nf_add_fwnode_remote(&isys->notifier, ep,
+							struct sensor_async_sd);
+		if (IS_ERR(s_asd)) {
+			ret = PTR_ERR(s_asd);
+			dev_err(dev, "add remove fwnode failed: %d\n", ret);
+			goto err_parse;
+		}
+
+		s_asd->csi2.port = vep.base.port;
+		s_asd->csi2.nlanes = vep.bus.mipi_csi2.num_data_lanes;
+
+		dev_dbg(dev, "remote endpoint port %d with %d lanes added\n",
+			s_asd->csi2.port, s_asd->csi2.nlanes);
+
+		fwnode_handle_put(ep);
+
+		continue;
+
+err_parse:
+		fwnode_handle_put(ep);
+		return ret;
+	}
+
+	isys->notifier.ops = &isys_async_ops;
+	ret = v4l2_async_nf_register(&isys->notifier);
+	if (ret) {
+		dev_err(dev, "failed to register async notifier : %d\n", ret);
+		v4l2_async_nf_cleanup(&isys->notifier);
+	}
+
+	return ret;
+}
+
+static void isys_notifier_cleanup(struct ipu6_isys *isys)
+{
+	v4l2_async_nf_unregister(&isys->notifier);
+	v4l2_async_nf_cleanup(&isys->notifier);
+}
+
+static int isys_register_devices(struct ipu6_isys *isys)
+{
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct pci_dev *pdev = isys->adev->isp->pdev;
+	int ret;
+
+	isys->media_dev.dev = dev;
+	media_device_pci_init(&isys->media_dev,
+			      pdev, IPU6_MEDIA_DEV_MODEL_NAME);
+
+	strscpy(isys->v4l2_dev.name, isys->media_dev.model,
+		sizeof(isys->v4l2_dev.name));
+
+	ret = media_device_register(&isys->media_dev);
+	if (ret < 0)
+		goto out_media_device_unregister;
+
+	isys->v4l2_dev.mdev = &isys->media_dev;
+	isys->v4l2_dev.ctrl_handler = NULL;
+
+	ret = v4l2_device_register(dev, &isys->v4l2_dev);
+	if (ret < 0)
+		goto out_media_device_unregister;
+
+	ret = isys_register_video_devices(isys);
+	if (ret)
+		goto out_v4l2_device_unregister;
+
+	ret = isys_csi2_register_subdevices(isys);
+	if (ret)
+		goto out_isys_unregister_video_device;
+
+	ret = isys_csi2_create_media_links(isys);
+	if (ret)
+		goto out_isys_unregister_subdevices;
+
+	ret = isys_notifier_init(isys);
+	if (ret)
+		goto out_isys_unregister_subdevices;
+
+	return 0;
+
+out_isys_unregister_subdevices:
+	isys_csi2_unregister_subdevices(isys);
+
+out_isys_unregister_video_device:
+	isys_unregister_video_devices(isys);
+
+out_v4l2_device_unregister:
+	v4l2_device_unregister(&isys->v4l2_dev);
+
+out_media_device_unregister:
+	media_device_unregister(&isys->media_dev);
+	media_device_cleanup(&isys->media_dev);
+
+	dev_err(dev, "failed to register isys devices\n");
+
+	return ret;
+}
+
+static void isys_unregister_devices(struct ipu6_isys *isys)
+{
+	isys_unregister_video_devices(isys);
+	isys_csi2_unregister_subdevices(isys);
+	v4l2_device_unregister(&isys->v4l2_dev);
+	media_device_unregister(&isys->media_dev);
+	media_device_cleanup(&isys->media_dev);
+}
+
+static int isys_runtime_pm_resume(struct device *dev)
+{
+	struct ipu6_bus_device *adev = to_ipu6_bus_device(dev);
+	struct ipu6_isys *isys = ipu6_bus_get_drvdata(adev);
+	struct ipu6_device *isp = adev->isp;
+	unsigned long flags;
+	int ret;
+
+	if (!isys)
+		return 0;
+
+	ret = ipu6_mmu_hw_init(adev->mmu);
+	if (ret)
+		return ret;
+
+	cpu_latency_qos_update_request(&isys->pm_qos, ISYS_PM_QOS_VALUE);
+
+	ret = ipu6_buttress_start_tsc_sync(isp);
+	if (ret)
+		return ret;
+
+	spin_lock_irqsave(&isys->power_lock, flags);
+	isys->power = 1;
+	spin_unlock_irqrestore(&isys->power_lock, flags);
+
+	isys_setup_hw(isys);
+
+	set_iwake_ltrdid(isys, 0, 0, LTR_ISYS_ON);
+
+	return 0;
+}
+
+static int isys_runtime_pm_suspend(struct device *dev)
+{
+	struct ipu6_bus_device *adev = to_ipu6_bus_device(dev);
+	struct ipu6_isys *isys;
+	unsigned long flags;
+
+	isys = dev_get_drvdata(dev);
+	if (!isys)
+		return 0;
+
+	spin_lock_irqsave(&isys->power_lock, flags);
+	isys->power = 0;
+	spin_unlock_irqrestore(&isys->power_lock, flags);
+
+	mutex_lock(&isys->mutex);
+	isys->need_reset = false;
+	mutex_unlock(&isys->mutex);
+
+	isys->phy_termcal_val = 0;
+	cpu_latency_qos_update_request(&isys->pm_qos, PM_QOS_DEFAULT_VALUE);
+
+	set_iwake_ltrdid(isys, 0, 0, LTR_ISYS_OFF);
+
+	ipu6_mmu_hw_cleanup(adev->mmu);
+
+	return 0;
+}
+
+static int isys_suspend(struct device *dev)
+{
+	struct ipu6_isys *isys = dev_get_drvdata(dev);
+
+	/* If stream is open, refuse to suspend */
+	if (isys->stream_opened)
+		return -EBUSY;
+
+	return 0;
+}
+
+static int isys_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops isys_pm_ops = {
+	.runtime_suspend = isys_runtime_pm_suspend,
+	.runtime_resume = isys_runtime_pm_resume,
+	.suspend = isys_suspend,
+	.resume = isys_resume,
+};
+
+static void free_fw_msg_bufs(struct ipu6_isys *isys)
+{
+	struct isys_fw_msgs *fwmsg, *safe;
+
+	list_for_each_entry_safe(fwmsg, safe, &isys->framebuflist, head)
+		ipu6_dma_free(isys->adev, sizeof(struct isys_fw_msgs), fwmsg,
+			      fwmsg->dma_addr, 0);
+
+	list_for_each_entry_safe(fwmsg, safe, &isys->framebuflist_fw, head)
+		ipu6_dma_free(isys->adev, sizeof(struct isys_fw_msgs), fwmsg,
+			      fwmsg->dma_addr, 0);
+}
+
+static int alloc_fw_msg_bufs(struct ipu6_isys *isys, int amount)
+{
+	struct isys_fw_msgs *addr;
+	dma_addr_t dma_addr;
+	unsigned long flags;
+	unsigned int i;
+
+	for (i = 0; i < amount; i++) {
+		addr = ipu6_dma_alloc(isys->adev, sizeof(*addr),
+				      &dma_addr, GFP_KERNEL, 0);
+		if (!addr)
+			break;
+		addr->dma_addr = dma_addr;
+
+		spin_lock_irqsave(&isys->listlock, flags);
+		list_add(&addr->head, &isys->framebuflist);
+		spin_unlock_irqrestore(&isys->listlock, flags);
+	}
+
+	if (i == amount)
+		return 0;
+
+	spin_lock_irqsave(&isys->listlock, flags);
+	while (!list_empty(&isys->framebuflist)) {
+		addr = list_first_entry(&isys->framebuflist,
+					struct isys_fw_msgs, head);
+		list_del(&addr->head);
+		spin_unlock_irqrestore(&isys->listlock, flags);
+		ipu6_dma_free(isys->adev, sizeof(struct isys_fw_msgs), addr,
+			      addr->dma_addr, 0);
+		spin_lock_irqsave(&isys->listlock, flags);
+	}
+	spin_unlock_irqrestore(&isys->listlock, flags);
+
+	return -ENOMEM;
+}
+
+struct isys_fw_msgs *ipu6_get_fw_msg_buf(struct ipu6_isys_stream *stream)
+{
+	struct ipu6_isys *isys = stream->isys;
+	struct device *dev = &isys->adev->auxdev.dev;
+	struct isys_fw_msgs *msg;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&isys->listlock, flags);
+	if (list_empty(&isys->framebuflist)) {
+		spin_unlock_irqrestore(&isys->listlock, flags);
+		dev_dbg(dev, "Frame list empty\n");
+
+		ret = alloc_fw_msg_bufs(isys, 5);
+		if (ret < 0)
+			return NULL;
+
+		spin_lock_irqsave(&isys->listlock, flags);
+		if (list_empty(&isys->framebuflist)) {
+			spin_unlock_irqrestore(&isys->listlock, flags);
+			dev_err(dev, "Frame list empty\n");
+			return NULL;
+		}
+	}
+	msg = list_last_entry(&isys->framebuflist, struct isys_fw_msgs, head);
+	list_move(&msg->head, &isys->framebuflist_fw);
+	spin_unlock_irqrestore(&isys->listlock, flags);
+	memset(&msg->fw_msg, 0, sizeof(msg->fw_msg));
+
+	return msg;
+}
+
+void ipu6_cleanup_fw_msg_bufs(struct ipu6_isys *isys)
+{
+	struct isys_fw_msgs *fwmsg, *fwmsg0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&isys->listlock, flags);
+	list_for_each_entry_safe(fwmsg, fwmsg0, &isys->framebuflist_fw, head)
+		list_move(&fwmsg->head, &isys->framebuflist);
+	spin_unlock_irqrestore(&isys->listlock, flags);
+}
+
+void ipu6_put_fw_msg_buf(struct ipu6_isys *isys, uintptr_t data)
+{
+	struct isys_fw_msgs *msg;
+	unsigned long flags;
+	void *ptr = (void *)data;
+
+	if (!ptr)
+		return;
+
+	spin_lock_irqsave(&isys->listlock, flags);
+	msg = container_of(ptr, struct isys_fw_msgs, fw_msg.dummy);
+	list_move(&msg->head, &isys->framebuflist);
+	spin_unlock_irqrestore(&isys->listlock, flags);
+}
+
+static int isys_probe(struct auxiliary_device *auxdev,
+		      const struct auxiliary_device_id *auxdev_id)
+{
+	const struct ipu6_isys_internal_csi2_pdata *csi2_pdata;
+	struct ipu6_bus_device *adev = auxdev_to_adev(auxdev);
+	struct ipu6_device *isp = adev->isp;
+	const struct firmware *fw;
+	struct ipu6_isys *isys;
+	unsigned int i;
+	int ret;
+
+	if (!isp->bus_ready_to_probe)
+		return -EPROBE_DEFER;
+
+	isys = devm_kzalloc(&auxdev->dev, sizeof(*isys), GFP_KERNEL);
+	if (!isys)
+		return -ENOMEM;
+
+	adev->auxdrv_data =
+		(const struct ipu6_auxdrv_data *)auxdev_id->driver_data;
+	adev->auxdrv = to_auxiliary_drv(auxdev->dev.driver);
+	isys->adev = adev;
+	isys->pdata = adev->pdata;
+	csi2_pdata = &isys->pdata->ipdata->csi2;
+
+	isys->csi2 = devm_kcalloc(&auxdev->dev, csi2_pdata->nports,
+				  sizeof(*isys->csi2), GFP_KERNEL);
+	if (!isys->csi2)
+		return -ENOMEM;
+
+	ret = ipu6_mmu_hw_init(adev->mmu);
+	if (ret)
+		return ret;
+
+	/* initial sensor type */
+	isys->sensor_type = isys->pdata->ipdata->sensor_type_start;
+
+	spin_lock_init(&isys->streams_lock);
+	spin_lock_init(&isys->power_lock);
+	isys->power = 0;
+	isys->phy_termcal_val = 0;
+
+	mutex_init(&isys->mutex);
+	mutex_init(&isys->stream_mutex);
+
+	spin_lock_init(&isys->listlock);
+	INIT_LIST_HEAD(&isys->framebuflist);
+	INIT_LIST_HEAD(&isys->framebuflist_fw);
+
+	isys->icache_prefetch = 0;
+
+	dev_set_drvdata(&auxdev->dev, isys);
+
+	isys_stream_init(isys);
+
+	if (!isp->secure_mode) {
+		fw = isp->cpd_fw;
+		ret = ipu6_buttress_map_fw_image(adev, fw, &adev->fw_sgt);
+		if (ret)
+			goto release_firmware;
+
+		ret = ipu6_cpd_create_pkg_dir(adev, isp->cpd_fw->data);
+		if (ret)
+			goto remove_shared_buffer;
+	}
+
+	cpu_latency_qos_add_request(&isys->pm_qos, PM_QOS_DEFAULT_VALUE);
+
+	ret = alloc_fw_msg_bufs(isys, 20);
+	if (ret < 0)
+		goto out_remove_pkg_dir_shared_buffer;
+
+	isys_iwake_watermark_init(isys);
+
+	if (is_ipu6se(adev->isp->hw_ver))
+		isys->phy_set_power = ipu6_isys_jsl_phy_set_power;
+	else if (is_ipu6ep_mtl(adev->isp->hw_ver))
+		isys->phy_set_power = ipu6_isys_dwc_phy_set_power;
+	else
+		isys->phy_set_power = ipu6_isys_mcd_phy_set_power;
+
+	ret = isys_register_devices(isys);
+	if (ret)
+		goto free_fw_msg_bufs;
+
+	ipu6_mmu_hw_cleanup(adev->mmu);
+
+	return 0;
+
+free_fw_msg_bufs:
+	free_fw_msg_bufs(isys);
+out_remove_pkg_dir_shared_buffer:
+	cpu_latency_qos_remove_request(&isys->pm_qos);
+	if (!isp->secure_mode)
+		ipu6_cpd_free_pkg_dir(adev);
+remove_shared_buffer:
+	if (!isp->secure_mode)
+		ipu6_buttress_unmap_fw_image(adev, &adev->fw_sgt);
+release_firmware:
+	if (!isp->secure_mode)
+		release_firmware(adev->fw);
+
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++)
+		mutex_destroy(&isys->streams[i].mutex);
+
+	mutex_destroy(&isys->mutex);
+	mutex_destroy(&isys->stream_mutex);
+
+	ipu6_mmu_hw_cleanup(adev->mmu);
+
+	return ret;
+}
+
+static void isys_remove(struct auxiliary_device *auxdev)
+{
+	struct ipu6_bus_device *adev = auxdev_to_adev(auxdev);
+	struct ipu6_isys *isys = dev_get_drvdata(&auxdev->dev);
+	struct ipu6_device *isp = adev->isp;
+	unsigned int i;
+
+	free_fw_msg_bufs(isys);
+
+	isys_unregister_devices(isys);
+	isys_notifier_cleanup(isys);
+
+	cpu_latency_qos_remove_request(&isys->pm_qos);
+
+	if (!isp->secure_mode) {
+		ipu6_cpd_free_pkg_dir(adev);
+		ipu6_buttress_unmap_fw_image(adev, &adev->fw_sgt);
+		release_firmware(adev->fw);
+	}
+
+	for (i = 0; i < IPU6_ISYS_MAX_STREAMS; i++)
+		mutex_destroy(&isys->streams[i].mutex);
+
+	isys_iwake_watermark_cleanup(isys);
+	mutex_destroy(&isys->stream_mutex);
+	mutex_destroy(&isys->mutex);
+}
+
+struct fwmsg {
+	int type;
+	char *msg;
+	bool valid_ts;
+};
+
+static const struct fwmsg fw_msg[] = {
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_OPEN_DONE, "STREAM_OPEN_DONE", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_CLOSE_ACK, "STREAM_CLOSE_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_START_ACK, "STREAM_START_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_ACK,
+	 "STREAM_START_AND_CAPTURE_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_STOP_ACK, "STREAM_STOP_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_FLUSH_ACK, "STREAM_FLUSH_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_PIN_DATA_READY, "PIN_DATA_READY", 1},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_ACK, "STREAM_CAPTURE_ACK", 0},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_DONE,
+	 "STREAM_START_AND_CAPTURE_DONE", 1},
+	{IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_DONE, "STREAM_CAPTURE_DONE", 1},
+	{IPU6_FW_ISYS_RESP_TYPE_FRAME_SOF, "FRAME_SOF", 1},
+	{IPU6_FW_ISYS_RESP_TYPE_FRAME_EOF, "FRAME_EOF", 1},
+	{IPU6_FW_ISYS_RESP_TYPE_STATS_DATA_READY, "STATS_READY", 1},
+	{-1, "UNKNOWN MESSAGE", 0}
+};
+
+static u32 resp_type_to_index(int type)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(fw_msg); i++)
+		if (fw_msg[i].type == type)
+			return i;
+
+	return  ARRAY_SIZE(fw_msg) - 1;
+}
+
+static int isys_isr_one(struct ipu6_bus_device *adev)
+{
+	struct ipu6_isys *isys = ipu6_bus_get_drvdata(adev);
+	struct ipu6_fw_isys_resp_info_abi *resp;
+	struct ipu6_isys_stream *stream;
+	struct ipu6_isys_csi2 *csi2 = NULL;
+	u32 index;
+	u64 ts;
+
+	if (!isys->fwcom)
+		return 1;
+
+	resp = ipu6_fw_isys_get_resp(isys->fwcom, IPU6_BASE_MSG_RECV_QUEUES);
+	if (!resp)
+		return 1;
+
+	ts = (u64)resp->timestamp[1] << 32 | resp->timestamp[0];
+
+	index = resp_type_to_index(resp->type);
+	dev_dbg(&adev->auxdev.dev,
+		"FW resp %02d %s, stream %u, ts 0x%16.16llx, pin %d\n",
+		resp->type, fw_msg[index].msg, resp->stream_handle,
+		fw_msg[index].valid_ts ? ts : 0, resp->pin_id);
+
+	if (resp->error_info.error == IPU6_FW_ISYS_ERROR_STREAM_IN_SUSPENSION)
+		/* Suspension is kind of special case: not enough buffers */
+		dev_dbg(&adev->auxdev.dev,
+			"FW error resp SUSPENSION, details %d\n",
+			resp->error_info.error_details);
+	else if (resp->error_info.error)
+		dev_dbg(&adev->auxdev.dev,
+			"FW error resp error %d, details %d\n",
+			resp->error_info.error, resp->error_info.error_details);
+
+	if (resp->stream_handle >= IPU6_ISYS_MAX_STREAMS) {
+		dev_err(&adev->auxdev.dev, "bad stream handle %u\n",
+			resp->stream_handle);
+		goto leave;
+	}
+
+	stream = ipu6_isys_query_stream_by_handle(isys, resp->stream_handle);
+	if (!stream) {
+		dev_err(&adev->auxdev.dev, "stream of stream_handle %u is unused\n",
+			resp->stream_handle);
+		goto leave;
+	}
+	stream->error = resp->error_info.error;
+
+	csi2 = ipu6_isys_subdev_to_csi2(stream->asd);
+
+	switch (resp->type) {
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_OPEN_DONE:
+		complete(&stream->stream_open_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_CLOSE_ACK:
+		complete(&stream->stream_close_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_START_ACK:
+		complete(&stream->stream_start_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_ACK:
+		complete(&stream->stream_start_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_STOP_ACK:
+		complete(&stream->stream_stop_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_FLUSH_ACK:
+		complete(&stream->stream_stop_completion);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_PIN_DATA_READY:
+		/*
+		 * firmware only release the capture msg until software
+		 * get pin_data_ready event
+		 */
+		ipu6_put_fw_msg_buf(ipu6_bus_get_drvdata(adev), resp->buf_id);
+		if (resp->pin_id < IPU6_ISYS_OUTPUT_PINS &&
+		    stream->output_pins_queue[resp->pin_id])
+			ipu6_isys_queue_buf_ready(stream, resp);
+		else
+			dev_warn(&adev->auxdev.dev,
+				 "%d:No queue for pin id %d\n",
+				 resp->stream_handle, resp->pin_id);
+		if (csi2)
+			ipu6_isys_csi2_error(csi2);
+
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_ACK:
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_START_AND_CAPTURE_DONE:
+	case IPU6_FW_ISYS_RESP_TYPE_STREAM_CAPTURE_DONE:
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_FRAME_SOF:
+
+		ipu6_isys_csi2_sof_event_by_stream(stream);
+		stream->seq[stream->seq_index].sequence =
+			atomic_read(&stream->sequence) - 1;
+		stream->seq[stream->seq_index].timestamp = ts;
+		dev_dbg(&adev->auxdev.dev,
+			"sof: handle %d: (index %u), timestamp 0x%16.16llx\n",
+			resp->stream_handle,
+			stream->seq[stream->seq_index].sequence, ts);
+		stream->seq_index = (stream->seq_index + 1)
+			% IPU6_ISYS_MAX_PARALLEL_SOF;
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_FRAME_EOF:
+		ipu6_isys_csi2_eof_event_by_stream(stream);
+		dev_dbg(&adev->auxdev.dev,
+			"eof: handle %d: (index %u), timestamp 0x%16.16llx\n",
+			resp->stream_handle,
+			stream->seq[stream->seq_index].sequence, ts);
+		break;
+	case IPU6_FW_ISYS_RESP_TYPE_STATS_DATA_READY:
+		break;
+	default:
+		dev_err(&adev->auxdev.dev, "%d:unknown response type %u\n",
+			resp->stream_handle, resp->type);
+		break;
+	}
+
+	ipu6_isys_put_stream(stream);
+leave:
+	ipu6_fw_isys_put_resp(isys->fwcom, IPU6_BASE_MSG_RECV_QUEUES);
+	return 0;
+}
+
+static const struct ipu6_auxdrv_data ipu6_isys_auxdrv_data = {
+	.isr = isys_isr,
+	.isr_threaded = NULL,
+	.wake_isr_thread = false,
+};
+
+static const struct auxiliary_device_id ipu6_isys_id_table[] = {
+	{
+		.name = "intel_ipu6.isys",
+		.driver_data = (kernel_ulong_t)&ipu6_isys_auxdrv_data,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(auxiliary, ipu6_isys_id_table);
+
+static struct auxiliary_driver isys_driver = {
+	.name = IPU6_ISYS_NAME,
+	.probe = isys_probe,
+	.remove = isys_remove,
+	.id_table = ipu6_isys_id_table,
+	.driver = {
+		.pm = &isys_pm_ops,
+	},
+};
+
+module_auxiliary_driver(isys_driver);
+
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_AUTHOR("Yunliang Ding <yunliang.ding@intel.com>");
+MODULE_AUTHOR("Hongju Wang <hongju.wang@intel.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Intel IPU6 input system driver");
+MODULE_IMPORT_NS("INTEL_IPU6");
+MODULE_IMPORT_NS("INTEL_IPU_BRIDGE");
diff --git a/drivers/media/pci/intel/ipu6/ipu6-isys.h b/drivers/media/pci/intel/ipu6/ipu6-isys.h
new file mode 100644
index 000000000000..f488e782c26e
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-isys.h
@@ -0,0 +1,202 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_ISYS_H
+#define IPU6_ISYS_H
+
+#include <linux/irqreturn.h>
+#include <linux/list.h>
+#include <linux/mutex.h>
+#include <linux/pm_qos.h>
+#include <linux/spinlock_types.h>
+#include <linux/types.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-device.h>
+
+#include "ipu6.h"
+#include "ipu6-fw-isys.h"
+#include "ipu6-isys-csi2.h"
+#include "ipu6-isys-video.h"
+
+struct ipu6_bus_device;
+
+#define IPU6_ISYS_ENTITY_PREFIX		"Intel IPU6"
+/* FW support max 16 streams */
+#define IPU6_ISYS_MAX_STREAMS		16
+#define ISYS_UNISPART_IRQS	(IPU6_ISYS_UNISPART_IRQ_SW |	\
+				 IPU6_ISYS_UNISPART_IRQ_CSI0 |	\
+				 IPU6_ISYS_UNISPART_IRQ_CSI1)
+
+/*
+ * Current message queue configuration. These must be big enough
+ * so that they never gets full. Queues are located in system memory
+ */
+#define IPU6_ISYS_SIZE_RECV_QUEUE 40
+#define IPU6_ISYS_SIZE_SEND_QUEUE 40
+#define IPU6_ISYS_SIZE_PROXY_RECV_QUEUE 5
+#define IPU6_ISYS_SIZE_PROXY_SEND_QUEUE 5
+#define IPU6_ISYS_NUM_RECV_QUEUE 1
+
+#define IPU6_ISYS_MIN_WIDTH		2U
+#define IPU6_ISYS_MIN_HEIGHT		2U
+#define IPU6_ISYS_MAX_WIDTH		4672U
+#define IPU6_ISYS_MAX_HEIGHT		3416U
+
+/* the threshold granularity is 2KB on IPU6 */
+#define IPU6_SRAM_GRANULARITY_SHIFT	11
+#define IPU6_SRAM_GRANULARITY_SIZE	2048
+/* the threshold granularity is 1KB on IPU6SE */
+#define IPU6SE_SRAM_GRANULARITY_SHIFT	10
+#define IPU6SE_SRAM_GRANULARITY_SIZE	1024
+/* IS pixel buffer is 256KB, MaxSRAMSize is 200KB on IPU6 */
+#define IPU6_MAX_SRAM_SIZE			(200 << 10)
+/* IS pixel buffer is 128KB, MaxSRAMSize is 96KB on IPU6SE */
+#define IPU6SE_MAX_SRAM_SIZE			(96 << 10)
+
+#define IPU6EP_LTR_VALUE			200
+#define IPU6EP_MIN_MEMOPEN_TH			0x4
+#define IPU6EP_MTL_LTR_VALUE			1023
+#define IPU6EP_MTL_MIN_MEMOPEN_TH		0xc
+
+struct ltr_did {
+	union {
+		u32 value;
+		struct {
+			u8 val0;
+			u8 val1;
+			u8 val2;
+			u8 val3;
+		} bits;
+	} lut_ltr;
+	union {
+		u32 value;
+		struct {
+			u8 th0;
+			u8 th1;
+			u8 th2;
+			u8 th3;
+		} bits;
+	} lut_fill_time;
+};
+
+struct isys_iwake_watermark {
+	bool iwake_enabled;
+	bool force_iwake_disable;
+	u32 iwake_threshold;
+	u64 isys_pixelbuffer_datarate;
+	struct ltr_did ltrdid;
+	struct mutex mutex; /* protect whole struct */
+	struct ipu6_isys *isys;
+	struct list_head video_list;
+};
+
+struct ipu6_isys_csi2_config {
+	u32 nlanes;
+	u32 port;
+};
+
+struct sensor_async_sd {
+	struct v4l2_async_connection asc;
+	struct ipu6_isys_csi2_config csi2;
+};
+
+/*
+ * struct ipu6_isys
+ *
+ * @media_dev: Media device
+ * @v4l2_dev: V4L2 device
+ * @adev: ISYS bus device
+ * @power: Is ISYS powered on or not?
+ * @isr_bits: Which bits does the ISR handle?
+ * @power_lock: Serialise access to power (power state in general)
+ * @csi2_rx_ctrl_cached: cached shared value between all CSI2 receivers
+ * @streams_lock: serialise access to streams
+ * @streams: streams per firmware stream ID
+ * @fwcom: fw communication layer private pointer
+ *         or optional external library private pointer
+ * @phy_termcal_val: the termination calibration value, only used for DWC PHY
+ * @need_reset: Isys requires d0i0->i3 transition
+ * @ref_count: total number of callers fw open
+ * @mutex: serialise access isys video open/release related operations
+ * @stream_mutex: serialise stream start and stop, queueing requests
+ * @pdata: platform data pointer
+ * @csi2: CSI-2 receivers
+ */
+struct ipu6_isys {
+	struct media_device media_dev;
+	struct v4l2_device v4l2_dev;
+	struct ipu6_bus_device *adev;
+
+	int power;
+	spinlock_t power_lock;
+	u32 isr_csi2_bits;
+	u32 csi2_rx_ctrl_cached;
+	spinlock_t streams_lock;
+	struct ipu6_isys_stream streams[IPU6_ISYS_MAX_STREAMS];
+	int streams_ref_count[IPU6_ISYS_MAX_STREAMS];
+	void *fwcom;
+	u32 phy_termcal_val;
+	bool need_reset;
+	bool icache_prefetch;
+	bool csi2_cse_ipc_not_supported;
+	unsigned int ref_count;
+	unsigned int stream_opened;
+	unsigned int sensor_type;
+
+	struct mutex mutex;
+	struct mutex stream_mutex;
+
+	struct ipu6_isys_pdata *pdata;
+
+	int (*phy_set_power)(struct ipu6_isys *isys,
+			     struct ipu6_isys_csi2_config *cfg,
+			     const struct ipu6_isys_csi2_timing *timing,
+			     bool on);
+
+	struct ipu6_isys_csi2 *csi2;
+
+	struct pm_qos_request pm_qos;
+	spinlock_t listlock;	/* Protect framebuflist */
+	struct list_head framebuflist;
+	struct list_head framebuflist_fw;
+	struct v4l2_async_notifier notifier;
+	struct isys_iwake_watermark iwake_watermark;
+};
+
+struct isys_fw_msgs {
+	union {
+		u64 dummy;
+		struct ipu6_fw_isys_frame_buff_set_abi frame;
+		struct ipu6_fw_isys_stream_cfg_data_abi stream;
+	} fw_msg;
+	struct list_head head;
+	dma_addr_t dma_addr;
+};
+
+struct isys_fw_msgs *ipu6_get_fw_msg_buf(struct ipu6_isys_stream *stream);
+void ipu6_put_fw_msg_buf(struct ipu6_isys *isys, uintptr_t data);
+void ipu6_cleanup_fw_msg_bufs(struct ipu6_isys *isys);
+
+extern const struct v4l2_ioctl_ops ipu6_isys_ioctl_ops;
+
+void isys_setup_hw(struct ipu6_isys *isys);
+irqreturn_t isys_isr(struct ipu6_bus_device *adev);
+void update_watermark_setting(struct ipu6_isys *isys);
+
+int ipu6_isys_mcd_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on);
+
+int ipu6_isys_dwc_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on);
+
+int ipu6_isys_jsl_phy_set_power(struct ipu6_isys *isys,
+				struct ipu6_isys_csi2_config *cfg,
+				const struct ipu6_isys_csi2_timing *timing,
+				bool on);
+#endif /* IPU6_ISYS_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-mmu.c b/drivers/media/pci/intel/ipu6/ipu6-mmu.c
new file mode 100644
index 000000000000..6d1c0b90169d
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-mmu.c
@@ -0,0 +1,806 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+#include <asm/barrier.h>
+
+#include <linux/align.h>
+#include <linux/atomic.h>
+#include <linux/bitops.h>
+#include <linux/bits.h>
+#include <linux/bug.h>
+#include <linux/cacheflush.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/gfp.h>
+#include <linux/io.h>
+#include <linux/iova.h>
+#include <linux/math.h>
+#include <linux/minmax.h>
+#include <linux/mm.h>
+#include <linux/pfn.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+#include <linux/vmalloc.h>
+
+#include "ipu6.h"
+#include "ipu6-dma.h"
+#include "ipu6-mmu.h"
+#include "ipu6-platform-regs.h"
+
+#define ISP_PAGE_SHIFT		12
+#define ISP_PAGE_SIZE		BIT(ISP_PAGE_SHIFT)
+#define ISP_PAGE_MASK		(~(ISP_PAGE_SIZE - 1))
+
+#define ISP_L1PT_SHIFT		22
+#define ISP_L1PT_MASK		(~((1U << ISP_L1PT_SHIFT) - 1))
+
+#define ISP_L2PT_SHIFT		12
+#define ISP_L2PT_MASK		(~(ISP_L1PT_MASK | (~(ISP_PAGE_MASK))))
+
+#define ISP_L1PT_PTES           1024
+#define ISP_L2PT_PTES           1024
+
+#define ISP_PADDR_SHIFT		12
+
+#define REG_TLB_INVALIDATE	0x0000
+
+#define REG_L1_PHYS		0x0004	/* 27-bit pfn */
+#define REG_INFO		0x0008
+
+#define TBL_PHYS_ADDR(a)	((phys_addr_t)(a) << ISP_PADDR_SHIFT)
+
+static void tlb_invalidate(struct ipu6_mmu *mmu)
+{
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&mmu->ready_lock, flags);
+	if (!mmu->ready) {
+		spin_unlock_irqrestore(&mmu->ready_lock, flags);
+		return;
+	}
+
+	for (i = 0; i < mmu->nr_mmus; i++) {
+		/*
+		 * To avoid the HW bug induced dead lock in some of the IPU6
+		 * MMUs on successive invalidate calls, we need to first do a
+		 * read to the page table base before writing the invalidate
+		 * register. MMUs which need to implement this WA, will have
+		 * the insert_read_before_invalidate flags set as true.
+		 * Disregard the return value of the read.
+		 */
+		if (mmu->mmu_hw[i].insert_read_before_invalidate)
+			readl(mmu->mmu_hw[i].base + REG_L1_PHYS);
+
+		writel(0xffffffff, mmu->mmu_hw[i].base +
+		       REG_TLB_INVALIDATE);
+		/*
+		 * The TLB invalidation is a "single cycle" (IOMMU clock cycles)
+		 * When the actual MMIO write reaches the IPU6 TLB Invalidate
+		 * register, wmb() will force the TLB invalidate out if the CPU
+		 * attempts to update the IOMMU page table (or sooner).
+		 */
+		wmb();
+	}
+	spin_unlock_irqrestore(&mmu->ready_lock, flags);
+}
+
+#ifdef DEBUG
+static void page_table_dump(struct ipu6_mmu_info *mmu_info)
+{
+	u32 l1_idx;
+
+	dev_dbg(mmu_info->dev, "begin IOMMU page table dump\n");
+
+	for (l1_idx = 0; l1_idx < ISP_L1PT_PTES; l1_idx++) {
+		u32 l2_idx;
+		u32 iova = (phys_addr_t)l1_idx << ISP_L1PT_SHIFT;
+		phys_addr_t l2_phys;
+
+		if (mmu_info->l1_pt[l1_idx] == mmu_info->dummy_l2_pteval)
+			continue;
+
+		l2_phys = TBL_PHYS_ADDR(mmu_info->l1_pt[l1_idx];)
+		dev_dbg(mmu_info->dev,
+			"l1 entry %u; iovas 0x%8.8x-0x%8.8x, at %pap\n",
+			l1_idx, iova, iova + ISP_PAGE_SIZE, &l2_phys);
+
+		for (l2_idx = 0; l2_idx < ISP_L2PT_PTES; l2_idx++) {
+			u32 *l2_pt = mmu_info->l2_pts[l1_idx];
+			u32 iova2 = iova + (l2_idx << ISP_L2PT_SHIFT);
+
+			if (l2_pt[l2_idx] == mmu_info->dummy_page_pteval)
+				continue;
+
+			dev_dbg(mmu_info->dev,
+				"\tl2 entry %u; iova 0x%8.8x, phys %pa\n",
+				l2_idx, iova2,
+				TBL_PHYS_ADDR(l2_pt[l2_idx]));
+		}
+	}
+
+	dev_dbg(mmu_info->dev, "end IOMMU page table dump\n");
+}
+#endif /* DEBUG */
+
+static dma_addr_t map_single(struct ipu6_mmu_info *mmu_info, void *ptr)
+{
+	dma_addr_t dma;
+
+	dma = dma_map_single(mmu_info->dev, ptr, PAGE_SIZE, DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(mmu_info->dev, dma))
+		return 0;
+
+	return dma;
+}
+
+static int get_dummy_page(struct ipu6_mmu_info *mmu_info)
+{
+	void *pt = (void *)get_zeroed_page(GFP_ATOMIC | GFP_DMA32);
+	dma_addr_t dma;
+
+	if (!pt)
+		return -ENOMEM;
+
+	dev_dbg(mmu_info->dev, "dummy_page: get_zeroed_page() == %p\n", pt);
+
+	dma = map_single(mmu_info, pt);
+	if (!dma) {
+		dev_err(mmu_info->dev, "Failed to map dummy page\n");
+		goto err_free_page;
+	}
+
+	mmu_info->dummy_page = pt;
+	mmu_info->dummy_page_pteval = dma >> ISP_PAGE_SHIFT;
+
+	return 0;
+
+err_free_page:
+	free_page((unsigned long)pt);
+	return -ENOMEM;
+}
+
+static void free_dummy_page(struct ipu6_mmu_info *mmu_info)
+{
+	dma_unmap_single(mmu_info->dev,
+			 TBL_PHYS_ADDR(mmu_info->dummy_page_pteval),
+			 PAGE_SIZE, DMA_BIDIRECTIONAL);
+	free_page((unsigned long)mmu_info->dummy_page);
+}
+
+static int alloc_dummy_l2_pt(struct ipu6_mmu_info *mmu_info)
+{
+	u32 *pt = (u32 *)get_zeroed_page(GFP_ATOMIC | GFP_DMA32);
+	dma_addr_t dma;
+	unsigned int i;
+
+	if (!pt)
+		return -ENOMEM;
+
+	dev_dbg(mmu_info->dev, "dummy_l2: get_zeroed_page() = %p\n", pt);
+
+	dma = map_single(mmu_info, pt);
+	if (!dma) {
+		dev_err(mmu_info->dev, "Failed to map l2pt page\n");
+		goto err_free_page;
+	}
+
+	for (i = 0; i < ISP_L2PT_PTES; i++)
+		pt[i] = mmu_info->dummy_page_pteval;
+
+	mmu_info->dummy_l2_pt = pt;
+	mmu_info->dummy_l2_pteval = dma >> ISP_PAGE_SHIFT;
+
+	return 0;
+
+err_free_page:
+	free_page((unsigned long)pt);
+	return -ENOMEM;
+}
+
+static void free_dummy_l2_pt(struct ipu6_mmu_info *mmu_info)
+{
+	dma_unmap_single(mmu_info->dev,
+			 TBL_PHYS_ADDR(mmu_info->dummy_l2_pteval),
+			 PAGE_SIZE, DMA_BIDIRECTIONAL);
+	free_page((unsigned long)mmu_info->dummy_l2_pt);
+}
+
+static u32 *alloc_l1_pt(struct ipu6_mmu_info *mmu_info)
+{
+	u32 *pt = (u32 *)get_zeroed_page(GFP_ATOMIC | GFP_DMA32);
+	dma_addr_t dma;
+	unsigned int i;
+
+	if (!pt)
+		return NULL;
+
+	dev_dbg(mmu_info->dev, "alloc_l1: get_zeroed_page() = %p\n", pt);
+
+	for (i = 0; i < ISP_L1PT_PTES; i++)
+		pt[i] = mmu_info->dummy_l2_pteval;
+
+	dma = map_single(mmu_info, pt);
+	if (!dma) {
+		dev_err(mmu_info->dev, "Failed to map l1pt page\n");
+		goto err_free_page;
+	}
+
+	mmu_info->l1_pt_dma = dma >> ISP_PADDR_SHIFT;
+	dev_dbg(mmu_info->dev, "l1 pt %p mapped at %pad\n", pt, &dma);
+
+	return pt;
+
+err_free_page:
+	free_page((unsigned long)pt);
+	return NULL;
+}
+
+static u32 *alloc_l2_pt(struct ipu6_mmu_info *mmu_info)
+{
+	u32 *pt = (u32 *)get_zeroed_page(GFP_ATOMIC | GFP_DMA32);
+	unsigned int i;
+
+	if (!pt)
+		return NULL;
+
+	dev_dbg(mmu_info->dev, "alloc_l2: get_zeroed_page() = %p\n", pt);
+
+	for (i = 0; i < ISP_L1PT_PTES; i++)
+		pt[i] = mmu_info->dummy_page_pteval;
+
+	return pt;
+}
+
+static void l2_unmap(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		     phys_addr_t dummy, size_t size)
+{
+	unsigned int l2_entries;
+	unsigned int l2_idx;
+	unsigned long flags;
+	u32 l1_idx;
+	u32 *l2_pt;
+
+	spin_lock_irqsave(&mmu_info->lock, flags);
+	for (l1_idx = iova >> ISP_L1PT_SHIFT;
+	     size > 0 && l1_idx < ISP_L1PT_PTES; l1_idx++) {
+		dev_dbg(mmu_info->dev,
+			"unmapping l2 pgtable (l1 index %u (iova 0x%8.8lx))\n",
+			l1_idx, iova);
+
+		if (mmu_info->l1_pt[l1_idx] == mmu_info->dummy_l2_pteval) {
+			dev_err(mmu_info->dev,
+				"unmap not mapped iova 0x%8.8lx l1 index %u\n",
+				iova, l1_idx);
+			continue;
+		}
+		l2_pt = mmu_info->l2_pts[l1_idx];
+
+		l2_entries = 0;
+		for (l2_idx = (iova & ISP_L2PT_MASK) >> ISP_L2PT_SHIFT;
+		     size > 0 && l2_idx < ISP_L2PT_PTES; l2_idx++) {
+			phys_addr_t pteval = TBL_PHYS_ADDR(l2_pt[l2_idx]);
+
+			dev_dbg(mmu_info->dev,
+				"unmap l2 index %u with pteval 0x%p\n",
+				l2_idx, &pteval);
+			l2_pt[l2_idx] = mmu_info->dummy_page_pteval;
+
+			iova += ISP_PAGE_SIZE;
+			size -= ISP_PAGE_SIZE;
+
+			l2_entries++;
+		}
+
+		WARN_ON_ONCE(!l2_entries);
+		clflush_cache_range(&l2_pt[l2_idx - l2_entries],
+				    sizeof(l2_pt[0]) * l2_entries);
+	}
+
+	WARN_ON_ONCE(size);
+	spin_unlock_irqrestore(&mmu_info->lock, flags);
+}
+
+static int l2_map(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		  phys_addr_t paddr, size_t size)
+{
+	struct device *dev = mmu_info->dev;
+	unsigned int l2_entries;
+	u32 *l2_pt, *l2_virt;
+	unsigned int l2_idx;
+	unsigned long flags;
+	size_t mapped = 0;
+	dma_addr_t dma;
+	u32 l1_entry;
+	u32 l1_idx;
+	int err = 0;
+
+	spin_lock_irqsave(&mmu_info->lock, flags);
+
+	paddr = ALIGN(paddr, ISP_PAGE_SIZE);
+	for (l1_idx = iova >> ISP_L1PT_SHIFT;
+	     size > 0 && l1_idx < ISP_L1PT_PTES; l1_idx++) {
+		dev_dbg(dev,
+			"mapping l2 page table for l1 index %u (iova %8.8x)\n",
+			l1_idx, (u32)iova);
+
+		l1_entry = mmu_info->l1_pt[l1_idx];
+		if (l1_entry == mmu_info->dummy_l2_pteval) {
+			l2_virt = mmu_info->l2_pts[l1_idx];
+			if (likely(!l2_virt)) {
+				l2_virt = alloc_l2_pt(mmu_info);
+				if (!l2_virt) {
+					err = -ENOMEM;
+					goto error;
+				}
+			}
+
+			dma = map_single(mmu_info, l2_virt);
+			if (!dma) {
+				dev_err(dev, "Failed to map l2pt page\n");
+				free_page((unsigned long)l2_virt);
+				err = -EINVAL;
+				goto error;
+			}
+
+			l1_entry = dma >> ISP_PADDR_SHIFT;
+
+			dev_dbg(dev, "page for l1_idx %u %p allocated\n",
+				l1_idx, l2_virt);
+			mmu_info->l1_pt[l1_idx] = l1_entry;
+			mmu_info->l2_pts[l1_idx] = l2_virt;
+
+			clflush_cache_range(&mmu_info->l1_pt[l1_idx],
+					    sizeof(mmu_info->l1_pt[l1_idx]));
+		}
+
+		l2_pt = mmu_info->l2_pts[l1_idx];
+		l2_entries = 0;
+
+		for (l2_idx = (iova & ISP_L2PT_MASK) >> ISP_L2PT_SHIFT;
+		     size > 0 && l2_idx < ISP_L2PT_PTES; l2_idx++) {
+			l2_pt[l2_idx] = paddr >> ISP_PADDR_SHIFT;
+
+			dev_dbg(dev, "l2 index %u mapped as 0x%8.8x\n", l2_idx,
+				l2_pt[l2_idx]);
+
+			iova += ISP_PAGE_SIZE;
+			paddr += ISP_PAGE_SIZE;
+			mapped += ISP_PAGE_SIZE;
+			size -= ISP_PAGE_SIZE;
+
+			l2_entries++;
+		}
+
+		WARN_ON_ONCE(!l2_entries);
+		clflush_cache_range(&l2_pt[l2_idx - l2_entries],
+				    sizeof(l2_pt[0]) * l2_entries);
+	}
+
+	spin_unlock_irqrestore(&mmu_info->lock, flags);
+
+	return 0;
+
+error:
+	spin_unlock_irqrestore(&mmu_info->lock, flags);
+	/* unroll mapping in case something went wrong */
+	if (mapped)
+		l2_unmap(mmu_info, iova - mapped, paddr - mapped, mapped);
+
+	return err;
+}
+
+static int __ipu6_mmu_map(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+			  phys_addr_t paddr, size_t size)
+{
+	u32 iova_start = round_down(iova, ISP_PAGE_SIZE);
+	u32 iova_end = ALIGN(iova + size, ISP_PAGE_SIZE);
+
+	dev_dbg(mmu_info->dev,
+		"mapping iova 0x%8.8x--0x%8.8x, size %zu at paddr %pap\n",
+		iova_start, iova_end, size, &paddr);
+
+	return l2_map(mmu_info, iova_start, paddr, size);
+}
+
+static void __ipu6_mmu_unmap(struct ipu6_mmu_info *mmu_info,
+			     unsigned long iova, size_t size)
+{
+	l2_unmap(mmu_info, iova, 0, size);
+}
+
+static int allocate_trash_buffer(struct ipu6_mmu *mmu)
+{
+	unsigned int n_pages = PFN_UP(IPU6_MMUV2_TRASH_RANGE);
+	struct iova *iova;
+	unsigned int i;
+	dma_addr_t dma;
+	unsigned long iova_addr;
+	int ret;
+
+	/* Allocate 8MB in iova range */
+	iova = alloc_iova(&mmu->dmap->iovad, n_pages,
+			  PHYS_PFN(mmu->dmap->mmu_info->aperture_end), 0);
+	if (!iova) {
+		dev_err(mmu->dev, "cannot allocate iova range for trash\n");
+		return -ENOMEM;
+	}
+
+	dma = dma_map_page(mmu->dmap->mmu_info->dev, mmu->trash_page, 0,
+			   PAGE_SIZE, DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(mmu->dmap->mmu_info->dev, dma)) {
+		dev_err(mmu->dmap->mmu_info->dev, "Failed to map trash page\n");
+		ret = -ENOMEM;
+		goto out_free_iova;
+	}
+
+	mmu->pci_trash_page = dma;
+
+	/*
+	 * Map the 8MB iova address range to the same physical trash page
+	 * mmu->trash_page which is already reserved at the probe
+	 */
+	iova_addr = iova->pfn_lo;
+	for (i = 0; i < n_pages; i++) {
+		ret = ipu6_mmu_map(mmu->dmap->mmu_info, PFN_PHYS(iova_addr),
+				   mmu->pci_trash_page, PAGE_SIZE);
+		if (ret) {
+			dev_err(mmu->dev,
+				"mapping trash buffer range failed\n");
+			goto out_unmap;
+		}
+
+		iova_addr++;
+	}
+
+	mmu->iova_trash_page = PFN_PHYS(iova->pfn_lo);
+	dev_dbg(mmu->dev, "iova trash buffer for MMUID: %d is %u\n",
+		mmu->mmid, (unsigned int)mmu->iova_trash_page);
+	return 0;
+
+out_unmap:
+	ipu6_mmu_unmap(mmu->dmap->mmu_info, PFN_PHYS(iova->pfn_lo),
+		       PFN_PHYS(iova_size(iova)));
+	dma_unmap_page(mmu->dmap->mmu_info->dev, mmu->pci_trash_page,
+		       PAGE_SIZE, DMA_BIDIRECTIONAL);
+out_free_iova:
+	__free_iova(&mmu->dmap->iovad, iova);
+	return ret;
+}
+
+int ipu6_mmu_hw_init(struct ipu6_mmu *mmu)
+{
+	struct ipu6_mmu_info *mmu_info;
+	unsigned long flags;
+	unsigned int i;
+
+	mmu_info = mmu->dmap->mmu_info;
+
+	/* Initialise the each MMU HW block */
+	for (i = 0; i < mmu->nr_mmus; i++) {
+		struct ipu6_mmu_hw *mmu_hw = &mmu->mmu_hw[i];
+		unsigned int j;
+		u16 block_addr;
+
+		/* Write page table address per MMU */
+		writel((phys_addr_t)mmu_info->l1_pt_dma,
+		       mmu->mmu_hw[i].base + REG_L1_PHYS);
+
+		/* Set info bits per MMU */
+		writel(mmu->mmu_hw[i].info_bits,
+		       mmu->mmu_hw[i].base + REG_INFO);
+
+		/* Configure MMU TLB stream configuration for L1 */
+		for (j = 0, block_addr = 0; j < mmu_hw->nr_l1streams;
+		     block_addr += mmu->mmu_hw[i].l1_block_sz[j], j++) {
+			if (block_addr > IPU6_MAX_LI_BLOCK_ADDR) {
+				dev_err(mmu->dev, "invalid L1 configuration\n");
+				return -EINVAL;
+			}
+
+			/* Write block start address for each streams */
+			writel(block_addr, mmu_hw->base +
+			       mmu_hw->l1_stream_id_reg_offset + 4 * j);
+		}
+
+		/* Configure MMU TLB stream configuration for L2 */
+		for (j = 0, block_addr = 0; j < mmu_hw->nr_l2streams;
+		     block_addr += mmu->mmu_hw[i].l2_block_sz[j], j++) {
+			if (block_addr > IPU6_MAX_L2_BLOCK_ADDR) {
+				dev_err(mmu->dev, "invalid L2 configuration\n");
+				return -EINVAL;
+			}
+
+			writel(block_addr, mmu_hw->base +
+			       mmu_hw->l2_stream_id_reg_offset + 4 * j);
+		}
+	}
+
+	if (!mmu->trash_page) {
+		int ret;
+
+		mmu->trash_page = alloc_page(GFP_KERNEL);
+		if (!mmu->trash_page) {
+			dev_err(mmu->dev, "insufficient memory for trash buffer\n");
+			return -ENOMEM;
+		}
+
+		ret = allocate_trash_buffer(mmu);
+		if (ret) {
+			__free_page(mmu->trash_page);
+			mmu->trash_page = NULL;
+			dev_err(mmu->dev, "trash buffer allocation failed\n");
+			return ret;
+		}
+	}
+
+	spin_lock_irqsave(&mmu->ready_lock, flags);
+	mmu->ready = true;
+	spin_unlock_irqrestore(&mmu->ready_lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_mmu_hw_init, "INTEL_IPU6");
+
+static struct ipu6_mmu_info *ipu6_mmu_alloc(struct ipu6_device *isp)
+{
+	struct ipu6_mmu_info *mmu_info;
+	int ret;
+
+	mmu_info = kzalloc(sizeof(*mmu_info), GFP_KERNEL);
+	if (!mmu_info)
+		return NULL;
+
+	mmu_info->aperture_start = 0;
+	mmu_info->aperture_end =
+		(dma_addr_t)DMA_BIT_MASK(isp->secure_mode ?
+					 IPU6_MMU_ADDR_BITS :
+					 IPU6_MMU_ADDR_BITS_NON_SECURE);
+	mmu_info->pgsize_bitmap = SZ_4K;
+	mmu_info->dev = &isp->pdev->dev;
+
+	ret = get_dummy_page(mmu_info);
+	if (ret)
+		goto err_free_info;
+
+	ret = alloc_dummy_l2_pt(mmu_info);
+	if (ret)
+		goto err_free_dummy_page;
+
+	mmu_info->l2_pts = vzalloc(ISP_L2PT_PTES * sizeof(*mmu_info->l2_pts));
+	if (!mmu_info->l2_pts)
+		goto err_free_dummy_l2_pt;
+
+	/*
+	 * We always map the L1 page table (a single page as well as
+	 * the L2 page tables).
+	 */
+	mmu_info->l1_pt = alloc_l1_pt(mmu_info);
+	if (!mmu_info->l1_pt)
+		goto err_free_l2_pts;
+
+	spin_lock_init(&mmu_info->lock);
+
+	dev_dbg(mmu_info->dev, "domain initialised\n");
+
+	return mmu_info;
+
+err_free_l2_pts:
+	vfree(mmu_info->l2_pts);
+err_free_dummy_l2_pt:
+	free_dummy_l2_pt(mmu_info);
+err_free_dummy_page:
+	free_dummy_page(mmu_info);
+err_free_info:
+	kfree(mmu_info);
+
+	return NULL;
+}
+
+void ipu6_mmu_hw_cleanup(struct ipu6_mmu *mmu)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&mmu->ready_lock, flags);
+	mmu->ready = false;
+	spin_unlock_irqrestore(&mmu->ready_lock, flags);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_mmu_hw_cleanup, "INTEL_IPU6");
+
+static struct ipu6_dma_mapping *alloc_dma_mapping(struct ipu6_device *isp)
+{
+	struct ipu6_dma_mapping *dmap;
+
+	dmap = kzalloc(sizeof(*dmap), GFP_KERNEL);
+	if (!dmap)
+		return NULL;
+
+	dmap->mmu_info = ipu6_mmu_alloc(isp);
+	if (!dmap->mmu_info) {
+		kfree(dmap);
+		return NULL;
+	}
+
+	init_iova_domain(&dmap->iovad, SZ_4K, 1);
+	dmap->mmu_info->dmap = dmap;
+
+	dev_dbg(&isp->pdev->dev, "alloc mapping\n");
+
+	iova_cache_get();
+
+	return dmap;
+}
+
+phys_addr_t ipu6_mmu_iova_to_phys(struct ipu6_mmu_info *mmu_info,
+				  dma_addr_t iova)
+{
+	phys_addr_t phy_addr;
+	unsigned long flags;
+	u32 *l2_pt;
+
+	spin_lock_irqsave(&mmu_info->lock, flags);
+	l2_pt = mmu_info->l2_pts[iova >> ISP_L1PT_SHIFT];
+	phy_addr = (phys_addr_t)l2_pt[(iova & ISP_L2PT_MASK) >> ISP_L2PT_SHIFT];
+	phy_addr <<= ISP_PAGE_SHIFT;
+	spin_unlock_irqrestore(&mmu_info->lock, flags);
+
+	return phy_addr;
+}
+
+void ipu6_mmu_unmap(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		    size_t size)
+{
+	unsigned int min_pagesz;
+
+	dev_dbg(mmu_info->dev, "unmapping iova 0x%lx size 0x%zx\n", iova, size);
+
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(mmu_info->pgsize_bitmap);
+
+	/*
+	 * The virtual address and the size of the mapping must be
+	 * aligned (at least) to the size of the smallest page supported
+	 * by the hardware
+	 */
+	if (!IS_ALIGNED(iova | size, min_pagesz)) {
+		dev_err(NULL, "unaligned: iova 0x%lx size 0x%zx min_pagesz 0x%x\n",
+			iova, size, min_pagesz);
+		return;
+	}
+
+	__ipu6_mmu_unmap(mmu_info, iova, size);
+}
+
+int ipu6_mmu_map(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		 phys_addr_t paddr, size_t size)
+{
+	unsigned int min_pagesz;
+
+	if (mmu_info->pgsize_bitmap == 0UL)
+		return -ENODEV;
+
+	/* find out the minimum page size supported */
+	min_pagesz = 1 << __ffs(mmu_info->pgsize_bitmap);
+
+	/*
+	 * both the virtual address and the physical one, as well as
+	 * the size of the mapping, must be aligned (at least) to the
+	 * size of the smallest page supported by the hardware
+	 */
+	if (!IS_ALIGNED(iova | paddr | size, min_pagesz)) {
+		dev_err(mmu_info->dev,
+			"unaligned: iova %lx pa %pa size %zx min_pagesz %x\n",
+			iova, &paddr, size, min_pagesz);
+		return -EINVAL;
+	}
+
+	dev_dbg(mmu_info->dev, "map: iova 0x%lx pa %pa size 0x%zx\n",
+		iova, &paddr, size);
+
+	return __ipu6_mmu_map(mmu_info, iova, paddr, size);
+}
+
+static void ipu6_mmu_destroy(struct ipu6_mmu *mmu)
+{
+	struct ipu6_dma_mapping *dmap = mmu->dmap;
+	struct ipu6_mmu_info *mmu_info = dmap->mmu_info;
+	struct iova *iova;
+	u32 l1_idx;
+
+	if (mmu->iova_trash_page) {
+		iova = find_iova(&dmap->iovad, PHYS_PFN(mmu->iova_trash_page));
+		if (iova) {
+			/* unmap and free the trash buffer iova */
+			ipu6_mmu_unmap(mmu_info, PFN_PHYS(iova->pfn_lo),
+				       PFN_PHYS(iova_size(iova)));
+			__free_iova(&dmap->iovad, iova);
+		} else {
+			dev_err(mmu->dev, "trash buffer iova not found.\n");
+		}
+
+		mmu->iova_trash_page = 0;
+		dma_unmap_page(mmu_info->dev, mmu->pci_trash_page,
+			       PAGE_SIZE, DMA_BIDIRECTIONAL);
+		mmu->pci_trash_page = 0;
+		__free_page(mmu->trash_page);
+	}
+
+	for (l1_idx = 0; l1_idx < ISP_L1PT_PTES; l1_idx++) {
+		if (mmu_info->l1_pt[l1_idx] != mmu_info->dummy_l2_pteval) {
+			dma_unmap_single(mmu_info->dev,
+					 TBL_PHYS_ADDR(mmu_info->l1_pt[l1_idx]),
+					 PAGE_SIZE, DMA_BIDIRECTIONAL);
+			free_page((unsigned long)mmu_info->l2_pts[l1_idx]);
+		}
+	}
+
+	vfree(mmu_info->l2_pts);
+	free_dummy_page(mmu_info);
+	dma_unmap_single(mmu_info->dev, TBL_PHYS_ADDR(mmu_info->l1_pt_dma),
+			 PAGE_SIZE, DMA_BIDIRECTIONAL);
+	free_page((unsigned long)mmu_info->dummy_l2_pt);
+	free_page((unsigned long)mmu_info->l1_pt);
+	kfree(mmu_info);
+}
+
+struct ipu6_mmu *ipu6_mmu_init(struct device *dev,
+			       void __iomem *base, int mmid,
+			       const struct ipu6_hw_variants *hw)
+{
+	struct ipu6_device *isp = pci_get_drvdata(to_pci_dev(dev));
+	struct ipu6_mmu_pdata *pdata;
+	struct ipu6_mmu *mmu;
+	unsigned int i;
+
+	if (hw->nr_mmus > IPU6_MMU_MAX_DEVICES)
+		return ERR_PTR(-EINVAL);
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	for (i = 0; i < hw->nr_mmus; i++) {
+		struct ipu6_mmu_hw *pdata_mmu = &pdata->mmu_hw[i];
+		const struct ipu6_mmu_hw *src_mmu = &hw->mmu_hw[i];
+
+		if (src_mmu->nr_l1streams > IPU6_MMU_MAX_TLB_L1_STREAMS ||
+		    src_mmu->nr_l2streams > IPU6_MMU_MAX_TLB_L2_STREAMS)
+			return ERR_PTR(-EINVAL);
+
+		*pdata_mmu = *src_mmu;
+		pdata_mmu->base = base + src_mmu->offset;
+	}
+
+	mmu = devm_kzalloc(dev, sizeof(*mmu), GFP_KERNEL);
+	if (!mmu)
+		return ERR_PTR(-ENOMEM);
+
+	mmu->mmid = mmid;
+	mmu->mmu_hw = pdata->mmu_hw;
+	mmu->nr_mmus = hw->nr_mmus;
+	mmu->tlb_invalidate = tlb_invalidate;
+	mmu->ready = false;
+	INIT_LIST_HEAD(&mmu->vma_list);
+	spin_lock_init(&mmu->ready_lock);
+
+	mmu->dmap = alloc_dma_mapping(isp);
+	if (!mmu->dmap) {
+		dev_err(dev, "can't alloc dma mapping\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	return mmu;
+}
+
+void ipu6_mmu_cleanup(struct ipu6_mmu *mmu)
+{
+	struct ipu6_dma_mapping *dmap = mmu->dmap;
+
+	ipu6_mmu_destroy(mmu);
+	mmu->dmap = NULL;
+	iova_cache_put();
+	put_iova_domain(&dmap->iovad);
+	kfree(dmap);
+}
diff --git a/drivers/media/pci/intel/ipu6/ipu6-mmu.h b/drivers/media/pci/intel/ipu6/ipu6-mmu.h
new file mode 100644
index 000000000000..8e40b4a66d7d
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-mmu.h
@@ -0,0 +1,73 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013--2024 Intel Corporation */
+
+#ifndef IPU6_MMU_H
+#define IPU6_MMU_H
+
+#define ISYS_MMID 1
+#define PSYS_MMID 0
+
+#include <linux/list.h>
+#include <linux/spinlock_types.h>
+#include <linux/types.h>
+
+struct device;
+struct page;
+struct ipu6_hw_variants;
+
+struct ipu6_mmu_info {
+	struct device *dev;
+
+	u32 *l1_pt;
+	u32 l1_pt_dma;
+	u32 **l2_pts;
+
+	u32 *dummy_l2_pt;
+	u32 dummy_l2_pteval;
+	void *dummy_page;
+	u32 dummy_page_pteval;
+
+	dma_addr_t aperture_start;
+	dma_addr_t aperture_end;
+	unsigned long pgsize_bitmap;
+
+	spinlock_t lock;	/* Serialize access to users */
+	struct ipu6_dma_mapping *dmap;
+};
+
+struct ipu6_mmu {
+	struct list_head node;
+
+	struct ipu6_mmu_hw *mmu_hw;
+	unsigned int nr_mmus;
+	unsigned int mmid;
+
+	phys_addr_t pgtbl;
+	struct device *dev;
+
+	struct ipu6_dma_mapping *dmap;
+	struct list_head vma_list;
+
+	struct page *trash_page;
+	dma_addr_t pci_trash_page; /* IOVA from PCI DMA services (parent) */
+	dma_addr_t iova_trash_page; /* IOVA for IPU6 child nodes to use */
+
+	bool ready;
+	spinlock_t ready_lock;	/* Serialize access to bool ready */
+
+	void (*tlb_invalidate)(struct ipu6_mmu *mmu);
+};
+
+struct ipu6_mmu *ipu6_mmu_init(struct device *dev,
+			       void __iomem *base, int mmid,
+			       const struct ipu6_hw_variants *hw);
+void ipu6_mmu_cleanup(struct ipu6_mmu *mmu);
+int ipu6_mmu_hw_init(struct ipu6_mmu *mmu);
+void ipu6_mmu_hw_cleanup(struct ipu6_mmu *mmu);
+int ipu6_mmu_map(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		 phys_addr_t paddr, size_t size);
+void ipu6_mmu_unmap(struct ipu6_mmu_info *mmu_info, unsigned long iova,
+		    size_t size);
+phys_addr_t ipu6_mmu_iova_to_phys(struct ipu6_mmu_info *mmu_info,
+				  dma_addr_t iova);
+#endif
diff --git a/drivers/media/pci/intel/ipu6/ipu6-platform-buttress-regs.h b/drivers/media/pci/intel/ipu6/ipu6-platform-buttress-regs.h
new file mode 100644
index 000000000000..efd65e494c16
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-platform-buttress-regs.h
@@ -0,0 +1,224 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2023--2024 Intel Corporation */
+
+#ifndef IPU6_PLATFORM_BUTTRESS_REGS_H
+#define IPU6_PLATFORM_BUTTRESS_REGS_H
+
+#include <linux/bits.h>
+
+/* IS_WORKPOINT_REQ */
+#define IPU6_BUTTRESS_REG_IS_FREQ_CTL		0x34
+/* PS_WORKPOINT_REQ */
+#define IPU6_BUTTRESS_REG_PS_FREQ_CTL		0x38
+
+/* should be tuned for real silicon */
+#define IPU6_IS_FREQ_CTL_DEFAULT_RATIO		0x08
+#define IPU6SE_IS_FREQ_CTL_DEFAULT_RATIO	0x0a
+#define IPU6_PS_FREQ_CTL_DEFAULT_RATIO		0x0d
+
+#define IPU6_IS_FREQ_CTL_DEFAULT_QOS_FLOOR_RATIO	0x10
+#define IPU6_PS_FREQ_CTL_DEFAULT_QOS_FLOOR_RATIO	0x0708
+
+#define IPU6_BUTTRESS_PWR_STATE_IS_PWR_SHIFT	3
+#define IPU6_BUTTRESS_PWR_STATE_IS_PWR_MASK	GENMASK(4, 3)
+
+#define IPU6_BUTTRESS_PWR_STATE_PS_PWR_SHIFT	6
+#define IPU6_BUTTRESS_PWR_STATE_PS_PWR_MASK	GENMASK(7, 6)
+
+#define IPU6_BUTTRESS_PWR_STATE_DN_DONE		0x0
+#define IPU6_BUTTRESS_PWR_STATE_UP_PROCESS	0x1
+#define IPU6_BUTTRESS_PWR_STATE_DN_PROCESS	0x2
+#define IPU6_BUTTRESS_PWR_STATE_UP_DONE		0x3
+
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_0	0x270
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_1	0x274
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_2	0x278
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_3	0x27c
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_4	0x280
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_5	0x284
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_6	0x288
+#define IPU6_BUTTRESS_REG_FPGA_SUPPORT_7	0x28c
+
+#define BUTTRESS_REG_WDT			0x8
+#define BUTTRESS_REG_BTRS_CTRL			0xc
+#define BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC0	BIT(0)
+#define BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC1	BIT(1)
+#define BUTTRESS_REG_BTRS_CTRL_REF_CLK_IND	GENMASK(9, 8)
+
+#define BUTTRESS_REG_FW_RESET_CTL	0x30
+#define BUTTRESS_FW_RESET_CTL_START	BIT(0)
+#define BUTTRESS_FW_RESET_CTL_DONE	BIT(1)
+
+#define BUTTRESS_REG_IS_FREQ_CTL	0x34
+#define BUTTRESS_REG_PS_FREQ_CTL	0x38
+
+#define BUTTRESS_FREQ_CTL_START		BIT(31)
+#define BUTTRESS_FREQ_CTL_ICCMAX_LEVEL		GENMASK(19, 16)
+#define BUTTRESS_FREQ_CTL_QOS_FLOOR_MASK	GENMASK(15, 8)
+#define BUTTRESS_FREQ_CTL_RATIO_MASK	GENMASK(7, 0)
+
+#define BUTTRESS_REG_PWR_STATE	0x5c
+
+#define BUTTRESS_PWR_STATE_RESET		0x0
+#define BUTTRESS_PWR_STATE_PWR_ON_DONE		0x1
+#define BUTTRESS_PWR_STATE_PWR_RDY		0x3
+#define BUTTRESS_PWR_STATE_PWR_IDLE		0x4
+
+#define BUTTRESS_PWR_STATE_HH_STATUS_MASK	GENMASK(12, 11)
+
+enum {
+	BUTTRESS_PWR_STATE_HH_STATE_IDLE,
+	BUTTRESS_PWR_STATE_HH_STATE_IN_PRGS,
+	BUTTRESS_PWR_STATE_HH_STATE_DONE,
+	BUTTRESS_PWR_STATE_HH_STATE_ERR,
+};
+
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_MASK	GENMASK(23, 19)
+
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_IDLE			0x0
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_PLL_CMP		0x1
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_CLKACK		0x2
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_PG_ACK		0x3
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_RST_ASSRT_CYCLES		0x4
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_STOP_CLK_CYCLES1		0x5
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_STOP_CLK_CYCLES2		0x6
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_RST_DEASSRT_CYCLES	0x7
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_FUSE_WR_CMP	0x8
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_BRK_POINT			0x9
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_IS_RDY			0xa
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_HALT_HALTED		0xb
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_RST_DURATION_CNT3		0xc
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_CLKACK_PD		0xd
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_PD_BRK_POINT		0xe
+#define BUTTRESS_PWR_STATE_IS_PWR_FSM_WAIT_4_PD_PG_ACK0		0xf
+
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_MASK	GENMASK(28, 24)
+
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_IDLE			0x0
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PU_PLL_IP_RDY	0x1
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_RO_PRE_CNT_EXH	0x2
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PU_VGI_PWRGOOD	0x3
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_RO_POST_CNT_EXH	0x4
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WR_PLL_RATIO		0x5
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PU_PLL_CMP		0x6
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PU_CLKACK		0x7
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_RST_ASSRT_CYCLES		0x8
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_STOP_CLK_CYCLES1		0x9
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_STOP_CLK_CYCLES2		0xa
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_RST_DEASSRT_CYCLES	0xb
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_PU_BRK_PNT		0xc
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_FUSE_ACCPT		0xd
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_PS_PWR_UP			0xf
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_4_HALTED		0x10
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_RESET_CNT3		0x11
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PD_CLKACK		0x12
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_PD_OFF_IND		0x13
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_DVFS_PH4		0x14
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_DVFS_PLL_CMP		0x15
+#define BUTTRESS_PWR_STATE_PS_PWR_FSM_WAIT_DVFS_CLKACK		0x16
+
+#define BUTTRESS_REG_SECURITY_CTL	0x300
+#define BUTTRESS_REG_SKU		0x314
+#define BUTTRESS_REG_SECURITY_TOUCH	0x318
+#define BUTTRESS_REG_CAMERA_MASK	0x84
+
+#define BUTTRESS_SECURITY_CTL_FW_SECURE_MODE	BIT(16)
+#define BUTTRESS_SECURITY_CTL_FW_SETUP_MASK	GENMASK(4, 0)
+
+#define BUTTRESS_SECURITY_CTL_FW_SETUP_DONE		BIT(0)
+#define BUTTRESS_SECURITY_CTL_AUTH_DONE			BIT(1)
+#define BUTTRESS_SECURITY_CTL_AUTH_FAILED		BIT(3)
+
+#define BUTTRESS_REG_FW_SOURCE_BASE_LO	0x78
+#define BUTTRESS_REG_FW_SOURCE_BASE_HI	0x7C
+#define BUTTRESS_REG_FW_SOURCE_SIZE	0x80
+
+#define BUTTRESS_REG_ISR_STATUS		0x90
+#define BUTTRESS_REG_ISR_ENABLED_STATUS	0x94
+#define BUTTRESS_REG_ISR_ENABLE		0x98
+#define BUTTRESS_REG_ISR_CLEAR		0x9C
+
+#define BUTTRESS_ISR_IS_IRQ			BIT(0)
+#define BUTTRESS_ISR_PS_IRQ			BIT(1)
+#define BUTTRESS_ISR_IPC_EXEC_DONE_BY_CSE	BIT(2)
+#define BUTTRESS_ISR_IPC_EXEC_DONE_BY_ISH	BIT(3)
+#define BUTTRESS_ISR_IPC_FROM_CSE_IS_WAITING	BIT(4)
+#define BUTTRESS_ISR_IPC_FROM_ISH_IS_WAITING	BIT(5)
+#define BUTTRESS_ISR_CSE_CSR_SET		BIT(6)
+#define BUTTRESS_ISR_ISH_CSR_SET		BIT(7)
+#define BUTTRESS_ISR_SPURIOUS_CMP		BIT(8)
+#define BUTTRESS_ISR_WATCHDOG_EXPIRED		BIT(9)
+#define BUTTRESS_ISR_PUNIT_2_IUNIT_IRQ		BIT(10)
+#define BUTTRESS_ISR_SAI_VIOLATION		BIT(11)
+#define BUTTRESS_ISR_HW_ASSERTION		BIT(12)
+#define BUTTRESS_ISR_IS_CORRECTABLE_MEM_ERR	BIT(13)
+#define BUTTRESS_ISR_IS_FATAL_MEM_ERR		BIT(14)
+#define BUTTRESS_ISR_IS_NON_FATAL_MEM_ERR	BIT(15)
+#define BUTTRESS_ISR_PS_CORRECTABLE_MEM_ERR	BIT(16)
+#define BUTTRESS_ISR_PS_FATAL_MEM_ERR		BIT(17)
+#define BUTTRESS_ISR_PS_NON_FATAL_MEM_ERR	BIT(18)
+#define BUTTRESS_ISR_PS_FAST_THROTTLE		BIT(19)
+#define BUTTRESS_ISR_UFI_ERROR			BIT(20)
+
+#define BUTTRESS_REG_IU2CSEDB0	0x100
+
+#define BUTTRESS_IU2CSEDB0_BUSY		BIT(31)
+#define BUTTRESS_IU2CSEDB0_IPC_CLIENT_ID_VAL	2
+
+#define BUTTRESS_REG_IU2CSEDATA0	0x104
+
+#define BUTTRESS_IU2CSEDATA0_IPC_BOOT_LOAD		1
+#define BUTTRESS_IU2CSEDATA0_IPC_AUTH_RUN		2
+#define BUTTRESS_IU2CSEDATA0_IPC_AUTH_REPLACE		3
+#define BUTTRESS_IU2CSEDATA0_IPC_UPDATE_SECURE_TOUCH	16
+
+#define BUTTRESS_CSE2IUDATA0_IPC_BOOT_LOAD_DONE			BIT(0)
+#define BUTTRESS_CSE2IUDATA0_IPC_AUTH_RUN_DONE			BIT(1)
+#define BUTTRESS_CSE2IUDATA0_IPC_AUTH_REPLACE_DONE		BIT(2)
+#define BUTTRESS_CSE2IUDATA0_IPC_UPDATE_SECURE_TOUCH_DONE	BIT(4)
+
+#define BUTTRESS_REG_IU2CSECSR		0x108
+
+#define BUTTRESS_IU2CSECSR_IPC_PEER_COMP_ACTIONS_RST_PHASE1		BIT(0)
+#define BUTTRESS_IU2CSECSR_IPC_PEER_COMP_ACTIONS_RST_PHASE2		BIT(1)
+#define BUTTRESS_IU2CSECSR_IPC_PEER_QUERIED_IP_COMP_ACTIONS_RST_PHASE	BIT(2)
+#define BUTTRESS_IU2CSECSR_IPC_PEER_ASSERTED_REG_VALID_REQ		BIT(3)
+#define BUTTRESS_IU2CSECSR_IPC_PEER_ACKED_REG_VALID			BIT(4)
+#define BUTTRESS_IU2CSECSR_IPC_PEER_DEASSERTED_REG_VALID_REQ		BIT(5)
+
+#define BUTTRESS_REG_CSE2IUDB0		0x304
+#define BUTTRESS_REG_CSE2IUCSR		0x30C
+#define BUTTRESS_REG_CSE2IUDATA0	0x308
+
+/* 0x20 == NACK, 0xf == unknown command */
+#define BUTTRESS_CSE2IUDATA0_IPC_NACK      0xf20
+#define BUTTRESS_CSE2IUDATA0_IPC_NACK_MASK GENMASK(15, 0)
+
+#define BUTTRESS_REG_ISH2IUCSR		0x50
+#define BUTTRESS_REG_ISH2IUDB0		0x54
+#define BUTTRESS_REG_ISH2IUDATA0	0x58
+
+#define BUTTRESS_REG_IU2ISHDB0		0x10C
+#define BUTTRESS_REG_IU2ISHDATA0	0x110
+#define BUTTRESS_REG_IU2ISHDATA1	0x114
+#define BUTTRESS_REG_IU2ISHCSR		0x118
+
+#define BUTTRESS_REG_FABRIC_CMD		0x88
+
+#define BUTTRESS_FABRIC_CMD_START_TSC_SYNC	BIT(0)
+#define BUTTRESS_FABRIC_CMD_IS_DRAIN		BIT(4)
+
+#define BUTTRESS_REG_TSW_CTL		0x120
+#define BUTTRESS_TSW_CTL_SOFT_RESET	BIT(8)
+
+#define BUTTRESS_REG_TSC_LO	0x164
+#define BUTTRESS_REG_TSC_HI	0x168
+
+#define BUTTRESS_IRQS		(BUTTRESS_ISR_IPC_FROM_CSE_IS_WAITING | \
+				 BUTTRESS_ISR_IPC_EXEC_DONE_BY_CSE |    \
+				 BUTTRESS_ISR_IS_IRQ | BUTTRESS_ISR_PS_IRQ)
+
+#define BUTTRESS_EVENT		 (BUTTRESS_ISR_IPC_FROM_CSE_IS_WAITING | \
+				  BUTTRESS_ISR_IPC_EXEC_DONE_BY_CSE |    \
+				  BUTTRESS_ISR_SAI_VIOLATION)
+#endif /* IPU6_PLATFORM_BUTTRESS_REGS_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-platform-isys-csi2-reg.h b/drivers/media/pci/intel/ipu6/ipu6-platform-isys-csi2-reg.h
new file mode 100644
index 000000000000..cc58377534dc
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-platform-isys-csi2-reg.h
@@ -0,0 +1,172 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2023--2024 Intel Corporation */
+
+#ifndef IPU6_PLATFORM_ISYS_CSI2_REG_H
+#define IPU6_PLATFORM_ISYS_CSI2_REG_H
+
+#include <linux/bits.h>
+
+#define CSI_REG_BASE			0x220000
+#define CSI_REG_PORT_BASE(id)		(CSI_REG_BASE + (id) * 0x1000)
+
+/* CSI Port Genral Purpose Registers */
+#define CSI_REG_PORT_GPREG_SRST                 0x0
+#define CSI_REG_PORT_GPREG_CSI2_SLV_REG_SRST    0x4
+#define CSI_REG_PORT_GPREG_CSI2_PORT_CONTROL    0x8
+
+/*
+ * Port IRQs mapping events:
+ * IRQ0 - CSI_FE event
+ * IRQ1 - CSI_SYNC
+ * IRQ2 - S2M_SIDS0TO7
+ * IRQ3 - S2M_SIDS8TO15
+ */
+#define CSI_PORT_REG_BASE_IRQ_CSI               0x80
+#define CSI_PORT_REG_BASE_IRQ_CSI_SYNC          0xA0
+#define CSI_PORT_REG_BASE_IRQ_S2M_SIDS0TOS7     0xC0
+#define CSI_PORT_REG_BASE_IRQ_S2M_SIDS8TOS15    0xE0
+
+#define CSI_PORT_REG_BASE_IRQ_EDGE_OFFSET	0x0
+#define CSI_PORT_REG_BASE_IRQ_MASK_OFFSET	0x4
+#define CSI_PORT_REG_BASE_IRQ_STATUS_OFFSET	0x8
+#define CSI_PORT_REG_BASE_IRQ_CLEAR_OFFSET	0xc
+#define CSI_PORT_REG_BASE_IRQ_ENABLE_OFFSET	0x10
+#define CSI_PORT_REG_BASE_IRQ_LEVEL_NOT_PULSE_OFFSET	0x14
+
+#define IPU6SE_CSI_RX_ERROR_IRQ_MASK		GENMASK(18, 0)
+#define IPU6_CSI_RX_ERROR_IRQ_MASK		GENMASK(19, 0)
+
+#define CSI_RX_NUM_ERRORS_IN_IRQ		20
+#define CSI_RX_NUM_IRQ				32
+
+#define IPU_CSI_RX_IRQ_FS_VC(chn)	(1 << ((chn) * 2))
+#define IPU_CSI_RX_IRQ_FE_VC(chn)	(2 << ((chn) * 2))
+
+/* PPI2CSI */
+#define CSI_REG_PPI2CSI_ENABLE				0x200
+#define CSI_REG_PPI2CSI_CONFIG_PPI_INTF			0x204
+#define PPI_INTF_CONFIG_NOF_ENABLED_DLANES_MASK		GENMASK(4, 3)
+#define CSI_REG_PPI2CSI_CONFIG_CSI_FEATURE		0x208
+
+enum CSI_PPI2CSI_CTRL {
+	CSI_PPI2CSI_DISABLE = 0,
+	CSI_PPI2CSI_ENABLE = 1,
+};
+
+/* CSI_FE */
+#define CSI_REG_CSI_FE_ENABLE                   0x280
+#define CSI_REG_CSI_FE_MODE                     0x284
+#define CSI_REG_CSI_FE_MUX_CTRL                 0x288
+#define CSI_REG_CSI_FE_SYNC_CNTR_SEL            0x290
+
+enum CSI_FE_ENABLE_TYPE {
+	CSI_FE_DISABLE = 0,
+	CSI_FE_ENABLE = 1,
+};
+
+enum CSI_FE_MODE_TYPE {
+	CSI_FE_DPHY_MODE = 0,
+	CSI_FE_CPHY_MODE = 1,
+};
+
+enum CSI_FE_INPUT_SELECTOR {
+	CSI_SENSOR_INPUT = 0,
+	CSI_MIPIGEN_INPUT = 1,
+};
+
+enum CSI_FE_SYNC_CNTR_SEL_TYPE {
+	CSI_CNTR_SENSOR_LINE_ID = BIT(0),
+	CSI_CNTR_INT_LINE_PKT_ID = ~CSI_CNTR_SENSOR_LINE_ID,
+	CSI_CNTR_SENSOR_FRAME_ID = BIT(1),
+	CSI_CNTR_INT_FRAME_PKT_ID = ~CSI_CNTR_SENSOR_FRAME_ID,
+};
+
+/* CSI HUB General Purpose Registers */
+#define CSI_REG_HUB_GPREG_SRST			(CSI_REG_BASE + 0x18000)
+#define CSI_REG_HUB_GPREG_SLV_REG_SRST		(CSI_REG_BASE + 0x18004)
+
+#define CSI_REG_HUB_DRV_ACCESS_PORT(id)	(CSI_REG_BASE + 0x18018 + (id) * 4)
+#define CSI_REG_HUB_FW_ACCESS_PORT_OFS		0x17000
+#define CSI_REG_HUB_FW_ACCESS_PORT_V6OFS	0x16000
+#define CSI_REG_HUB_FW_ACCESS_PORT(ofs, id)	\
+					(CSI_REG_BASE + (ofs) + (id) * 4)
+
+enum CSI_PORT_CLK_GATING_SWITCH {
+	CSI_PORT_CLK_GATING_OFF = 0,
+	CSI_PORT_CLK_GATING_ON = 1,
+};
+
+#define CSI_REG_BASE_HUB_IRQ                        0x18200
+
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_EDGE			0x238200
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_MASK			0x238204
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_STATUS			0x238208
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_CLEAR			0x23820c
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_ENABLE			0x238210
+#define IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_LEVEL_NOT_PULSE		0x238214
+
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_EDGE			0x238220
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_MASK			0x238224
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_STATUS			0x238228
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_CLEAR			0x23822c
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_ENABLE			0x238230
+#define IPU6_REG_ISYS_CSI_TOP_CTRL1_IRQ_LEVEL_NOT_PULSE		0x238234
+
+/* MTL IPU6V6 irq ctrl0 & ctrl1 */
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_EDGE			0x238700
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_MASK			0x238704
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_STATUS		0x238708
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_CLEAR			0x23870c
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_ENABLE		0x238710
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_LEVEL_NOT_PULSE	0x238714
+
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_EDGE			0x238720
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_MASK			0x238724
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_STATUS		0x238728
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_CLEAR			0x23872c
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_ENABLE		0x238730
+#define IPU6V6_REG_ISYS_CSI_TOP_CTRL1_IRQ_LEVEL_NOT_PULSE	0x238734
+
+/*
+ * 3:0 CSI_PORT.irq_out[3:0] CSI_PORT_CTRL0 IRQ outputs (4bits)
+ * [0] CSI_PORT.IRQ_CTRL0_csi
+ * [1] CSI_PORT.IRQ_CTRL1_csi_sync
+ * [2] CSI_PORT.IRQ_CTRL2_s2m_sids0to7
+ * [3] CSI_PORT.IRQ_CTRL3_s2m_sids8to15
+ */
+#define IPU6_ISYS_UNISPART_IRQ_CSI2(port)		\
+				   (0x3 << ((port) * IPU6_CSI_IRQ_NUM_PER_PIPE))
+
+/*
+ * ipu6se support 2 front ends, 2 port per front end, 4 ports 0..3
+ * sip0 - 0, 1
+ * sip1 - 2, 3
+ * 0 and 2 support 4 data lanes, 1 and 3 support 2 data lanes
+ * all offset are base from isys base address
+ */
+
+#define CSI2_HUB_GPREG_SIP_SRST(sip)			(0x238038 + (sip) * 4)
+#define CSI2_HUB_GPREG_SIP_FB_PORT_CFG(sip)		(0x238050 + (sip) * 4)
+
+#define CSI2_HUB_GPREG_DPHY_TIMER_INCR			0x238040
+#define CSI2_HUB_GPREG_HPLL_FREQ			0x238044
+#define CSI2_HUB_GPREG_IS_CLK_RATIO			0x238048
+#define CSI2_HUB_GPREG_HPLL_FREQ_ISCLK_RATE_OVERRIDE	0x23804c
+#define CSI2_HUB_GPREG_PORT_CLKGATING_DISABLE		0x238058
+#define CSI2_HUB_GPREG_SIP0_CSI_RX_A_CONTROL		0x23805c
+#define CSI2_HUB_GPREG_SIP0_CSI_RX_B_CONTROL		0x238088
+#define CSI2_HUB_GPREG_SIP1_CSI_RX_A_CONTROL		0x2380a4
+#define CSI2_HUB_GPREG_SIP1_CSI_RX_B_CONTROL		0x2380d0
+
+#define CSI2_SIP_TOP_CSI_RX_BASE(sip)		(0x23805c + (sip) * 0x48)
+#define CSI2_SIP_TOP_CSI_RX_PORT_BASE_0(port)	(0x23805c + ((port) / 2) * 0x48)
+#define CSI2_SIP_TOP_CSI_RX_PORT_BASE_1(port)	(0x238088 + ((port) / 2) * 0x48)
+
+/* offset from port base */
+#define CSI2_SIP_TOP_CSI_RX_PORT_CONTROL		0x0
+#define CSI2_SIP_TOP_CSI_RX_DLY_CNT_TERMEN_CLANE	0x4
+#define CSI2_SIP_TOP_CSI_RX_DLY_CNT_SETTLE_CLANE	0x8
+#define CSI2_SIP_TOP_CSI_RX_DLY_CNT_TERMEN_DLANE(lane)	(0xc + (lane) * 8)
+#define CSI2_SIP_TOP_CSI_RX_DLY_CNT_SETTLE_DLANE(lane)	(0x10 + (lane) * 8)
+
+#endif /* IPU6_ISYS_CSI2_REG_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6-platform-regs.h b/drivers/media/pci/intel/ipu6/ipu6-platform-regs.h
new file mode 100644
index 000000000000..b883385adb44
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6-platform-regs.h
@@ -0,0 +1,179 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2018 - 2024 Intel Corporation */
+
+#ifndef IPU6_PLATFORM_REGS_H
+#define IPU6_PLATFORM_REGS_H
+
+#include <linux/bits.h>
+
+/*
+ * IPU6 uses uniform address within IPU6, therefore all subsystem registers
+ * locates in one single space starts from 0 but in different sctions with
+ * different addresses, the subsystem offsets are defined to 0 as the
+ * register definition will have the address offset to 0.
+ */
+#define IPU6_UNIFIED_OFFSET			0
+
+#define IPU6_ISYS_IOMMU0_OFFSET		0x2e0000
+#define IPU6_ISYS_IOMMU1_OFFSET		0x2e0500
+#define IPU6_ISYS_IOMMUI_OFFSET		0x2e0a00
+
+#define IPU6_PSYS_IOMMU0_OFFSET		0x1b0000
+#define IPU6_PSYS_IOMMU1_OFFSET		0x1b0700
+#define IPU6_PSYS_IOMMU1R_OFFSET	0x1b0e00
+#define IPU6_PSYS_IOMMUI_OFFSET		0x1b1500
+
+/* the offset from IOMMU base register */
+#define IPU6_MMU_L1_STREAM_ID_REG_OFFSET	0x0c
+#define IPU6_MMU_L2_STREAM_ID_REG_OFFSET	0x4c
+#define IPU6_PSYS_MMU1W_L2_STREAM_ID_REG_OFFSET	0x8c
+
+#define IPU6_MMU_INFO_OFFSET		0x8
+
+#define IPU6_ISYS_SPC_OFFSET		0x210000
+
+#define IPU6SE_PSYS_SPC_OFFSET		0x110000
+#define IPU6_PSYS_SPC_OFFSET		0x118000
+
+#define IPU6_ISYS_DMEM_OFFSET		0x200000
+#define IPU6_PSYS_DMEM_OFFSET		0x100000
+
+#define IPU6_REG_ISYS_UNISPART_IRQ_EDGE			0x27c000
+#define IPU6_REG_ISYS_UNISPART_IRQ_MASK			0x27c004
+#define IPU6_REG_ISYS_UNISPART_IRQ_STATUS		0x27c008
+#define IPU6_REG_ISYS_UNISPART_IRQ_CLEAR		0x27c00c
+#define IPU6_REG_ISYS_UNISPART_IRQ_ENABLE		0x27c010
+#define IPU6_REG_ISYS_UNISPART_IRQ_LEVEL_NOT_PULSE	0x27c014
+#define IPU6_REG_ISYS_UNISPART_SW_IRQ_REG		0x27c414
+#define IPU6_REG_ISYS_UNISPART_SW_IRQ_MUX_REG		0x27c418
+#define IPU6_ISYS_UNISPART_IRQ_CSI0			BIT(2)
+#define IPU6_ISYS_UNISPART_IRQ_CSI1			BIT(3)
+#define IPU6_ISYS_UNISPART_IRQ_SW			BIT(22)
+
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_EDGE			0x2b0200
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_MASK			0x2b0204
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_STATUS		0x2b0208
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_CLEAR			0x2b020c
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_ENABLE		0x2b0210
+#define IPU6_REG_ISYS_ISL_TOP_IRQ_LEVEL_NOT_PULSE	0x2b0214
+
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_EDGE			0x2d2100
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_MASK			0x2d2104
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_STATUS		0x2d2108
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_CLEAR		0x2d210c
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_ENABLE		0x2d2110
+#define IPU6_REG_ISYS_CMPR_TOP_IRQ_LEVEL_NOT_PULSE	0x2d2114
+
+/* CDC Burst collector thresholds for isys - 3 FIFOs i = 0..2 */
+#define IPU6_REG_ISYS_CDC_THRESHOLD(i)		(0x27c400 + ((i) * 4))
+
+#define IPU6_CSI_IRQ_NUM_PER_PIPE			4
+#define IPU6SE_ISYS_CSI_PORT_NUM			4
+#define IPU6_ISYS_CSI_PORT_NUM				8
+
+#define IPU6_ISYS_CSI_PORT_IRQ(irq_num)		BIT(irq_num)
+
+/* PKG DIR OFFSET in IMR in secure mode */
+#define IPU6_PKG_DIR_IMR_OFFSET			0x40
+
+#define IPU6_ISYS_REG_SPC_STATUS_CTRL		0x0
+
+#define IPU6_ISYS_SPC_STATUS_START			BIT(1)
+#define IPU6_ISYS_SPC_STATUS_RUN			BIT(3)
+#define IPU6_ISYS_SPC_STATUS_READY			BIT(5)
+#define IPU6_ISYS_SPC_STATUS_CTRL_ICACHE_INVALIDATE	BIT(12)
+#define IPU6_ISYS_SPC_STATUS_ICACHE_PREFETCH		BIT(13)
+
+#define IPU6_PSYS_REG_SPC_STATUS_CTRL			0x0
+#define IPU6_PSYS_REG_SPC_START_PC			0x4
+#define IPU6_PSYS_REG_SPC_ICACHE_BASE			0x10
+#define IPU6_REG_PSYS_INFO_SEG_0_CONFIG_ICACHE_MASTER	0x14
+
+#define IPU6_PSYS_SPC_STATUS_START			BIT(1)
+#define IPU6_PSYS_SPC_STATUS_RUN			BIT(3)
+#define IPU6_PSYS_SPC_STATUS_READY			BIT(5)
+#define IPU6_PSYS_SPC_STATUS_CTRL_ICACHE_INVALIDATE	BIT(12)
+#define IPU6_PSYS_SPC_STATUS_ICACHE_PREFETCH		BIT(13)
+
+#define IPU6_PSYS_REG_SPP0_STATUS_CTRL			0x20000
+
+#define IPU6_INFO_ENABLE_SNOOP			BIT(0)
+#define IPU6_INFO_DEC_FORCE_FLUSH		BIT(1)
+#define IPU6_INFO_DEC_PASS_THROUGH		BIT(2)
+#define IPU6_INFO_ZLW				BIT(3)
+#define IPU6_INFO_REQUEST_DESTINATION_IOSF	BIT(9)
+#define IPU6_INFO_IMR_BASE			BIT(10)
+#define IPU6_INFO_IMR_DESTINED			BIT(11)
+
+#define IPU6_INFO_REQUEST_DESTINATION_PRIMARY IPU6_INFO_REQUEST_DESTINATION_IOSF
+
+/*
+ * s2m_pixel_soc_pixel_remapping is dedicated for the enabling of the
+ * pixel s2m remp ability.Remap here  means that s2m rearange the order
+ * of the pixels in each 4 pixels group.
+ * For examle, mirroring remping means that if input's 4 first pixels
+ * are 1 2 3 4 then in output we should see 4 3 2 1 in this 4 first pixels.
+ * 0xE4 is from s2m MAS document. It means no remapping.
+ */
+#define S2M_PIXEL_SOC_PIXEL_REMAPPING_FLAG_NO_REMAPPING 0xe4
+/*
+ * csi_be_soc_pixel_remapping is for the enabling of the pixel remapping.
+ * This remapping is exactly like the stream2mmio remapping.
+ */
+#define CSI_BE_SOC_PIXEL_REMAPPING_FLAG_NO_REMAPPING    0xe4
+
+#define IPU6_REG_DMA_TOP_AB_GROUP1_BASE_ADDR		0x1ae000
+#define IPU6_REG_DMA_TOP_AB_GROUP2_BASE_ADDR		0x1af000
+#define IPU6_REG_DMA_TOP_AB_RING_MIN_OFFSET(n)		(0x4 + (n) * 0xc)
+#define IPU6_REG_DMA_TOP_AB_RING_MAX_OFFSET(n)		(0x8 + (n) * 0xc)
+#define IPU6_REG_DMA_TOP_AB_RING_ACCESS_OFFSET(n)	(0xc + (n) * 0xc)
+
+enum ipu6_device_ab_group1_target_id {
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R0_SPC_DMEM,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R1_SPC_DMEM,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R2_SPC_DMEM,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R3_SPC_STATUS_REG,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R4_SPC_MASTER_BASE_ADDR,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R5_SPC_PC_STALL,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R6_SPC_EQ,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R7_SPC_RESERVED,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R8_SPC_RESERVED,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R9_SPP0,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R10_SPP1,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R11_CENTRAL_R1,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R12_IRQ,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R13_CENTRAL_R2,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R14_DMA,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R15_DMA,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R16_GP,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R17_ZLW_INSERTER,
+	IPU6_DEVICE_AB_GROUP1_TARGET_ID_R18_AB,
+};
+
+enum nci_ab_access_mode {
+	NCI_AB_ACCESS_MODE_RW,	/* read & write */
+	NCI_AB_ACCESS_MODE_RO,	/* read only */
+	NCI_AB_ACCESS_MODE_WO,	/* write only */
+	NCI_AB_ACCESS_MODE_NA,	/* No access at all */
+};
+
+/* IRQ-related registers in PSYS */
+#define IPU6_REG_PSYS_GPDEV_IRQ_EDGE		0x1aa200
+#define IPU6_REG_PSYS_GPDEV_IRQ_MASK		0x1aa204
+#define IPU6_REG_PSYS_GPDEV_IRQ_STATUS		0x1aa208
+#define IPU6_REG_PSYS_GPDEV_IRQ_CLEAR		0x1aa20c
+#define IPU6_REG_PSYS_GPDEV_IRQ_ENABLE		0x1aa210
+#define IPU6_REG_PSYS_GPDEV_IRQ_LEVEL_NOT_PULSE	0x1aa214
+/* There are 8 FW interrupts, n = 0..7 */
+#define IPU6_PSYS_GPDEV_FWIRQ0			5
+#define IPU6_PSYS_GPDEV_FWIRQ1			6
+#define IPU6_PSYS_GPDEV_FWIRQ2			7
+#define IPU6_PSYS_GPDEV_FWIRQ3			8
+#define IPU6_PSYS_GPDEV_FWIRQ4			9
+#define IPU6_PSYS_GPDEV_FWIRQ5			10
+#define IPU6_PSYS_GPDEV_FWIRQ6			11
+#define IPU6_PSYS_GPDEV_FWIRQ7			12
+#define IPU6_PSYS_GPDEV_IRQ_FWIRQ(n)		BIT(n)
+#define IPU6_REG_PSYS_GPDEV_FWIRQ(n)		(4 * (n) + 0x1aa100)
+
+#endif /* IPU6_PLATFORM_REGS_H */
diff --git a/drivers/media/pci/intel/ipu6/ipu6.c b/drivers/media/pci/intel/ipu6/ipu6.c
new file mode 100644
index 000000000000..1f4f20b9c94d
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6.c
@@ -0,0 +1,846 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2013--2024 Intel Corporation
+ */
+
+#include <linux/bitfield.h>
+#include <linux/bits.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/firmware.h>
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/pci-ats.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/scatterlist.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include <media/ipu-bridge.h>
+#include <media/ipu6-pci-table.h>
+
+#include "ipu6.h"
+#include "ipu6-bus.h"
+#include "ipu6-buttress.h"
+#include "ipu6-cpd.h"
+#include "ipu6-isys.h"
+#include "ipu6-mmu.h"
+#include "ipu6-platform-buttress-regs.h"
+#include "ipu6-platform-isys-csi2-reg.h"
+#include "ipu6-platform-regs.h"
+
+#define IPU6_PCI_BAR		0
+
+struct ipu6_cell_program {
+	u32 magic_number;
+
+	u32 blob_offset;
+	u32 blob_size;
+
+	u32 start[3];
+
+	u32 icache_source;
+	u32 icache_target;
+	u32 icache_size;
+
+	u32 pmem_source;
+	u32 pmem_target;
+	u32 pmem_size;
+
+	u32 data_source;
+	u32 data_target;
+	u32 data_size;
+
+	u32 bss_target;
+	u32 bss_size;
+
+	u32 cell_id;
+	u32 regs_addr;
+
+	u32 cell_pmem_data_bus_address;
+	u32 cell_dmem_data_bus_address;
+	u32 cell_pmem_control_bus_address;
+	u32 cell_dmem_control_bus_address;
+
+	u32 next;
+	u32 dummy[2];
+};
+
+static struct ipu6_isys_internal_pdata isys_ipdata = {
+	.hw_variant = {
+		.offset = IPU6_UNIFIED_OFFSET,
+		.nr_mmus = 3,
+		.mmu_hw = {
+			{
+				.offset = IPU6_ISYS_IOMMU0_OFFSET,
+				.info_bits = IPU6_INFO_REQUEST_DESTINATION_IOSF,
+				.nr_l1streams = 16,
+				.l1_block_sz = {
+					3, 8, 2, 2, 2, 2, 2, 2, 1, 1,
+					1, 1, 1, 1, 1, 1
+				},
+				.nr_l2streams = 16,
+				.l2_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.insert_read_before_invalidate = false,
+				.l1_stream_id_reg_offset =
+				IPU6_MMU_L1_STREAM_ID_REG_OFFSET,
+				.l2_stream_id_reg_offset =
+				IPU6_MMU_L2_STREAM_ID_REG_OFFSET,
+			},
+			{
+				.offset = IPU6_ISYS_IOMMU1_OFFSET,
+				.info_bits = 0,
+				.nr_l1streams = 16,
+				.l1_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 1, 1, 4
+				},
+				.nr_l2streams = 16,
+				.l2_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.insert_read_before_invalidate = false,
+				.l1_stream_id_reg_offset =
+				IPU6_MMU_L1_STREAM_ID_REG_OFFSET,
+				.l2_stream_id_reg_offset =
+				IPU6_MMU_L2_STREAM_ID_REG_OFFSET,
+			},
+			{
+				.offset = IPU6_ISYS_IOMMUI_OFFSET,
+				.info_bits = 0,
+				.nr_l1streams = 0,
+				.nr_l2streams = 0,
+				.insert_read_before_invalidate = false,
+			},
+		},
+		.cdc_fifos = 3,
+		.cdc_fifo_threshold = {6, 8, 2},
+		.dmem_offset = IPU6_ISYS_DMEM_OFFSET,
+		.spc_offset = IPU6_ISYS_SPC_OFFSET,
+	},
+	.isys_dma_overshoot = IPU6_ISYS_OVERALLOC_MIN,
+};
+
+static struct ipu6_psys_internal_pdata psys_ipdata = {
+	.hw_variant = {
+		.offset = IPU6_UNIFIED_OFFSET,
+		.nr_mmus = 4,
+		.mmu_hw = {
+			{
+				.offset = IPU6_PSYS_IOMMU0_OFFSET,
+				.info_bits =
+				IPU6_INFO_REQUEST_DESTINATION_IOSF,
+				.nr_l1streams = 16,
+				.l1_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.nr_l2streams = 16,
+				.l2_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.insert_read_before_invalidate = false,
+				.l1_stream_id_reg_offset =
+				IPU6_MMU_L1_STREAM_ID_REG_OFFSET,
+				.l2_stream_id_reg_offset =
+				IPU6_MMU_L2_STREAM_ID_REG_OFFSET,
+			},
+			{
+				.offset = IPU6_PSYS_IOMMU1_OFFSET,
+				.info_bits = 0,
+				.nr_l1streams = 32,
+				.l1_block_sz = {
+					1, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 10,
+					5, 4, 14, 6, 4, 14, 6, 4, 8,
+					4, 2, 1, 1, 1, 1, 14
+				},
+				.nr_l2streams = 32,
+				.l2_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.insert_read_before_invalidate = false,
+				.l1_stream_id_reg_offset =
+				IPU6_MMU_L1_STREAM_ID_REG_OFFSET,
+				.l2_stream_id_reg_offset =
+				IPU6_PSYS_MMU1W_L2_STREAM_ID_REG_OFFSET,
+			},
+			{
+				.offset = IPU6_PSYS_IOMMU1R_OFFSET,
+				.info_bits = 0,
+				.nr_l1streams = 16,
+				.l1_block_sz = {
+					1, 4, 4, 4, 4, 16, 8, 4, 32,
+					16, 16, 2, 2, 2, 1, 12
+				},
+				.nr_l2streams = 16,
+				.l2_block_sz = {
+					2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+					2, 2, 2, 2, 2, 2
+				},
+				.insert_read_before_invalidate = false,
+				.l1_stream_id_reg_offset =
+				IPU6_MMU_L1_STREAM_ID_REG_OFFSET,
+				.l2_stream_id_reg_offset =
+				IPU6_MMU_L2_STREAM_ID_REG_OFFSET,
+			},
+			{
+				.offset = IPU6_PSYS_IOMMUI_OFFSET,
+				.info_bits = 0,
+				.nr_l1streams = 0,
+				.nr_l2streams = 0,
+				.insert_read_before_invalidate = false,
+			},
+		},
+		.dmem_offset = IPU6_PSYS_DMEM_OFFSET,
+	},
+};
+
+static const struct ipu6_buttress_ctrl isys_buttress_ctrl = {
+	.ratio = IPU6_IS_FREQ_CTL_DEFAULT_RATIO,
+	.qos_floor = IPU6_IS_FREQ_CTL_DEFAULT_QOS_FLOOR_RATIO,
+	.freq_ctl = IPU6_BUTTRESS_REG_IS_FREQ_CTL,
+	.pwr_sts_shift = IPU6_BUTTRESS_PWR_STATE_IS_PWR_SHIFT,
+	.pwr_sts_mask = IPU6_BUTTRESS_PWR_STATE_IS_PWR_MASK,
+	.pwr_sts_on = IPU6_BUTTRESS_PWR_STATE_UP_DONE,
+	.pwr_sts_off = IPU6_BUTTRESS_PWR_STATE_DN_DONE,
+};
+
+static const struct ipu6_buttress_ctrl psys_buttress_ctrl = {
+	.ratio = IPU6_PS_FREQ_CTL_DEFAULT_RATIO,
+	.qos_floor = IPU6_PS_FREQ_CTL_DEFAULT_QOS_FLOOR_RATIO,
+	.freq_ctl = IPU6_BUTTRESS_REG_PS_FREQ_CTL,
+	.pwr_sts_shift = IPU6_BUTTRESS_PWR_STATE_PS_PWR_SHIFT,
+	.pwr_sts_mask = IPU6_BUTTRESS_PWR_STATE_PS_PWR_MASK,
+	.pwr_sts_on = IPU6_BUTTRESS_PWR_STATE_UP_DONE,
+	.pwr_sts_off = IPU6_BUTTRESS_PWR_STATE_DN_DONE,
+};
+
+static void
+ipu6_pkg_dir_configure_spc(struct ipu6_device *isp,
+			   const struct ipu6_hw_variants *hw_variant,
+			   int pkg_dir_idx, void __iomem *base,
+			   u64 *pkg_dir, dma_addr_t pkg_dir_vied_address)
+{
+	struct ipu6_cell_program *prog;
+	void __iomem *spc_base;
+	u32 server_fw_addr;
+	dma_addr_t dma_addr;
+	u32 pg_offset;
+
+	server_fw_addr = lower_32_bits(*(pkg_dir + (pkg_dir_idx + 1) * 2));
+	if (pkg_dir_idx == IPU6_CPD_PKG_DIR_ISYS_SERVER_IDX)
+		dma_addr = sg_dma_address(isp->isys->fw_sgt.sgl);
+	else
+		dma_addr = sg_dma_address(isp->psys->fw_sgt.sgl);
+
+	pg_offset = server_fw_addr - dma_addr;
+	prog = (struct ipu6_cell_program *)((uintptr_t)isp->cpd_fw->data +
+					    pg_offset);
+	spc_base = base + prog->regs_addr;
+	if (spc_base != (base + hw_variant->spc_offset))
+		dev_warn(&isp->pdev->dev,
+			 "SPC reg addr %p not matching value from CPD %p\n",
+			 base + hw_variant->spc_offset, spc_base);
+	writel(server_fw_addr + prog->blob_offset +
+	       prog->icache_source, spc_base + IPU6_PSYS_REG_SPC_ICACHE_BASE);
+	writel(IPU6_INFO_REQUEST_DESTINATION_IOSF,
+	       spc_base + IPU6_REG_PSYS_INFO_SEG_0_CONFIG_ICACHE_MASTER);
+	writel(prog->start[1], spc_base + IPU6_PSYS_REG_SPC_START_PC);
+	writel(pkg_dir_vied_address, base + hw_variant->dmem_offset);
+}
+
+void ipu6_configure_spc(struct ipu6_device *isp,
+			const struct ipu6_hw_variants *hw_variant,
+			int pkg_dir_idx, void __iomem *base, u64 *pkg_dir,
+			dma_addr_t pkg_dir_dma_addr)
+{
+	void __iomem *dmem_base = base + hw_variant->dmem_offset;
+	void __iomem *spc_regs_base = base + hw_variant->spc_offset;
+	u32 val;
+
+	val = readl(spc_regs_base + IPU6_PSYS_REG_SPC_STATUS_CTRL);
+	val |= IPU6_PSYS_SPC_STATUS_CTRL_ICACHE_INVALIDATE;
+	writel(val, spc_regs_base + IPU6_PSYS_REG_SPC_STATUS_CTRL);
+
+	if (isp->secure_mode)
+		writel(IPU6_PKG_DIR_IMR_OFFSET, dmem_base);
+	else
+		ipu6_pkg_dir_configure_spc(isp, hw_variant, pkg_dir_idx, base,
+					   pkg_dir, pkg_dir_dma_addr);
+}
+EXPORT_SYMBOL_NS_GPL(ipu6_configure_spc, "INTEL_IPU6");
+
+#define IPU6_ISYS_CSI2_NPORTS		4
+#define IPU6SE_ISYS_CSI2_NPORTS		4
+#define IPU6_TGL_ISYS_CSI2_NPORTS	8
+#define IPU6EP_MTL_ISYS_CSI2_NPORTS	6
+
+static void ipu6_internal_pdata_init(struct ipu6_device *isp)
+{
+	u8 hw_ver = isp->hw_ver;
+
+	isys_ipdata.num_parallel_streams = IPU6_ISYS_NUM_STREAMS;
+	isys_ipdata.sram_gran_shift = IPU6_SRAM_GRANULARITY_SHIFT;
+	isys_ipdata.sram_gran_size = IPU6_SRAM_GRANULARITY_SIZE;
+	isys_ipdata.max_sram_size = IPU6_MAX_SRAM_SIZE;
+	isys_ipdata.sensor_type_start = IPU6_FW_ISYS_SENSOR_TYPE_START;
+	isys_ipdata.sensor_type_end = IPU6_FW_ISYS_SENSOR_TYPE_END;
+	isys_ipdata.max_streams = IPU6_ISYS_NUM_STREAMS;
+	isys_ipdata.max_send_queues = IPU6_N_MAX_SEND_QUEUES;
+	isys_ipdata.max_sram_blocks = IPU6_NOF_SRAM_BLOCKS_MAX;
+	isys_ipdata.max_devq_size = IPU6_DEV_SEND_QUEUE_SIZE;
+	isys_ipdata.csi2.nports = IPU6_ISYS_CSI2_NPORTS;
+	isys_ipdata.csi2.irq_mask = IPU6_CSI_RX_ERROR_IRQ_MASK;
+	isys_ipdata.csi2.ctrl0_irq_edge = IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_EDGE;
+	isys_ipdata.csi2.ctrl0_irq_clear =
+		IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_CLEAR;
+	isys_ipdata.csi2.ctrl0_irq_mask = IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_MASK;
+	isys_ipdata.csi2.ctrl0_irq_enable =
+		IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_ENABLE;
+	isys_ipdata.csi2.ctrl0_irq_status =
+		IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_STATUS;
+	isys_ipdata.csi2.ctrl0_irq_lnp =
+		IPU6_REG_ISYS_CSI_TOP_CTRL0_IRQ_LEVEL_NOT_PULSE;
+	isys_ipdata.enhanced_iwake = is_ipu6ep_mtl(hw_ver) || is_ipu6ep(hw_ver);
+	psys_ipdata.hw_variant.spc_offset = IPU6_PSYS_SPC_OFFSET;
+	isys_ipdata.csi2.fw_access_port_ofs = CSI_REG_HUB_FW_ACCESS_PORT_OFS;
+
+	if (is_ipu6ep(hw_ver)) {
+		isys_ipdata.ltr = IPU6EP_LTR_VALUE;
+		isys_ipdata.memopen_threshold = IPU6EP_MIN_MEMOPEN_TH;
+	}
+
+	if (is_ipu6_tgl(hw_ver))
+		isys_ipdata.csi2.nports = IPU6_TGL_ISYS_CSI2_NPORTS;
+
+	if (is_ipu6ep_mtl(hw_ver)) {
+		isys_ipdata.csi2.nports = IPU6EP_MTL_ISYS_CSI2_NPORTS;
+
+		isys_ipdata.csi2.ctrl0_irq_edge =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_EDGE;
+		isys_ipdata.csi2.ctrl0_irq_clear =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_CLEAR;
+		isys_ipdata.csi2.ctrl0_irq_mask =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_MASK;
+		isys_ipdata.csi2.ctrl0_irq_enable =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_ENABLE;
+		isys_ipdata.csi2.ctrl0_irq_lnp =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_LEVEL_NOT_PULSE;
+		isys_ipdata.csi2.ctrl0_irq_status =
+			IPU6V6_REG_ISYS_CSI_TOP_CTRL0_IRQ_STATUS;
+		isys_ipdata.csi2.fw_access_port_ofs =
+			CSI_REG_HUB_FW_ACCESS_PORT_V6OFS;
+		isys_ipdata.ltr = IPU6EP_MTL_LTR_VALUE;
+		isys_ipdata.memopen_threshold = IPU6EP_MTL_MIN_MEMOPEN_TH;
+	}
+
+	if (is_ipu6se(hw_ver)) {
+		isys_ipdata.csi2.nports = IPU6SE_ISYS_CSI2_NPORTS;
+		isys_ipdata.csi2.irq_mask = IPU6SE_CSI_RX_ERROR_IRQ_MASK;
+		isys_ipdata.num_parallel_streams = IPU6SE_ISYS_NUM_STREAMS;
+		isys_ipdata.sram_gran_shift = IPU6SE_SRAM_GRANULARITY_SHIFT;
+		isys_ipdata.sram_gran_size = IPU6SE_SRAM_GRANULARITY_SIZE;
+		isys_ipdata.max_sram_size = IPU6SE_MAX_SRAM_SIZE;
+		isys_ipdata.sensor_type_start =
+			IPU6SE_FW_ISYS_SENSOR_TYPE_START;
+		isys_ipdata.sensor_type_end = IPU6SE_FW_ISYS_SENSOR_TYPE_END;
+		isys_ipdata.max_streams = IPU6SE_ISYS_NUM_STREAMS;
+		isys_ipdata.max_send_queues = IPU6SE_N_MAX_SEND_QUEUES;
+		isys_ipdata.max_sram_blocks = IPU6SE_NOF_SRAM_BLOCKS_MAX;
+		isys_ipdata.max_devq_size = IPU6SE_DEV_SEND_QUEUE_SIZE;
+		psys_ipdata.hw_variant.spc_offset = IPU6SE_PSYS_SPC_OFFSET;
+	}
+}
+
+static struct ipu6_bus_device *
+ipu6_isys_init(struct pci_dev *pdev, struct device *parent,
+	       struct ipu6_buttress_ctrl *ctrl, void __iomem *base,
+	       const struct ipu6_isys_internal_pdata *ipdata)
+{
+	struct device *dev = &pdev->dev;
+	struct ipu6_bus_device *isys_adev;
+	struct ipu6_isys_pdata *pdata;
+	int ret;
+
+	ret = ipu_bridge_init(dev, ipu_bridge_parse_ssdb);
+	if (ret) {
+		dev_err_probe(dev, ret, "IPU6 bridge init failed\n");
+		return ERR_PTR(ret);
+	}
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->base = base;
+	pdata->ipdata = ipdata;
+
+	isys_adev = ipu6_bus_initialize_device(pdev, parent, pdata, ctrl,
+					       IPU6_ISYS_NAME);
+	if (IS_ERR(isys_adev)) {
+		kfree(pdata);
+		return dev_err_cast_probe(dev, isys_adev,
+				"ipu6_bus_initialize_device isys failed\n");
+	}
+
+	isys_adev->mmu = ipu6_mmu_init(dev, base, ISYS_MMID,
+				       &ipdata->hw_variant);
+	if (IS_ERR(isys_adev->mmu)) {
+		put_device(&isys_adev->auxdev.dev);
+		kfree(pdata);
+		return dev_err_cast_probe(dev, isys_adev->mmu,
+				"ipu6_mmu_init(isys_adev->mmu) failed\n");
+	}
+
+	isys_adev->mmu->dev = &isys_adev->auxdev.dev;
+
+	ret = ipu6_bus_add_device(isys_adev);
+	if (ret) {
+		kfree(pdata);
+		return ERR_PTR(ret);
+	}
+
+	return isys_adev;
+}
+
+static struct ipu6_bus_device *
+ipu6_psys_init(struct pci_dev *pdev, struct device *parent,
+	       struct ipu6_buttress_ctrl *ctrl, void __iomem *base,
+	       const struct ipu6_psys_internal_pdata *ipdata)
+{
+	struct ipu6_bus_device *psys_adev;
+	struct ipu6_psys_pdata *pdata;
+	int ret;
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->base = base;
+	pdata->ipdata = ipdata;
+
+	psys_adev = ipu6_bus_initialize_device(pdev, parent, pdata, ctrl,
+					       IPU6_PSYS_NAME);
+	if (IS_ERR(psys_adev)) {
+		kfree(pdata);
+		return dev_err_cast_probe(&pdev->dev, psys_adev,
+				"ipu6_bus_initialize_device psys failed\n");
+	}
+
+	psys_adev->mmu = ipu6_mmu_init(&pdev->dev, base, PSYS_MMID,
+				       &ipdata->hw_variant);
+	if (IS_ERR(psys_adev->mmu)) {
+		put_device(&psys_adev->auxdev.dev);
+		kfree(pdata);
+		return dev_err_cast_probe(&pdev->dev, psys_adev->mmu,
+				"ipu6_mmu_init(psys_adev->mmu) failed\n");
+	}
+
+	psys_adev->mmu->dev = &psys_adev->auxdev.dev;
+
+	ret = ipu6_bus_add_device(psys_adev);
+	if (ret) {
+		kfree(pdata);
+		return ERR_PTR(ret);
+	}
+
+	return psys_adev;
+}
+
+static int ipu6_pci_config_setup(struct pci_dev *dev, u8 hw_ver)
+{
+	int ret;
+
+	/* No PCI msi capability for IPU6EP */
+	if (is_ipu6ep(hw_ver) || is_ipu6ep_mtl(hw_ver)) {
+		/* likely do nothing as msi not enabled by default */
+		pci_disable_msi(dev);
+		return 0;
+	}
+
+	ret = pci_alloc_irq_vectors(dev, 1, 1, PCI_IRQ_MSI);
+	if (ret < 0)
+		return dev_err_probe(&dev->dev, ret, "Request msi failed");
+
+	return 0;
+}
+
+static void ipu6_configure_vc_mechanism(struct ipu6_device *isp)
+{
+	u32 val = readl(isp->base + BUTTRESS_REG_BTRS_CTRL);
+
+	if (IPU6_BTRS_ARB_STALL_MODE_VC0 == IPU6_BTRS_ARB_MODE_TYPE_STALL)
+		val |= BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC0;
+	else
+		val &= ~BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC0;
+
+	if (IPU6_BTRS_ARB_STALL_MODE_VC1 == IPU6_BTRS_ARB_MODE_TYPE_STALL)
+		val |= BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC1;
+	else
+		val &= ~BUTTRESS_REG_BTRS_CTRL_STALL_MODE_VC1;
+
+	writel(val, isp->base + BUTTRESS_REG_BTRS_CTRL);
+}
+
+static int ipu6_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
+{
+	struct ipu6_buttress_ctrl *isys_ctrl = NULL, *psys_ctrl = NULL;
+	struct device *dev = &pdev->dev;
+	void __iomem *isys_base = NULL;
+	void __iomem *psys_base = NULL;
+	struct ipu6_device *isp;
+	phys_addr_t phys;
+	u32 val, version, sku_id;
+	int ret;
+
+	isp = devm_kzalloc(dev, sizeof(*isp), GFP_KERNEL);
+	if (!isp)
+		return -ENOMEM;
+
+	isp->pdev = pdev;
+	INIT_LIST_HEAD(&isp->devices);
+
+	ret = pcim_enable_device(pdev);
+	if (ret)
+		return dev_err_probe(dev, ret, "Enable PCI device failed\n");
+
+	phys = pci_resource_start(pdev, IPU6_PCI_BAR);
+	dev_dbg(dev, "IPU6 PCI bar[%u] = %pa\n", IPU6_PCI_BAR, &phys);
+
+	isp->base = pcim_iomap_region(pdev, IPU6_PCI_BAR, IPU6_NAME);
+	if (IS_ERR(isp->base))
+		return dev_err_probe(dev, PTR_ERR(isp->base),
+				     "Failed to I/O mem remapping\n");
+
+	pci_set_drvdata(pdev, isp);
+	pci_set_master(pdev);
+
+	isp->cpd_metadata_cmpnt_size = sizeof(struct ipu6_cpd_metadata_cmpnt);
+	switch (id->device) {
+	case PCI_DEVICE_ID_INTEL_IPU6:
+		isp->hw_ver = IPU6_VER_6;
+		isp->cpd_fw_name = IPU6_FIRMWARE_NAME;
+		break;
+	case PCI_DEVICE_ID_INTEL_IPU6SE:
+		isp->hw_ver = IPU6_VER_6SE;
+		isp->cpd_fw_name = IPU6SE_FIRMWARE_NAME;
+		isp->cpd_metadata_cmpnt_size =
+			sizeof(struct ipu6se_cpd_metadata_cmpnt);
+		break;
+	case PCI_DEVICE_ID_INTEL_IPU6EP_ADLP:
+	case PCI_DEVICE_ID_INTEL_IPU6EP_RPLP:
+		isp->hw_ver = IPU6_VER_6EP;
+		isp->cpd_fw_name = IPU6EP_FIRMWARE_NAME;
+		break;
+	case PCI_DEVICE_ID_INTEL_IPU6EP_ADLN:
+		isp->hw_ver = IPU6_VER_6EP;
+		isp->cpd_fw_name = IPU6EPADLN_FIRMWARE_NAME;
+		break;
+	case PCI_DEVICE_ID_INTEL_IPU6EP_MTL:
+		isp->hw_ver = IPU6_VER_6EP_MTL;
+		isp->cpd_fw_name = IPU6EPMTL_FIRMWARE_NAME;
+		break;
+	default:
+		return dev_err_probe(dev, -ENODEV,
+				     "Unsupported IPU6 device %x\n",
+				     id->device);
+	}
+
+	ipu6_internal_pdata_init(isp);
+
+	isys_base = isp->base + isys_ipdata.hw_variant.offset;
+	psys_base = isp->base + psys_ipdata.hw_variant.offset;
+
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(39));
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to set DMA mask\n");
+
+	dma_set_max_seg_size(dev, UINT_MAX);
+
+	ret = ipu6_pci_config_setup(pdev, isp->hw_ver);
+	if (ret)
+		return ret;
+
+	ret = ipu6_buttress_init(isp);
+	if (ret)
+		return ret;
+
+	ret = request_firmware(&isp->cpd_fw, isp->cpd_fw_name, dev);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret,
+			      "Requesting signed firmware %s failed\n",
+			      isp->cpd_fw_name);
+		goto buttress_exit;
+	}
+
+	ret = ipu6_cpd_validate_cpd_file(isp, isp->cpd_fw->data,
+					 isp->cpd_fw->size);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret,
+			      "Failed to validate cpd\n");
+		goto out_ipu6_bus_del_devices;
+	}
+
+	isys_ctrl = devm_kmemdup(dev, &isys_buttress_ctrl,
+				 sizeof(isys_buttress_ctrl), GFP_KERNEL);
+	if (!isys_ctrl) {
+		ret = -ENOMEM;
+		goto out_ipu6_bus_del_devices;
+	}
+
+	isp->isys = ipu6_isys_init(pdev, dev, isys_ctrl, isys_base,
+				   &isys_ipdata);
+	if (IS_ERR(isp->isys)) {
+		ret = PTR_ERR(isp->isys);
+		goto out_ipu6_bus_del_devices;
+	}
+
+	psys_ctrl = devm_kmemdup(dev, &psys_buttress_ctrl,
+				 sizeof(psys_buttress_ctrl), GFP_KERNEL);
+	if (!psys_ctrl) {
+		ret = -ENOMEM;
+		goto out_ipu6_bus_del_devices;
+	}
+
+	isp->psys = ipu6_psys_init(pdev, &isp->isys->auxdev.dev, psys_ctrl,
+				   psys_base, &psys_ipdata);
+	if (IS_ERR(isp->psys)) {
+		ret = PTR_ERR(isp->psys);
+		goto out_ipu6_bus_del_devices;
+	}
+
+	ret = pm_runtime_resume_and_get(&isp->psys->auxdev.dev);
+	if (ret < 0)
+		goto out_ipu6_bus_del_devices;
+
+	ret = ipu6_mmu_hw_init(isp->psys->mmu);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret,
+			      "Failed to set MMU hardware\n");
+		goto out_ipu6_bus_del_devices;
+	}
+
+	ret = ipu6_buttress_map_fw_image(isp->psys, isp->cpd_fw,
+					 &isp->psys->fw_sgt);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret, "failed to map fw image\n");
+		goto out_ipu6_bus_del_devices;
+	}
+
+	ret = ipu6_cpd_create_pkg_dir(isp->psys, isp->cpd_fw->data);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret,
+			      "failed to create pkg dir\n");
+		goto out_ipu6_bus_del_devices;
+	}
+
+	ret = devm_request_threaded_irq(dev, pdev->irq, ipu6_buttress_isr,
+					ipu6_buttress_isr_threaded,
+					IRQF_SHARED, IPU6_NAME, isp);
+	if (ret) {
+		dev_err_probe(dev, ret, "Requesting irq failed\n");
+		goto out_ipu6_bus_del_devices;
+	}
+
+	ret = ipu6_buttress_authenticate(isp);
+	if (ret) {
+		dev_err_probe(&isp->pdev->dev, ret,
+			      "FW authentication failed\n");
+		goto out_free_irq;
+	}
+
+	ipu6_mmu_hw_cleanup(isp->psys->mmu);
+	pm_runtime_put(&isp->psys->auxdev.dev);
+
+	/* Configure the arbitration mechanisms for VC requests */
+	ipu6_configure_vc_mechanism(isp);
+
+	val = readl(isp->base + BUTTRESS_REG_SKU);
+	sku_id = FIELD_GET(GENMASK(6, 4), val);
+	version = FIELD_GET(GENMASK(3, 0), val);
+	dev_info(dev, "IPU%u-v%u[%x] hardware version %d\n", version, sku_id,
+		 pdev->device, isp->hw_ver);
+
+	pm_runtime_put_noidle(dev);
+	pm_runtime_allow(dev);
+
+	isp->bus_ready_to_probe = true;
+
+	return 0;
+
+out_free_irq:
+	devm_free_irq(dev, pdev->irq, isp);
+out_ipu6_bus_del_devices:
+	if (isp->psys) {
+		ipu6_cpd_free_pkg_dir(isp->psys);
+		ipu6_buttress_unmap_fw_image(isp->psys, &isp->psys->fw_sgt);
+	}
+	if (!IS_ERR_OR_NULL(isp->psys) && !IS_ERR_OR_NULL(isp->psys->mmu))
+		ipu6_mmu_cleanup(isp->psys->mmu);
+	if (!IS_ERR_OR_NULL(isp->isys) && !IS_ERR_OR_NULL(isp->isys->mmu))
+		ipu6_mmu_cleanup(isp->isys->mmu);
+	ipu6_bus_del_devices(pdev);
+	release_firmware(isp->cpd_fw);
+buttress_exit:
+	ipu6_buttress_exit(isp);
+
+	return ret;
+}
+
+static void ipu6_pci_remove(struct pci_dev *pdev)
+{
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+	struct ipu6_mmu *isys_mmu = isp->isys->mmu;
+	struct ipu6_mmu *psys_mmu = isp->psys->mmu;
+
+	devm_free_irq(&pdev->dev, pdev->irq, isp);
+	ipu6_cpd_free_pkg_dir(isp->psys);
+
+	ipu6_buttress_unmap_fw_image(isp->psys, &isp->psys->fw_sgt);
+	ipu6_buttress_exit(isp);
+
+	ipu6_bus_del_devices(pdev);
+
+	pm_runtime_forbid(&pdev->dev);
+	pm_runtime_get_noresume(&pdev->dev);
+
+	release_firmware(isp->cpd_fw);
+
+	ipu6_mmu_cleanup(psys_mmu);
+	ipu6_mmu_cleanup(isys_mmu);
+}
+
+static void ipu6_pci_reset_prepare(struct pci_dev *pdev)
+{
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+
+	pm_runtime_forbid(&isp->pdev->dev);
+}
+
+static void ipu6_pci_reset_done(struct pci_dev *pdev)
+{
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+
+	ipu6_buttress_restore(isp);
+	if (isp->secure_mode)
+		ipu6_buttress_reset_authentication(isp);
+
+	isp->need_ipc_reset = true;
+	pm_runtime_allow(&isp->pdev->dev);
+}
+
+/*
+ * PCI base driver code requires driver to provide these to enable
+ * PCI device level PM state transitions (D0<->D3)
+ */
+static int ipu6_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+
+	synchronize_irq(pdev->irq);
+	return 0;
+}
+
+static int ipu6_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+	struct ipu6_buttress *b = &isp->buttress;
+	int ret;
+
+	/* Configure the arbitration mechanisms for VC requests */
+	ipu6_configure_vc_mechanism(isp);
+
+	isp->secure_mode = ipu6_buttress_get_secure_mode(isp);
+	dev_info(dev, "IPU6 in %s mode\n",
+		 isp->secure_mode ? "secure" : "non-secure");
+
+	ipu6_buttress_restore(isp);
+
+	ret = ipu6_buttress_ipc_reset(isp, &b->cse);
+	if (ret)
+		dev_err(&isp->pdev->dev, "IPC reset protocol failed!\n");
+
+	ret = pm_runtime_resume_and_get(&isp->psys->auxdev.dev);
+	if (ret < 0) {
+		dev_err(&isp->psys->auxdev.dev, "Failed to get runtime PM\n");
+		return 0;
+	}
+
+	ret = ipu6_buttress_authenticate(isp);
+	if (ret)
+		dev_err(&isp->pdev->dev, "FW authentication failed(%d)\n", ret);
+
+	pm_runtime_put(&isp->psys->auxdev.dev);
+
+	return 0;
+}
+
+static int ipu6_runtime_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct ipu6_device *isp = pci_get_drvdata(pdev);
+	int ret;
+
+	ipu6_configure_vc_mechanism(isp);
+	ipu6_buttress_restore(isp);
+
+	if (isp->need_ipc_reset) {
+		struct ipu6_buttress *b = &isp->buttress;
+
+		isp->need_ipc_reset = false;
+		ret = ipu6_buttress_ipc_reset(isp, &b->cse);
+		if (ret)
+			dev_err(&isp->pdev->dev, "IPC reset protocol failed\n");
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops ipu6_pm_ops = {
+	SYSTEM_SLEEP_PM_OPS(&ipu6_suspend, &ipu6_resume)
+	RUNTIME_PM_OPS(&ipu6_suspend, &ipu6_runtime_resume, NULL)
+};
+
+MODULE_DEVICE_TABLE(pci, ipu6_pci_tbl);
+
+static const struct pci_error_handlers pci_err_handlers = {
+	.reset_prepare = ipu6_pci_reset_prepare,
+	.reset_done = ipu6_pci_reset_done,
+};
+
+static struct pci_driver ipu6_pci_driver = {
+	.name = IPU6_NAME,
+	.id_table = ipu6_pci_tbl,
+	.probe = ipu6_pci_probe,
+	.remove = ipu6_pci_remove,
+	.driver = {
+		.pm = pm_ptr(&ipu6_pm_ops),
+	},
+	.err_handler = &pci_err_handlers,
+};
+
+module_pci_driver(ipu6_pci_driver);
+
+MODULE_IMPORT_NS("INTEL_IPU_BRIDGE");
+MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
+MODULE_AUTHOR("Tianshu Qiu <tian.shu.qiu@intel.com>");
+MODULE_AUTHOR("Bingbu Cao <bingbu.cao@intel.com>");
+MODULE_AUTHOR("Qingwu Zhang <qingwu.zhang@intel.com>");
+MODULE_AUTHOR("Yunliang Ding <yunliang.ding@intel.com>");
+MODULE_AUTHOR("Hongju Wang <hongju.wang@intel.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Intel IPU6 PCI driver");
diff --git a/drivers/media/pci/intel/ipu6/ipu6.h b/drivers/media/pci/intel/ipu6/ipu6.h
new file mode 100644
index 000000000000..92e3c3414c91
--- /dev/null
+++ b/drivers/media/pci/intel/ipu6/ipu6.h
@@ -0,0 +1,342 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/* Copyright (C) 2013 - 2024 Intel Corporation */
+
+#ifndef IPU6_H
+#define IPU6_H
+
+#include <linux/list.h>
+#include <linux/pci.h>
+#include <linux/types.h>
+
+#include "ipu6-buttress.h"
+
+struct firmware;
+struct pci_dev;
+struct ipu6_bus_device;
+
+#define IPU6_NAME			"intel-ipu6"
+#define IPU6_MEDIA_DEV_MODEL_NAME	"ipu6"
+
+#define IPU6SE_FIRMWARE_NAME		"intel/ipu/ipu6se_fw.bin"
+#define IPU6EP_FIRMWARE_NAME		"intel/ipu/ipu6ep_fw.bin"
+#define IPU6_FIRMWARE_NAME		"intel/ipu/ipu6_fw.bin"
+#define IPU6EPMTL_FIRMWARE_NAME		"intel/ipu/ipu6epmtl_fw.bin"
+#define IPU6EPADLN_FIRMWARE_NAME	"intel/ipu/ipu6epadln_fw.bin"
+
+enum ipu6_version {
+	IPU6_VER_INVALID = 0,
+	IPU6_VER_6 = 1,
+	IPU6_VER_6SE = 3,
+	IPU6_VER_6EP = 5,
+	IPU6_VER_6EP_MTL = 6,
+};
+
+/*
+ * IPU6 - TGL
+ * IPU6SE - JSL
+ * IPU6EP - ADL/RPL
+ * IPU6EP_MTL - MTL
+ */
+static inline bool is_ipu6se(u8 hw_ver)
+{
+	return hw_ver == IPU6_VER_6SE;
+}
+
+static inline bool is_ipu6ep(u8 hw_ver)
+{
+	return hw_ver == IPU6_VER_6EP;
+}
+
+static inline bool is_ipu6ep_mtl(u8 hw_ver)
+{
+	return hw_ver == IPU6_VER_6EP_MTL;
+}
+
+static inline bool is_ipu6_tgl(u8 hw_ver)
+{
+	return hw_ver == IPU6_VER_6;
+}
+
+/*
+ * ISYS DMA can overshoot. For higher resolutions over allocation is one line
+ * but it must be at minimum 1024 bytes. Value could be different in
+ * different versions / generations thus provide it via platform data.
+ */
+#define IPU6_ISYS_OVERALLOC_MIN		1024
+
+/* Physical pages in GDA is 128, page size is 2K for IPU6, 1K for others */
+#define IPU6_DEVICE_GDA_NR_PAGES		128
+
+/* Virtualization factor to calculate the available virtual pages */
+#define IPU6_DEVICE_GDA_VIRT_FACTOR	32
+
+struct ipu6_device {
+	struct pci_dev *pdev;
+	struct list_head devices;
+	struct ipu6_bus_device *isys;
+	struct ipu6_bus_device *psys;
+	struct ipu6_buttress buttress;
+
+	const struct firmware *cpd_fw;
+	const char *cpd_fw_name;
+	u32 cpd_metadata_cmpnt_size;
+
+	void __iomem *base;
+	bool need_ipc_reset;
+	bool secure_mode;
+	u8 hw_ver;
+	bool bus_ready_to_probe;
+};
+
+#define IPU6_ISYS_NAME "isys"
+#define IPU6_PSYS_NAME "psys"
+
+#define IPU6_MMU_MAX_DEVICES		4
+#define IPU6_MMU_ADDR_BITS		32
+/* The firmware is accessible within the first 2 GiB only in non-secure mode. */
+#define IPU6_MMU_ADDR_BITS_NON_SECURE	31
+
+#define IPU6_MMU_MAX_TLB_L1_STREAMS	32
+#define IPU6_MMU_MAX_TLB_L2_STREAMS	32
+#define IPU6_MAX_LI_BLOCK_ADDR		128
+#define IPU6_MAX_L2_BLOCK_ADDR		64
+
+#define IPU6SE_ISYS_NUM_STREAMS          IPU6SE_NONSECURE_STREAM_ID_MAX
+#define IPU6_ISYS_NUM_STREAMS            IPU6_NONSECURE_STREAM_ID_MAX
+
+/*
+ * To maximize the IOSF utlization, IPU6 need to send requests in bursts.
+ * At the DMA interface with the buttress, there are CDC FIFOs with burst
+ * collection capability. CDC FIFO burst collectors have a configurable
+ * threshold and is configured based on the outcome of performance measurements.
+ *
+ * isys has 3 ports with IOSF interface for VC0, VC1 and VC2
+ * psys has 4 ports with IOSF interface for VC0, VC1w, VC1r and VC2
+ *
+ * Threshold values are pre-defined and are arrived at after performance
+ * evaluations on a type of IPU6
+ */
+#define IPU6_MAX_VC_IOSF_PORTS		4
+
+/*
+ * IPU6 must configure correct arbitration mechanism related to the IOSF VC
+ * requests. There are two options per VC0 and VC1 - > 0 means rearbitrate on
+ * stall and 1 means stall until the request is completed.
+ */
+#define IPU6_BTRS_ARB_MODE_TYPE_REARB	0
+#define IPU6_BTRS_ARB_MODE_TYPE_STALL	1
+
+/* Currently chosen arbitration mechanism for VC0 */
+#define IPU6_BTRS_ARB_STALL_MODE_VC0	\
+			IPU6_BTRS_ARB_MODE_TYPE_REARB
+
+/* Currently chosen arbitration mechanism for VC1 */
+#define IPU6_BTRS_ARB_STALL_MODE_VC1	\
+			IPU6_BTRS_ARB_MODE_TYPE_REARB
+
+/*
+ * MMU Invalidation HW bug workaround by ZLW mechanism
+ *
+ * Old IPU6 MMUV2 has a bug in the invalidation mechanism which might result in
+ * wrong translation or replication of the translation. This will cause data
+ * corruption. So we cannot directly use the MMU V2 invalidation registers
+ * to invalidate the MMU. Instead, whenever an invalidate is called, we need to
+ * clear the TLB by evicting all the valid translations by filling it with trash
+ * buffer (which is guaranteed not to be used by any other processes). ZLW is
+ * used to fill the L1 and L2 caches with the trash buffer translations. ZLW
+ * or Zero length write, is pre-fetch mechanism to pre-fetch the pages in
+ * advance to the L1 and L2 caches without triggering any memory operations.
+ *
+ * In MMU V2, L1 -> 16 streams and 64 blocks, maximum 16 blocks per stream
+ * One L1 block has 16 entries, hence points to 16 * 4K pages
+ * L2 -> 16 streams and 32 blocks. 2 blocks per streams
+ * One L2 block maps to 1024 L1 entries, hence points to 4MB address range
+ * 2 blocks per L2 stream means, 1 stream points to 8MB range
+ *
+ * As we need to clear the caches and 8MB being the biggest cache size, we need
+ * to have trash buffer which points to 8MB address range. As these trash
+ * buffers are not used for any memory transactions, we need only the least
+ * amount of physical memory. So we reserve 8MB IOVA address range but only
+ * one page is reserved from physical memory. Each of this 8MB IOVA address
+ * range is then mapped to the same physical memory page.
+ */
+/* One L2 entry maps 1024 L1 entries and one L1 entry per page */
+#define IPU6_MMUV2_L2_RANGE		(1024 * PAGE_SIZE)
+/* Max L2 blocks per stream */
+#define IPU6_MMUV2_MAX_L2_BLOCKS	2
+/* Max L1 blocks per stream */
+#define IPU6_MMUV2_MAX_L1_BLOCKS	16
+#define IPU6_MMUV2_TRASH_RANGE	(IPU6_MMUV2_L2_RANGE * IPU6_MMUV2_MAX_L2_BLOCKS)
+/* Entries per L1 block */
+#define MMUV2_ENTRIES_PER_L1_BLOCK	16
+#define MMUV2_TRASH_L1_BLOCK_OFFSET	(MMUV2_ENTRIES_PER_L1_BLOCK * PAGE_SIZE)
+#define MMUV2_TRASH_L2_BLOCK_OFFSET	IPU6_MMUV2_L2_RANGE
+
+/*
+ * In some of the IPU6 MMUs, there is provision to configure L1 and L2 page
+ * table caches. Both these L1 and L2 caches are divided into multiple sections
+ * called streams. There is maximum 16 streams for both caches. Each of these
+ * sections are subdivided into multiple blocks. When nr_l1streams = 0 and
+ * nr_l2streams = 0, means the MMU is of type MMU_V1 and do not support
+ * L1/L2 page table caches.
+ *
+ * L1 stream per block sizes are configurable and varies per usecase.
+ * L2 has constant block sizes - 2 blocks per stream.
+ *
+ * MMU1 support pre-fetching of the pages to have less cache lookup misses. To
+ * enable the pre-fetching, MMU1 AT (Address Translator) device registers
+ * need to be configured.
+ *
+ * There are four types of memory accesses which requires ZLW configuration.
+ * ZLW(Zero Length Write) is a mechanism to enable VT-d pre-fetching on IOMMU.
+ *
+ * 1. Sequential Access or 1D mode
+ *	Set ZLW_EN -> 1
+ *	set ZLW_PAGE_CROSS_1D -> 1
+ *	Set ZLW_N to "N" pages so that ZLW will be inserte N pages ahead where
+ *		  N is pre-defined and hardcoded in the platform data
+ *	Set ZLW_2D -> 0
+ *
+ * 2. ZLW 2D mode
+ *	Set ZLW_EN -> 1
+ *	set ZLW_PAGE_CROSS_1D -> 1,
+ *	Set ZLW_N -> 0
+ *	Set ZLW_2D -> 1
+ *
+ * 3. ZLW Enable (no 1D or 2D mode)
+ *	Set ZLW_EN -> 1
+ *	set ZLW_PAGE_CROSS_1D -> 0,
+ *	Set ZLW_N -> 0
+ *	Set ZLW_2D -> 0
+ *
+ * 4. ZLW disable
+ *	Set ZLW_EN -> 0
+ *	set ZLW_PAGE_CROSS_1D -> 0,
+ *	Set ZLW_N -> 0
+ *	Set ZLW_2D -> 0
+ *
+ * To configure the ZLW for the above memory access, four registers are
+ * available. Hence to track these four settings, we have the following entries
+ * in the struct ipu6_mmu_hw. Each of these entries are per stream and
+ * available only for the L1 streams.
+ *
+ * a. l1_zlw_en -> To track zlw enabled per stream (ZLW_EN)
+ * b. l1_zlw_1d_mode -> Track 1D mode per stream. ZLW inserted at page boundary
+ * c. l1_ins_zlw_ahead_pages -> to track how advance the ZLW need to be inserted
+ *			Insert ZLW request N pages ahead address.
+ * d. l1_zlw_2d_mode -> To track 2D mode per stream (ZLW_2D)
+ *
+ *
+ * Currently L1/L2 streams, blocks, AT ZLW configurations etc. are pre-defined
+ * as per the usecase specific calculations. Any change to this pre-defined
+ * table has to happen in sync with IPU6 FW.
+ */
+struct ipu6_mmu_hw {
+	union {
+		unsigned long offset;
+		void __iomem *base;
+	};
+	u32 info_bits;
+	u8 nr_l1streams;
+	/*
+	 * L1 has variable blocks per stream - total of 64 blocks and maximum of
+	 * 16 blocks per stream. Configurable by using the block start address
+	 * per stream. Block start address is calculated from the block size
+	 */
+	u8 l1_block_sz[IPU6_MMU_MAX_TLB_L1_STREAMS];
+	/* Is ZLW is enabled in each stream */
+	bool l1_zlw_en[IPU6_MMU_MAX_TLB_L1_STREAMS];
+	bool l1_zlw_1d_mode[IPU6_MMU_MAX_TLB_L1_STREAMS];
+	u8 l1_ins_zlw_ahead_pages[IPU6_MMU_MAX_TLB_L1_STREAMS];
+	bool l1_zlw_2d_mode[IPU6_MMU_MAX_TLB_L1_STREAMS];
+
+	u32 l1_stream_id_reg_offset;
+	u32 l2_stream_id_reg_offset;
+
+	u8 nr_l2streams;
+	/*
+	 * L2 has fixed 2 blocks per stream. Block address is calculated
+	 * from the block size
+	 */
+	u8 l2_block_sz[IPU6_MMU_MAX_TLB_L2_STREAMS];
+	/* flag to track if WA is needed for successive invalidate HW bug */
+	bool insert_read_before_invalidate;
+};
+
+struct ipu6_mmu_pdata {
+	u32 nr_mmus;
+	struct ipu6_mmu_hw mmu_hw[IPU6_MMU_MAX_DEVICES];
+	int mmid;
+};
+
+struct ipu6_isys_csi2_pdata {
+	void __iomem *base;
+};
+
+struct ipu6_isys_internal_csi2_pdata {
+	u32 nports;
+	u32 irq_mask;
+	u32 ctrl0_irq_edge;
+	u32 ctrl0_irq_clear;
+	u32 ctrl0_irq_mask;
+	u32 ctrl0_irq_enable;
+	u32 ctrl0_irq_lnp;
+	u32 ctrl0_irq_status;
+	u32 fw_access_port_ofs;
+};
+
+struct ipu6_isys_internal_tpg_pdata {
+	u32 ntpgs;
+	u32 *offsets;
+	u32 *sels;
+};
+
+struct ipu6_hw_variants {
+	unsigned long offset;
+	u32 nr_mmus;
+	struct ipu6_mmu_hw mmu_hw[IPU6_MMU_MAX_DEVICES];
+	u8 cdc_fifos;
+	u8 cdc_fifo_threshold[IPU6_MAX_VC_IOSF_PORTS];
+	u32 dmem_offset;
+	u32 spc_offset;
+};
+
+struct ipu6_isys_internal_pdata {
+	struct ipu6_isys_internal_csi2_pdata csi2;
+	struct ipu6_hw_variants hw_variant;
+	u32 num_parallel_streams;
+	u32 isys_dma_overshoot;
+	u32 sram_gran_shift;
+	u32 sram_gran_size;
+	u32 max_sram_size;
+	u32 max_streams;
+	u32 max_send_queues;
+	u32 max_sram_blocks;
+	u32 max_devq_size;
+	u32 sensor_type_start;
+	u32 sensor_type_end;
+	u32 ltr;
+	u32 memopen_threshold;
+	bool enhanced_iwake;
+};
+
+struct ipu6_isys_pdata {
+	void __iomem *base;
+	const struct ipu6_isys_internal_pdata *ipdata;
+};
+
+struct ipu6_psys_internal_pdata {
+	struct ipu6_hw_variants hw_variant;
+};
+
+struct ipu6_psys_pdata {
+	void __iomem *base;
+	const struct ipu6_psys_internal_pdata *ipdata;
+};
+
+int ipu6_fw_authenticate(void *data, u64 val);
+void ipu6_configure_spc(struct ipu6_device *isp,
+			const struct ipu6_hw_variants *hw_variant,
+			int pkg_dir_idx, void __iomem *base, u64 *pkg_dir,
+			dma_addr_t pkg_dir_dma_addr);
+#endif /* IPU6_H */
diff --git a/drivers/media/pci/intel/ivsc/Kconfig b/drivers/media/pci/intel/ivsc/Kconfig
index 407a800c81bc..a7d9607ecdc6 100644
--- a/drivers/media/pci/intel/ivsc/Kconfig
+++ b/drivers/media/pci/intel/ivsc/Kconfig
@@ -4,6 +4,7 @@
 config INTEL_VSC
 	tristate "Intel Visual Sensing Controller"
 	depends on INTEL_MEI && ACPI && VIDEO_DEV
+	depends on IPU_BRIDGE || !IPU_BRIDGE
 	select MEDIA_CONTROLLER
 	select VIDEO_V4L2_SUBDEV_API
 	select V4L2_FWNODE
diff --git a/drivers/media/pci/intel/ivsc/mei_csi.c b/drivers/media/pci/intel/ivsc/mei_csi.c
index 55e0c60c420c..92d871a378ba 100644
--- a/drivers/media/pci/intel/ivsc/mei_csi.c
+++ b/drivers/media/pci/intel/ivsc/mei_csi.c
@@ -19,12 +19,15 @@
 #include <linux/mei_cl_bus.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/pci.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/units.h>
 #include <linux/uuid.h>
 #include <linux/workqueue.h>
 
+#include <media/ipu-bridge.h>
+#include <media/ipu6-pci-table.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fwnode.h>
@@ -32,8 +35,6 @@
 
 #define MEI_CSI_ENTITY_NAME "Intel IVSC CSI"
 
-#define MEI_CSI_LINK_FREQ_400MHZ 400000000ULL
-
 /* the 5s used here is based on experiment */
 #define CSI_CMD_TIMEOUT (5 * HZ)
 /* to setup CSI-2 link an extra delay needed and determined experimentally */
@@ -118,12 +119,13 @@ struct mei_csi {
 	struct mutex lock;
 
 	struct v4l2_subdev subdev;
-	struct v4l2_subdev *remote;
+	struct media_pad *remote;
 	struct v4l2_async_notifier notifier;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct v4l2_ctrl *freq_ctrl;
 	struct v4l2_ctrl *privacy_ctrl;
-	unsigned int remote_pad;
+	/* lock for v4l2 controls */
+	struct mutex ctrl_lock;
 	/* start streaming or not */
 	int streaming;
 
@@ -133,9 +135,6 @@ struct mei_csi {
 	u32 nr_of_lanes;
 	/* frequency of the CSI-2 link */
 	u64 link_freq;
-
-	/* privacy status */
-	enum ivsc_privacy_status status;
 };
 
 static const struct v4l2_mbus_framefmt mei_csi_format_mbus_default = {
@@ -145,10 +144,6 @@ static const struct v4l2_mbus_framefmt mei_csi_format_mbus_default = {
 	.field = V4L2_FIELD_NONE,
 };
 
-static s64 link_freq_menu_items[] = {
-	MEI_CSI_LINK_FREQ_400MHZ
-};
-
 static inline struct mei_csi *notifier_to_csi(struct v4l2_async_notifier *n)
 {
 	return container_of(n, struct mei_csi, notifier);
@@ -159,11 +154,6 @@ static inline struct mei_csi *sd_to_csi(struct v4l2_subdev *sd)
 	return container_of(sd, struct mei_csi, subdev);
 }
 
-static inline struct mei_csi *ctrl_to_csi(struct v4l2_ctrl *ctrl)
-{
-	return container_of(ctrl->handler, struct mei_csi, ctrl_handler);
-}
-
 /* send a command to firmware and mutex must be held by caller */
 static int mei_csi_send(struct mei_csi *csi, u8 *buf, size_t len)
 {
@@ -187,7 +177,11 @@ static int mei_csi_send(struct mei_csi *csi, u8 *buf, size_t len)
 
 	/* command response status */
 	ret = csi->cmd_response.status;
-	if (ret) {
+	if (ret == -1) {
+		/* notify privacy on instead of reporting error */
+		ret = 0;
+		v4l2_ctrl_s_ctrl(csi->privacy_ctrl, 1);
+	} else if (ret) {
 		ret = -EINVAL;
 		goto out;
 	}
@@ -262,10 +256,9 @@ static void mei_csi_rx(struct mei_cl_device *cldev)
 
 	switch (notif.cmd_id) {
 	case CSI_PRIVACY_NOTIF:
-		if (notif.cont.cont < CSI_PRIVACY_MAX) {
-			csi->status = notif.cont.cont;
-			v4l2_ctrl_s_ctrl(csi->privacy_ctrl, csi->status);
-		}
+		if (notif.cont.cont < CSI_PRIVACY_MAX)
+			v4l2_ctrl_s_ctrl(csi->privacy_ctrl,
+					 notif.cont.cont == CSI_PRIVACY_ON);
 		break;
 	case CSI_SET_OWNER:
 	case CSI_SET_CONF:
@@ -281,11 +274,13 @@ static void mei_csi_rx(struct mei_cl_device *cldev)
 static int mei_csi_set_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct mei_csi *csi = sd_to_csi(sd);
+	struct v4l2_subdev *remote_sd =
+		media_entity_to_v4l2_subdev(csi->remote->entity);
 	s64 freq;
 	int ret;
 
 	if (enable && csi->streaming == 0) {
-		freq = v4l2_get_link_freq(csi->remote->ctrl_handler, 0, 0);
+		freq = v4l2_get_link_freq(csi->remote, 0, 0);
 		if (freq < 0) {
 			dev_err(&csi->cldev->dev,
 				"error %lld, invalid link_freq\n", freq);
@@ -304,11 +299,11 @@ static int mei_csi_set_stream(struct v4l2_subdev *sd, int enable)
 		if (ret < 0)
 			goto err_switch;
 
-		ret = v4l2_subdev_call(csi->remote, video, s_stream, 1);
+		ret = v4l2_subdev_call(remote_sd, video, s_stream, 1);
 		if (ret)
 			goto err_switch;
 	} else if (!enable && csi->streaming == 1) {
-		v4l2_subdev_call(csi->remote, video, s_stream, 0);
+		v4l2_subdev_call(remote_sd, video, s_stream, 0);
 
 		/* switch CSI-2 link to IVSC */
 		ret = csi_set_link_owner(csi, CSI_LINK_IVSC);
@@ -465,34 +460,30 @@ static int mei_csi_set_fmt(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int mei_csi_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+static int mei_csi_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
+				   struct v4l2_mbus_config *mbus_config)
 {
-	struct mei_csi *csi = ctrl_to_csi(ctrl);
+	struct mei_csi *csi = sd_to_csi(sd);
+	unsigned int i;
 	s64 freq;
 
-	if (ctrl->id == V4L2_CID_LINK_FREQ) {
-		if (!csi->remote)
-			return -EINVAL;
-
-		freq = v4l2_get_link_freq(csi->remote->ctrl_handler, 0, 0);
-		if (freq < 0) {
-			dev_err(&csi->cldev->dev,
-				"error %lld, invalid link_freq\n", freq);
-			return -EINVAL;
-		}
+	mbus_config->type = V4L2_MBUS_CSI2_DPHY;
+	for (i = 0; i < V4L2_MBUS_CSI2_MAX_DATA_LANES; i++)
+		mbus_config->bus.mipi_csi2.data_lanes[i] = i + 1;
+	mbus_config->bus.mipi_csi2.num_data_lanes = csi->nr_of_lanes;
 
-		link_freq_menu_items[0] = freq;
-		ctrl->val = 0;
-
-		return 0;
+	freq = v4l2_get_link_freq(csi->remote, 0, 0);
+	if (freq < 0) {
+		dev_err(&csi->cldev->dev,
+			"error %lld, invalid link_freq\n", freq);
+		return -EINVAL;
 	}
 
-	return -EINVAL;
-}
+	csi->link_freq = freq;
+	mbus_config->link_freq = freq;
 
-static const struct v4l2_ctrl_ops mei_csi_ctrl_ops = {
-	.g_volatile_ctrl = mei_csi_g_volatile_ctrl,
-};
+	return 0;
+}
 
 static const struct v4l2_subdev_video_ops mei_csi_video_ops = {
 	.s_stream = mei_csi_set_stream,
@@ -501,6 +492,7 @@ static const struct v4l2_subdev_video_ops mei_csi_video_ops = {
 static const struct v4l2_subdev_pad_ops mei_csi_pad_ops = {
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = mei_csi_set_fmt,
+	.get_mbus_config = mei_csi_get_mbus_config,
 };
 
 static const struct v4l2_subdev_ops mei_csi_subdev_ops = {
@@ -528,8 +520,7 @@ static int mei_csi_notify_bound(struct v4l2_async_notifier *notifier,
 	if (pad < 0)
 		return pad;
 
-	csi->remote = subdev;
-	csi->remote_pad = pad;
+	csi->remote = &subdev->entity.pads[pad];
 
 	return media_create_pad_link(&subdev->entity, pad,
 				     &csi->subdev.entity, CSI_PAD_SINK,
@@ -553,25 +544,15 @@ static const struct v4l2_async_notifier_operations mei_csi_notify_ops = {
 
 static int mei_csi_init_controls(struct mei_csi *csi)
 {
-	u32 max;
 	int ret;
 
-	ret = v4l2_ctrl_handler_init(&csi->ctrl_handler, 2);
+	mutex_init(&csi->ctrl_lock);
+
+	ret = v4l2_ctrl_handler_init(&csi->ctrl_handler, 1);
 	if (ret)
 		return ret;
 
-	csi->ctrl_handler.lock = &csi->lock;
-
-	max = ARRAY_SIZE(link_freq_menu_items) - 1;
-	csi->freq_ctrl = v4l2_ctrl_new_int_menu(&csi->ctrl_handler,
-						&mei_csi_ctrl_ops,
-						V4L2_CID_LINK_FREQ,
-						max,
-						0,
-						link_freq_menu_items);
-	if (csi->freq_ctrl)
-		csi->freq_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY |
-					 V4L2_CTRL_FLAG_VOLATILE;
+	csi->ctrl_handler.lock = &csi->ctrl_lock;
 
 	csi->privacy_ctrl = v4l2_ctrl_new_std(&csi->ctrl_handler, NULL,
 					      V4L2_CID_PRIVACY, 0, 1, 1, 0);
@@ -661,11 +642,26 @@ static int mei_csi_probe(struct mei_cl_device *cldev,
 			 const struct mei_cl_device_id *id)
 {
 	struct device *dev = &cldev->dev;
+	struct pci_dev *ipu;
 	struct mei_csi *csi;
+	unsigned int i;
 	int ret;
 
-	if (!dev_fwnode(dev))
-		return -EPROBE_DEFER;
+	for (i = 0, ipu = NULL; !ipu && ipu6_pci_tbl[i].vendor; i++)
+		ipu = pci_get_device(ipu6_pci_tbl[i].vendor,
+				     ipu6_pci_tbl[i].device, NULL);
+
+	if (!ipu)
+		return -ENODEV;
+
+	ret = ipu_bridge_init(&ipu->dev, ipu_bridge_parse_ssdb);
+	put_device(&ipu->dev);
+	if (ret < 0)
+		return ret;
+	if (!dev_fwnode(dev)) {
+		dev_err(dev, "mei-csi probed without device fwnode!\n");
+		return -ENXIO;
+	}
 
 	csi = devm_kzalloc(dev, sizeof(struct mei_csi), GFP_KERNEL);
 	if (!csi)
@@ -737,6 +733,7 @@ err_entity:
 
 err_ctrl_handler:
 	v4l2_ctrl_handler_free(&csi->ctrl_handler);
+	mutex_destroy(&csi->ctrl_lock);
 	v4l2_async_nf_unregister(&csi->notifier);
 	v4l2_async_nf_cleanup(&csi->notifier);
 
@@ -756,6 +753,7 @@ static void mei_csi_remove(struct mei_cl_device *cldev)
 	v4l2_async_nf_unregister(&csi->notifier);
 	v4l2_async_nf_cleanup(&csi->notifier);
 	v4l2_ctrl_handler_free(&csi->ctrl_handler);
+	mutex_destroy(&csi->ctrl_lock);
 	v4l2_async_unregister_subdev(&csi->subdev);
 	v4l2_subdev_cleanup(&csi->subdev);
 	media_entity_cleanup(&csi->subdev.entity);
@@ -784,6 +782,7 @@ static struct mei_cl_driver mei_csi_driver = {
 
 module_mei_cl_driver(mei_csi_driver);
 
+MODULE_IMPORT_NS("INTEL_IPU_BRIDGE");
 MODULE_AUTHOR("Wentong Wu <wentong.wu@intel.com>");
 MODULE_AUTHOR("Zhifeng Wang <zhifeng.wang@intel.com>");
 MODULE_DESCRIPTION("Device driver for IVSC CSI");
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index ba503d820e48..ecc20cd89926 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -371,33 +371,6 @@ int ivtv_msleep_timeout(unsigned int msecs, int intr)
 	return 0;
 }
 
-/* Release ioremapped memory */
-static void ivtv_iounmap(struct ivtv *itv)
-{
-	if (itv == NULL)
-		return;
-
-	/* Release registers memory */
-	if (itv->reg_mem != NULL) {
-		IVTV_DEBUG_INFO("releasing reg_mem\n");
-		iounmap(itv->reg_mem);
-		itv->reg_mem = NULL;
-	}
-	/* Release io memory */
-	if (itv->has_cx23415 && itv->dec_mem != NULL) {
-		IVTV_DEBUG_INFO("releasing dec_mem\n");
-		iounmap(itv->dec_mem);
-	}
-	itv->dec_mem = NULL;
-
-	/* Release io memory */
-	if (itv->enc_mem != NULL) {
-		IVTV_DEBUG_INFO("releasing enc_mem\n");
-		iounmap(itv->enc_mem);
-		itv->enc_mem = NULL;
-	}
-}
-
 /* Hauppauge card? get values from tveeprom */
 void ivtv_read_eeprom(struct ivtv *itv, struct tveeprom *tv)
 {
@@ -833,7 +806,7 @@ static int ivtv_setup_pci(struct ivtv *itv, struct pci_dev *pdev,
 
 	IVTV_DEBUG_INFO("Enabling pci device\n");
 
-	if (pci_enable_device(pdev)) {
+	if (pcim_enable_device(pdev)) {
 		IVTV_ERR("Can't enable device!\n");
 		return -EIO;
 	}
@@ -841,24 +814,24 @@ static int ivtv_setup_pci(struct ivtv *itv, struct pci_dev *pdev,
 		IVTV_ERR("No suitable DMA available.\n");
 		return -EIO;
 	}
-	if (!request_mem_region(itv->base_addr, IVTV_ENCODER_SIZE, "ivtv encoder")) {
+	if (!devm_request_mem_region(&pdev->dev, itv->base_addr,
+				     IVTV_ENCODER_SIZE, "ivtv encoder")) {
 		IVTV_ERR("Cannot request encoder memory region.\n");
 		return -EIO;
 	}
 
-	if (!request_mem_region(itv->base_addr + IVTV_REG_OFFSET,
-				IVTV_REG_SIZE, "ivtv registers")) {
+	if (!devm_request_mem_region(&pdev->dev,
+				     itv->base_addr + IVTV_REG_OFFSET,
+				     IVTV_REG_SIZE, "ivtv registers")) {
 		IVTV_ERR("Cannot request register memory region.\n");
-		release_mem_region(itv->base_addr, IVTV_ENCODER_SIZE);
 		return -EIO;
 	}
 
 	if (itv->has_cx23415 &&
-	    !request_mem_region(itv->base_addr + IVTV_DECODER_OFFSET,
-				IVTV_DECODER_SIZE, "ivtv decoder")) {
+	    !devm_request_mem_region(&pdev->dev,
+				     itv->base_addr + IVTV_DECODER_OFFSET,
+				     IVTV_DECODER_SIZE, "ivtv decoder")) {
 		IVTV_ERR("Cannot request decoder memory region.\n");
-		release_mem_region(itv->base_addr, IVTV_ENCODER_SIZE);
-		release_mem_region(itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
 		return -EIO;
 	}
 
@@ -870,11 +843,6 @@ static int ivtv_setup_pci(struct ivtv *itv, struct pci_dev *pdev,
 		pci_read_config_word(pdev, PCI_COMMAND, &cmd);
 		if (!(cmd & PCI_COMMAND_MASTER)) {
 			IVTV_ERR("Bus Mastering is not enabled\n");
-			if (itv->has_cx23415)
-				release_mem_region(itv->base_addr + IVTV_DECODER_OFFSET,
-						   IVTV_DECODER_SIZE);
-			release_mem_region(itv->base_addr, IVTV_ENCODER_SIZE);
-			release_mem_region(itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
 			return -ENXIO;
 		}
 	}
@@ -1033,37 +1001,37 @@ static int ivtv_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
 
 	/* PCI Device Setup */
 	retval = ivtv_setup_pci(itv, pdev, pci_id);
-	if (retval == -EIO)
+	if (retval == -EIO || retval == -ENXIO)
 		goto free_worker;
-	if (retval == -ENXIO)
-		goto free_mem;
 
 	/* map io memory */
 	IVTV_DEBUG_INFO("attempting ioremap at 0x%llx len 0x%08x\n",
 		   (u64)itv->base_addr + IVTV_ENCODER_OFFSET, IVTV_ENCODER_SIZE);
-	itv->enc_mem = ioremap(itv->base_addr + IVTV_ENCODER_OFFSET,
-				       IVTV_ENCODER_SIZE);
+	itv->enc_mem = devm_ioremap(&pdev->dev,
+				    itv->base_addr + IVTV_ENCODER_OFFSET,
+				    IVTV_ENCODER_SIZE);
 	if (!itv->enc_mem) {
 		IVTV_ERR("ioremap failed. Can't get a window into CX23415/6 encoder memory\n");
 		IVTV_ERR("Each capture card with a CX23415/6 needs 8 MB of vmalloc address space for this window\n");
 		IVTV_ERR("Check the output of 'grep Vmalloc /proc/meminfo'\n");
 		IVTV_ERR("Use the vmalloc= kernel command line option to set VmallocTotal to a larger value\n");
 		retval = -ENOMEM;
-		goto free_mem;
+		goto free_worker;
 	}
 
 	if (itv->has_cx23415) {
 		IVTV_DEBUG_INFO("attempting ioremap at 0x%llx len 0x%08x\n",
 				(u64)itv->base_addr + IVTV_DECODER_OFFSET, IVTV_DECODER_SIZE);
-		itv->dec_mem = ioremap(itv->base_addr + IVTV_DECODER_OFFSET,
-				IVTV_DECODER_SIZE);
+		itv->dec_mem = devm_ioremap(&pdev->dev,
+					    itv->base_addr + IVTV_DECODER_OFFSET,
+					    IVTV_DECODER_SIZE);
 		if (!itv->dec_mem) {
 			IVTV_ERR("ioremap failed. Can't get a window into CX23415 decoder memory\n");
 			IVTV_ERR("Each capture card with a CX23415 needs 8 MB of vmalloc address space for this window\n");
 			IVTV_ERR("Check the output of 'grep Vmalloc /proc/meminfo'\n");
 			IVTV_ERR("Use the vmalloc= kernel command line option to set VmallocTotal to a larger value\n");
 			retval = -ENOMEM;
-			goto free_mem;
+			goto free_worker;
 		}
 	}
 	else {
@@ -1073,26 +1041,27 @@ static int ivtv_probe(struct pci_dev *pdev, const struct pci_device_id *pci_id)
 	/* map registers memory */
 	IVTV_DEBUG_INFO("attempting ioremap at 0x%llx len 0x%08x\n",
 		   (u64)itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
-	itv->reg_mem =
-	    ioremap(itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
+	itv->reg_mem = devm_ioremap(&pdev->dev,
+				    itv->base_addr + IVTV_REG_OFFSET,
+				    IVTV_REG_SIZE);
 	if (!itv->reg_mem) {
 		IVTV_ERR("ioremap failed. Can't get a window into CX23415/6 register space\n");
 		IVTV_ERR("Each capture card with a CX23415/6 needs 64 kB of vmalloc address space for this window\n");
 		IVTV_ERR("Check the output of 'grep Vmalloc /proc/meminfo'\n");
 		IVTV_ERR("Use the vmalloc= kernel command line option to set VmallocTotal to a larger value\n");
 		retval = -ENOMEM;
-		goto free_io;
+		goto free_worker;
 	}
 
 	retval = ivtv_gpio_init(itv);
 	if (retval)
-		goto free_io;
+		goto free_worker;
 
 	/* active i2c  */
 	IVTV_DEBUG_INFO("activating i2c...\n");
 	if (init_ivtv_i2c(itv)) {
 		IVTV_ERR("Could not initialize i2c\n");
-		goto free_io;
+		goto free_worker;
 	}
 
 	if (itv->card->hw_all & IVTV_HW_TVEEPROM) {
@@ -1277,13 +1246,6 @@ free_irq:
 free_i2c:
 	v4l2_ctrl_handler_free(&itv->cxhdl.hdl);
 	exit_ivtv_i2c(itv);
-free_io:
-	ivtv_iounmap(itv);
-free_mem:
-	release_mem_region(itv->base_addr, IVTV_ENCODER_SIZE);
-	release_mem_region(itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
-	if (itv->has_cx23415)
-		release_mem_region(itv->base_addr + IVTV_DECODER_OFFSET, IVTV_DECODER_SIZE);
 free_worker:
 	kthread_stop(itv->irq_worker_task);
 err:
@@ -1439,14 +1401,7 @@ static void ivtv_remove(struct pci_dev *pdev)
 	exit_ivtv_i2c(itv);
 
 	free_irq(itv->pdev->irq, (void *)itv);
-	ivtv_iounmap(itv);
-
-	release_mem_region(itv->base_addr, IVTV_ENCODER_SIZE);
-	release_mem_region(itv->base_addr + IVTV_REG_OFFSET, IVTV_REG_SIZE);
-	if (itv->has_cx23415)
-		release_mem_region(itv->base_addr + IVTV_DECODER_OFFSET, IVTV_DECODER_SIZE);
 
-	pci_disable_device(itv->pdev);
 	for (i = 0; i < IVTV_VBI_FRAMES; i++)
 		kfree(itv->vbi.sliced_mpeg_data[i]);
 
diff --git a/drivers/media/pci/ivtv/ivtv-fileops.c b/drivers/media/pci/ivtv/ivtv-fileops.c
index 4202c3a47d33..cfa28d035586 100644
--- a/drivers/media/pci/ivtv/ivtv-fileops.c
+++ b/drivers/media/pci/ivtv/ivtv-fileops.c
@@ -21,6 +21,7 @@
 #include "ivtv-ioctl.h"
 #include "ivtv-cards.h"
 #include "ivtv-firmware.h"
+#include <linux/lockdep.h>
 #include <media/v4l2-event.h>
 #include <media/i2c/saa7115.h>
 
@@ -190,12 +191,27 @@ static void ivtv_update_pgm_info(struct ivtv *itv)
 	itv->pgm_info_write_idx = (itv->pgm_info_write_idx + i) % itv->pgm_info_num;
 }
 
+static void ivtv_schedule(struct ivtv_stream *s)
+{
+	struct ivtv *itv = s->itv;
+	DEFINE_WAIT(wait);
+
+	lockdep_assert_held(&itv->serialize_lock);
+
+	mutex_unlock(&itv->serialize_lock);
+	prepare_to_wait(&s->waitq, &wait, TASK_INTERRUPTIBLE);
+	/* New buffers might have become free before we were added to the waitqueue */
+	if (!s->q_free.buffers)
+		schedule();
+	finish_wait(&s->waitq, &wait);
+	mutex_lock(&itv->serialize_lock);
+}
+
 static struct ivtv_buffer *ivtv_get_buffer(struct ivtv_stream *s, int non_block, int *err)
 {
 	struct ivtv *itv = s->itv;
 	struct ivtv_stream *s_vbi = &itv->streams[IVTV_ENC_STREAM_TYPE_VBI];
 	struct ivtv_buffer *buf;
-	DEFINE_WAIT(wait);
 
 	*err = 0;
 	while (1) {
@@ -258,13 +274,7 @@ static struct ivtv_buffer *ivtv_get_buffer(struct ivtv_stream *s, int non_block,
 		}
 
 		/* wait for more data to arrive */
-		mutex_unlock(&itv->serialize_lock);
-		prepare_to_wait(&s->waitq, &wait, TASK_INTERRUPTIBLE);
-		/* New buffers might have become available before we were added to the waitqueue */
-		if (!s->q_full.buffers)
-			schedule();
-		finish_wait(&s->waitq, &wait);
-		mutex_lock(&itv->serialize_lock);
+		ivtv_schedule(s);
 		if (signal_pending(current)) {
 			/* return if a signal was received */
 			IVTV_DEBUG_INFO("User stopped %s\n", s->name);
@@ -533,6 +543,25 @@ int ivtv_start_decoding(struct ivtv_open_id *id, int speed)
 	return 0;
 }
 
+static int ivtv_schedule_dma(struct ivtv_stream *s)
+{
+	struct ivtv *itv = s->itv;
+	int got_sig;
+	DEFINE_WAIT(wait);
+
+	lockdep_assert_held(&itv->serialize_lock);
+
+	mutex_unlock(&itv->serialize_lock);
+	prepare_to_wait(&itv->dma_waitq, &wait, TASK_INTERRUPTIBLE);
+	while (!(got_sig = signal_pending(current)) &&
+	       test_bit(IVTV_F_S_DMA_PENDING, &s->s_flags))
+		schedule();
+	finish_wait(&itv->dma_waitq, &wait);
+	mutex_lock(&itv->serialize_lock);
+
+	return got_sig;
+}
+
 static ssize_t ivtv_write(struct file *filp, const char __user *user_buf, size_t count, loff_t *pos)
 {
 	struct ivtv_open_id *id = fh2id(filp->private_data);
@@ -544,7 +573,6 @@ static ssize_t ivtv_write(struct file *filp, const char __user *user_buf, size_t
 	int bytes_written = 0;
 	int mode;
 	int rc;
-	DEFINE_WAIT(wait);
 
 	IVTV_DEBUG_HI_FILE("write %zd bytes to %s\n", count, s->name);
 
@@ -618,13 +646,7 @@ retry:
 			break;
 		if (filp->f_flags & O_NONBLOCK)
 			return -EAGAIN;
-		mutex_unlock(&itv->serialize_lock);
-		prepare_to_wait(&s->waitq, &wait, TASK_INTERRUPTIBLE);
-		/* New buffers might have become free before we were added to the waitqueue */
-		if (!s->q_free.buffers)
-			schedule();
-		finish_wait(&s->waitq, &wait);
-		mutex_lock(&itv->serialize_lock);
+		ivtv_schedule(s);
 		if (signal_pending(current)) {
 			IVTV_DEBUG_INFO("User stopped %s\n", s->name);
 			return -EINTR;
@@ -674,20 +696,10 @@ retry:
 
 	if (test_bit(IVTV_F_S_NEEDS_DATA, &s->s_flags)) {
 		if (s->q_full.length >= itv->dma_data_req_size) {
-			int got_sig;
-
 			if (mode == OUT_YUV)
 				ivtv_yuv_setup_stream_frame(itv);
 
-			mutex_unlock(&itv->serialize_lock);
-			prepare_to_wait(&itv->dma_waitq, &wait, TASK_INTERRUPTIBLE);
-			while (!(got_sig = signal_pending(current)) &&
-					test_bit(IVTV_F_S_DMA_PENDING, &s->s_flags)) {
-				schedule();
-			}
-			finish_wait(&itv->dma_waitq, &wait);
-			mutex_lock(&itv->serialize_lock);
-			if (got_sig) {
+			if (ivtv_schedule_dma(s)) {
 				IVTV_DEBUG_INFO("User interrupted %s\n", s->name);
 				return -EINTR;
 			}
diff --git a/drivers/media/pci/ivtv/ivtv-irq.c b/drivers/media/pci/ivtv/ivtv-irq.c
index b7aaa8b4a784..748c14e87963 100644
--- a/drivers/media/pci/ivtv/ivtv-irq.c
+++ b/drivers/media/pci/ivtv/ivtv-irq.c
@@ -532,7 +532,7 @@ static void ivtv_irq_dma_read(struct ivtv *itv)
 
 	IVTV_DEBUG_HI_IRQ("DEC DMA READ\n");
 
-	del_timer(&itv->dma_timer);
+	timer_delete(&itv->dma_timer);
 
 	if (!test_bit(IVTV_F_I_UDMA, &itv->i_flags) && itv->cur_dma_stream < 0)
 		return;
@@ -597,7 +597,7 @@ static void ivtv_irq_enc_dma_complete(struct ivtv *itv)
 	ivtv_api_get_data(&itv->enc_mbox, IVTV_MBOX_DMA_END, 2, data);
 	IVTV_DEBUG_HI_IRQ("ENC DMA COMPLETE %x %d (%d)\n", data[0], data[1], itv->cur_dma_stream);
 
-	del_timer(&itv->dma_timer);
+	timer_delete(&itv->dma_timer);
 
 	if (itv->cur_dma_stream < 0)
 		return;
@@ -670,7 +670,7 @@ static void ivtv_irq_dma_err(struct ivtv *itv)
 	u32 data[CX2341X_MBOX_MAX_DATA];
 	u32 status;
 
-	del_timer(&itv->dma_timer);
+	timer_delete(&itv->dma_timer);
 
 	ivtv_api_get_data(&itv->enc_mbox, IVTV_MBOX_DMA_END, 2, data);
 	status = read_reg(IVTV_REG_DMASTATUS);
@@ -1064,7 +1064,7 @@ irqreturn_t ivtv_irq_handler(int irq, void *dev_id)
 
 void ivtv_unfinished_dma(struct timer_list *t)
 {
-	struct ivtv *itv = from_timer(itv, t, dma_timer);
+	struct ivtv *itv = timer_container_of(itv, t, dma_timer);
 
 	if (!test_bit(IVTV_F_I_DMA, &itv->i_flags))
 		return;
diff --git a/drivers/media/pci/ivtv/ivtv-streams.c b/drivers/media/pci/ivtv/ivtv-streams.c
index af9e6235c74d..ac085925d3cb 100644
--- a/drivers/media/pci/ivtv/ivtv-streams.c
+++ b/drivers/media/pci/ivtv/ivtv-streams.c
@@ -891,7 +891,7 @@ int ivtv_stop_v4l2_encode_stream(struct ivtv_stream *s, int gop_end)
 
 	/* Set the following Interrupt mask bits for capture */
 	ivtv_set_irq_mask(itv, IVTV_IRQ_MASK_CAPTURE);
-	del_timer(&itv->dma_timer);
+	timer_delete(&itv->dma_timer);
 
 	/* event notification (off) */
 	if (test_and_clear_bit(IVTV_F_I_DIG_RST, &itv->i_flags)) {
@@ -956,7 +956,7 @@ int ivtv_stop_v4l2_decode_stream(struct ivtv_stream *s, int flags, u64 pts)
 	ivtv_vapi(itv, CX2341X_DEC_SET_EVENT_NOTIFICATION, 4, 0, 0, IVTV_IRQ_DEC_AUD_MODE_CHG, -1);
 
 	ivtv_set_irq_mask(itv, IVTV_IRQ_MASK_DECODE);
-	del_timer(&itv->dma_timer);
+	timer_delete(&itv->dma_timer);
 
 	clear_bit(IVTV_F_S_NEEDS_DATA, &s->s_flags);
 	clear_bit(IVTV_F_S_STREAMING, &s->s_flags);
diff --git a/drivers/media/pci/ivtv/ivtv-udma.c b/drivers/media/pci/ivtv/ivtv-udma.c
index 99b9f55ca829..f467a00492f4 100644
--- a/drivers/media/pci/ivtv/ivtv-udma.c
+++ b/drivers/media/pci/ivtv/ivtv-udma.c
@@ -131,6 +131,8 @@ int ivtv_udma_setup(struct ivtv *itv, unsigned long ivtv_dest_addr,
 
 	/* Fill SG List with new values */
 	if (ivtv_udma_fill_sg_list(dma, &user_dma, 0) < 0) {
+		IVTV_DEBUG_WARN("%s: could not allocate bounce buffers for highmem userspace buffers\n",
+				__func__);
 		unpin_user_pages(dma->map, dma->page_count);
 		dma->page_count = 0;
 		return -ENOMEM;
@@ -139,6 +141,12 @@ int ivtv_udma_setup(struct ivtv *itv, unsigned long ivtv_dest_addr,
 	/* Map SG List */
 	dma->SG_length = dma_map_sg(&itv->pdev->dev, dma->SGlist,
 				    dma->page_count, DMA_TO_DEVICE);
+	if (!dma->SG_length) {
+		IVTV_DEBUG_WARN("%s: DMA map error, SG_length is 0\n", __func__);
+		unpin_user_pages(dma->map, dma->page_count);
+		dma->page_count = 0;
+		return -EINVAL;
+	}
 
 	/* Fill SG Array with new values */
 	ivtv_udma_fill_sg_array (dma, ivtv_dest_addr, 0, -1);
diff --git a/drivers/media/pci/ivtv/ivtv-yuv.c b/drivers/media/pci/ivtv/ivtv-yuv.c
index 582146f8d70d..2d9274537725 100644
--- a/drivers/media/pci/ivtv/ivtv-yuv.c
+++ b/drivers/media/pci/ivtv/ivtv-yuv.c
@@ -114,6 +114,12 @@ static int ivtv_yuv_prep_user_dma(struct ivtv *itv, struct ivtv_user_dma *dma,
 	}
 	dma->SG_length = dma_map_sg(&itv->pdev->dev, dma->SGlist,
 				    dma->page_count, DMA_TO_DEVICE);
+	if (!dma->SG_length) {
+		IVTV_DEBUG_WARN("%s: DMA map error, SG_length is 0\n", __func__);
+		unpin_user_pages(dma->map, dma->page_count);
+		dma->page_count = 0;
+		return -EINVAL;
+	}
 
 	/* Fill SG Array with new values */
 	ivtv_udma_fill_sg_array(dma, y_buffer_offset, uv_buffer_offset, y_size);
diff --git a/drivers/media/pci/ivtv/ivtvfb.c b/drivers/media/pci/ivtv/ivtvfb.c
index 410477e3e621..90c584cf97c2 100644
--- a/drivers/media/pci/ivtv/ivtvfb.c
+++ b/drivers/media/pci/ivtv/ivtvfb.c
@@ -104,6 +104,7 @@ MODULE_PARM_DESC(osd_xres,
 		 "\t\t\tdefault 640");
 
 MODULE_AUTHOR("Kevin Thayer, Chris Kennedy, Hans Verkuil, John Harvey, Ian Armstrong");
+MODULE_DESCRIPTION("Conexant cx23415 framebuffer support");
 MODULE_LICENSE("GPL");
 
 /* --------------------------------------------------------------------- */
@@ -281,10 +282,10 @@ static int ivtvfb_prep_dec_dma_to_device(struct ivtv *itv,
 	/* Map User DMA */
 	if (ivtv_udma_setup(itv, ivtv_dest_addr, userbuf, size_in_bytes) <= 0) {
 		mutex_unlock(&itv->udma.lock);
-		IVTVFB_WARN("ivtvfb_prep_dec_dma_to_device, Error with pin_user_pages: %d bytes, %d pages returned\n",
-			       size_in_bytes, itv->udma.page_count);
+		IVTVFB_WARN("%s, Error in ivtv_udma_setup: %d bytes, %d pages returned\n",
+			       __func__, size_in_bytes, itv->udma.page_count);
 
-		/* pin_user_pages must have failed completely */
+		/* pin_user_pages or DMA must have failed completely */
 		return -EIO;
 	}
 
diff --git a/drivers/media/pci/mantis/hopper_cards.c b/drivers/media/pci/mantis/hopper_cards.c
index c0bd5d7e148b..b85aef4e2b24 100644
--- a/drivers/media/pci/mantis/hopper_cards.c
+++ b/drivers/media/pci/mantis/hopper_cards.c
@@ -116,7 +116,7 @@ static irqreturn_t hopper_irq_handler(int irq, void *dev_id)
 	if (stat & MANTIS_INT_RISCI) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[8]);
 		mantis->busy_block = (stat & MANTIS_INT_RISCSTAT) >> 28;
-		tasklet_schedule(&mantis->tasklet);
+		queue_work(system_bh_wq, &mantis->bh_work);
 	}
 	if (stat & MANTIS_INT_I2CDONE) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[9]);
diff --git a/drivers/media/pci/mantis/mantis_cards.c b/drivers/media/pci/mantis/mantis_cards.c
index 906e4500d87d..b44b4cf42f86 100644
--- a/drivers/media/pci/mantis/mantis_cards.c
+++ b/drivers/media/pci/mantis/mantis_cards.c
@@ -125,7 +125,7 @@ static irqreturn_t mantis_irq_handler(int irq, void *dev_id)
 	if (stat & MANTIS_INT_RISCI) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[8]);
 		mantis->busy_block = (stat & MANTIS_INT_RISCSTAT) >> 28;
-		tasklet_schedule(&mantis->tasklet);
+		queue_work(system_bh_wq, &mantis->bh_work);
 	}
 	if (stat & MANTIS_INT_I2CDONE) {
 		dprintk(MANTIS_DEBUG, 0, "<%s>", label[9]);
diff --git a/drivers/media/pci/mantis/mantis_common.h b/drivers/media/pci/mantis/mantis_common.h
index d88ac280226c..6e563ecd94e8 100644
--- a/drivers/media/pci/mantis/mantis_common.h
+++ b/drivers/media/pci/mantis/mantis_common.h
@@ -125,7 +125,7 @@ struct mantis_pci {
 	__le32			*risc_cpu;
 	dma_addr_t		risc_dma;
 
-	struct tasklet_struct	tasklet;
+	struct work_struct	bh_work;
 	spinlock_t		intmask_lock;
 
 	struct i2c_adapter	adapter;
diff --git a/drivers/media/pci/mantis/mantis_core.h b/drivers/media/pci/mantis/mantis_core.h
deleted file mode 100644
index 93c89a10a2c7..000000000000
--- a/drivers/media/pci/mantis/mantis_core.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-or-later */
-/*
-	Mantis PCI bridge driver
-
-	Copyright (C) Manu Abraham (abraham.manu@gmail.com)
-
-*/
-
-#ifndef __MANTIS_CORE_H
-#define __MANTIS_CORE_H
-
-#include "mantis_common.h"
-
-
-#define FE_TYPE_SAT	0
-#define FE_TYPE_CAB	1
-#define FE_TYPE_TER	2
-
-#define FE_TYPE_TS204	0
-#define FE_TYPE_TS188	1
-
-
-struct vendorname {
-	u8  *sub_vendor_name;
-	u32 sub_vendor_id;
-};
-
-struct devicetype {
-	u8  *sub_device_name;
-	u32 sub_device_id;
-	u8  device_type;
-	u32 type_flags;
-};
-
-
-extern int mantis_dma_init(struct mantis_pci *mantis);
-extern int mantis_dma_exit(struct mantis_pci *mantis);
-extern void mantis_dma_start(struct mantis_pci *mantis);
-extern void mantis_dma_stop(struct mantis_pci *mantis);
-extern int mantis_i2c_init(struct mantis_pci *mantis);
-extern int mantis_i2c_exit(struct mantis_pci *mantis);
-
-#endif /* __MANTIS_CORE_H */
diff --git a/drivers/media/pci/mantis/mantis_dma.c b/drivers/media/pci/mantis/mantis_dma.c
index 80c843936493..eeb210a2862f 100644
--- a/drivers/media/pci/mantis/mantis_dma.c
+++ b/drivers/media/pci/mantis/mantis_dma.c
@@ -200,9 +200,9 @@ void mantis_dma_stop(struct mantis_pci *mantis)
 }
 
 
-void mantis_dma_xfer(struct tasklet_struct *t)
+void mantis_dma_xfer(struct work_struct *t)
 {
-	struct mantis_pci *mantis = from_tasklet(mantis, t, tasklet);
+	struct mantis_pci *mantis = from_work(mantis, t, bh_work);
 	struct mantis_hwconfig *config = mantis->hwconfig;
 
 	while (mantis->last_block != mantis->busy_block) {
diff --git a/drivers/media/pci/mantis/mantis_dma.h b/drivers/media/pci/mantis/mantis_dma.h
index 37da982c9c29..5db0d3728f15 100644
--- a/drivers/media/pci/mantis/mantis_dma.h
+++ b/drivers/media/pci/mantis/mantis_dma.h
@@ -13,6 +13,6 @@ extern int mantis_dma_init(struct mantis_pci *mantis);
 extern int mantis_dma_exit(struct mantis_pci *mantis);
 extern void mantis_dma_start(struct mantis_pci *mantis);
 extern void mantis_dma_stop(struct mantis_pci *mantis);
-extern void mantis_dma_xfer(struct tasklet_struct *t);
+extern void mantis_dma_xfer(struct work_struct *t);
 
 #endif /* __MANTIS_DMA_H */
diff --git a/drivers/media/pci/mantis/mantis_dvb.c b/drivers/media/pci/mantis/mantis_dvb.c
index c7ba4a76e608..398e32f44692 100644
--- a/drivers/media/pci/mantis/mantis_dvb.c
+++ b/drivers/media/pci/mantis/mantis_dvb.c
@@ -105,7 +105,7 @@ static int mantis_dvb_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 	if (mantis->feeds == 1)	 {
 		dprintk(MANTIS_DEBUG, 1, "mantis start feed & dma");
 		mantis_dma_start(mantis);
-		tasklet_enable(&mantis->tasklet);
+		enable_and_queue_work(system_bh_wq, &mantis->bh_work);
 	}
 
 	return mantis->feeds;
@@ -125,7 +125,7 @@ static int mantis_dvb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	mantis->feeds--;
 	if (mantis->feeds == 0) {
 		dprintk(MANTIS_DEBUG, 1, "mantis stop feed and dma");
-		tasklet_disable(&mantis->tasklet);
+		disable_work_sync(&mantis->bh_work);
 		mantis_dma_stop(mantis);
 	}
 
@@ -205,8 +205,8 @@ int mantis_dvb_init(struct mantis_pci *mantis)
 	}
 
 	dvb_net_init(&mantis->dvb_adapter, &mantis->dvbnet, &mantis->demux.dmx);
-	tasklet_setup(&mantis->tasklet, mantis_dma_xfer);
-	tasklet_disable(&mantis->tasklet);
+	INIT_WORK(&mantis->bh_work, mantis_dma_xfer);
+	disable_work_sync(&mantis->bh_work);
 	if (mantis->hwconfig) {
 		result = config->frontend_init(mantis, mantis->fe);
 		if (result < 0) {
@@ -235,7 +235,7 @@ int mantis_dvb_init(struct mantis_pci *mantis)
 
 	/* Error conditions ..	*/
 err5:
-	tasklet_kill(&mantis->tasklet);
+	cancel_work_sync(&mantis->bh_work);
 	dvb_net_release(&mantis->dvbnet);
 	if (mantis->fe) {
 		dvb_unregister_frontend(mantis->fe);
@@ -273,7 +273,7 @@ int mantis_dvb_exit(struct mantis_pci *mantis)
 		dvb_frontend_detach(mantis->fe);
 	}
 
-	tasklet_kill(&mantis->tasklet);
+	cancel_work_sync(&mantis->bh_work);
 	dvb_net_release(&mantis->dvbnet);
 
 	mantis->demux.dmx.remove_frontend(&mantis->demux.dmx, &mantis->fe_mem);
diff --git a/drivers/media/pci/mgb4/mgb4_cmt.c b/drivers/media/pci/mgb4/mgb4_cmt.c
index 70dc78ef193c..c22ef51436ed 100644
--- a/drivers/media/pci/mgb4/mgb4_cmt.c
+++ b/drivers/media/pci/mgb4/mgb4_cmt.c
@@ -135,8 +135,8 @@ static const u16 cmt_vals_out[][15] = {
 };
 
 static const u16 cmt_vals_in[][13] = {
-	{0x1082, 0x0000, 0x5104, 0x0000, 0x11C7, 0x0000, 0x1041, 0x02BC, 0x7C01, 0xFFE9, 0x9900, 0x9908, 0x8100},
 	{0x1104, 0x0000, 0x9208, 0x0000, 0x138E, 0x0000, 0x1041, 0x015E, 0x7C01, 0xFFE9, 0x0100, 0x0908, 0x1000},
+	{0x1082, 0x0000, 0x5104, 0x0000, 0x11C7, 0x0000, 0x1041, 0x02BC, 0x7C01, 0xFFE9, 0x9900, 0x9908, 0x8100},
 };
 
 static const u32 cmt_addrs_out[][15] = {
@@ -206,10 +206,11 @@ u32 mgb4_cmt_set_vout_freq(struct mgb4_vout_dev *voutdev, unsigned int freq)
 
 	mgb4_write_reg(video, regs->config, 0x1 | (config & ~0x3));
 
+	mgb4_mask_reg(video, regs->config, 0x100, 0x100);
+
 	for (i = 0; i < ARRAY_SIZE(cmt_addrs_out[0]); i++)
 		mgb4_write_reg(&voutdev->mgbdev->cmt, addr[i], reg_set[i]);
 
-	mgb4_mask_reg(video, regs->config, 0x100, 0x100);
 	mgb4_mask_reg(video, regs->config, 0x100, 0x0);
 
 	mgb4_write_reg(video, regs->config, config & ~0x1);
@@ -227,6 +228,8 @@ void mgb4_cmt_set_vin_freq_range(struct mgb4_vin_dev *vindev,
 	u32 config;
 	size_t i;
 
+	freq_range = array_index_nospec(freq_range, ARRAY_SIZE(cmt_vals_in));
+
 	addr = cmt_addrs_in[vindev->config->id];
 	reg_set = cmt_vals_in[freq_range];
 
@@ -234,10 +237,11 @@ void mgb4_cmt_set_vin_freq_range(struct mgb4_vin_dev *vindev,
 
 	mgb4_write_reg(video, regs->config, 0x1 | (config & ~0x3));
 
+	mgb4_mask_reg(video, regs->config, 0x1000, 0x1000);
+
 	for (i = 0; i < ARRAY_SIZE(cmt_addrs_in[0]); i++)
 		mgb4_write_reg(&vindev->mgbdev->cmt, addr[i], reg_set[i]);
 
-	mgb4_mask_reg(video, regs->config, 0x1000, 0x1000);
 	mgb4_mask_reg(video, regs->config, 0x1000, 0x0);
 
 	mgb4_write_reg(video, regs->config, config & ~0x1);
diff --git a/drivers/media/pci/mgb4/mgb4_core.c b/drivers/media/pci/mgb4/mgb4_core.c
index 9bcf10a77fd3..3ce6b717ca32 100644
--- a/drivers/media/pci/mgb4/mgb4_core.c
+++ b/drivers/media/pci/mgb4/mgb4_core.c
@@ -40,7 +40,9 @@
 #include "mgb4_trigger.h"
 #include "mgb4_core.h"
 
-#define MGB4_USER_IRQS 16
+#define MGB4_USER_IRQS  16
+#define MGB4_MGB4_BAR_ID 0
+#define MGB4_XDMA_BAR_ID 1
 
 #define DIGITEQ_VID 0x1ed8
 #define T100_DID    0x0101
@@ -123,7 +125,7 @@ static const struct hwmon_chip_info temp_chip_info = {
 };
 #endif
 
-static int match_i2c_adap(struct device *dev, void *data)
+static int match_i2c_adap(struct device *dev, const void *data)
 {
 	return i2c_verify_adapter(dev) ? 1 : 0;
 }
@@ -139,7 +141,7 @@ static struct i2c_adapter *get_i2c_adap(struct platform_device *pdev)
 	return dev ? to_i2c_adapter(dev) : NULL;
 }
 
-static int match_spi_adap(struct device *dev, void *data)
+static int match_spi_adap(struct device *dev, const void *data)
 {
 	return to_spi_device(dev) ? 1 : 0;
 }
@@ -302,7 +304,7 @@ static int init_i2c(struct mgb4_dev *mgbdev)
 	/* create dummy clock required by the xiic-i2c adapter */
 	snprintf(clk_name, sizeof(clk_name), "xiic-i2c.%d", id);
 	mgbdev->i2c_clk = clk_hw_register_fixed_rate(NULL, clk_name, NULL,
-						     0, 125000000);
+						     0, MGB4_HW_FREQ);
 	if (IS_ERR(mgbdev->i2c_clk)) {
 		dev_err(dev, "failed to register I2C clock\n");
 		return PTR_ERR(mgbdev->i2c_clk);
@@ -404,8 +406,9 @@ static int get_module_version(struct mgb4_dev *mgbdev)
 		dev_err(dev, "unknown module type\n");
 		return -EINVAL;
 	}
-	fw_version = mgb4_read_reg(&mgbdev->video, 0xC4);
-	if (fw_version >> 24 != mgbdev->module_version >> 4) {
+	fw_version = mgb4_read_reg(&mgbdev->video, 0xC4) >> 24;
+	if ((MGB4_IS_FPDL3(mgbdev) && fw_version != 1) ||
+	    (MGB4_IS_GMSL(mgbdev) && fw_version != 2)) {
 		dev_err(dev, "module/firmware type mismatch\n");
 		return -EINVAL;
 	}
@@ -493,13 +496,13 @@ static int mgb4_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	struct mgb4_dev *mgbdev;
 	struct resource video = {
 		.start	= 0x0,
-		.end	= 0x100,
+		.end	= 0xff,
 		.flags	= IORESOURCE_MEM,
 		.name	= "mgb4-video",
 	};
 	struct resource cmt = {
 		.start	= 0x1000,
-		.end	= 0x1800,
+		.end	= 0x17ff,
 		.flags	= IORESOURCE_MEM,
 		.name	= "mgb4-cmt",
 	};
@@ -582,9 +585,7 @@ static int mgb4_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 							    NULL);
 #endif
 
-#ifdef CONFIG_DEBUG_FS
 	mgbdev->debugfs = debugfs_create_dir(dev_name(&pdev->dev), NULL);
-#endif
 
 	/* Get card serial number. On systems without MTD flash support we may
 	 * get an error thus ignore the return value. An invalid serial number
@@ -599,14 +600,18 @@ static int mgb4_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	rv = get_module_version(mgbdev);
 	if (rv < 0)
 		goto exit;
+	/* Propagate the module type(version) to the FPGA */
+	mgb4_write_reg(&mgbdev->video, 0xD4, mgbdev->module_version);
 
 	/* Video input v4l2 devices */
 	for (i = 0; i < MGB4_VIN_DEVICES; i++)
 		mgbdev->vin[i] = mgb4_vin_create(mgbdev, i);
 
 	/* Video output v4l2 devices */
-	for (i = 0; i < MGB4_VOUT_DEVICES; i++)
-		mgbdev->vout[i] = mgb4_vout_create(mgbdev, i);
+	if (MGB4_HAS_VOUT(mgbdev)) {
+		for (i = 0; i < MGB4_VOUT_DEVICES; i++)
+			mgbdev->vout[i] = mgb4_vout_create(mgbdev, i);
+	}
 
 	/* Triggers */
 	mgbdev->indio_dev = mgb4_trigger_create(mgbdev);
@@ -642,13 +647,12 @@ static void mgb4_remove(struct pci_dev *pdev)
 	struct mgb4_dev *mgbdev = pci_get_drvdata(pdev);
 	int i;
 
-#ifdef CONFIG_DEBUG_FS
-	debugfs_remove_recursive(mgbdev->debugfs);
-#endif
 #if IS_REACHABLE(CONFIG_HWMON)
 	hwmon_device_unregister(mgbdev->hwmon_dev);
 #endif
 
+	debugfs_remove_recursive(mgbdev->debugfs);
+
 	if (mgbdev->indio_dev)
 		mgb4_trigger_free(mgbdev->indio_dev);
 
diff --git a/drivers/media/pci/mgb4/mgb4_core.h b/drivers/media/pci/mgb4/mgb4_core.h
index 2a946e46aec1..cc24068400a2 100644
--- a/drivers/media/pci/mgb4/mgb4_core.h
+++ b/drivers/media/pci/mgb4/mgb4_core.h
@@ -13,16 +13,19 @@
 #include <linux/dmaengine.h>
 #include "mgb4_regs.h"
 
+#define MGB4_HW_FREQ 125000000
+
 #define MGB4_VIN_DEVICES  2
 #define MGB4_VOUT_DEVICES 2
 
-#define MGB4_MGB4_BAR_ID  0
-#define MGB4_XDMA_BAR_ID  1
-
 #define MGB4_IS_GMSL(mgbdev) \
-	((mgbdev)->module_version >> 4 == 2)
+	((((mgbdev)->module_version >> 4) >= 2) && \
+	 (((mgbdev)->module_version >> 4) <= 4))
 #define MGB4_IS_FPDL3(mgbdev) \
-	((mgbdev)->module_version >> 4 == 1)
+	(((mgbdev)->module_version >> 4) == 1)
+#define MGB4_HAS_VOUT(mgbdev) \
+	((((mgbdev)->module_version >> 4) >= 1) && \
+	 (((mgbdev)->module_version >> 4) <= 3))
 
 struct mgb4_dma_channel {
 	struct dma_chan *chan;
@@ -66,9 +69,7 @@ struct mgb4_dev {
 	u8 module_version;
 	u32 serial_number;
 
-#ifdef CONFIG_DEBUG_FS
 	struct dentry *debugfs;
-#endif
 };
 
 #endif
diff --git a/drivers/media/pci/mgb4/mgb4_io.h b/drivers/media/pci/mgb4/mgb4_io.h
index 8698db1be4a9..dd8696d7df31 100644
--- a/drivers/media/pci/mgb4/mgb4_io.h
+++ b/drivers/media/pci/mgb4/mgb4_io.h
@@ -7,11 +7,9 @@
 #ifndef __MGB4_IO_H__
 #define __MGB4_IO_H__
 
+#include <linux/math64.h>
 #include <media/v4l2-dev.h>
-
-#define MGB4_DEFAULT_WIDTH     1280
-#define MGB4_DEFAULT_HEIGHT    640
-#define MGB4_DEFAULT_PERIOD    (125000000 / 60)
+#include "mgb4_core.h"
 
 /* Register access error indication */
 #define MGB4_ERR_NO_REG        0xFFFFFFFE
@@ -20,6 +18,9 @@
 #define MGB4_ERR_QUEUE_EMPTY   0xFFFFFFFC
 #define MGB4_ERR_QUEUE_FULL    0xFFFFFFFB
 
+#define MGB4_PERIOD(numerator, denominator) \
+	((u32)div_u64((MGB4_HW_FREQ * (u64)(numerator)), (denominator)))
+
 struct mgb4_frame_buffer {
 	struct vb2_v4l2_buffer vb;
 	struct list_head list;
@@ -30,4 +31,24 @@ static inline struct mgb4_frame_buffer *to_frame_buffer(struct vb2_v4l2_buffer *
 	return container_of(vbuf, struct mgb4_frame_buffer, vb);
 }
 
+static inline bool has_yuv_and_timeperframe(struct mgb4_regs *video)
+{
+	u32 status = mgb4_read_reg(video, 0xD0);
+
+	return (status & (1U << 8));
+}
+
+#define has_yuv(video) has_yuv_and_timeperframe(video)
+#define has_timeperframe(video) has_yuv_and_timeperframe(video)
+
+static inline u32 pixel_size(struct v4l2_dv_timings *timings)
+{
+	struct v4l2_bt_timings *bt = &timings->bt;
+
+	u32 height = bt->height + bt->vfrontporch + bt->vsync + bt->vbackporch;
+	u32 width = bt->width + bt->hfrontporch + bt->hsync + bt->hbackporch;
+
+	return width * height;
+}
+
 #endif
diff --git a/drivers/media/pci/mgb4/mgb4_regs.c b/drivers/media/pci/mgb4/mgb4_regs.c
index 53d4e4503a74..b45537dbfafa 100644
--- a/drivers/media/pci/mgb4/mgb4_regs.c
+++ b/drivers/media/pci/mgb4/mgb4_regs.c
@@ -5,12 +5,13 @@
  */
 
 #include <linux/ioport.h>
+#include <linux/errno.h>
 #include "mgb4_regs.h"
 
 int mgb4_regs_map(struct resource *res, struct mgb4_regs *regs)
 {
 	regs->mapbase = res->start;
-	regs->mapsize = res->end - res->start;
+	regs->mapsize = resource_size(res);
 
 	if (!request_mem_region(regs->mapbase, regs->mapsize, res->name))
 		return -EINVAL;
diff --git a/drivers/media/pci/mgb4/mgb4_sysfs_in.c b/drivers/media/pci/mgb4/mgb4_sysfs_in.c
index 0ba66a2cf145..9626fa59e3d3 100644
--- a/drivers/media/pci/mgb4/mgb4_sysfs_in.c
+++ b/drivers/media/pci/mgb4/mgb4_sysfs_in.c
@@ -333,7 +333,7 @@ static ssize_t hsync_width_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal);
+				vindev->config->regs.hsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x00FF0000) >> 16);
 }
@@ -344,7 +344,7 @@ static ssize_t vsync_width_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal2);
+				vindev->config->regs.vsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x00FF0000) >> 16);
 }
@@ -355,7 +355,7 @@ static ssize_t hback_porch_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal);
+				vindev->config->regs.hsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x0000FF00) >> 8);
 }
@@ -366,7 +366,7 @@ static ssize_t hfront_porch_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal);
+				vindev->config->regs.hsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x000000FF));
 }
@@ -377,7 +377,7 @@ static ssize_t vback_porch_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal2);
+				vindev->config->regs.vsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x0000FF00) >> 8);
 }
@@ -388,7 +388,7 @@ static ssize_t vfront_porch_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vin_dev *vindev = video_get_drvdata(vdev);
 	u32 sig = mgb4_read_reg(&vindev->mgbdev->video,
-				vindev->config->regs.signal2);
+				vindev->config->regs.vsync);
 
 	return sprintf(buf, "%u\n", (sig & 0x000000FF));
 }
diff --git a/drivers/media/pci/mgb4/mgb4_sysfs_out.c b/drivers/media/pci/mgb4/mgb4_sysfs_out.c
index 9f6e81c57726..573aa61c69d4 100644
--- a/drivers/media/pci/mgb4/mgb4_sysfs_out.c
+++ b/drivers/media/pci/mgb4/mgb4_sysfs_out.c
@@ -229,9 +229,9 @@ static ssize_t frame_rate_show(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vout_dev *voutdev = video_get_drvdata(vdev);
 	u32 period = mgb4_read_reg(&voutdev->mgbdev->video,
-				   voutdev->config->regs.frame_period);
+				   voutdev->config->regs.frame_limit);
 
-	return sprintf(buf, "%u\n", 125000000 / period);
+	return sprintf(buf, "%u\n", period ? MGB4_HW_FREQ / period : 0);
 }
 
 /*
@@ -245,14 +245,15 @@ static ssize_t frame_rate_store(struct device *dev,
 	struct video_device *vdev = to_video_device(dev);
 	struct mgb4_vout_dev *voutdev = video_get_drvdata(vdev);
 	unsigned long val;
-	int ret;
+	int limit, ret;
 
 	ret = kstrtoul(buf, 10, &val);
 	if (ret)
 		return ret;
 
+	limit = val ? MGB4_HW_FREQ / val : 0;
 	mgb4_write_reg(&voutdev->mgbdev->video,
-		       voutdev->config->regs.frame_period, 125000000 / val);
+		       voutdev->config->regs.frame_limit, limit);
 
 	return count;
 }
diff --git a/drivers/media/pci/mgb4/mgb4_vin.c b/drivers/media/pci/mgb4/mgb4_vin.c
index 2cd78c539889..989e93f67f75 100644
--- a/drivers/media/pci/mgb4/mgb4_vin.c
+++ b/drivers/media/pci/mgb4/mgb4_vin.c
@@ -18,6 +18,7 @@
 #include <linux/workqueue.h>
 #include <linux/align.h>
 #include <linux/dma/amd_xdma.h>
+#include <linux/v4l2-dv-timings.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-v4l2.h>
 #include <media/videobuf2-dma-sg.h>
@@ -34,8 +35,8 @@ ATTRIBUTE_GROUPS(mgb4_fpdl3_in);
 ATTRIBUTE_GROUPS(mgb4_gmsl_in);
 
 static const struct mgb4_vin_config vin_cfg[] = {
-	{0, 0, 0, 6, {0x10, 0x00, 0x04, 0x08, 0x1C, 0x14, 0x18, 0x20, 0x24, 0x28}},
-	{1, 1, 1, 7, {0x40, 0x30, 0x34, 0x38, 0x4C, 0x44, 0x48, 0x50, 0x54, 0x58}}
+	{0, 0, 0, 6, {0x10, 0x00, 0x04, 0x08, 0x1C, 0x14, 0x18, 0x20, 0x24, 0x28, 0xE8}},
+	{1, 1, 1, 7, {0x40, 0x30, 0x34, 0x38, 0x4C, 0x44, 0x48, 0x50, 0x54, 0x58, 0xEC}}
 };
 
 static const struct i2c_board_info fpdl3_deser_info[] = {
@@ -76,6 +77,9 @@ static const struct v4l2_dv_timings_cap video_timings_cap = {
 	},
 };
 
+/* Dummy timings when no signal present */
+static const struct v4l2_dv_timings cea1080p60 = V4L2_DV_BT_CEA_1920X1080P60;
+
 /*
  * Returns the video output connected with the given video input if the input
  * is in loopback mode.
@@ -139,8 +143,8 @@ static int get_timings(struct mgb4_vin_dev *vindev,
 
 	u32 status = mgb4_read_reg(video, regs->status);
 	u32 pclk = mgb4_read_reg(video, regs->pclk);
-	u32 signal = mgb4_read_reg(video, regs->signal);
-	u32 signal2 = mgb4_read_reg(video, regs->signal2);
+	u32 hsync = mgb4_read_reg(video, regs->hsync);
+	u32 vsync = mgb4_read_reg(video, regs->vsync);
 	u32 resolution = mgb4_read_reg(video, regs->resolution);
 
 	if (!(status & (1U << 2)))
@@ -157,12 +161,12 @@ static int get_timings(struct mgb4_vin_dev *vindev,
 	if (status & (1U << 13))
 		timings->bt.polarities |= V4L2_DV_VSYNC_POS_POL;
 	timings->bt.pixelclock = pclk * 1000;
-	timings->bt.hsync = (signal & 0x00FF0000) >> 16;
-	timings->bt.vsync = (signal2 & 0x00FF0000) >> 16;
-	timings->bt.hbackporch = (signal & 0x0000FF00) >> 8;
-	timings->bt.hfrontporch = signal & 0x000000FF;
-	timings->bt.vbackporch = (signal2 & 0x0000FF00) >> 8;
-	timings->bt.vfrontporch = signal2 & 0x000000FF;
+	timings->bt.hsync = (hsync & 0x00FF0000) >> 16;
+	timings->bt.vsync = (vsync & 0x00FF0000) >> 16;
+	timings->bt.hbackporch = (hsync & 0x0000FF00) >> 8;
+	timings->bt.hfrontporch = hsync & 0x000000FF;
+	timings->bt.vbackporch = (vsync & 0x0000FF00) >> 8;
+	timings->bt.vfrontporch = vsync & 0x000000FF;
 
 	return 0;
 }
@@ -186,8 +190,11 @@ static int queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
 		       struct device *alloc_devs[])
 {
 	struct mgb4_vin_dev *vindev = vb2_get_drv_priv(q);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
+	u32 config = mgb4_read_reg(video, vindev->config->regs.config);
+	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
 	unsigned int size = (vindev->timings.bt.width + vindev->padding)
-	 * vindev->timings.bt.height * 4;
+			    * vindev->timings.bt.height * pixelsize;
 
 	/*
 	 * If I/O reconfiguration is in process, do not allow to start
@@ -220,9 +227,12 @@ static int buffer_init(struct vb2_buffer *vb)
 static int buffer_prepare(struct vb2_buffer *vb)
 {
 	struct mgb4_vin_dev *vindev = vb2_get_drv_priv(vb->vb2_queue);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
 	struct device *dev = &vindev->mgbdev->pdev->dev;
+	u32 config = mgb4_read_reg(video, vindev->config->regs.config);
+	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
 	unsigned int size = (vindev->timings.bt.width + vindev->padding)
-	 * vindev->timings.bt.height * 4;
+			    * vindev->timings.bt.height * pixelsize;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(dev, "buffer too small (%lu < %u)\n",
@@ -250,6 +260,7 @@ static void buffer_queue(struct vb2_buffer *vb)
 static void stop_streaming(struct vb2_queue *vq)
 {
 	struct mgb4_vin_dev *vindev = vb2_get_drv_priv(vq);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
 	const struct mgb4_vin_config *config = vindev->config;
 	int irq = xdma_get_user_irq(vindev->mgbdev->xdev, config->vin_irq);
 
@@ -263,6 +274,9 @@ static void stop_streaming(struct vb2_queue *vq)
 		mgb4_mask_reg(&vindev->mgbdev->video, config->regs.config, 0x2,
 			      0x0);
 
+	mgb4_write_reg(video, vindev->config->regs.padding, 0);
+	set_loopback_padding(vindev, 0);
+
 	cancel_work_sync(&vindev->dma_work);
 	return_all_buffers(vindev, VB2_BUF_STATE_ERROR);
 }
@@ -270,6 +284,7 @@ static void stop_streaming(struct vb2_queue *vq)
 static int start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct mgb4_vin_dev *vindev = vb2_get_drv_priv(vq);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
 	const struct mgb4_vin_config *config = vindev->config;
 	int irq = xdma_get_user_irq(vindev->mgbdev->xdev, config->vin_irq);
 
@@ -282,6 +297,9 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		mgb4_mask_reg(&vindev->mgbdev->video, config->regs.config, 0x2,
 			      0x2);
 
+	mgb4_write_reg(video, vindev->config->regs.padding, vindev->padding);
+	set_loopback_padding(vindev, vindev->padding);
+
 	xdma_enable_user_irq(vindev->mgbdev->xdev, irq);
 
 	return 0;
@@ -294,8 +312,6 @@ static const struct vb2_ops queue_ops = {
 	.buf_queue = buffer_queue,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish
 };
 
 static int fh_open(struct file *file)
@@ -312,35 +328,18 @@ static int fh_open(struct file *file)
 	if (!v4l2_fh_is_singular_file(file))
 		goto out;
 
-	get_timings(vindev, &vindev->timings);
-	set_loopback_padding(vindev, vindev->padding);
+	if (get_timings(vindev, &vindev->timings) < 0)
+		vindev->timings = cea1080p60;
 
 out:
 	mutex_unlock(&vindev->lock);
 	return rv;
 }
 
-static int fh_release(struct file *file)
-{
-	struct mgb4_vin_dev *vindev = video_drvdata(file);
-	int rv;
-
-	mutex_lock(&vindev->lock);
-
-	if (v4l2_fh_is_singular_file(file))
-		set_loopback_padding(vindev, 0);
-
-	rv = _vb2_fop_release(file, NULL);
-
-	mutex_unlock(&vindev->lock);
-
-	return rv;
-}
-
 static const struct v4l2_file_operations video_fops = {
 	.owner = THIS_MODULE,
 	.open = fh_open,
-	.release = fh_release,
+	.release = vb2_fop_release,
 	.unlocked_ioctl = video_ioctl2,
 	.read = vb2_fop_read,
 	.mmap = vb2_fop_mmap,
@@ -359,33 +358,42 @@ static int vidioc_querycap(struct file *file, void *priv,
 static int vidioc_enum_fmt(struct file *file, void *priv,
 			   struct v4l2_fmtdesc *f)
 {
-	if (f->index != 0)
-		return -EINVAL;
-
-	f->pixelformat = V4L2_PIX_FMT_ABGR32;
+	struct mgb4_vin_dev *vindev = video_drvdata(file);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
 
-	return 0;
+	if (f->index == 0) {
+		f->pixelformat = V4L2_PIX_FMT_ABGR32;
+		return 0;
+	} else if (f->index == 1 && has_yuv(video)) {
+		f->pixelformat = V4L2_PIX_FMT_YUYV;
+		return 0;
+	} else {
+		return -EINVAL;
+	}
 }
 
 static int vidioc_enum_frameintervals(struct file *file, void *priv,
 				      struct v4l2_frmivalenum *ival)
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
 
 	if (ival->index != 0)
 		return -EINVAL;
-	if (ival->pixel_format != V4L2_PIX_FMT_ABGR32)
+	if (!(ival->pixel_format == V4L2_PIX_FMT_ABGR32 ||
+	      ((has_yuv(video) && ival->pixel_format == V4L2_PIX_FMT_YUYV))))
 		return -EINVAL;
 	if (ival->width != vindev->timings.bt.width ||
 	    ival->height != vindev->timings.bt.height)
 		return -EINVAL;
 
-	ival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
-	ival->stepwise.min.denominator = 60;
-	ival->stepwise.min.numerator = 1;
-	ival->stepwise.max.denominator = 1;
-	ival->stepwise.max.numerator = 1;
-	ival->stepwise.step = ival->stepwise.max;
+	ival->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+	ival->stepwise.max.denominator = MGB4_HW_FREQ;
+	ival->stepwise.max.numerator = 0xFFFFFFFF;
+	ival->stepwise.min.denominator = vindev->timings.bt.pixelclock;
+	ival->stepwise.min.numerator = pixel_size(&vindev->timings);
+	ival->stepwise.step.denominator = MGB4_HW_FREQ;
+	ival->stepwise.step.numerator = 1;
 
 	return 0;
 }
@@ -393,13 +401,29 @@ static int vidioc_enum_frameintervals(struct file *file, void *priv,
 static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
+	u32 config = mgb4_read_reg(video, vindev->config->regs.config);
 
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
 	f->fmt.pix.width = vindev->timings.bt.width;
 	f->fmt.pix.height = vindev->timings.bt.height;
 	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width + vindev->padding) * 4;
+
+	if (config & (1U << 16)) {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+		if (config & (1U << 20)) {
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+		} else {
+			if (config & (1U << 19))
+				f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+			else
+				f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+		}
+		f->fmt.pix.bytesperline = (f->fmt.pix.width + vindev->padding) * 2;
+	} else {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
+		f->fmt.pix.bytesperline = (f->fmt.pix.width + vindev->padding) * 4;
+	}
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
 
 	return 0;
@@ -408,14 +432,30 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
+	u32 pixelsize;
 
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
 	f->fmt.pix.width = vindev->timings.bt.width;
 	f->fmt.pix.height = vindev->timings.bt.height;
 	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
-	f->fmt.pix.bytesperline = max(f->fmt.pix.width * 4,
-				      ALIGN_DOWN(f->fmt.pix.bytesperline, 4));
+
+	if (has_yuv(video) && f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+		pixelsize = 2;
+		if (!(f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709 ||
+		      f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M))
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+	} else {
+		pixelsize = 4;
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
+	}
+
+	if (f->fmt.pix.bytesperline > f->fmt.pix.width * pixelsize &&
+	    f->fmt.pix.bytesperline < f->fmt.pix.width * pixelsize * 2)
+		f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.bytesperline,
+						pixelsize);
+	else
+		f->fmt.pix.bytesperline = f->fmt.pix.width * pixelsize;
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
 
 	return 0;
@@ -425,15 +465,36 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
 	struct mgb4_regs *video = &vindev->mgbdev->video;
+	u32 config, pixelsize;
 
 	if (vb2_is_busy(&vindev->queue))
 		return -EBUSY;
 
 	vidioc_try_fmt(file, priv, f);
 
-	vindev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width * 4)) / 4;
-	mgb4_write_reg(video, vindev->config->regs.padding, vindev->padding);
-	set_loopback_padding(vindev, vindev->padding);
+	config = mgb4_read_reg(video, vindev->config->regs.config);
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+		pixelsize = 2;
+		config |= 1U << 16;
+
+		if (f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709) {
+			config |= 1U << 20;
+			config |= 1U << 19;
+		} else if (f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M) {
+			config &= ~(1U << 20);
+			config |= 1U << 19;
+		} else {
+			config &= ~(1U << 20);
+			config &= ~(1U << 19);
+		}
+	} else {
+		pixelsize = 4;
+		config &= ~(1U << 16);
+	}
+	mgb4_write_reg(video, vindev->config->regs.config, config);
+
+	vindev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width
+			   * pixelsize)) / pixelsize;
 
 	return 0;
 }
@@ -467,7 +528,8 @@ static int vidioc_enum_framesizes(struct file *file, void *fh,
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
 
-	if (fsize->index != 0 || fsize->pixel_format != V4L2_PIX_FMT_ABGR32)
+	if (fsize->index != 0 || !(fsize->pixel_format == V4L2_PIX_FMT_ABGR32 ||
+				   fsize->pixel_format == V4L2_PIX_FMT_YUYV))
 		return -EINVAL;
 
 	fsize->discrete.width = vindev->timings.bt.width;
@@ -488,27 +550,56 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 	return 0;
 }
 
-static int vidioc_parm(struct file *file, void *priv,
-		       struct v4l2_streamparm *parm)
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
 {
 	struct mgb4_vin_dev *vindev = video_drvdata(file);
 	struct mgb4_regs *video = &vindev->mgbdev->video;
-	const struct mgb4_vin_regs *regs = &vindev->config->regs;
-	struct v4l2_fract timeperframe = {
-		.numerator = mgb4_read_reg(video, regs->frame_period),
-		.denominator = 125000000,
-	};
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
+	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
+	u32 timer;
 
 	parm->parm.capture.readbuffers = 2;
-	parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
-	parm->parm.capture.timeperframe = timeperframe;
+
+	if (has_timeperframe(video)) {
+		timer = mgb4_read_reg(video, vindev->config->regs.timer);
+		if (timer < 0xFFFF) {
+			tpf->numerator = pixel_size(&vindev->timings);
+			tpf->denominator = vindev->timings.bt.pixelclock;
+		} else {
+			tpf->numerator = timer;
+			tpf->denominator = MGB4_HW_FREQ;
+		}
+
+		parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	}
 
 	return 0;
 }
 
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct mgb4_vin_dev *vindev = video_drvdata(file);
+	struct mgb4_regs *video = &vindev->mgbdev->video;
+	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
+	u32 period, timer;
+
+	if (has_timeperframe(video)) {
+		timer = tpf->denominator ?
+			MGB4_PERIOD(tpf->numerator, tpf->denominator) : 0;
+		if (timer) {
+			period = MGB4_PERIOD(pixel_size(&vindev->timings),
+					     vindev->timings.bt.pixelclock);
+			if (timer < period)
+				timer = 0;
+		}
+
+		mgb4_write_reg(video, vindev->config->regs.timer, timer);
+	}
+
+	return vidioc_g_parm(file, priv, parm);
+}
+
 static int vidioc_s_dv_timings(struct file *file, void *fh,
 			       struct v4l2_dv_timings *timings)
 {
@@ -550,7 +641,14 @@ static int vidioc_query_dv_timings(struct file *file, void *fh,
 static int vidioc_enum_dv_timings(struct file *file, void *fh,
 				  struct v4l2_enum_dv_timings *timings)
 {
-	return v4l2_enum_dv_timings_cap(timings, &video_timings_cap, NULL, NULL);
+	struct mgb4_vin_dev *vindev = video_drvdata(file);
+
+	if (timings->index != 0)
+		return -EINVAL;
+	if (get_timings(vindev, &timings->timings) < 0)
+		return -ENODATA;
+
+	return 0;
 }
 
 static int vidioc_dv_timings_cap(struct file *file, void *fh,
@@ -592,8 +690,8 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_expbuf = vb2_ioctl_expbuf,
 	.vidioc_streamon = vb2_ioctl_streamon,
 	.vidioc_streamoff = vb2_ioctl_streamoff,
-	.vidioc_g_parm = vidioc_parm,
-	.vidioc_s_parm = vidioc_parm,
+	.vidioc_g_parm = vidioc_g_parm,
+	.vidioc_s_parm = vidioc_s_parm,
 	.vidioc_dv_timings_cap = vidioc_dv_timings_cap,
 	.vidioc_enum_dv_timings = vidioc_enum_dv_timings,
 	.vidioc_g_dv_timings = vidioc_g_dv_timings,
@@ -658,14 +756,14 @@ static void signal_change(struct work_struct *work)
 	u32 width = resolution >> 16;
 	u32 height = resolution & 0xFFFF;
 
-	if (timings->width != width || timings->height != height) {
-		static const struct v4l2_event ev = {
-			.type = V4L2_EVENT_SOURCE_CHANGE,
-			.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
-		};
+	static const struct v4l2_event ev = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
 
-		v4l2_event_queue(&vindev->vdev, &ev);
+	v4l2_event_queue(&vindev->vdev, &ev);
 
+	if (timings->width != width || timings->height != height) {
 		if (vb2_is_streaming(&vindev->queue))
 			vb2_queue_error(&vindev->queue);
 	}
@@ -748,14 +846,16 @@ static void fpga_init(struct mgb4_vin_dev *vindev)
 	mgb4_write_reg(video, regs->config, 1U << 9);
 }
 
-#ifdef CONFIG_DEBUG_FS
-static void debugfs_init(struct mgb4_vin_dev *vindev)
+static void create_debugfs(struct mgb4_vin_dev *vindev)
 {
+#ifdef CONFIG_DEBUG_FS
 	struct mgb4_regs *video = &vindev->mgbdev->video;
+	struct dentry *entry;
 
-	vindev->debugfs = debugfs_create_dir(vindev->vdev.name,
-					     vindev->mgbdev->debugfs);
-	if (!vindev->debugfs)
+	if (IS_ERR_OR_NULL(vindev->mgbdev->debugfs))
+		return;
+	entry = debugfs_create_dir(vindev->vdev.name, vindev->mgbdev->debugfs);
+	if (IS_ERR(entry))
 		return;
 
 	vindev->regs[0].name = "CONFIG";
@@ -771,20 +871,25 @@ static void debugfs_init(struct mgb4_vin_dev *vindev)
 	vindev->regs[5].name = "PCLK_FREQUENCY";
 	vindev->regs[5].offset = vindev->config->regs.pclk;
 	vindev->regs[6].name = "VIDEO_PARAMS_1";
-	vindev->regs[6].offset = vindev->config->regs.signal;
+	vindev->regs[6].offset = vindev->config->regs.hsync;
 	vindev->regs[7].name = "VIDEO_PARAMS_2";
-	vindev->regs[7].offset = vindev->config->regs.signal2;
+	vindev->regs[7].offset = vindev->config->regs.vsync;
 	vindev->regs[8].name = "PADDING_PIXELS";
 	vindev->regs[8].offset = vindev->config->regs.padding;
+	if (has_timeperframe(video)) {
+		vindev->regs[9].name = "TIMER";
+		vindev->regs[9].offset = vindev->config->regs.timer;
+		vindev->regset.nregs = 10;
+	} else {
+		vindev->regset.nregs = 9;
+	}
 
 	vindev->regset.base = video->membase;
 	vindev->regset.regs = vindev->regs;
-	vindev->regset.nregs = ARRAY_SIZE(vindev->regs);
 
-	debugfs_create_regset32("registers", 0444, vindev->debugfs,
-				&vindev->regset);
-}
+	debugfs_create_regset32("registers", 0444, entry, &vindev->regset);
 #endif
+}
 
 struct mgb4_vin_dev *mgb4_vin_create(struct mgb4_dev *mgbdev, int id)
 {
@@ -890,9 +995,7 @@ struct mgb4_vin_dev *mgb4_vin_create(struct mgb4_dev *mgbdev, int id)
 		goto err_video_dev;
 	}
 
-#ifdef CONFIG_DEBUG_FS
-	debugfs_init(vindev);
-#endif
+	create_debugfs(vindev);
 
 	return vindev;
 
@@ -923,10 +1026,6 @@ void mgb4_vin_free(struct mgb4_vin_dev *vindev)
 	free_irq(vin_irq, vindev);
 	free_irq(err_irq, vindev);
 
-#ifdef CONFIG_DEBUG_FS
-	debugfs_remove_recursive(vindev->debugfs);
-#endif
-
 	groups = MGB4_IS_GMSL(vindev->mgbdev)
 	  ? mgb4_gmsl_in_groups : mgb4_fpdl3_in_groups;
 	device_remove_groups(&vindev->vdev.dev, groups);
diff --git a/drivers/media/pci/mgb4/mgb4_vin.h b/drivers/media/pci/mgb4/mgb4_vin.h
index 0249b400ad4d..2a2c829914ce 100644
--- a/drivers/media/pci/mgb4/mgb4_vin.h
+++ b/drivers/media/pci/mgb4/mgb4_vin.h
@@ -22,9 +22,10 @@ struct mgb4_vin_regs {
 	u32 frame_period;
 	u32 sync;
 	u32 pclk;
-	u32 signal;
-	u32 signal2;
+	u32 hsync;
+	u32 vsync;
 	u32 padding;
+	u32 timer;
 };
 
 struct mgb4_vin_config {
@@ -57,9 +58,8 @@ struct mgb4_vin_dev {
 	const struct mgb4_vin_config *config;
 
 #ifdef CONFIG_DEBUG_FS
-	struct dentry *debugfs;
 	struct debugfs_regset32 regset;
-	struct debugfs_reg32 regs[9];
+	struct debugfs_reg32 regs[sizeof(struct mgb4_vin_regs) / 4];
 #endif
 };
 
diff --git a/drivers/media/pci/mgb4/mgb4_vout.c b/drivers/media/pci/mgb4/mgb4_vout.c
index 241353ee77a5..14c5725bd4d8 100644
--- a/drivers/media/pci/mgb4/mgb4_vout.c
+++ b/drivers/media/pci/mgb4/mgb4_vout.c
@@ -16,6 +16,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-v4l2.h>
 #include <media/videobuf2-dma-sg.h>
+#include <media/v4l2-dv-timings.h>
 #include "mgb4_core.h"
 #include "mgb4_dma.h"
 #include "mgb4_sysfs.h"
@@ -27,8 +28,8 @@ ATTRIBUTE_GROUPS(mgb4_fpdl3_out);
 ATTRIBUTE_GROUPS(mgb4_gmsl_out);
 
 static const struct mgb4_vout_config vout_cfg[] = {
-	{0, 0, 8, {0x78, 0x60, 0x64, 0x68, 0x74, 0x6C, 0x70, 0x7c}},
-	{1, 1, 9, {0x98, 0x80, 0x84, 0x88, 0x94, 0x8c, 0x90, 0x9c}}
+	{0, 0, 8, {0x78, 0x60, 0x64, 0x68, 0x74, 0x6C, 0x70, 0x7C, 0xE0}},
+	{1, 1, 9, {0x98, 0x80, 0x84, 0x88, 0x94, 0x8C, 0x90, 0x9C, 0xE4}}
 };
 
 static const struct i2c_board_info fpdl3_ser_info[] = {
@@ -40,6 +41,49 @@ static const struct mgb4_i2c_kv fpdl3_i2c[] = {
 	{0x05, 0xFF, 0x04}, {0x06, 0xFF, 0x01}, {0xC2, 0xFF, 0x80}
 };
 
+static const struct v4l2_dv_timings_cap video_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	.bt = {
+		.min_width = 320,
+		.max_width = 4096,
+		.min_height = 240,
+		.max_height = 2160,
+		.min_pixelclock = 1843200, /* 320 x 240 x 24Hz */
+		.max_pixelclock = 530841600, /* 4096 x 2160 x 60Hz */
+		.standards = V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+			V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF,
+		.capabilities = V4L2_DV_BT_CAP_PROGRESSIVE |
+			V4L2_DV_BT_CAP_CUSTOM,
+	},
+};
+
+static void get_timings(struct mgb4_vout_dev *voutdev,
+			struct v4l2_dv_timings *timings)
+{
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	const struct mgb4_vout_regs *regs = &voutdev->config->regs;
+
+	u32 hsync = mgb4_read_reg(video, regs->hsync);
+	u32 vsync = mgb4_read_reg(video, regs->vsync);
+	u32 resolution = mgb4_read_reg(video, regs->resolution);
+
+	memset(timings, 0, sizeof(*timings));
+	timings->type = V4L2_DV_BT_656_1120;
+	timings->bt.width = resolution >> 16;
+	timings->bt.height = resolution & 0xFFFF;
+	if (hsync & (1U << 31))
+		timings->bt.polarities |= V4L2_DV_HSYNC_POS_POL;
+	if (vsync & (1U << 31))
+		timings->bt.polarities |= V4L2_DV_VSYNC_POS_POL;
+	timings->bt.pixelclock = voutdev->freq * 1000;
+	timings->bt.hsync = (hsync & 0x00FF0000) >> 16;
+	timings->bt.vsync = (vsync & 0x00FF0000) >> 16;
+	timings->bt.hbackporch = (hsync & 0x0000FF00) >> 8;
+	timings->bt.hfrontporch = hsync & 0x000000FF;
+	timings->bt.vbackporch = (vsync & 0x0000FF00) >> 8;
+	timings->bt.vfrontporch = vsync & 0x000000FF;
+}
+
 static void return_all_buffers(struct mgb4_vout_dev *voutdev,
 			       enum vb2_buffer_state state)
 {
@@ -59,7 +103,11 @@ static int queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
 		       struct device *alloc_devs[])
 {
 	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(q);
-	unsigned int size;
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
+	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
+	unsigned int size = (voutdev->width + voutdev->padding) * voutdev->height
+			    * pixelsize;
 
 	/*
 	 * If I/O reconfiguration is in process, do not allow to start
@@ -69,8 +117,6 @@ static int queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
 	if (test_bit(0, &voutdev->mgbdev->io_reconfig))
 		return -EBUSY;
 
-	size = (voutdev->width + voutdev->padding) * voutdev->height * 4;
-
 	if (*nplanes)
 		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
@@ -93,9 +139,11 @@ static int buffer_prepare(struct vb2_buffer *vb)
 {
 	struct mgb4_vout_dev *voutdev = vb2_get_drv_priv(vb->vb2_queue);
 	struct device *dev = &voutdev->mgbdev->pdev->dev;
-	unsigned int size;
-
-	size = (voutdev->width + voutdev->padding) * voutdev->height * 4;
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
+	u32 pixelsize = (config & (1U << 16)) ? 2 : 4;
+	unsigned int size = (voutdev->width + voutdev->padding) * voutdev->height
+			    * pixelsize;
 
 	if (vb2_plane_size(vb, 0) < size) {
 		dev_err(dev, "buffer too small (%lu < %u)\n",
@@ -128,7 +176,10 @@ static void stop_streaming(struct vb2_queue *vq)
 
 	xdma_disable_user_irq(mgbdev->xdev, irq);
 	cancel_work_sync(&voutdev->dma_work);
+
 	mgb4_mask_reg(&mgbdev->video, voutdev->config->regs.config, 0x2, 0x0);
+	mgb4_write_reg(&mgbdev->video, voutdev->config->regs.padding, 0);
+
 	return_all_buffers(voutdev, VB2_BUF_STATE_ERROR);
 }
 
@@ -144,6 +195,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	int rv;
 	u32 addr;
 
+	mgb4_write_reg(video, config->regs.padding, voutdev->padding);
 	mgb4_mask_reg(video, config->regs.config, 0x2, 0x2);
 
 	addr = mgb4_read_reg(video, config->regs.address);
@@ -178,8 +230,6 @@ static const struct vb2_ops queue_ops = {
 	.buf_queue = buffer_queue,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish
 };
 
 static int vidioc_querycap(struct file *file, void *priv,
@@ -194,24 +244,47 @@ static int vidioc_querycap(struct file *file, void *priv,
 static int vidioc_enum_fmt(struct file *file, void *priv,
 			   struct v4l2_fmtdesc *f)
 {
-	if (f->index != 0)
-		return -EINVAL;
-
-	f->pixelformat = V4L2_PIX_FMT_ABGR32;
+	struct mgb4_vin_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
 
-	return 0;
+	if (f->index == 0) {
+		f->pixelformat = V4L2_PIX_FMT_ABGR32;
+		return 0;
+	} else if (f->index == 1 && has_yuv(video)) {
+		f->pixelformat = V4L2_PIX_FMT_YUYV;
+		return 0;
+	} else {
+		return -EINVAL;
+	}
 }
 
 static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	u32 config = mgb4_read_reg(video, voutdev->config->regs.config);
 
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
 	f->fmt.pix.width = voutdev->width;
 	f->fmt.pix.height = voutdev->height;
 	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
-	f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 4;
+
+	if (config & (1U << 16)) {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+		if (config & (1U << 20)) {
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+		} else {
+			if (config & (1U << 19))
+				f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+			else
+				f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+		}
+		f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 2;
+	} else {
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
+		f->fmt.pix.bytesperline = (f->fmt.pix.width + voutdev->padding) * 4;
+	}
+
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
 
 	return 0;
@@ -220,14 +293,30 @@ static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 static int vidioc_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	u32 pixelsize;
 
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
 	f->fmt.pix.width = voutdev->width;
 	f->fmt.pix.height = voutdev->height;
 	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
-	f->fmt.pix.bytesperline = max(f->fmt.pix.width * 4,
-				      ALIGN_DOWN(f->fmt.pix.bytesperline, 4));
+
+	if (has_yuv(video) && f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+		pixelsize = 2;
+		if (!(f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709 ||
+		      f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M))
+			f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
+	} else {
+		pixelsize = 4;
+		f->fmt.pix.pixelformat = V4L2_PIX_FMT_ABGR32;
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_RAW;
+	}
+
+	if (f->fmt.pix.bytesperline > f->fmt.pix.width * pixelsize &&
+	    f->fmt.pix.bytesperline < f->fmt.pix.width * pixelsize * 2)
+		f->fmt.pix.bytesperline = ALIGN(f->fmt.pix.bytesperline,
+						pixelsize);
+	else
+		f->fmt.pix.bytesperline = f->fmt.pix.width * pixelsize;
 	f->fmt.pix.sizeimage = f->fmt.pix.bytesperline * f->fmt.pix.height;
 
 	return 0;
@@ -237,14 +326,39 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 {
 	struct mgb4_vout_dev *voutdev = video_drvdata(file);
 	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	u32 config, pixelsize;
+	int ret;
 
 	if (vb2_is_busy(&voutdev->queue))
 		return -EBUSY;
 
-	vidioc_try_fmt(file, priv, f);
+	ret = vidioc_try_fmt(file, priv, f);
+	if (ret < 0)
+		return ret;
+
+	config = mgb4_read_reg(video, voutdev->config->regs.config);
+	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+		pixelsize = 2;
+		config |= 1U << 16;
+
+		if (f->fmt.pix.colorspace == V4L2_COLORSPACE_REC709) {
+			config |= 1U << 20;
+			config |= 1U << 19;
+		} else if (f->fmt.pix.colorspace == V4L2_COLORSPACE_SMPTE170M) {
+			config &= ~(1U << 20);
+			config |= 1U << 19;
+		} else {
+			config &= ~(1U << 20);
+			config &= ~(1U << 19);
+		}
+	} else {
+		pixelsize = 4;
+		config &= ~(1U << 16);
+	}
+	mgb4_write_reg(video, voutdev->config->regs.config, config);
 
-	voutdev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width * 4)) / 4;
-	mgb4_write_reg(video, voutdev->config->regs.padding, voutdev->padding);
+	voutdev->padding = (f->fmt.pix.bytesperline - (f->fmt.pix.width
+			    * pixelsize)) / pixelsize;
 
 	return 0;
 }
@@ -267,11 +381,128 @@ static int vidioc_enum_output(struct file *file, void *priv,
 		return -EINVAL;
 
 	out->type = V4L2_OUTPUT_TYPE_ANALOG;
+	out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
 	strscpy(out->name, "MGB4", sizeof(out->name));
 
 	return 0;
 }
 
+static int vidioc_enum_frameintervals(struct file *file, void *priv,
+				      struct v4l2_frmivalenum *ival)
+{
+	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	struct v4l2_dv_timings timings;
+
+	if (ival->index != 0)
+		return -EINVAL;
+	if (!(ival->pixel_format == V4L2_PIX_FMT_ABGR32 ||
+	      ((has_yuv(video) && ival->pixel_format == V4L2_PIX_FMT_YUYV))))
+		return -EINVAL;
+	if (ival->width != voutdev->width || ival->height != voutdev->height)
+		return -EINVAL;
+
+	get_timings(voutdev, &timings);
+
+	ival->type = V4L2_FRMIVAL_TYPE_STEPWISE;
+	ival->stepwise.max.denominator = MGB4_HW_FREQ;
+	ival->stepwise.max.numerator = 0xFFFFFFFF;
+	ival->stepwise.min.denominator = timings.bt.pixelclock;
+	ival->stepwise.min.numerator = pixel_size(&timings);
+	ival->stepwise.step.denominator = MGB4_HW_FREQ;
+	ival->stepwise.step.numerator = 1;
+
+	return 0;
+}
+
+static int vidioc_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
+	struct v4l2_dv_timings timings;
+	u32 timer;
+
+	parm->parm.output.writebuffers = 2;
+
+	if (has_timeperframe(video)) {
+		timer = mgb4_read_reg(video, voutdev->config->regs.timer);
+		if (timer < 0xFFFF) {
+			get_timings(voutdev, &timings);
+			tpf->numerator = pixel_size(&timings);
+			tpf->denominator = timings.bt.pixelclock;
+		} else {
+			tpf->numerator = timer;
+			tpf->denominator = MGB4_HW_FREQ;
+		}
+
+		parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+	}
+
+	return 0;
+}
+
+static int vidioc_s_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	struct v4l2_fract *tpf = &parm->parm.output.timeperframe;
+	struct v4l2_dv_timings timings;
+	u32 timer, period;
+
+	if (has_timeperframe(video)) {
+		timer = tpf->denominator ?
+			MGB4_PERIOD(tpf->numerator, tpf->denominator) : 0;
+		if (timer) {
+			get_timings(voutdev, &timings);
+			period = MGB4_PERIOD(pixel_size(&timings),
+					     timings.bt.pixelclock);
+			if (timer < period)
+				timer = 0;
+		}
+
+		mgb4_write_reg(video, voutdev->config->regs.timer, timer);
+	}
+
+	return vidioc_g_parm(file, priv, parm);
+}
+
+static int vidioc_g_dv_timings(struct file *file, void *fh,
+			       struct v4l2_dv_timings *timings)
+{
+	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+
+	get_timings(voutdev, timings);
+
+	return 0;
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *fh,
+			       struct v4l2_dv_timings *timings)
+{
+	struct mgb4_vout_dev *voutdev = video_drvdata(file);
+
+	get_timings(voutdev, timings);
+
+	return 0;
+}
+
+static int vidioc_enum_dv_timings(struct file *file, void *fh,
+				  struct v4l2_enum_dv_timings *timings)
+{
+	return v4l2_enum_dv_timings_cap(timings, &video_timings_cap, NULL, NULL);
+}
+
+static int vidioc_dv_timings_cap(struct file *file, void *fh,
+				 struct v4l2_dv_timings_cap *cap)
+{
+	*cap = video_timings_cap;
+
+	return 0;
+}
+
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_querycap = vidioc_querycap,
 	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt,
@@ -279,8 +510,15 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_s_fmt_vid_out = vidioc_s_fmt,
 	.vidioc_g_fmt_vid_out = vidioc_g_fmt,
 	.vidioc_enum_output = vidioc_enum_output,
+	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
 	.vidioc_g_output = vidioc_g_output,
 	.vidioc_s_output = vidioc_s_output,
+	.vidioc_g_parm = vidioc_g_parm,
+	.vidioc_s_parm = vidioc_s_parm,
+	.vidioc_dv_timings_cap = vidioc_dv_timings_cap,
+	.vidioc_enum_dv_timings = vidioc_enum_dv_timings,
+	.vidioc_g_dv_timings = vidioc_g_dv_timings,
+	.vidioc_s_dv_timings = vidioc_s_dv_timings,
 	.vidioc_reqbufs = vb2_ioctl_reqbufs,
 	.vidioc_create_bufs = vb2_ioctl_create_bufs,
 	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
@@ -422,27 +660,28 @@ static void fpga_init(struct mgb4_vout_dev *voutdev)
 	const struct mgb4_vout_regs *regs = &voutdev->config->regs;
 
 	mgb4_write_reg(video, regs->config, 0x00000011);
-	mgb4_write_reg(video, regs->resolution,
-		       (MGB4_DEFAULT_WIDTH << 16) | MGB4_DEFAULT_HEIGHT);
-	mgb4_write_reg(video, regs->hsync, 0x00102020);
-	mgb4_write_reg(video, regs->vsync, 0x40020202);
-	mgb4_write_reg(video, regs->frame_period, MGB4_DEFAULT_PERIOD);
+	mgb4_write_reg(video, regs->resolution, (1280 << 16) | 640);
+	mgb4_write_reg(video, regs->hsync, 0x00283232);
+	mgb4_write_reg(video, regs->vsync, 0x40141F1E);
+	mgb4_write_reg(video, regs->frame_limit, MGB4_HW_FREQ / 60);
 	mgb4_write_reg(video, regs->padding, 0x00000000);
 
-	voutdev->freq = mgb4_cmt_set_vout_freq(voutdev, 70000 >> 1) << 1;
+	voutdev->freq = mgb4_cmt_set_vout_freq(voutdev, 61150 >> 1) << 1;
 
 	mgb4_write_reg(video, regs->config,
 		       (voutdev->config->id + MGB4_VIN_DEVICES) << 2 | 1 << 4);
 }
 
-#ifdef CONFIG_DEBUG_FS
-static void debugfs_init(struct mgb4_vout_dev *voutdev)
+static void create_debugfs(struct mgb4_vout_dev *voutdev)
 {
+#ifdef CONFIG_DEBUG_FS
 	struct mgb4_regs *video = &voutdev->mgbdev->video;
+	struct dentry *entry;
 
-	voutdev->debugfs = debugfs_create_dir(voutdev->vdev.name,
-					      voutdev->mgbdev->debugfs);
-	if (!voutdev->debugfs)
+	if (IS_ERR_OR_NULL(voutdev->mgbdev->debugfs))
+		return;
+	entry = debugfs_create_dir(voutdev->vdev.name, voutdev->mgbdev->debugfs);
+	if (IS_ERR(entry))
 		return;
 
 	voutdev->regs[0].name = "CONFIG";
@@ -455,19 +694,24 @@ static void debugfs_init(struct mgb4_vout_dev *voutdev)
 	voutdev->regs[3].offset = voutdev->config->regs.hsync;
 	voutdev->regs[4].name = "VIDEO_PARAMS_2";
 	voutdev->regs[4].offset = voutdev->config->regs.vsync;
-	voutdev->regs[5].name = "FRAME_PERIOD";
-	voutdev->regs[5].offset = voutdev->config->regs.frame_period;
-	voutdev->regs[6].name = "PADDING";
+	voutdev->regs[5].name = "FRAME_LIMIT";
+	voutdev->regs[5].offset = voutdev->config->regs.frame_limit;
+	voutdev->regs[6].name = "PADDING_PIXELS";
 	voutdev->regs[6].offset = voutdev->config->regs.padding;
+	if (has_timeperframe(video)) {
+		voutdev->regs[7].name = "TIMER";
+		voutdev->regs[7].offset = voutdev->config->regs.timer;
+		voutdev->regset.nregs = 8;
+	} else {
+		voutdev->regset.nregs = 7;
+	}
 
 	voutdev->regset.base = video->membase;
 	voutdev->regset.regs = voutdev->regs;
-	voutdev->regset.nregs = ARRAY_SIZE(voutdev->regs);
 
-	debugfs_create_regset32("registers", 0444, voutdev->debugfs,
-				&voutdev->regset);
-}
+	debugfs_create_regset32("registers", 0444, entry, &voutdev->regset);
 #endif
+}
 
 struct mgb4_vout_dev *mgb4_vout_create(struct mgb4_dev *mgbdev, int id)
 {
@@ -561,9 +805,7 @@ struct mgb4_vout_dev *mgb4_vout_create(struct mgb4_dev *mgbdev, int id)
 		goto err_video_dev;
 	}
 
-#ifdef CONFIG_DEBUG_FS
-	debugfs_init(voutdev);
-#endif
+	create_debugfs(voutdev);
 
 	return voutdev;
 
@@ -586,10 +828,6 @@ void mgb4_vout_free(struct mgb4_vout_dev *voutdev)
 
 	free_irq(irq, voutdev);
 
-#ifdef CONFIG_DEBUG_FS
-	debugfs_remove_recursive(voutdev->debugfs);
-#endif
-
 	groups = MGB4_IS_GMSL(voutdev->mgbdev)
 	  ? mgb4_gmsl_out_groups : mgb4_fpdl3_out_groups;
 	device_remove_groups(&voutdev->vdev.dev, groups);
diff --git a/drivers/media/pci/mgb4/mgb4_vout.h b/drivers/media/pci/mgb4/mgb4_vout.h
index b163dee711fd..a07eeabdcf34 100644
--- a/drivers/media/pci/mgb4/mgb4_vout.h
+++ b/drivers/media/pci/mgb4/mgb4_vout.h
@@ -19,10 +19,11 @@ struct mgb4_vout_regs {
 	u32 config;
 	u32 status;
 	u32 resolution;
-	u32 frame_period;
+	u32 frame_limit;
 	u32 hsync;
 	u32 vsync;
 	u32 padding;
+	u32 timer;
 };
 
 struct mgb4_vout_config {
@@ -53,9 +54,8 @@ struct mgb4_vout_dev {
 	const struct mgb4_vout_config *config;
 
 #ifdef CONFIG_DEBUG_FS
-	struct dentry *debugfs;
 	struct debugfs_regset32 regset;
-	struct debugfs_reg32 regs[7];
+	struct debugfs_reg32 regs[sizeof(struct mgb4_vout_regs) / 4];
 #endif
 };
 
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
index 557985ba25db..9f2ac33cffa7 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
@@ -637,7 +637,7 @@ static void netup_unidvb_queue_cleanup(struct netup_dma *dma)
 
 static void netup_unidvb_dma_timeout(struct timer_list *t)
 {
-	struct netup_dma *dma = from_timer(dma, t, timeout);
+	struct netup_dma *dma = timer_container_of(dma, t, timeout);
 	struct netup_unidvb_dev *ndev = dma->ndev;
 
 	dev_dbg(&ndev->pci_dev->dev, "%s()\n", __func__);
@@ -698,7 +698,7 @@ static void netup_unidvb_dma_fini(struct netup_unidvb_dev *ndev, int num)
 	netup_unidvb_dma_enable(dma, 0);
 	msleep(50);
 	cancel_work_sync(&dma->work);
-	del_timer_sync(&dma->timeout);
+	timer_delete_sync(&dma->timeout);
 }
 
 static int netup_unidvb_dma_setup(struct netup_unidvb_dev *ndev)
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c b/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
index 46676f2c89c7..1c885d620b75 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_i2c.c
@@ -135,7 +135,7 @@ static void netup_i2c_fifo_tx(struct netup_i2c *i2c)
 		(readw(&i2c->regs->tx_fifo.stat_ctrl) & 0x3f);
 	u32 msg_length = i2c->msg->len - i2c->xmit_size;
 
-	msg_length = (msg_length < fifo_space ? msg_length : fifo_space);
+	msg_length = min(msg_length, fifo_space);
 	while (msg_length--) {
 		data = i2c->msg->buf[i2c->xmit_size++];
 		writeb(data, &i2c->regs->tx_fifo.data8);
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_spi.c b/drivers/media/pci/netup_unidvb/netup_unidvb_spi.c
index e90aa1c1584c..34d6d52f1e4e 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_spi.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_spi.c
@@ -175,11 +175,11 @@ int netup_spi_init(struct netup_unidvb_dev *ndev)
 	struct spi_controller *ctlr;
 	struct netup_spi *nspi;
 
-	ctlr = devm_spi_alloc_master(&ndev->pci_dev->dev,
-					 sizeof(struct netup_spi));
+	ctlr = devm_spi_alloc_host(&ndev->pci_dev->dev,
+				   sizeof(struct netup_spi));
 	if (!ctlr) {
 		dev_err(&ndev->pci_dev->dev,
-			"%s(): unable to alloc SPI master\n", __func__);
+			"%s(): unable to alloc SPI host\n", __func__);
 		return -EINVAL;
 	}
 	nspi = spi_controller_get_devdata(ctlr);
diff --git a/drivers/media/pci/ngene/ngene-core.c b/drivers/media/pci/ngene/ngene-core.c
index 7481f553f959..db6796240bce 100644
--- a/drivers/media/pci/ngene/ngene-core.c
+++ b/drivers/media/pci/ngene/ngene-core.c
@@ -50,9 +50,9 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 /* nGene interrupt handler **************************************************/
 /****************************************************************************/
 
-static void event_tasklet(struct tasklet_struct *t)
+static void event_bh_work(struct work_struct *t)
 {
-	struct ngene *dev = from_tasklet(dev, t, event_tasklet);
+	struct ngene *dev = from_work(dev, t, event_bh_work);
 
 	while (dev->EventQueueReadIndex != dev->EventQueueWriteIndex) {
 		struct EVENT_BUFFER Event =
@@ -68,9 +68,9 @@ static void event_tasklet(struct tasklet_struct *t)
 	}
 }
 
-static void demux_tasklet(struct tasklet_struct *t)
+static void demux_bh_work(struct work_struct *t)
 {
-	struct ngene_channel *chan = from_tasklet(chan, t, demux_tasklet);
+	struct ngene_channel *chan = from_work(chan, t, demux_bh_work);
 	struct device *pdev = &chan->dev->pci_dev->dev;
 	struct SBufferHeader *Cur = chan->nextBuffer;
 
@@ -204,7 +204,7 @@ static irqreturn_t irq_handler(int irq, void *dev_id)
 			dev->EventQueueOverflowFlag = 1;
 		}
 		dev->EventBuffer->EventStatus &= ~0x80;
-		tasklet_schedule(&dev->event_tasklet);
+		queue_work(system_bh_wq, &dev->event_bh_work);
 		rc = IRQ_HANDLED;
 	}
 
@@ -217,8 +217,8 @@ static irqreturn_t irq_handler(int irq, void *dev_id)
 			     ngeneBuffer.SR.Flags & 0xC0) == 0x80) {
 				dev->channel[i].nextBuffer->
 					ngeneBuffer.SR.Flags |= 0x40;
-				tasklet_schedule(
-					&dev->channel[i].demux_tasklet);
+				queue_work(system_bh_wq,
+					   &dev->channel[i].demux_bh_work);
 				rc = IRQ_HANDLED;
 			}
 		}
@@ -1181,7 +1181,7 @@ static void ngene_init(struct ngene *dev)
 	struct device *pdev = &dev->pci_dev->dev;
 	int i;
 
-	tasklet_setup(&dev->event_tasklet, event_tasklet);
+	INIT_WORK(&dev->event_bh_work, event_bh_work);
 
 	memset_io(dev->iomem + 0xc000, 0x00, 0x220);
 	memset_io(dev->iomem + 0xc400, 0x00, 0x100);
@@ -1395,7 +1395,7 @@ static void release_channel(struct ngene_channel *chan)
 	if (chan->running)
 		set_transfer(chan, 0);
 
-	tasklet_kill(&chan->demux_tasklet);
+	cancel_work_sync(&chan->demux_bh_work);
 
 	if (chan->ci_dev) {
 		dvb_unregister_device(chan->ci_dev);
@@ -1445,7 +1445,7 @@ static int init_channel(struct ngene_channel *chan)
 	struct ngene_info *ni = dev->card_info;
 	int io = ni->io_type[nr];
 
-	tasklet_setup(&chan->demux_tasklet, demux_tasklet);
+	INIT_WORK(&chan->demux_bh_work, demux_bh_work);
 	chan->users = 0;
 	chan->type = io;
 	chan->mode = chan->type;	/* for now only one mode */
@@ -1488,7 +1488,9 @@ static int init_channel(struct ngene_channel *chan)
 	}
 
 	if (dev->ci.en && (io & NGENE_IO_TSOUT)) {
-		dvb_ca_en50221_init(adapter, dev->ci.en, 0, 1);
+		ret = dvb_ca_en50221_init(adapter, dev->ci.en, 0, 1);
+		if (ret != 0)
+			goto err;
 		set_transfer(chan, 1);
 		chan->dev->channel[2].DataFormatFlags = DF_SWAP32;
 		set_transfer(&chan->dev->channel[2], 1);
@@ -1647,7 +1649,7 @@ void ngene_remove(struct pci_dev *pdev)
 	struct ngene *dev = pci_get_drvdata(pdev);
 	int i;
 
-	tasklet_kill(&dev->event_tasklet);
+	cancel_work_sync(&dev->event_bh_work);
 	for (i = MAX_STREAM - 1; i >= 0; i--)
 		release_channel(&dev->channel[i]);
 	if (dev->ci.en)
diff --git a/drivers/media/pci/ngene/ngene.h b/drivers/media/pci/ngene/ngene.h
index d1d7da84cd9d..9f989420b9e7 100644
--- a/drivers/media/pci/ngene/ngene.h
+++ b/drivers/media/pci/ngene/ngene.h
@@ -16,6 +16,7 @@
 #include <linux/scatterlist.h>
 
 #include <linux/dvb/frontend.h>
+#include <linux/workqueue.h>
 
 #include <media/dmxdev.h>
 #include <media/dvbdev.h>
@@ -621,7 +622,7 @@ struct ngene_channel {
 	int                   users;
 	struct video_device  *v4l_dev;
 	struct dvb_device    *ci_dev;
-	struct tasklet_struct demux_tasklet;
+	struct work_struct    demux_bh_work;
 
 	struct SBufferHeader *nextBuffer;
 	enum KSSTATE          State;
@@ -717,7 +718,7 @@ struct ngene {
 	struct EVENT_BUFFER   EventQueue[EVENT_QUEUE_SIZE];
 	int                   EventQueueOverflowCount;
 	int                   EventQueueOverflowFlag;
-	struct tasklet_struct event_tasklet;
+	struct work_struct    event_bh_work;
 	struct EVENT_BUFFER  *EventBuffer;
 	int                   EventQueueWriteIndex;
 	int                   EventQueueReadIndex;
diff --git a/drivers/media/pci/pt3/pt3.c b/drivers/media/pci/pt3/pt3.c
index 246f73b8a9e7..c55aa782b72c 100644
--- a/drivers/media/pci/pt3/pt3.c
+++ b/drivers/media/pci/pt3/pt3.c
@@ -692,6 +692,7 @@ static int pt3_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	u8 rev;
 	u32 ver;
 	int i, ret;
+	void __iomem *iomem;
 	struct pt3_board *pt3;
 	struct i2c_adapter *i2c;
 
@@ -703,10 +704,6 @@ static int pt3_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		return -ENODEV;
 	pci_set_master(pdev);
 
-	ret = pcim_iomap_regions(pdev, BIT(0) | BIT(2), DRV_NAME);
-	if (ret < 0)
-		return ret;
-
 	ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to set DMA mask\n");
@@ -719,8 +716,16 @@ static int pt3_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	pci_set_drvdata(pdev, pt3);
 	pt3->pdev = pdev;
 	mutex_init(&pt3->lock);
-	pt3->regs[0] = pcim_iomap_table(pdev)[0];
-	pt3->regs[1] = pcim_iomap_table(pdev)[2];
+
+	iomem = pcim_iomap_region(pdev, 0, DRV_NAME);
+	if (IS_ERR(iomem))
+		return PTR_ERR(iomem);
+	pt3->regs[0] = iomem;
+
+	iomem = pcim_iomap_region(pdev, 2, DRV_NAME);
+	if (IS_ERR(iomem))
+		return PTR_ERR(iomem);
+	pt3->regs[1] = iomem;
 
 	ver = ioread32(pt3->regs[0] + REG_VERSION);
 	if ((ver >> 16) != 0x0301) {
diff --git a/drivers/media/pci/saa7134/saa7134-alsa.c b/drivers/media/pci/saa7134/saa7134-alsa.c
index d3cde05a6eba..f86a44dfe6e3 100644
--- a/drivers/media/pci/saa7134/saa7134-alsa.c
+++ b/drivers/media/pci/saa7134/saa7134-alsa.c
@@ -1096,9 +1096,6 @@ static void snd_saa7134_free(struct snd_card * card)
 	if (chip->dev->dmasound.priv_data == NULL)
 		return;
 
-	if (chip->irq >= 0)
-		free_irq(chip->irq, &chip->dev->dmasound);
-
 	chip->dev->dmasound.priv_data = NULL;
 
 }
@@ -1147,10 +1144,8 @@ static int alsa_card_saa7134_create(struct saa7134_dev *dev, int devnum)
 	chip->iobase = pci_resource_start(dev->pci, 0);
 
 
-	err = request_irq(dev->pci->irq, saa7134_alsa_irq,
-				IRQF_SHARED, dev->name,
-				(void*) &dev->dmasound);
-
+	err = devm_request_irq(&dev->pci->dev, dev->pci->irq, saa7134_alsa_irq,
+			       IRQF_SHARED, dev->name, &dev->dmasound);
 	if (err < 0) {
 		pr_err("%s: can't get IRQ %d for ALSA\n",
 			dev->name, dev->pci->irq);
@@ -1259,5 +1254,6 @@ static void saa7134_alsa_exit(void)
 /* We initialize this late, to make sure the sound system is up and running */
 late_initcall(saa7134_alsa_init);
 module_exit(saa7134_alsa_exit);
+MODULE_DESCRIPTION("Philips SAA7134 DMA audio support");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ricardo Cerqueira");
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index 1280696f65f2..e80fb4ebfda6 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -5152,7 +5152,7 @@ struct saa7134_board saa7134_boards[] = {
 		},
 	},
 	[SAA7134_BOARD_AVERMEDIA_STUDIO_507UA] = {
-		/* Andy Shevchenko <andy@smile.org.ua> */
+		/* Andy Shevchenko <andy@kernel.org> */
 		.name           = "Avermedia AVerTV Studio 507UA",
 		.audio_clock    = 0x00187de7,
 		.tuner_type     = TUNER_PHILIPS_FM1216ME_MK3, /* Should be MK5 */
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index ea0585e43abb..537aa65acdc8 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -322,13 +322,13 @@ void saa7134_buffer_next(struct saa7134_dev *dev,
 		/* nothing to do -- just stop DMA */
 		core_dbg("buffer_next %p\n", NULL);
 		saa7134_set_dmabits(dev);
-		del_timer(&q->timeout);
+		timer_delete(&q->timeout);
 	}
 }
 
 void saa7134_buffer_timeout(struct timer_list *t)
 {
-	struct saa7134_dmaqueue *q = from_timer(q, t, timeout);
+	struct saa7134_dmaqueue *q = timer_container_of(q, t, timeout);
 	struct saa7134_dev *dev = q->dev;
 	unsigned long flags;
 
@@ -364,7 +364,7 @@ void saa7134_stop_streaming(struct saa7134_dev *dev, struct saa7134_dmaqueue *q)
 		tmp = NULL;
 	}
 	spin_unlock_irqrestore(&dev->slock, flags);
-	saa7134_buffer_timeout(&q->timeout); /* also calls del_timer(&q->timeout) */
+	saa7134_buffer_timeout(&q->timeout); /* also calls timer_delete(&q->timeout) */
 }
 EXPORT_SYMBOL_GPL(saa7134_stop_streaming);
 
@@ -1390,9 +1390,9 @@ static int __maybe_unused saa7134_suspend(struct device *dev_d)
 	/* Disable timeout timers - if we have active buffers, we will
 	   fill them on resume*/
 
-	del_timer(&dev->video_q.timeout);
-	del_timer(&dev->vbi_q.timeout);
-	del_timer(&dev->ts_q.timeout);
+	timer_delete(&dev->video_q.timeout);
+	timer_delete(&dev->vbi_q.timeout);
+	timer_delete(&dev->ts_q.timeout);
 
 	if (dev->remote && dev->remote->dev->users)
 		saa7134_ir_close(dev->remote->dev);
diff --git a/drivers/media/pci/saa7134/saa7134-dvb.c b/drivers/media/pci/saa7134/saa7134-dvb.c
index 9c6cfef03331..6de8a02314af 100644
--- a/drivers/media/pci/saa7134/saa7134-dvb.c
+++ b/drivers/media/pci/saa7134/saa7134-dvb.c
@@ -52,6 +52,7 @@
 #include "s5h1411.h"
 
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
+MODULE_DESCRIPTION("DVB/ATSC Support for saa7134 based TV cards");
 MODULE_LICENSE("GPL");
 
 static unsigned int antenna_pwr;
@@ -466,7 +467,9 @@ static int philips_europa_tuner_sleep(struct dvb_frontend *fe)
 	/* switch the board to analog mode */
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	i2c_transfer(&dev->i2c_adap, &analog_msg, 1);
+	if (i2c_transfer(&dev->i2c_adap, &analog_msg, 1) != 1)
+		return -EIO;
+
 	return 0;
 }
 
@@ -1018,7 +1021,9 @@ static int md8800_set_voltage2(struct dvb_frontend *fe,
 	else
 		wbuf[1] = rbuf & 0xef;
 	msg[0].len = 2;
-	i2c_transfer(&dev->i2c_adap, msg, 1);
+	if (i2c_transfer(&dev->i2c_adap, msg, 1) != 1)
+		return -EIO;
+
 	return 0;
 }
 
diff --git a/drivers/media/pci/saa7134/saa7134-empress.c b/drivers/media/pci/saa7134/saa7134-empress.c
index 434fa1ee1c33..8c4f70e4177d 100644
--- a/drivers/media/pci/saa7134/saa7134-empress.c
+++ b/drivers/media/pci/saa7134/saa7134-empress.c
@@ -19,6 +19,7 @@
 /* ------------------------------------------------------------------ */
 
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
+MODULE_DESCRIPTION("Philips SAA7134 empress support");
 MODULE_LICENSE("GPL");
 
 static unsigned int empress_nr[] = {[0 ... (SAA7134_MAXBOARDS - 1)] = UNSET };
@@ -77,8 +78,6 @@ static const struct vb2_ops saa7134_empress_qops = {
 	.buf_init	= saa7134_ts_buffer_init,
 	.buf_prepare	= saa7134_ts_buffer_prepare,
 	.buf_queue	= saa7134_vb2_buffer_queue,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
 };
diff --git a/drivers/media/pci/saa7134/saa7134-go7007.c b/drivers/media/pci/saa7134/saa7134-go7007.c
index da83893ffee9..bd37db5ce363 100644
--- a/drivers/media/pci/saa7134/saa7134-go7007.c
+++ b/drivers/media/pci/saa7134/saa7134-go7007.c
@@ -516,4 +516,5 @@ static void __exit saa7134_go7007_mod_cleanup(void)
 module_init(saa7134_go7007_mod_init);
 module_exit(saa7134_go7007_mod_cleanup);
 
+MODULE_DESCRIPTION("go7007 support for saa7134 based TV cards");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/pci/saa7134/saa7134-input.c b/drivers/media/pci/saa7134/saa7134-input.c
index 8610eb473b39..468dbe8d552f 100644
--- a/drivers/media/pci/saa7134/saa7134-input.c
+++ b/drivers/media/pci/saa7134/saa7134-input.c
@@ -431,7 +431,7 @@ void saa7134_input_irq(struct saa7134_dev *dev)
 
 static void saa7134_input_timer(struct timer_list *t)
 {
-	struct saa7134_card_ir *ir = from_timer(ir, t, timer);
+	struct saa7134_card_ir *ir = timer_container_of(ir, t, timer);
 	struct saa7134_dev *dev = ir->dev->priv;
 
 	build_key(dev);
@@ -496,7 +496,7 @@ void saa7134_ir_close(struct rc_dev *rc)
 	struct saa7134_card_ir *ir = dev->remote;
 
 	if (ir->polling)
-		del_timer_sync(&ir->timer);
+		timer_delete_sync(&ir->timer);
 
 	ir->running = false;
 }
diff --git a/drivers/media/pci/saa7134/saa7134-ts.c b/drivers/media/pci/saa7134/saa7134-ts.c
index 437dbe5e75e2..1b44033067c5 100644
--- a/drivers/media/pci/saa7134/saa7134-ts.c
+++ b/drivers/media/pci/saa7134/saa7134-ts.c
@@ -166,8 +166,6 @@ struct vb2_ops saa7134_ts_qops = {
 	.buf_init	= saa7134_ts_buffer_init,
 	.buf_prepare	= saa7134_ts_buffer_prepare,
 	.buf_queue	= saa7134_vb2_buffer_queue,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.stop_streaming = saa7134_ts_stop_streaming,
 };
 EXPORT_SYMBOL_GPL(saa7134_ts_qops);
@@ -300,7 +298,7 @@ int saa7134_ts_start(struct saa7134_dev *dev)
 
 int saa7134_ts_fini(struct saa7134_dev *dev)
 {
-	del_timer_sync(&dev->ts_q.timeout);
+	timer_delete_sync(&dev->ts_q.timeout);
 	saa7134_pgtable_free(dev->pci, &dev->ts_q.pt);
 	return 0;
 }
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index 3e773690468b..28bf77449bdb 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -161,8 +161,6 @@ const struct vb2_ops saa7134_vbi_qops = {
 	.buf_init	= buffer_init,
 	.buf_prepare	= buffer_prepare,
 	.buf_queue	= saa7134_vb2_buffer_queue,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.start_streaming = saa7134_vb2_start_streaming,
 	.stop_streaming = saa7134_vb2_stop_streaming,
 };
@@ -185,7 +183,7 @@ int saa7134_vbi_init1(struct saa7134_dev *dev)
 int saa7134_vbi_fini(struct saa7134_dev *dev)
 {
 	/* nothing */
-	del_timer_sync(&dev->vbi_q.timeout);
+	timer_delete_sync(&dev->vbi_q.timeout);
 	return 0;
 }
 
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 56b4481a40e6..c88939bce56b 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -844,8 +844,6 @@ static const struct vb2_ops vb2_qops = {
 	.buf_init	= buffer_init,
 	.buf_prepare	= buffer_prepare,
 	.buf_queue	= saa7134_vb2_buffer_queue,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.start_streaming = saa7134_vb2_start_streaming,
 	.stop_streaming = saa7134_vb2_stop_streaming,
 };
@@ -1743,7 +1741,7 @@ int saa7134_video_init1(struct saa7134_dev *dev)
 
 void saa7134_video_fini(struct saa7134_dev *dev)
 {
-	del_timer_sync(&dev->video_q.timeout);
+	timer_delete_sync(&dev->video_q.timeout);
 	/* free stuff */
 	saa7134_pgtable_free(dev->pci, &dev->video_q.pt);
 	saa7134_pgtable_free(dev->pci, &dev->vbi_q.pt);
diff --git a/drivers/media/pci/saa7164/saa7164-vbi.c b/drivers/media/pci/saa7164/saa7164-vbi.c
index a6738baab688..ac958a5fca78 100644
--- a/drivers/media/pci/saa7164/saa7164-vbi.c
+++ b/drivers/media/pci/saa7164/saa7164-vbi.c
@@ -77,9 +77,7 @@ static int saa7164_vbi_buffers_alloc(struct saa7164_port *port)
 	/* TODO: NTSC SPECIFIC */
 	/* Init and establish defaults */
 	params->samplesperline = 1440;
-	params->numberoflines = 12;
 	params->numberoflines = 18;
-	params->pitch = 1600;
 	params->pitch = 1440;
 	params->numpagetables = 2 +
 		((params->numberoflines * params->pitch) / PAGE_SIZE);
diff --git a/drivers/media/pci/smipcie/smipcie-main.c b/drivers/media/pci/smipcie/smipcie-main.c
index 0c300d019d9c..7db6d443fc54 100644
--- a/drivers/media/pci/smipcie/smipcie-main.c
+++ b/drivers/media/pci/smipcie/smipcie-main.c
@@ -279,10 +279,10 @@ static void smi_port_clearInterrupt(struct smi_port *port)
 		(port->_dmaInterruptCH0 | port->_dmaInterruptCH1));
 }
 
-/* tasklet handler: DMA data to dmx.*/
-static void smi_dma_xfer(struct tasklet_struct *t)
+/* BH work handler: DMA data to dmx.*/
+static void smi_dma_xfer(struct work_struct *t)
 {
-	struct smi_port *port = from_tasklet(port, t, tasklet);
+	struct smi_port *port = from_work(port, t, bh_work);
 	struct smi_dev *dev = port->dev;
 	u32 intr_status, finishedData, dmaManagement;
 	u8 dmaChan0State, dmaChan1State;
@@ -426,8 +426,8 @@ static int smi_port_init(struct smi_port *port, int dmaChanUsed)
 	}
 
 	smi_port_disableInterrupt(port);
-	tasklet_setup(&port->tasklet, smi_dma_xfer);
-	tasklet_disable(&port->tasklet);
+	INIT_WORK(&port->bh_work, smi_dma_xfer);
+	disable_work_sync(&port->bh_work);
 	port->enable = 1;
 	return 0;
 err:
@@ -438,7 +438,7 @@ err:
 static void smi_port_exit(struct smi_port *port)
 {
 	smi_port_disableInterrupt(port);
-	tasklet_kill(&port->tasklet);
+	cancel_work_sync(&port->bh_work);
 	smi_port_dma_free(port);
 	port->enable = 0;
 }
@@ -452,7 +452,7 @@ static int smi_port_irq(struct smi_port *port, u32 int_status)
 		smi_port_disableInterrupt(port);
 		port->_int_status = int_status;
 		smi_port_clearInterrupt(port);
-		tasklet_schedule(&port->tasklet);
+		queue_work(system_bh_wq, &port->bh_work);
 		handled = 1;
 	}
 	return handled;
@@ -823,7 +823,7 @@ static int smi_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 		smi_port_clearInterrupt(port);
 		smi_port_enableInterrupt(port);
 		smi_write(port->DMA_MANAGEMENT, dmaManagement);
-		tasklet_enable(&port->tasklet);
+		enable_and_queue_work(system_bh_wq, &port->bh_work);
 	}
 	return port->users;
 }
@@ -837,7 +837,7 @@ static int smi_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	if (--port->users)
 		return port->users;
 
-	tasklet_disable(&port->tasklet);
+	disable_work_sync(&port->bh_work);
 	smi_port_disableInterrupt(port);
 	smi_clear(port->DMA_MANAGEMENT, 0x30003);
 	return 0;
diff --git a/drivers/media/pci/smipcie/smipcie.h b/drivers/media/pci/smipcie/smipcie.h
index 2b5e0154814c..98e44edaab9e 100644
--- a/drivers/media/pci/smipcie/smipcie.h
+++ b/drivers/media/pci/smipcie/smipcie.h
@@ -17,6 +17,7 @@
 #include <linux/pci.h>
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
+#include <linux/workqueue.h>
 #include <media/rc-core.h>
 
 #include <media/demux.h>
@@ -257,7 +258,7 @@ struct smi_port {
 	u32 _dmaInterruptCH0;
 	u32 _dmaInterruptCH1;
 	u32 _int_status;
-	struct tasklet_struct tasklet;
+	struct work_struct bh_work;
 	/* dvb */
 	struct dmx_frontend hw_frontend;
 	struct dmx_frontend mem_frontend;
diff --git a/drivers/media/pci/solo6x10/solo6x10-core.c b/drivers/media/pci/solo6x10/solo6x10-core.c
index 6d87fbb0ee04..febb2c156cf6 100644
--- a/drivers/media/pci/solo6x10/solo6x10-core.c
+++ b/drivers/media/pci/solo6x10/solo6x10-core.c
@@ -144,11 +144,8 @@ static void free_solo_dev(struct solo_dev *solo_dev)
 
 		/* Now cleanup the PCI device */
 		solo_irq_off(solo_dev, ~0);
-		free_irq(pdev->irq, solo_dev);
-		pci_iounmap(pdev, solo_dev->reg_base);
 	}
 
-	pci_release_regions(pdev);
 	pci_disable_device(pdev);
 	v4l2_device_unregister(&solo_dev->v4l2_dev);
 	pci_set_drvdata(pdev, NULL);
@@ -365,7 +362,7 @@ static ssize_t sdram_offsets_show(struct device *dev,
 }
 
 static ssize_t sdram_show(struct file *file, struct kobject *kobj,
-			  struct bin_attribute *a, char *buf,
+			  const struct bin_attribute *a, char *buf,
 			  loff_t off, size_t count)
 {
 	struct device *dev = kobj_to_dev(kobj);
@@ -435,7 +432,7 @@ static int solo_sysfs_init(struct solo_dev *solo_dev)
 	sysfs_attr_init(&sdram_attr->attr);
 	sdram_attr->attr.name = "sdram";
 	sdram_attr->attr.mode = 0440;
-	sdram_attr->read = sdram_show;
+	sdram_attr->read_new = sdram_show;
 	sdram_attr->size = solo_dev->sdram_size;
 
 	if (device_create_bin_file(dev, sdram_attr)) {
@@ -480,16 +477,11 @@ static int solo_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	pci_write_config_byte(pdev, 0x40, 0x00);
 	pci_write_config_byte(pdev, 0x41, 0x00);
 
-	ret = pci_request_regions(pdev, SOLO6X10_NAME);
+	solo_dev->reg_base = pcim_iomap_region(pdev, 0, SOLO6X10_NAME);
+	ret = PTR_ERR_OR_ZERO(solo_dev->reg_base);
 	if (ret)
 		goto fail_probe;
 
-	solo_dev->reg_base = pci_ioremap_bar(pdev, 0);
-	if (solo_dev->reg_base == NULL) {
-		ret = -ENOMEM;
-		goto fail_probe;
-	}
-
 	chip_id = solo_reg_read(solo_dev, SOLO_CHIP_OPTION) &
 				SOLO_CHIP_ID_MASK;
 	switch (chip_id) {
@@ -551,8 +543,8 @@ static int solo_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* PLL locking time of 1ms */
 	mdelay(1);
 
-	ret = request_irq(pdev->irq, solo_isr, IRQF_SHARED, SOLO6X10_NAME,
-			  solo_dev);
+	ret = devm_request_irq(&pdev->dev, pdev->irq, solo_isr, IRQF_SHARED,
+			       SOLO6X10_NAME, solo_dev);
 	if (ret)
 		goto fail_probe;
 
diff --git a/drivers/media/pci/solo6x10/solo6x10-p2m.c b/drivers/media/pci/solo6x10/solo6x10-p2m.c
index ca70a864a3ef..5f100e5e03d9 100644
--- a/drivers/media/pci/solo6x10/solo6x10-p2m.c
+++ b/drivers/media/pci/solo6x10/solo6x10-p2m.c
@@ -57,7 +57,7 @@ int solo_p2m_dma_desc(struct solo_dev *solo_dev,
 		      int desc_cnt)
 {
 	struct solo_p2m_dev *p2m_dev;
-	unsigned int timeout;
+	unsigned long time_left;
 	unsigned int config = 0;
 	int ret = 0;
 	unsigned int p2m_id = 0;
@@ -99,12 +99,12 @@ int solo_p2m_dma_desc(struct solo_dev *solo_dev,
 			       desc[1].ctrl);
 	}
 
-	timeout = wait_for_completion_timeout(&p2m_dev->completion,
-					      solo_dev->p2m_jiffies);
+	time_left = wait_for_completion_timeout(&p2m_dev->completion,
+						solo_dev->p2m_jiffies);
 
 	if (WARN_ON_ONCE(p2m_dev->error))
 		ret = -EIO;
-	else if (timeout == 0) {
+	else if (time_left == 0) {
 		solo_dev->p2m_timeouts++;
 		ret = -EAGAIN;
 	}
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
index 0adf3d80f248..5ee59b3844cc 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
@@ -756,8 +756,6 @@ static const struct vb2_ops solo_enc_video_qops = {
 	.buf_finish	= solo_enc_buf_finish,
 	.start_streaming = solo_enc_start_streaming,
 	.stop_streaming = solo_enc_stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 static int solo_enc_querycap(struct file *file, void  *priv,
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2.c b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
index e18cc41fca83..35715b21dbdf 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
@@ -365,8 +365,6 @@ static const struct vb2_ops solo_video_qops = {
 	.buf_queue	= solo_buf_queue,
 	.start_streaming = solo_start_streaming,
 	.stop_streaming = solo_stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 static int solo_querycap(struct file *file, void  *priv,
diff --git a/drivers/media/pci/sta2x11/Kconfig b/drivers/media/pci/sta2x11/Kconfig
deleted file mode 100644
index 118b922c08c3..000000000000
--- a/drivers/media/pci/sta2x11/Kconfig
+++ /dev/null
@@ -1,16 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0-only
-config STA2X11_VIP
-	tristate "STA2X11 VIP Video For Linux"
-	depends on PCI && VIDEO_DEV && I2C
-	depends on STA2X11 || COMPILE_TEST
-	select GPIOLIB if MEDIA_SUBDRV_AUTOSELECT
-	select VIDEO_ADV7180 if MEDIA_SUBDRV_AUTOSELECT
-	select VIDEOBUF2_DMA_CONTIG
-	select MEDIA_CONTROLLER
-	select VIDEO_V4L2_SUBDEV_API
-	help
-	  Say Y for support for STA2X11 VIP (Video Input Port) capture
-	  device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called sta2x11_vip.
diff --git a/drivers/media/pci/sta2x11/Makefile b/drivers/media/pci/sta2x11/Makefile
deleted file mode 100644
index bb684a7b6270..000000000000
--- a/drivers/media/pci/sta2x11/Makefile
+++ /dev/null
@@ -1,2 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0-only
-obj-$(CONFIG_STA2X11_VIP) += sta2x11_vip.o
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
deleted file mode 100644
index 364ce9e57018..000000000000
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ /dev/null
@@ -1,1272 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- * This is the driver for the STA2x11 Video Input Port.
- *
- * Copyright (C) 2012       ST Microelectronics
- *     author: Federico Vaga <federico.vaga@gmail.com>
- * Copyright (C) 2010       WindRiver Systems, Inc.
- *     authors: Andreas Kies <andreas.kies@windriver.com>
- *              Vlad Lungu   <vlad.lungu@windriver.com>
- */
-
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/videodev2.h>
-#include <linux/kmod.h>
-#include <linux/pci.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/gpio/consumer.h>
-#include <linux/gpio.h>
-#include <linux/i2c.h>
-#include <linux/delay.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-fh.h>
-#include <media/v4l2-event.h>
-#include <media/videobuf2-dma-contig.h>
-
-#include "sta2x11_vip.h"
-
-#define DRV_VERSION "1.3"
-
-#ifndef PCI_DEVICE_ID_STMICRO_VIP
-#define PCI_DEVICE_ID_STMICRO_VIP 0xCC0D
-#endif
-
-#define MAX_FRAMES 4
-
-/*Register offsets*/
-#define DVP_CTL		0x00
-#define DVP_TFO		0x04
-#define DVP_TFS		0x08
-#define DVP_BFO		0x0C
-#define DVP_BFS		0x10
-#define DVP_VTP		0x14
-#define DVP_VBP		0x18
-#define DVP_VMP		0x1C
-#define DVP_ITM		0x98
-#define DVP_ITS		0x9C
-#define DVP_STA		0xA0
-#define DVP_HLFLN	0xA8
-#define DVP_RGB		0xC0
-#define DVP_PKZ		0xF0
-
-/*Register fields*/
-#define DVP_CTL_ENA	0x00000001
-#define DVP_CTL_RST	0x80000000
-#define DVP_CTL_DIS	(~0x00040001)
-
-#define DVP_IT_VSB	0x00000008
-#define DVP_IT_VST	0x00000010
-#define DVP_IT_FIFO	0x00000020
-
-#define DVP_HLFLN_SD	0x00000001
-
-#define SAVE_COUNT 8
-#define AUX_COUNT 3
-#define IRQ_COUNT 1
-
-
-struct vip_buffer {
-	struct vb2_v4l2_buffer vb;
-	struct list_head	list;
-	dma_addr_t		dma;
-};
-static inline struct vip_buffer *to_vip_buffer(struct vb2_v4l2_buffer *vb2)
-{
-	return container_of(vb2, struct vip_buffer, vb);
-}
-
-/**
- * struct sta2x11_vip - All internal data for one instance of device
- * @v4l2_dev: device registered in v4l layer
- * @video_dev: properties of our device
- * @pdev: PCI device
- * @adapter: contains I2C adapter information
- * @register_save_area: All relevant register are saved here during suspend
- * @decoder: contains information about video DAC
- * @ctrl_hdl: handler for control framework
- * @format: pixel format, fixed UYVY
- * @std: video standard (e.g. PAL/NTSC)
- * @input: input line for video signal ( 0 or 1 )
- * @disabled: Device is in power down state
- * @slock: for excluse access of registers
- * @vb_vidq: queue maintained by videobuf2 layer
- * @buffer_list: list of buffer in use
- * @sequence: sequence number of acquired buffer
- * @active: current active buffer
- * @lock: used in videobuf2 callback
- * @v4l_lock: serialize its video4linux ioctls
- * @tcount: Number of top frames
- * @bcount: Number of bottom frames
- * @overflow: Number of FIFO overflows
- * @iomem: hardware base address
- * @config: I2C and gpio config from platform
- *
- * All non-local data is accessed via this structure.
- */
-struct sta2x11_vip {
-	struct v4l2_device v4l2_dev;
-	struct video_device video_dev;
-	struct pci_dev *pdev;
-	struct i2c_adapter *adapter;
-	unsigned int register_save_area[IRQ_COUNT + SAVE_COUNT + AUX_COUNT];
-	struct v4l2_subdev *decoder;
-	struct v4l2_ctrl_handler ctrl_hdl;
-
-
-	struct v4l2_pix_format format;
-	v4l2_std_id std;
-	unsigned int input;
-	int disabled;
-	spinlock_t slock;
-
-	struct vb2_queue vb_vidq;
-	struct list_head buffer_list;
-	unsigned int sequence;
-	struct vip_buffer *active; /* current active buffer */
-	spinlock_t lock; /* Used in videobuf2 callback */
-	struct mutex v4l_lock;
-
-	/* Interrupt counters */
-	int tcount, bcount;
-	int overflow;
-
-	void __iomem *iomem;	/* I/O Memory */
-	struct vip_config *config;
-};
-
-static const unsigned int registers_to_save[AUX_COUNT] = {
-	DVP_HLFLN, DVP_RGB, DVP_PKZ
-};
-
-static struct v4l2_pix_format formats_50[] = {
-	{			/*PAL interlaced */
-	 .width = 720,
-	 .height = 576,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_INTERLACED,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 576,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-	{			/*PAL top */
-	 .width = 720,
-	 .height = 288,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_TOP,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 288,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-	{			/*PAL bottom */
-	 .width = 720,
-	 .height = 288,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_BOTTOM,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 288,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-
-};
-
-static struct v4l2_pix_format formats_60[] = {
-	{			/*NTSC interlaced */
-	 .width = 720,
-	 .height = 480,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_INTERLACED,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 480,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-	{			/*NTSC top */
-	 .width = 720,
-	 .height = 240,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_TOP,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 240,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-	{			/*NTSC bottom */
-	 .width = 720,
-	 .height = 240,
-	 .pixelformat = V4L2_PIX_FMT_UYVY,
-	 .field = V4L2_FIELD_BOTTOM,
-	 .bytesperline = 720 * 2,
-	 .sizeimage = 720 * 2 * 240,
-	 .colorspace = V4L2_COLORSPACE_SMPTE170M},
-};
-
-/* Write VIP register */
-static inline void reg_write(struct sta2x11_vip *vip, unsigned int reg, u32 val)
-{
-	iowrite32((val), (vip->iomem)+(reg));
-}
-/* Read VIP register */
-static inline u32 reg_read(struct sta2x11_vip *vip, unsigned int reg)
-{
-	return  ioread32((vip->iomem)+(reg));
-}
-/* Start DMA acquisition */
-static void start_dma(struct sta2x11_vip *vip, struct vip_buffer *vip_buf)
-{
-	unsigned long offset = 0;
-
-	if (vip->format.field == V4L2_FIELD_INTERLACED)
-		offset = vip->format.width * 2;
-
-	spin_lock_irq(&vip->slock);
-	/* Enable acquisition */
-	reg_write(vip, DVP_CTL, reg_read(vip, DVP_CTL) | DVP_CTL_ENA);
-	/* Set Top and Bottom Field memory address */
-	reg_write(vip, DVP_VTP, (u32)vip_buf->dma);
-	reg_write(vip, DVP_VBP, (u32)vip_buf->dma + offset);
-	spin_unlock_irq(&vip->slock);
-}
-
-/* Fetch the next buffer to activate */
-static void vip_active_buf_next(struct sta2x11_vip *vip)
-{
-	/* Get the next buffer */
-	spin_lock(&vip->lock);
-	if (list_empty(&vip->buffer_list)) {/* No available buffer */
-		spin_unlock(&vip->lock);
-		return;
-	}
-	vip->active = list_first_entry(&vip->buffer_list,
-				       struct vip_buffer,
-				       list);
-	/* Reset Top and Bottom counter */
-	vip->tcount = 0;
-	vip->bcount = 0;
-	spin_unlock(&vip->lock);
-	if (vb2_is_streaming(&vip->vb_vidq)) {	/* streaming is on */
-		start_dma(vip, vip->active);	/* start dma capture */
-	}
-}
-
-
-/* Videobuf2 Operations */
-static int queue_setup(struct vb2_queue *vq,
-		       unsigned int *nbuffers, unsigned int *nplanes,
-		       unsigned int sizes[], struct device *alloc_devs[])
-{
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vq);
-
-	if (!(*nbuffers) || *nbuffers < MAX_FRAMES)
-		*nbuffers = MAX_FRAMES;
-
-	*nplanes = 1;
-	sizes[0] = vip->format.sizeimage;
-
-	vip->sequence = 0;
-	vip->active = NULL;
-	vip->tcount = 0;
-	vip->bcount = 0;
-
-	return 0;
-};
-static int buffer_init(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct vip_buffer *vip_buf = to_vip_buffer(vbuf);
-
-	vip_buf->dma = vb2_dma_contig_plane_dma_addr(vb, 0);
-	INIT_LIST_HEAD(&vip_buf->list);
-	return 0;
-}
-
-static int buffer_prepare(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vb->vb2_queue);
-	struct vip_buffer *vip_buf = to_vip_buffer(vbuf);
-	unsigned long size;
-
-	size = vip->format.sizeimage;
-	if (vb2_plane_size(vb, 0) < size) {
-		v4l2_err(&vip->v4l2_dev, "buffer too small (%lu < %lu)\n",
-			 vb2_plane_size(vb, 0), size);
-		return -EINVAL;
-	}
-
-	vb2_set_plane_payload(&vip_buf->vb.vb2_buf, 0, size);
-
-	return 0;
-}
-static void buffer_queue(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vb->vb2_queue);
-	struct vip_buffer *vip_buf = to_vip_buffer(vbuf);
-
-	spin_lock(&vip->lock);
-	list_add_tail(&vip_buf->list, &vip->buffer_list);
-	if (!vip->active) {	/* No active buffer, active the first one */
-		vip->active = list_first_entry(&vip->buffer_list,
-					       struct vip_buffer,
-					       list);
-		if (vb2_is_streaming(&vip->vb_vidq))	/* streaming is on */
-			start_dma(vip, vip_buf);	/* start dma capture */
-	}
-	spin_unlock(&vip->lock);
-}
-static void buffer_finish(struct vb2_buffer *vb)
-{
-	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vb->vb2_queue);
-	struct vip_buffer *vip_buf = to_vip_buffer(vbuf);
-
-	/* Buffer handled, remove it from the list */
-	spin_lock(&vip->lock);
-	list_del_init(&vip_buf->list);
-	spin_unlock(&vip->lock);
-
-	if (vb2_is_streaming(vb->vb2_queue))
-		vip_active_buf_next(vip);
-}
-
-static int start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vq);
-
-	spin_lock_irq(&vip->slock);
-	/* Enable interrupt VSYNC Top and Bottom*/
-	reg_write(vip, DVP_ITM, DVP_IT_VSB | DVP_IT_VST);
-	spin_unlock_irq(&vip->slock);
-
-	if (count)
-		start_dma(vip, vip->active);
-
-	return 0;
-}
-
-/* abort streaming and wait for last buffer */
-static void stop_streaming(struct vb2_queue *vq)
-{
-	struct sta2x11_vip *vip = vb2_get_drv_priv(vq);
-	struct vip_buffer *vip_buf, *node;
-
-	/* Disable acquisition */
-	reg_write(vip, DVP_CTL, reg_read(vip, DVP_CTL) & ~DVP_CTL_ENA);
-	/* Disable all interrupts */
-	reg_write(vip, DVP_ITM, 0);
-
-	/* Release all active buffers */
-	spin_lock(&vip->lock);
-	list_for_each_entry_safe(vip_buf, node, &vip->buffer_list, list) {
-		vb2_buffer_done(&vip_buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-		list_del(&vip_buf->list);
-	}
-	spin_unlock(&vip->lock);
-}
-
-static const struct vb2_ops vip_video_qops = {
-	.queue_setup		= queue_setup,
-	.buf_init		= buffer_init,
-	.buf_prepare		= buffer_prepare,
-	.buf_finish		= buffer_finish,
-	.buf_queue		= buffer_queue,
-	.start_streaming	= start_streaming,
-	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
-};
-
-
-/* File Operations */
-static const struct v4l2_file_operations vip_fops = {
-	.owner = THIS_MODULE,
-	.open = v4l2_fh_open,
-	.release = vb2_fop_release,
-	.unlocked_ioctl = video_ioctl2,
-	.read = vb2_fop_read,
-	.mmap = vb2_fop_mmap,
-	.poll = vb2_fop_poll
-};
-
-
-/**
- * vidioc_querycap - return capabilities of device
- * @file: descriptor of device
- * @cap: contains return values
- * @priv: unused
- *
- * the capabilities of the device are returned
- *
- * return value: 0, no error.
- */
-static int vidioc_querycap(struct file *file, void *priv,
-			   struct v4l2_capability *cap)
-{
-	strscpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
-	strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
-	return 0;
-}
-
-/**
- * vidioc_s_std - set video standard
- * @file: descriptor of device
- * @std: contains standard to be set
- * @priv: unused
- *
- * the video standard is set
- *
- * return value: 0, no error.
- *
- * -EIO, no input signal detected
- *
- * other, returned from video DAC.
- */
-static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id std)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-
-	/*
-	 * This is here for backwards compatibility only.
-	 * The use of V4L2_STD_ALL to trigger a querystd is non-standard.
-	 */
-	if (std == V4L2_STD_ALL) {
-		v4l2_subdev_call(vip->decoder, video, querystd, &std);
-		if (std == V4L2_STD_UNKNOWN)
-			return -EIO;
-	}
-
-	if (vip->std != std) {
-		vip->std = std;
-		if (V4L2_STD_525_60 & std)
-			vip->format = formats_60[0];
-		else
-			vip->format = formats_50[0];
-	}
-
-	return v4l2_subdev_call(vip->decoder, video, s_std, std);
-}
-
-/**
- * vidioc_g_std - get video standard
- * @file: descriptor of device
- * @priv: unused
- * @std: contains return values
- *
- * the current video standard is returned
- *
- * return value: 0, no error.
- */
-static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *std)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-
-	*std = vip->std;
-	return 0;
-}
-
-/**
- * vidioc_querystd - get possible video standards
- * @file: descriptor of device
- * @priv: unused
- * @std: contains return values
- *
- * all possible video standards are returned
- *
- * return value: delivered by video DAC routine.
- */
-static int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *std)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-
-	return v4l2_subdev_call(vip->decoder, video, querystd, std);
-}
-
-static int vidioc_enum_input(struct file *file, void *priv,
-			     struct v4l2_input *inp)
-{
-	if (inp->index > 1)
-		return -EINVAL;
-
-	inp->type = V4L2_INPUT_TYPE_CAMERA;
-	inp->std = V4L2_STD_ALL;
-	sprintf(inp->name, "Camera %u", inp->index);
-
-	return 0;
-}
-
-/**
- * vidioc_s_input - set input line
- * @file: descriptor of device
- * @priv: unused
- * @i: new input line number
- *
- * the current active input line is set
- *
- * return value: 0, no error.
- *
- * -EINVAL, line number out of range
- */
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-	int ret;
-
-	if (i > 1)
-		return -EINVAL;
-	ret = v4l2_subdev_call(vip->decoder, video, s_routing, i, 0, 0);
-
-	if (!ret)
-		vip->input = i;
-
-	return 0;
-}
-
-/**
- * vidioc_g_input - return input line
- * @file: descriptor of device
- * @priv: unused
- * @i: returned input line number
- *
- * the current active input line is returned
- *
- * return value: always 0.
- */
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-
-	*i = vip->input;
-	return 0;
-}
-
-/**
- * vidioc_enum_fmt_vid_cap - return video capture format
- * @file: descriptor of device
- * @priv: unused
- * @f: returned format information
- *
- * returns name and format of video capture
- * Only UYVY is supported by hardware.
- *
- * return value: always 0.
- */
-static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-				   struct v4l2_fmtdesc *f)
-{
-
-	if (f->index != 0)
-		return -EINVAL;
-
-	f->pixelformat = V4L2_PIX_FMT_UYVY;
-	return 0;
-}
-
-/**
- * vidioc_try_fmt_vid_cap - set video capture format
- * @file: descriptor of device
- * @priv: unused
- * @f: new format
- *
- * new video format is set which includes width and
- * field type. width is fixed to 720, no scaling.
- * Only UYVY is supported by this hardware.
- * the minimum height is 200, the maximum is 576 (PAL)
- *
- * return value: 0, no error
- *
- * -EINVAL, pixel or field format not supported
- *
- */
-static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-				  struct v4l2_format *f)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-	int interlace_lim;
-
-	if (V4L2_PIX_FMT_UYVY != f->fmt.pix.pixelformat) {
-		v4l2_warn(&vip->v4l2_dev, "Invalid format, only UYVY supported\n");
-		return -EINVAL;
-	}
-
-	if (V4L2_STD_525_60 & vip->std)
-		interlace_lim = 240;
-	else
-		interlace_lim = 288;
-
-	switch (f->fmt.pix.field) {
-	default:
-	case V4L2_FIELD_ANY:
-		if (interlace_lim < f->fmt.pix.height)
-			f->fmt.pix.field = V4L2_FIELD_INTERLACED;
-		else
-			f->fmt.pix.field = V4L2_FIELD_BOTTOM;
-		break;
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-		if (interlace_lim < f->fmt.pix.height)
-			f->fmt.pix.height = interlace_lim;
-		break;
-	case V4L2_FIELD_INTERLACED:
-		break;
-	}
-
-	/* It is the only supported format */
-	f->fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
-	f->fmt.pix.height &= ~1;
-	if (2 * interlace_lim < f->fmt.pix.height)
-		f->fmt.pix.height = 2 * interlace_lim;
-	if (200 > f->fmt.pix.height)
-		f->fmt.pix.height = 200;
-	f->fmt.pix.width = 720;
-	f->fmt.pix.bytesperline = f->fmt.pix.width * 2;
-	f->fmt.pix.sizeimage = f->fmt.pix.width * 2 * f->fmt.pix.height;
-	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	return 0;
-}
-
-/**
- * vidioc_s_fmt_vid_cap - set current video format parameters
- * @file: descriptor of device
- * @priv: unused
- * @f: returned format information
- *
- * set new capture format
- * return value: 0, no error
- *
- * other, delivered by video DAC routine.
- */
-static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-	unsigned int t_stop, b_stop, pitch;
-	int ret;
-
-	ret = vidioc_try_fmt_vid_cap(file, priv, f);
-	if (ret)
-		return ret;
-
-	if (vb2_is_busy(&vip->vb_vidq)) {
-		/* Can't change format during acquisition */
-		v4l2_err(&vip->v4l2_dev, "device busy\n");
-		return -EBUSY;
-	}
-	vip->format = f->fmt.pix;
-	switch (vip->format.field) {
-	case V4L2_FIELD_INTERLACED:
-		t_stop = ((vip->format.height / 2 - 1) << 16) |
-			 (2 * vip->format.width - 1);
-		b_stop = t_stop;
-		pitch = 4 * vip->format.width;
-		break;
-	case V4L2_FIELD_TOP:
-		t_stop = ((vip->format.height - 1) << 16) |
-			 (2 * vip->format.width - 1);
-		b_stop = (0 << 16) | (2 * vip->format.width - 1);
-		pitch = 2 * vip->format.width;
-		break;
-	case V4L2_FIELD_BOTTOM:
-		t_stop = (0 << 16) | (2 * vip->format.width - 1);
-		b_stop = (vip->format.height << 16) |
-			 (2 * vip->format.width - 1);
-		pitch = 2 * vip->format.width;
-		break;
-	default:
-		v4l2_err(&vip->v4l2_dev, "unknown field format\n");
-		return -EINVAL;
-	}
-
-	spin_lock_irq(&vip->slock);
-	/* Y-X Top Field Offset */
-	reg_write(vip, DVP_TFO, 0);
-	/* Y-X Bottom Field Offset */
-	reg_write(vip, DVP_BFO, 0);
-	/* Y-X Top Field Stop*/
-	reg_write(vip, DVP_TFS, t_stop);
-	/* Y-X Bottom Field Stop */
-	reg_write(vip, DVP_BFS, b_stop);
-	/* Video Memory Pitch */
-	reg_write(vip, DVP_VMP, pitch);
-	spin_unlock_irq(&vip->slock);
-
-	return 0;
-}
-
-/**
- * vidioc_g_fmt_vid_cap - get current video format parameters
- * @file: descriptor of device
- * @priv: unused
- * @f: contains format information
- *
- * returns current video format parameters
- *
- * return value: 0, always successful
- */
-static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct sta2x11_vip *vip = video_drvdata(file);
-
-	f->fmt.pix = vip->format;
-
-	return 0;
-}
-
-static const struct v4l2_ioctl_ops vip_ioctl_ops = {
-	.vidioc_querycap = vidioc_querycap,
-	/* FMT handling */
-	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
-	/* Buffer handlers */
-	.vidioc_create_bufs = vb2_ioctl_create_bufs,
-	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
-	.vidioc_reqbufs = vb2_ioctl_reqbufs,
-	.vidioc_querybuf = vb2_ioctl_querybuf,
-	.vidioc_qbuf = vb2_ioctl_qbuf,
-	.vidioc_dqbuf = vb2_ioctl_dqbuf,
-	/* Stream on/off */
-	.vidioc_streamon = vb2_ioctl_streamon,
-	.vidioc_streamoff = vb2_ioctl_streamoff,
-	/* Standard handling */
-	.vidioc_g_std = vidioc_g_std,
-	.vidioc_s_std = vidioc_s_std,
-	.vidioc_querystd = vidioc_querystd,
-	/* Input handling */
-	.vidioc_enum_input = vidioc_enum_input,
-	.vidioc_g_input = vidioc_g_input,
-	.vidioc_s_input = vidioc_s_input,
-	/* Log status ioctl */
-	.vidioc_log_status = v4l2_ctrl_log_status,
-	/* Event handling */
-	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
-	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
-};
-
-static const struct video_device video_dev_template = {
-	.name = KBUILD_MODNAME,
-	.release = video_device_release_empty,
-	.fops = &vip_fops,
-	.ioctl_ops = &vip_ioctl_ops,
-	.tvnorms = V4L2_STD_ALL,
-	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
-		       V4L2_CAP_STREAMING,
-};
-
-/**
- * vip_irq - interrupt routine
- * @irq: Number of interrupt ( not used, correct number is assumed )
- * @data: local data structure containing all information
- *
- * check for both frame interrupts set ( top and bottom ).
- * check FIFO overflow, but limit number of log messages after open.
- * signal a complete buffer if done
- *
- * return value: IRQ_NONE, interrupt was not generated by VIP
- *
- * IRQ_HANDLED, interrupt done.
- */
-static irqreturn_t vip_irq(int irq, void *data)
-{
-	struct sta2x11_vip *vip = data;
-	unsigned int status;
-
-	status = reg_read(vip, DVP_ITS);
-
-	if (!status)		/* No interrupt to handle */
-		return IRQ_NONE;
-
-	if (status & DVP_IT_FIFO)
-		if (vip->overflow++ > 5)
-			pr_info("VIP: fifo overflow\n");
-
-	if ((status & DVP_IT_VST) && (status & DVP_IT_VSB)) {
-		/* this is bad, we are too slow, hope the condition is gone
-		 * on the next frame */
-		return IRQ_HANDLED;
-	}
-
-	if (status & DVP_IT_VST)
-		if ((++vip->tcount) < 2)
-			return IRQ_HANDLED;
-	if (status & DVP_IT_VSB) {
-		vip->bcount++;
-		return IRQ_HANDLED;
-	}
-
-	if (vip->active) { /* Acquisition is over on this buffer */
-		/* Disable acquisition */
-		reg_write(vip, DVP_CTL, reg_read(vip, DVP_CTL) & ~DVP_CTL_ENA);
-		/* Remove the active buffer from the list */
-		vip->active->vb.vb2_buf.timestamp = ktime_get_ns();
-		vip->active->vb.sequence = vip->sequence++;
-		vb2_buffer_done(&vip->active->vb.vb2_buf, VB2_BUF_STATE_DONE);
-	}
-
-	return IRQ_HANDLED;
-}
-
-static void sta2x11_vip_init_register(struct sta2x11_vip *vip)
-{
-	/* Register initialization */
-	spin_lock_irq(&vip->slock);
-	/* Clean interrupt */
-	reg_read(vip, DVP_ITS);
-	/* Enable Half Line per vertical */
-	reg_write(vip, DVP_HLFLN, DVP_HLFLN_SD);
-	/* Reset VIP control */
-	reg_write(vip, DVP_CTL, DVP_CTL_RST);
-	/* Clear VIP control */
-	reg_write(vip, DVP_CTL, 0);
-	spin_unlock_irq(&vip->slock);
-}
-static void sta2x11_vip_clear_register(struct sta2x11_vip *vip)
-{
-	spin_lock_irq(&vip->slock);
-	/* Disable interrupt */
-	reg_write(vip, DVP_ITM, 0);
-	/* Reset VIP Control */
-	reg_write(vip, DVP_CTL, DVP_CTL_RST);
-	/* Clear VIP Control */
-	reg_write(vip, DVP_CTL, 0);
-	/* Clean VIP Interrupt */
-	reg_read(vip, DVP_ITS);
-	spin_unlock_irq(&vip->slock);
-}
-static int sta2x11_vip_init_buffer(struct sta2x11_vip *vip)
-{
-	int err;
-
-	err = dma_set_coherent_mask(&vip->pdev->dev, DMA_BIT_MASK(29));
-	if (err) {
-		v4l2_err(&vip->v4l2_dev, "Cannot configure coherent mask");
-		return err;
-	}
-	memset(&vip->vb_vidq, 0, sizeof(struct vb2_queue));
-	vip->vb_vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	vip->vb_vidq.io_modes = VB2_MMAP | VB2_READ;
-	vip->vb_vidq.drv_priv = vip;
-	vip->vb_vidq.buf_struct_size = sizeof(struct vip_buffer);
-	vip->vb_vidq.ops = &vip_video_qops;
-	vip->vb_vidq.mem_ops = &vb2_dma_contig_memops;
-	vip->vb_vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	vip->vb_vidq.dev = &vip->pdev->dev;
-	vip->vb_vidq.lock = &vip->v4l_lock;
-	err = vb2_queue_init(&vip->vb_vidq);
-	if (err)
-		return err;
-	INIT_LIST_HEAD(&vip->buffer_list);
-	spin_lock_init(&vip->lock);
-	return 0;
-}
-
-static int sta2x11_vip_init_controls(struct sta2x11_vip *vip)
-{
-	/*
-	 * Inititialize an empty control so VIP can inerithing controls
-	 * from ADV7180
-	 */
-	v4l2_ctrl_handler_init(&vip->ctrl_hdl, 0);
-
-	vip->v4l2_dev.ctrl_handler = &vip->ctrl_hdl;
-	if (vip->ctrl_hdl.error) {
-		int err = vip->ctrl_hdl.error;
-
-		v4l2_ctrl_handler_free(&vip->ctrl_hdl);
-		return err;
-	}
-
-	return 0;
-}
-
-/**
- * vip_gpio_reserve - reserve gpio pin
- * @dev: device
- * @pin: GPIO pin number
- * @dir: direction, input or output
- * @name: GPIO pin name
- *
- */
-static int vip_gpio_reserve(struct device *dev, int pin, int dir,
-			    const char *name)
-{
-	struct gpio_desc *desc = gpio_to_desc(pin);
-	int ret = -ENODEV;
-
-	if (!gpio_is_valid(pin))
-		return ret;
-
-	ret = gpio_request(pin, name);
-	if (ret) {
-		dev_err(dev, "Failed to allocate pin %d (%s)\n", pin, name);
-		return ret;
-	}
-
-	ret = gpiod_direction_output(desc, dir);
-	if (ret) {
-		dev_err(dev, "Failed to set direction for pin %d (%s)\n",
-			pin, name);
-		gpio_free(pin);
-		return ret;
-	}
-
-	ret = gpiod_export(desc, false);
-	if (ret) {
-		dev_err(dev, "Failed to export pin %d (%s)\n", pin, name);
-		gpio_free(pin);
-		return ret;
-	}
-
-	return 0;
-}
-
-/**
- * vip_gpio_release - release gpio pin
- * @dev: device
- * @pin: GPIO pin number
- * @name: GPIO pin name
- *
- */
-static void vip_gpio_release(struct device *dev, int pin, const char *name)
-{
-	if (gpio_is_valid(pin)) {
-		struct gpio_desc *desc = gpio_to_desc(pin);
-
-		dev_dbg(dev, "releasing pin %d (%s)\n",	pin, name);
-		gpiod_unexport(desc);
-		gpio_free(pin);
-	}
-}
-
-/**
- * sta2x11_vip_init_one - init one instance of video device
- * @pdev: PCI device
- * @ent: (not used)
- *
- * allocate reset pins for DAC.
- * Reset video DAC, this is done via reset line.
- * allocate memory for managing device
- * request interrupt
- * map IO region
- * register device
- * find and initialize video DAC
- *
- * return value: 0, no error
- *
- * -ENOMEM, no memory
- *
- * -ENODEV, device could not be detected or registered
- */
-static int sta2x11_vip_init_one(struct pci_dev *pdev,
-				const struct pci_device_id *ent)
-{
-	int ret;
-	struct sta2x11_vip *vip;
-	struct vip_config *config;
-
-	/* Check if hardware support 26-bit DMA */
-	if (dma_set_mask(&pdev->dev, DMA_BIT_MASK(26))) {
-		dev_err(&pdev->dev, "26-bit DMA addressing not available\n");
-		return -EINVAL;
-	}
-	/* Enable PCI */
-	ret = pci_enable_device(pdev);
-	if (ret)
-		return ret;
-
-	/* Get VIP platform data */
-	config = dev_get_platdata(&pdev->dev);
-	if (!config) {
-		dev_info(&pdev->dev, "VIP slot disabled\n");
-		ret = -EINVAL;
-		goto disable;
-	}
-
-	/* Power configuration */
-	ret = vip_gpio_reserve(&pdev->dev, config->pwr_pin, 0,
-			       config->pwr_name);
-	if (ret)
-		goto disable;
-
-	ret = vip_gpio_reserve(&pdev->dev, config->reset_pin, 0,
-			       config->reset_name);
-	if (ret) {
-		vip_gpio_release(&pdev->dev, config->pwr_pin,
-				 config->pwr_name);
-		goto disable;
-	}
-
-	if (gpio_is_valid(config->pwr_pin)) {
-		/* Datasheet says 5ms between PWR and RST */
-		usleep_range(5000, 25000);
-		gpio_direction_output(config->pwr_pin, 1);
-	}
-
-	if (gpio_is_valid(config->reset_pin)) {
-		/* Datasheet says 5ms between PWR and RST */
-		usleep_range(5000, 25000);
-		gpio_direction_output(config->reset_pin, 1);
-	}
-	usleep_range(5000, 25000);
-
-	/* Allocate a new VIP instance */
-	vip = kzalloc(sizeof(struct sta2x11_vip), GFP_KERNEL);
-	if (!vip) {
-		ret = -ENOMEM;
-		goto release_gpios;
-	}
-	vip->pdev = pdev;
-	vip->std = V4L2_STD_PAL;
-	vip->format = formats_50[0];
-	vip->config = config;
-	mutex_init(&vip->v4l_lock);
-
-	ret = sta2x11_vip_init_controls(vip);
-	if (ret)
-		goto free_mem;
-	ret = v4l2_device_register(&pdev->dev, &vip->v4l2_dev);
-	if (ret)
-		goto free_mem;
-
-	dev_dbg(&pdev->dev, "BAR #0 at 0x%lx 0x%lx irq %d\n",
-		(unsigned long)pci_resource_start(pdev, 0),
-		(unsigned long)pci_resource_len(pdev, 0), pdev->irq);
-
-	pci_set_master(pdev);
-
-	ret = pci_request_regions(pdev, KBUILD_MODNAME);
-	if (ret)
-		goto unreg;
-
-	vip->iomem = pci_iomap(pdev, 0, 0x100);
-	if (!vip->iomem) {
-		ret = -ENOMEM;
-		goto release;
-	}
-
-	pci_enable_msi(pdev);
-
-	/* Initialize buffer */
-	ret = sta2x11_vip_init_buffer(vip);
-	if (ret)
-		goto unmap;
-
-	spin_lock_init(&vip->slock);
-
-	ret = request_irq(pdev->irq, vip_irq, IRQF_SHARED, KBUILD_MODNAME, vip);
-	if (ret) {
-		dev_err(&pdev->dev, "request_irq failed\n");
-		ret = -ENODEV;
-		goto release_buf;
-	}
-
-	/* Initialize and register video device */
-	vip->video_dev = video_dev_template;
-	vip->video_dev.v4l2_dev = &vip->v4l2_dev;
-	vip->video_dev.queue = &vip->vb_vidq;
-	vip->video_dev.lock = &vip->v4l_lock;
-	video_set_drvdata(&vip->video_dev, vip);
-
-	ret = video_register_device(&vip->video_dev, VFL_TYPE_VIDEO, -1);
-	if (ret)
-		goto vrelease;
-
-	/* Get ADV7180 subdevice */
-	vip->adapter = i2c_get_adapter(vip->config->i2c_id);
-	if (!vip->adapter) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "no I2C adapter found\n");
-		goto vunreg;
-	}
-
-	vip->decoder = v4l2_i2c_new_subdev(&vip->v4l2_dev, vip->adapter,
-					   "adv7180", vip->config->i2c_addr,
-					   NULL);
-	if (!vip->decoder) {
-		ret = -ENODEV;
-		dev_err(&pdev->dev, "no decoder found\n");
-		goto vunreg;
-	}
-
-	i2c_put_adapter(vip->adapter);
-	v4l2_subdev_call(vip->decoder, core, init, 0);
-
-	sta2x11_vip_init_register(vip);
-
-	dev_info(&pdev->dev, "STA2X11 Video Input Port (VIP) loaded\n");
-	return 0;
-
-vunreg:
-	video_set_drvdata(&vip->video_dev, NULL);
-vrelease:
-	vb2_video_unregister_device(&vip->video_dev);
-	free_irq(pdev->irq, vip);
-release_buf:
-	pci_disable_msi(pdev);
-unmap:
-	pci_iounmap(pdev, vip->iomem);
-release:
-	pci_release_regions(pdev);
-unreg:
-	v4l2_device_unregister(&vip->v4l2_dev);
-free_mem:
-	kfree(vip);
-release_gpios:
-	vip_gpio_release(&pdev->dev, config->reset_pin, config->reset_name);
-	vip_gpio_release(&pdev->dev, config->pwr_pin, config->pwr_name);
-disable:
-	/*
-	 * do not call pci_disable_device on sta2x11 because it break all
-	 * other Bus masters on this EP
-	 */
-	return ret;
-}
-
-/**
- * sta2x11_vip_remove_one - release device
- * @pdev: PCI device
- *
- * Undo everything done in .._init_one
- *
- * unregister video device
- * free interrupt
- * unmap ioadresses
- * free memory
- * free GPIO pins
- */
-static void sta2x11_vip_remove_one(struct pci_dev *pdev)
-{
-	struct v4l2_device *v4l2_dev = pci_get_drvdata(pdev);
-	struct sta2x11_vip *vip =
-	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
-
-	sta2x11_vip_clear_register(vip);
-
-	video_set_drvdata(&vip->video_dev, NULL);
-	vb2_video_unregister_device(&vip->video_dev);
-	free_irq(pdev->irq, vip);
-	pci_disable_msi(pdev);
-	pci_iounmap(pdev, vip->iomem);
-	pci_release_regions(pdev);
-
-	v4l2_device_unregister(&vip->v4l2_dev);
-
-	vip_gpio_release(&pdev->dev, vip->config->pwr_pin,
-			 vip->config->pwr_name);
-	vip_gpio_release(&pdev->dev, vip->config->reset_pin,
-			 vip->config->reset_name);
-
-	kfree(vip);
-	/*
-	 * do not call pci_disable_device on sta2x11 because it break all
-	 * other Bus masters on this EP
-	 */
-}
-
-/**
- * sta2x11_vip_suspend - set device into power save mode
- * @dev_d: PCI device
- *
- * all relevant registers are saved and an attempt to set a new state is made.
- *
- * return value: 0 always indicate success,
- * even if device could not be disabled. (workaround for hardware problem)
- */
-static int __maybe_unused sta2x11_vip_suspend(struct device *dev_d)
-{
-	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev_d);
-	struct sta2x11_vip *vip =
-	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
-	unsigned long flags;
-	int i;
-
-	spin_lock_irqsave(&vip->slock, flags);
-	vip->register_save_area[0] = reg_read(vip, DVP_CTL);
-	reg_write(vip, DVP_CTL, vip->register_save_area[0] & DVP_CTL_DIS);
-	vip->register_save_area[SAVE_COUNT] = reg_read(vip, DVP_ITM);
-	reg_write(vip, DVP_ITM, 0);
-	for (i = 1; i < SAVE_COUNT; i++)
-		vip->register_save_area[i] = reg_read(vip, 4 * i);
-	for (i = 0; i < AUX_COUNT; i++)
-		vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i] =
-		    reg_read(vip, registers_to_save[i]);
-	spin_unlock_irqrestore(&vip->slock, flags);
-
-	vip->disabled = 1;
-
-	pr_info("VIP: suspend\n");
-	return 0;
-}
-
-/**
- * sta2x11_vip_resume - resume device operation
- * @dev_d : PCI device
- *
- * return value: 0, no error.
- *
- * other, could not set device to power on state.
- */
-static int __maybe_unused sta2x11_vip_resume(struct device *dev_d)
-{
-	struct v4l2_device *v4l2_dev = dev_get_drvdata(dev_d);
-	struct sta2x11_vip *vip =
-	    container_of(v4l2_dev, struct sta2x11_vip, v4l2_dev);
-	unsigned long flags;
-	int i;
-
-	pr_info("VIP: resume\n");
-
-	vip->disabled = 0;
-
-	spin_lock_irqsave(&vip->slock, flags);
-	for (i = 1; i < SAVE_COUNT; i++)
-		reg_write(vip, 4 * i, vip->register_save_area[i]);
-	for (i = 0; i < AUX_COUNT; i++)
-		reg_write(vip, registers_to_save[i],
-			  vip->register_save_area[SAVE_COUNT + IRQ_COUNT + i]);
-	reg_write(vip, DVP_CTL, vip->register_save_area[0]);
-	reg_write(vip, DVP_ITM, vip->register_save_area[SAVE_COUNT]);
-	spin_unlock_irqrestore(&vip->slock, flags);
-	return 0;
-}
-
-static const struct pci_device_id sta2x11_vip_pci_tbl[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_STMICRO, PCI_DEVICE_ID_STMICRO_VIP)},
-	{0,}
-};
-
-static SIMPLE_DEV_PM_OPS(sta2x11_vip_pm_ops,
-			 sta2x11_vip_suspend,
-			 sta2x11_vip_resume);
-
-static struct pci_driver sta2x11_vip_driver = {
-	.name = KBUILD_MODNAME,
-	.probe = sta2x11_vip_init_one,
-	.remove = sta2x11_vip_remove_one,
-	.id_table = sta2x11_vip_pci_tbl,
-	.driver.pm = &sta2x11_vip_pm_ops,
-};
-
-static int __init sta2x11_vip_init_module(void)
-{
-	return pci_register_driver(&sta2x11_vip_driver);
-}
-
-static void __exit sta2x11_vip_exit_module(void)
-{
-	pci_unregister_driver(&sta2x11_vip_driver);
-}
-
-#ifdef MODULE
-module_init(sta2x11_vip_init_module);
-module_exit(sta2x11_vip_exit_module);
-#else
-late_initcall_sync(sta2x11_vip_init_module);
-#endif
-
-MODULE_DESCRIPTION("STA2X11 Video Input Port driver");
-MODULE_AUTHOR("Wind River");
-MODULE_LICENSE("GPL v2");
-MODULE_VERSION(DRV_VERSION);
-MODULE_DEVICE_TABLE(pci, sta2x11_vip_pci_tbl);
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.h b/drivers/media/pci/sta2x11/sta2x11_vip.h
deleted file mode 100644
index de6000e7943e..000000000000
--- a/drivers/media/pci/sta2x11/sta2x11_vip.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- * Copyright (c) 2011 Wind River Systems, Inc.
- *
- * Author:  Anders Wallin <anders.wallin@windriver.com>
- */
-
-#ifndef __STA2X11_VIP_H
-#define __STA2X11_VIP_H
-
-/**
- * struct vip_config - video input configuration data
- * @pwr_name: ADV powerdown name
- * @pwr_pin: ADV powerdown pin
- * @reset_name: ADV reset name
- * @reset_pin: ADV reset pin
- * @i2c_id: ADV i2c adapter ID
- * @i2c_addr: ADV i2c address
- */
-struct vip_config {
-	const char *pwr_name;
-	int pwr_pin;
-	const char *reset_name;
-	int reset_pin;
-	int i2c_id;
-	int i2c_addr;
-};
-
-#endif /* __STA2X11_VIP_H */
diff --git a/drivers/media/pci/ttpci/budget-av.c b/drivers/media/pci/ttpci/budget-av.c
index a47c5850ef87..69f5e810f9b5 100644
--- a/drivers/media/pci/ttpci/budget-av.c
+++ b/drivers/media/pci/ttpci/budget-av.c
@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 /*
- * budget-av.c: driver for the SAA7146 based Budget DVB cards
- *              with analog video in
+ * budget-av.ko: driver for the SAA7146 based Budget DVB cards
+ *               with analog video input (and optionally with CI)
  *
  * Compiled from various sources by Michael Hunold <michael@mihu.de>
  *
@@ -16,7 +16,6 @@
  * the project's page is at https://linuxtv.org
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include "budget.h"
 #include "stv0299.h"
@@ -37,6 +36,7 @@
 #include <linux/interrupt.h>
 #include <linux/input.h>
 #include <linux/spinlock.h>
+#include <linux/workqueue.h>
 
 #include <media/dvb_ca_en50221.h>
 
@@ -55,7 +55,7 @@ struct budget_av {
 	struct video_device vd;
 	int cur_input;
 	int has_saa7113;
-	struct tasklet_struct ciintf_irq_tasklet;
+	struct work_struct ciintf_irq_bh_work;
 	int slot_status;
 	struct dvb_ca_en50221 ca;
 	u8 reinitialise_demod:1;
@@ -63,8 +63,8 @@ struct budget_av {
 
 static int ciintf_slot_shutdown(struct dvb_ca_en50221 *ca, int slot);
 
-
-/* GPIO Connections:
+/*
+ * GPIO Connections:
  * 0 - Vcc/Reset (Reset is controlled by capacitor). Resets the frontend *AS WELL*!
  * 1 - CI memory select 0=>IO memory, 1=>Attribute Memory
  * 2 - CI Card Enable (Active Low)
@@ -95,12 +95,12 @@ static u8 i2c_readreg(struct i2c_adapter *i2c, u8 id, u8 reg)
 	return mm2[0];
 }
 
-static int i2c_readregs(struct i2c_adapter *i2c, u8 id, u8 reg, u8 * buf, u8 len)
+static int i2c_readregs(struct i2c_adapter *i2c, u8 id, u8 reg, u8 *buf, u8 len)
 {
 	u8 mm1[] = { reg };
 	struct i2c_msg msgs[2] = {
-		{.addr = id / 2,.flags = 0,.buf = mm1,.len = 1},
-		{.addr = id / 2,.flags = I2C_M_RD,.buf = buf,.len = len}
+		{.addr = id / 2, .flags = 0, .buf = mm1, .len = 1},
+		{.addr = id / 2, .flags = I2C_M_RD, .buf = buf, .len = len}
 	};
 
 	if (i2c_transfer(i2c, msgs, 2) != 2)
@@ -206,7 +206,7 @@ static int ciintf_slot_reset(struct dvb_ca_en50221 *ca, int slot)
 	if (slot != 0)
 		return -EINVAL;
 
-	dprintk(1, "ciintf_slot_reset\n");
+	dprintk(1, "ci slot reset\n");
 	budget_av->slot_status = SLOTSTATUS_RESET;
 
 	saa7146_setgpio(saa, 2, SAA7146_GPIO_OUTHI); /* disable card */
@@ -235,7 +235,7 @@ static int ciintf_slot_shutdown(struct dvb_ca_en50221 *ca, int slot)
 	if (slot != 0)
 		return -EINVAL;
 
-	dprintk(1, "ciintf_slot_shutdown\n");
+	dprintk(1, "ci slot shutdown\n");
 
 	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTB);
 	budget_av->slot_status = SLOTSTATUS_NONE;
@@ -251,7 +251,7 @@ static int ciintf_slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
 	if (slot != 0)
 		return -EINVAL;
 
-	dprintk(1, "ciintf_slot_ts_enable: %d\n", budget_av->slot_status);
+	dprintk(1, "ci slot status: %d\n", budget_av->slot_status);
 
 	ttpci_budget_set_video_port(saa, BUDGET_VIDEO_PORTA);
 
@@ -267,8 +267,10 @@ static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open
 	if (slot != 0)
 		return -EINVAL;
 
-	/* test the card detect line - needs to be done carefully
-	 * since it never goes high for some CAMs on this interface (e.g. topuptv) */
+	/*
+	 * test the card detect line - needs to be done carefully
+	 * since it never goes high for some CAMs on this interface (e.g. topuptv)
+	 */
 	if (budget_av->slot_status == SLOTSTATUS_NONE) {
 		saa7146_setgpio(saa, 3, SAA7146_GPIO_INPUT);
 		udelay(1);
@@ -281,12 +283,14 @@ static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open
 		saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTLO);
 	}
 
-	/* We also try and read from IO memory to work round the above detection bug. If
+	/*
+	 * We also try and read from IO memory to work round the above detection bug. If
 	 * there is no CAM, we will get a timeout. Only done if there is no cam
 	 * present, since this test actually breaks some cams :(
 	 *
 	 * if the CI interface is not open, we also do the above test since we
-	 * don't care if the cam has problems - we'll be resetting it on open() anyway */
+	 * don't care if the cam has problems - we'll be resetting it on open() anyway
+	 */
 	if ((budget_av->slot_status == SLOTSTATUS_NONE) || (!open)) {
 		saa7146_setgpio(budget_av->budget.dev, 1, SAA7146_GPIO_OUTLO);
 		result = ttpci_budget_debiread(&budget_av->budget, DEBICICAM, 0, 1, 0, 1);
@@ -305,16 +309,14 @@ static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open
 	/* read from attribute memory in reset/ready state to know when the CAM is ready */
 	if (budget_av->slot_status == SLOTSTATUS_RESET) {
 		result = ciintf_read_attribute_mem(ca, slot, 0);
-		if (result == 0x1d) {
+		if (result == 0x1d)
 			budget_av->slot_status = SLOTSTATUS_READY;
-		}
 	}
 
 	/* work out correct return code */
 	if (budget_av->slot_status != SLOTSTATUS_NONE) {
-		if (budget_av->slot_status & SLOTSTATUS_READY) {
+		if (budget_av->slot_status & SLOTSTATUS_READY)
 			return DVB_CA_EN50221_POLL_CAM_PRESENT | DVB_CA_EN50221_POLL_CAM_READY;
-		}
 		return DVB_CA_EN50221_POLL_CAM_PRESENT;
 	}
 	return 0;
@@ -349,8 +351,9 @@ static int ciintf_init(struct budget_av *budget_av)
 	budget_av->budget.ci_present = 1;
 	budget_av->slot_status = SLOTSTATUS_NONE;
 
-	if ((result = dvb_ca_en50221_init(&budget_av->budget.dvb_adapter,
-					  &budget_av->ca, 0, 1)) != 0) {
+	result = dvb_ca_en50221_init(&budget_av->budget.dvb_adapter,
+				     &budget_av->ca, 0, 1);
+	if (result != 0) {
 		pr_err("ci initialisation failed\n");
 		goto error;
 	}
@@ -439,7 +442,7 @@ static int saa7113_setinput(struct budget_av *budget_av, int input)
 {
 	struct budget *budget = &budget_av->budget;
 
-	if (1 != budget_av->has_saa7113)
+	if (budget_av->has_saa7113 != 1)
 		return -ENODEV;
 
 	if (input == 1) {
@@ -448,8 +451,9 @@ static int saa7113_setinput(struct budget_av *budget_av, int input)
 	} else if (input == 0) {
 		i2c_writereg(&budget->i2c_adap, 0x4a, 0x02, 0xc0);
 		i2c_writereg(&budget->i2c_adap, 0x4a, 0x09, 0x00);
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	budget_av->cur_input = input;
 	return 0;
@@ -492,7 +496,7 @@ static int philips_su1278_ty_ci_tuner_set_params(struct dvb_frontend *fe)
 	u32 div;
 	u8 buf[4];
 	struct budget *budget = fe->dvb->priv;
-	struct i2c_msg msg = {.addr = 0x61,.flags = 0,.buf = buf,.len = sizeof(buf) };
+	struct i2c_msg msg = {.addr = 0x61, .flags = 0, .buf = buf, .len = sizeof(buf) };
 
 	if ((c->frequency < 950000) || (c->frequency > 2150000))
 		return -EINVAL;
@@ -606,7 +610,7 @@ static int philips_cu1216_tuner_set_params(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct budget *budget = fe->dvb->priv;
 	u8 buf[6];
-	struct i2c_msg msg = {.addr = 0x60,.flags = 0,.buf = buf,.len = sizeof(buf) };
+	struct i2c_msg msg = {.addr = 0x60, .flags = 0, .buf = buf, .len = sizeof(buf) };
 	int i;
 
 #define CU1216_IF 36125000
@@ -670,7 +674,7 @@ static int philips_tu1216_tuner_init(struct dvb_frontend *fe)
 {
 	struct budget *budget = fe->dvb->priv;
 	static u8 tu1216_init[] = { 0x0b, 0xf5, 0x85, 0xab };
-	struct i2c_msg tuner_msg = {.addr = 0x60,.flags = 0,.buf = tu1216_init,.len = sizeof(tu1216_init) };
+	struct i2c_msg tuner_msg = {.addr = 0x60, .flags = 0, .buf = tu1216_init, .len = sizeof(tu1216_init) };
 
 	// setup PLL configuration
 	if (fe->ops.i2c_gate_ctrl)
@@ -687,7 +691,7 @@ static int philips_tu1216_tuner_set_params(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct budget *budget = fe->dvb->priv;
 	u8 tuner_buf[4];
-	struct i2c_msg tuner_msg = {.addr = 0x60,.flags = 0,.buf = tuner_buf,.len =
+	struct i2c_msg tuner_msg = {.addr = 0x60, .flags = 0, .buf = tuner_buf, .len =
 			sizeof(tuner_buf) };
 	int tuner_frequency = 0;
 	u8 band, cp, filter;
@@ -865,7 +869,7 @@ static int philips_sd1878_ci_set_symbol_rate(struct dvb_frontend *fe,
 
 static const struct stv0299_config philips_sd1878_config = {
 	.demod_address = 0x68,
-     .inittab = philips_sd1878_inittab,
+	.inittab = philips_sd1878_inittab,
 	.mclk = 88000000UL,
 	.invert = 0,
 	.skip_reinit = 0,
@@ -878,222 +882,222 @@ static const struct stv0299_config philips_sd1878_config = {
 /* KNC1 DVB-S (STB0899) Inittab	*/
 static const struct stb0899_s1_reg knc1_stb0899_s1_init_1[] = {
 
-	{ STB0899_DEV_ID		, 0x81 },
-	{ STB0899_DISCNTRL1		, 0x32 },
-	{ STB0899_DISCNTRL2		, 0x80 },
-	{ STB0899_DISRX_ST0		, 0x04 },
-	{ STB0899_DISRX_ST1		, 0x00 },
-	{ STB0899_DISPARITY		, 0x00 },
-	{ STB0899_DISSTATUS		, 0x20 },
-	{ STB0899_DISF22		, 0x8c },
-	{ STB0899_DISF22RX		, 0x9a },
-	{ STB0899_SYSREG		, 0x0b },
-	{ STB0899_ACRPRESC		, 0x11 },
-	{ STB0899_ACRDIV1		, 0x0a },
-	{ STB0899_ACRDIV2		, 0x05 },
-	{ STB0899_DACR1			, 0x00 },
-	{ STB0899_DACR2			, 0x00 },
-	{ STB0899_OUTCFG		, 0x00 },
-	{ STB0899_MODECFG		, 0x00 },
-	{ STB0899_IRQSTATUS_3		, 0x30 },
-	{ STB0899_IRQSTATUS_2		, 0x00 },
-	{ STB0899_IRQSTATUS_1		, 0x00 },
-	{ STB0899_IRQSTATUS_0		, 0x00 },
-	{ STB0899_IRQMSK_3		, 0xf3 },
-	{ STB0899_IRQMSK_2		, 0xfc },
-	{ STB0899_IRQMSK_1		, 0xff },
-	{ STB0899_IRQMSK_0		, 0xff },
-	{ STB0899_IRQCFG		, 0x00 },
-	{ STB0899_I2CCFG		, 0x88 },
-	{ STB0899_I2CRPT		, 0x58 }, /* Repeater=8, Stop=disabled */
-	{ STB0899_IOPVALUE5		, 0x00 },
-	{ STB0899_IOPVALUE4		, 0x20 },
-	{ STB0899_IOPVALUE3		, 0xc9 },
-	{ STB0899_IOPVALUE2		, 0x90 },
-	{ STB0899_IOPVALUE1		, 0x40 },
-	{ STB0899_IOPVALUE0		, 0x00 },
-	{ STB0899_GPIO00CFG		, 0x82 },
-	{ STB0899_GPIO01CFG		, 0x82 },
-	{ STB0899_GPIO02CFG		, 0x82 },
-	{ STB0899_GPIO03CFG		, 0x82 },
-	{ STB0899_GPIO04CFG		, 0x82 },
-	{ STB0899_GPIO05CFG		, 0x82 },
-	{ STB0899_GPIO06CFG		, 0x82 },
-	{ STB0899_GPIO07CFG		, 0x82 },
-	{ STB0899_GPIO08CFG		, 0x82 },
-	{ STB0899_GPIO09CFG		, 0x82 },
-	{ STB0899_GPIO10CFG		, 0x82 },
-	{ STB0899_GPIO11CFG		, 0x82 },
-	{ STB0899_GPIO12CFG		, 0x82 },
-	{ STB0899_GPIO13CFG		, 0x82 },
-	{ STB0899_GPIO14CFG		, 0x82 },
-	{ STB0899_GPIO15CFG		, 0x82 },
-	{ STB0899_GPIO16CFG		, 0x82 },
-	{ STB0899_GPIO17CFG		, 0x82 },
-	{ STB0899_GPIO18CFG		, 0x82 },
-	{ STB0899_GPIO19CFG		, 0x82 },
-	{ STB0899_GPIO20CFG		, 0x82 },
-	{ STB0899_SDATCFG		, 0xb8 },
-	{ STB0899_SCLTCFG		, 0xba },
-	{ STB0899_AGCRFCFG		, 0x08 }, /* 0x1c */
-	{ STB0899_GPIO22		, 0x82 }, /* AGCBB2CFG */
-	{ STB0899_GPIO21		, 0x91 }, /* AGCBB1CFG */
-	{ STB0899_DIRCLKCFG		, 0x82 },
-	{ STB0899_CLKOUT27CFG		, 0x7e },
-	{ STB0899_STDBYCFG		, 0x82 },
-	{ STB0899_CS0CFG		, 0x82 },
-	{ STB0899_CS1CFG		, 0x82 },
-	{ STB0899_DISEQCOCFG		, 0x20 },
-	{ STB0899_GPIO32CFG		, 0x82 },
-	{ STB0899_GPIO33CFG		, 0x82 },
-	{ STB0899_GPIO34CFG		, 0x82 },
-	{ STB0899_GPIO35CFG		, 0x82 },
-	{ STB0899_GPIO36CFG		, 0x82 },
-	{ STB0899_GPIO37CFG		, 0x82 },
-	{ STB0899_GPIO38CFG		, 0x82 },
-	{ STB0899_GPIO39CFG		, 0x82 },
-	{ STB0899_NCOARSE		, 0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
-	{ STB0899_SYNTCTRL		, 0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
-	{ STB0899_FILTCTRL		, 0x00 },
-	{ STB0899_SYSCTRL		, 0x00 },
-	{ STB0899_STOPCLK1		, 0x20 },
-	{ STB0899_STOPCLK2		, 0x00 },
-	{ STB0899_INTBUFSTATUS		, 0x00 },
-	{ STB0899_INTBUFCTRL		, 0x0a },
-	{ 0xffff			, 0xff },
+	{ STB0899_DEV_ID,		0x81 },
+	{ STB0899_DISCNTRL1,		0x32 },
+	{ STB0899_DISCNTRL2,		0x80 },
+	{ STB0899_DISRX_ST0,		0x04 },
+	{ STB0899_DISRX_ST1,		0x00 },
+	{ STB0899_DISPARITY,		0x00 },
+	{ STB0899_DISSTATUS,		0x20 },
+	{ STB0899_DISF22,		0x8c },
+	{ STB0899_DISF22RX,		0x9a },
+	{ STB0899_SYSREG,		0x0b },
+	{ STB0899_ACRPRESC,		0x11 },
+	{ STB0899_ACRDIV1,		0x0a },
+	{ STB0899_ACRDIV2,		0x05 },
+	{ STB0899_DACR1,		0x00 },
+	{ STB0899_DACR2,		0x00 },
+	{ STB0899_OUTCFG,		0x00 },
+	{ STB0899_MODECFG,		0x00 },
+	{ STB0899_IRQSTATUS_3,		0x30 },
+	{ STB0899_IRQSTATUS_2,		0x00 },
+	{ STB0899_IRQSTATUS_1,		0x00 },
+	{ STB0899_IRQSTATUS_0,		0x00 },
+	{ STB0899_IRQMSK_3,		0xf3 },
+	{ STB0899_IRQMSK_2,		0xfc },
+	{ STB0899_IRQMSK_1,		0xff },
+	{ STB0899_IRQMSK_0,		0xff },
+	{ STB0899_IRQCFG,		0x00 },
+	{ STB0899_I2CCFG,		0x88 },
+	{ STB0899_I2CRPT,		0x58 }, /* Repeater=8, Stop=disabled */
+	{ STB0899_IOPVALUE5,		0x00 },
+	{ STB0899_IOPVALUE4,		0x20 },
+	{ STB0899_IOPVALUE3,		0xc9 },
+	{ STB0899_IOPVALUE2,		0x90 },
+	{ STB0899_IOPVALUE1,		0x40 },
+	{ STB0899_IOPVALUE0,		0x00 },
+	{ STB0899_GPIO00CFG,		0x82 },
+	{ STB0899_GPIO01CFG,		0x82 },
+	{ STB0899_GPIO02CFG,		0x82 },
+	{ STB0899_GPIO03CFG,		0x82 },
+	{ STB0899_GPIO04CFG,		0x82 },
+	{ STB0899_GPIO05CFG,		0x82 },
+	{ STB0899_GPIO06CFG,		0x82 },
+	{ STB0899_GPIO07CFG,		0x82 },
+	{ STB0899_GPIO08CFG,		0x82 },
+	{ STB0899_GPIO09CFG,		0x82 },
+	{ STB0899_GPIO10CFG,		0x82 },
+	{ STB0899_GPIO11CFG,		0x82 },
+	{ STB0899_GPIO12CFG,		0x82 },
+	{ STB0899_GPIO13CFG,		0x82 },
+	{ STB0899_GPIO14CFG,		0x82 },
+	{ STB0899_GPIO15CFG,		0x82 },
+	{ STB0899_GPIO16CFG,		0x82 },
+	{ STB0899_GPIO17CFG,		0x82 },
+	{ STB0899_GPIO18CFG,		0x82 },
+	{ STB0899_GPIO19CFG,		0x82 },
+	{ STB0899_GPIO20CFG,		0x82 },
+	{ STB0899_SDATCFG,		0xb8 },
+	{ STB0899_SCLTCFG,		0xba },
+	{ STB0899_AGCRFCFG,		0x08 }, /* 0x1c */
+	{ STB0899_GPIO22,		0x82 }, /* AGCBB2CFG */
+	{ STB0899_GPIO21,		0x91 }, /* AGCBB1CFG */
+	{ STB0899_DIRCLKCFG,		0x82 },
+	{ STB0899_CLKOUT27CFG,		0x7e },
+	{ STB0899_STDBYCFG,		0x82 },
+	{ STB0899_CS0CFG,		0x82 },
+	{ STB0899_CS1CFG,		0x82 },
+	{ STB0899_DISEQCOCFG,		0x20 },
+	{ STB0899_GPIO32CFG,		0x82 },
+	{ STB0899_GPIO33CFG,		0x82 },
+	{ STB0899_GPIO34CFG,		0x82 },
+	{ STB0899_GPIO35CFG,		0x82 },
+	{ STB0899_GPIO36CFG,		0x82 },
+	{ STB0899_GPIO37CFG,		0x82 },
+	{ STB0899_GPIO38CFG,		0x82 },
+	{ STB0899_GPIO39CFG,		0x82 },
+	{ STB0899_NCOARSE,		0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
+	{ STB0899_SYNTCTRL,		0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
+	{ STB0899_FILTCTRL,		0x00 },
+	{ STB0899_SYSCTRL,		0x00 },
+	{ STB0899_STOPCLK1,		0x20 },
+	{ STB0899_STOPCLK2,		0x00 },
+	{ STB0899_INTBUFSTATUS,		0x00 },
+	{ STB0899_INTBUFCTRL,		0x0a },
+	{ 0xffff,			0xff },
 };
 
 static const struct stb0899_s1_reg knc1_stb0899_s1_init_3[] = {
-	{ STB0899_DEMOD			, 0x00 },
-	{ STB0899_RCOMPC		, 0xc9 },
-	{ STB0899_AGC1CN		, 0x41 },
-	{ STB0899_AGC1REF		, 0x08 },
-	{ STB0899_RTC			, 0x7a },
-	{ STB0899_TMGCFG		, 0x4e },
-	{ STB0899_AGC2REF		, 0x33 },
-	{ STB0899_TLSR			, 0x84 },
-	{ STB0899_CFD			, 0xee },
-	{ STB0899_ACLC			, 0x87 },
-	{ STB0899_BCLC			, 0x94 },
-	{ STB0899_EQON			, 0x41 },
-	{ STB0899_LDT			, 0xdd },
-	{ STB0899_LDT2			, 0xc9 },
-	{ STB0899_EQUALREF		, 0xb4 },
-	{ STB0899_TMGRAMP		, 0x10 },
-	{ STB0899_TMGTHD		, 0x30 },
-	{ STB0899_IDCCOMP		, 0xfb },
-	{ STB0899_QDCCOMP		, 0x03 },
-	{ STB0899_POWERI		, 0x3b },
-	{ STB0899_POWERQ		, 0x3d },
-	{ STB0899_RCOMP			, 0x81 },
-	{ STB0899_AGCIQIN		, 0x80 },
-	{ STB0899_AGC2I1		, 0x04 },
-	{ STB0899_AGC2I2		, 0xf5 },
-	{ STB0899_TLIR			, 0x25 },
-	{ STB0899_RTF			, 0x80 },
-	{ STB0899_DSTATUS		, 0x00 },
-	{ STB0899_LDI			, 0xca },
-	{ STB0899_CFRM			, 0xf1 },
-	{ STB0899_CFRL			, 0xf3 },
-	{ STB0899_NIRM			, 0x2a },
-	{ STB0899_NIRL			, 0x05 },
-	{ STB0899_ISYMB			, 0x17 },
-	{ STB0899_QSYMB			, 0xfa },
-	{ STB0899_SFRH			, 0x2f },
-	{ STB0899_SFRM			, 0x68 },
-	{ STB0899_SFRL			, 0x40 },
-	{ STB0899_SFRUPH		, 0x2f },
-	{ STB0899_SFRUPM		, 0x68 },
-	{ STB0899_SFRUPL		, 0x40 },
-	{ STB0899_EQUAI1		, 0xfd },
-	{ STB0899_EQUAQ1		, 0x04 },
-	{ STB0899_EQUAI2		, 0x0f },
-	{ STB0899_EQUAQ2		, 0xff },
-	{ STB0899_EQUAI3		, 0xdf },
-	{ STB0899_EQUAQ3		, 0xfa },
-	{ STB0899_EQUAI4		, 0x37 },
-	{ STB0899_EQUAQ4		, 0x0d },
-	{ STB0899_EQUAI5		, 0xbd },
-	{ STB0899_EQUAQ5		, 0xf7 },
-	{ STB0899_DSTATUS2		, 0x00 },
-	{ STB0899_VSTATUS		, 0x00 },
-	{ STB0899_VERROR		, 0xff },
-	{ STB0899_IQSWAP		, 0x2a },
-	{ STB0899_ECNT1M		, 0x00 },
-	{ STB0899_ECNT1L		, 0x00 },
-	{ STB0899_ECNT2M		, 0x00 },
-	{ STB0899_ECNT2L		, 0x00 },
-	{ STB0899_ECNT3M		, 0x00 },
-	{ STB0899_ECNT3L		, 0x00 },
-	{ STB0899_FECAUTO1		, 0x06 },
-	{ STB0899_FECM			, 0x01 },
-	{ STB0899_VTH12			, 0xf0 },
-	{ STB0899_VTH23			, 0xa0 },
-	{ STB0899_VTH34			, 0x78 },
-	{ STB0899_VTH56			, 0x4e },
-	{ STB0899_VTH67			, 0x48 },
-	{ STB0899_VTH78			, 0x38 },
-	{ STB0899_PRVIT			, 0xff },
-	{ STB0899_VITSYNC		, 0x19 },
-	{ STB0899_RSULC			, 0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
-	{ STB0899_TSULC			, 0x42 },
-	{ STB0899_RSLLC			, 0x40 },
-	{ STB0899_TSLPL			, 0x12 },
-	{ STB0899_TSCFGH		, 0x0c },
-	{ STB0899_TSCFGM		, 0x00 },
-	{ STB0899_TSCFGL		, 0x0c },
-	{ STB0899_TSOUT			, 0x4d }, /* 0x0d for CAM */
-	{ STB0899_RSSYNCDEL		, 0x00 },
-	{ STB0899_TSINHDELH		, 0x02 },
-	{ STB0899_TSINHDELM		, 0x00 },
-	{ STB0899_TSINHDELL		, 0x00 },
-	{ STB0899_TSLLSTKM		, 0x00 },
-	{ STB0899_TSLLSTKL		, 0x00 },
-	{ STB0899_TSULSTKM		, 0x00 },
-	{ STB0899_TSULSTKL		, 0xab },
-	{ STB0899_PCKLENUL		, 0x00 },
-	{ STB0899_PCKLENLL		, 0xcc },
-	{ STB0899_RSPCKLEN		, 0xcc },
-	{ STB0899_TSSTATUS		, 0x80 },
-	{ STB0899_ERRCTRL1		, 0xb6 },
-	{ STB0899_ERRCTRL2		, 0x96 },
-	{ STB0899_ERRCTRL3		, 0x89 },
-	{ STB0899_DMONMSK1		, 0x27 },
-	{ STB0899_DMONMSK0		, 0x03 },
-	{ STB0899_DEMAPVIT		, 0x5c },
-	{ STB0899_PLPARM		, 0x1f },
-	{ STB0899_PDELCTRL		, 0x48 },
-	{ STB0899_PDELCTRL2		, 0x00 },
-	{ STB0899_BBHCTRL1		, 0x00 },
-	{ STB0899_BBHCTRL2		, 0x00 },
-	{ STB0899_HYSTTHRESH		, 0x77 },
-	{ STB0899_MATCSTM		, 0x00 },
-	{ STB0899_MATCSTL		, 0x00 },
-	{ STB0899_UPLCSTM		, 0x00 },
-	{ STB0899_UPLCSTL		, 0x00 },
-	{ STB0899_DFLCSTM		, 0x00 },
-	{ STB0899_DFLCSTL		, 0x00 },
-	{ STB0899_SYNCCST		, 0x00 },
-	{ STB0899_SYNCDCSTM		, 0x00 },
-	{ STB0899_SYNCDCSTL		, 0x00 },
-	{ STB0899_ISI_ENTRY		, 0x00 },
-	{ STB0899_ISI_BIT_EN		, 0x00 },
-	{ STB0899_MATSTRM		, 0x00 },
-	{ STB0899_MATSTRL		, 0x00 },
-	{ STB0899_UPLSTRM		, 0x00 },
-	{ STB0899_UPLSTRL		, 0x00 },
-	{ STB0899_DFLSTRM		, 0x00 },
-	{ STB0899_DFLSTRL		, 0x00 },
-	{ STB0899_SYNCSTR		, 0x00 },
-	{ STB0899_SYNCDSTRM		, 0x00 },
-	{ STB0899_SYNCDSTRL		, 0x00 },
-	{ STB0899_CFGPDELSTATUS1	, 0x10 },
-	{ STB0899_CFGPDELSTATUS2	, 0x00 },
-	{ STB0899_BBFERRORM		, 0x00 },
-	{ STB0899_BBFERRORL		, 0x00 },
-	{ STB0899_UPKTERRORM		, 0x00 },
-	{ STB0899_UPKTERRORL		, 0x00 },
-	{ 0xffff			, 0xff },
+	{ STB0899_DEMOD,		0x00 },
+	{ STB0899_RCOMPC,		0xc9 },
+	{ STB0899_AGC1CN,		0x41 },
+	{ STB0899_AGC1REF,		0x08 },
+	{ STB0899_RTC,			0x7a },
+	{ STB0899_TMGCFG,		0x4e },
+	{ STB0899_AGC2REF,		0x33 },
+	{ STB0899_TLSR,			0x84 },
+	{ STB0899_CFD,			0xee },
+	{ STB0899_ACLC,			0x87 },
+	{ STB0899_BCLC,			0x94 },
+	{ STB0899_EQON,			0x41 },
+	{ STB0899_LDT,			0xdd },
+	{ STB0899_LDT2,			0xc9 },
+	{ STB0899_EQUALREF,		0xb4 },
+	{ STB0899_TMGRAMP,		0x10 },
+	{ STB0899_TMGTHD,		0x30 },
+	{ STB0899_IDCCOMP,		0xfb },
+	{ STB0899_QDCCOMP,		0x03 },
+	{ STB0899_POWERI,		0x3b },
+	{ STB0899_POWERQ,		0x3d },
+	{ STB0899_RCOMP,		0x81 },
+	{ STB0899_AGCIQIN,		0x80 },
+	{ STB0899_AGC2I1,		0x04 },
+	{ STB0899_AGC2I2,		0xf5 },
+	{ STB0899_TLIR,			0x25 },
+	{ STB0899_RTF,			0x80 },
+	{ STB0899_DSTATUS,		0x00 },
+	{ STB0899_LDI,			0xca },
+	{ STB0899_CFRM,			0xf1 },
+	{ STB0899_CFRL,			0xf3 },
+	{ STB0899_NIRM,			0x2a },
+	{ STB0899_NIRL,			0x05 },
+	{ STB0899_ISYMB,		0x17 },
+	{ STB0899_QSYMB,		0xfa },
+	{ STB0899_SFRH,			0x2f },
+	{ STB0899_SFRM,			0x68 },
+	{ STB0899_SFRL,			0x40 },
+	{ STB0899_SFRUPH,		0x2f },
+	{ STB0899_SFRUPM,		0x68 },
+	{ STB0899_SFRUPL,		0x40 },
+	{ STB0899_EQUAI1,		0xfd },
+	{ STB0899_EQUAQ1,		0x04 },
+	{ STB0899_EQUAI2,		0x0f },
+	{ STB0899_EQUAQ2,		0xff },
+	{ STB0899_EQUAI3,		0xdf },
+	{ STB0899_EQUAQ3,		0xfa },
+	{ STB0899_EQUAI4,		0x37 },
+	{ STB0899_EQUAQ4,		0x0d },
+	{ STB0899_EQUAI5,		0xbd },
+	{ STB0899_EQUAQ5,		0xf7 },
+	{ STB0899_DSTATUS2,		0x00 },
+	{ STB0899_VSTATUS,		0x00 },
+	{ STB0899_VERROR,		0xff },
+	{ STB0899_IQSWAP,		0x2a },
+	{ STB0899_ECNT1M,		0x00 },
+	{ STB0899_ECNT1L,		0x00 },
+	{ STB0899_ECNT2M,		0x00 },
+	{ STB0899_ECNT2L,		0x00 },
+	{ STB0899_ECNT3M,		0x00 },
+	{ STB0899_ECNT3L,		0x00 },
+	{ STB0899_FECAUTO1,		0x06 },
+	{ STB0899_FECM,			0x01 },
+	{ STB0899_VTH12,		0xf0 },
+	{ STB0899_VTH23,		0xa0 },
+	{ STB0899_VTH34,		0x78 },
+	{ STB0899_VTH56,		0x4e },
+	{ STB0899_VTH67,		0x48 },
+	{ STB0899_VTH78,		0x38 },
+	{ STB0899_PRVIT,		0xff },
+	{ STB0899_VITSYNC,		0x19 },
+	{ STB0899_RSULC,		0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
+	{ STB0899_TSULC,		0x42 },
+	{ STB0899_RSLLC,		0x40 },
+	{ STB0899_TSLPL,		0x12 },
+	{ STB0899_TSCFGH,		0x0c },
+	{ STB0899_TSCFGM,		0x00 },
+	{ STB0899_TSCFGL,		0x0c },
+	{ STB0899_TSOUT,		0x4d }, /* 0x0d for CAM */
+	{ STB0899_RSSYNCDEL,		0x00 },
+	{ STB0899_TSINHDELH,		0x02 },
+	{ STB0899_TSINHDELM,		0x00 },
+	{ STB0899_TSINHDELL,		0x00 },
+	{ STB0899_TSLLSTKM,		0x00 },
+	{ STB0899_TSLLSTKL,		0x00 },
+	{ STB0899_TSULSTKM,		0x00 },
+	{ STB0899_TSULSTKL,		0xab },
+	{ STB0899_PCKLENUL,		0x00 },
+	{ STB0899_PCKLENLL,		0xcc },
+	{ STB0899_RSPCKLEN,		0xcc },
+	{ STB0899_TSSTATUS,		0x80 },
+	{ STB0899_ERRCTRL1,		0xb6 },
+	{ STB0899_ERRCTRL2,		0x96 },
+	{ STB0899_ERRCTRL3,		0x89 },
+	{ STB0899_DMONMSK1,		0x27 },
+	{ STB0899_DMONMSK0,		0x03 },
+	{ STB0899_DEMAPVIT,		0x5c },
+	{ STB0899_PLPARM,		0x1f },
+	{ STB0899_PDELCTRL,		0x48 },
+	{ STB0899_PDELCTRL2,		0x00 },
+	{ STB0899_BBHCTRL1,		0x00 },
+	{ STB0899_BBHCTRL2,		0x00 },
+	{ STB0899_HYSTTHRESH,		0x77 },
+	{ STB0899_MATCSTM,		0x00 },
+	{ STB0899_MATCSTL,		0x00 },
+	{ STB0899_UPLCSTM,		0x00 },
+	{ STB0899_UPLCSTL,		0x00 },
+	{ STB0899_DFLCSTM,		0x00 },
+	{ STB0899_DFLCSTL,		0x00 },
+	{ STB0899_SYNCCST,		0x00 },
+	{ STB0899_SYNCDCSTM,		0x00 },
+	{ STB0899_SYNCDCSTL,		0x00 },
+	{ STB0899_ISI_ENTRY,		0x00 },
+	{ STB0899_ISI_BIT_EN,		0x00 },
+	{ STB0899_MATSTRM,		0x00 },
+	{ STB0899_MATSTRL,		0x00 },
+	{ STB0899_UPLSTRM,		0x00 },
+	{ STB0899_UPLSTRL,		0x00 },
+	{ STB0899_DFLSTRM,		0x00 },
+	{ STB0899_DFLSTRL,		0x00 },
+	{ STB0899_SYNCSTR,		0x00 },
+	{ STB0899_SYNCDSTRM,		0x00 },
+	{ STB0899_SYNCDSTRL,		0x00 },
+	{ STB0899_CFGPDELSTATUS1,	0x10 },
+	{ STB0899_CFGPDELSTATUS2,	0x00 },
+	{ STB0899_BBFERRORM,		0x00 },
+	{ STB0899_BBFERRORL,		0x00 },
+	{ STB0899_UPKTERRORM,		0x00 },
+	{ STB0899_UPKTERRORL,		0x00 },
+	{ 0xffff,			0xff },
 };
 
 /* STB0899 demodulator config for the KNC1 and clones */
@@ -1153,8 +1157,8 @@ static u8 read_pwm(struct budget_av *budget_av)
 {
 	u8 b = 0xff;
 	u8 pwm;
-	struct i2c_msg msg[] = { {.addr = 0x50,.flags = 0,.buf = &b,.len = 1},
-	{.addr = 0x50,.flags = I2C_M_RD,.buf = &pwm,.len = 1}
+	struct i2c_msg msg[] = { {.addr = 0x50, .flags = 0, .buf = &b, .len = 1},
+	{.addr = 0x50, .flags = I2C_M_RD, .buf = &pwm, .len = 1}
 	};
 
 	if ((i2c_transfer(&budget_av->budget.i2c_adap, msg, 2) != 2)
@@ -1196,8 +1200,8 @@ static u8 read_pwm(struct budget_av *budget_av)
 
 static void frontend_init(struct budget_av *budget_av)
 {
-	struct saa7146_dev * saa = budget_av->budget.dev;
-	struct dvb_frontend * fe = NULL;
+	struct saa7146_dev *saa = budget_av->budget.dev;
+	struct dvb_frontend *fe = NULL;
 
 	/* Enable / PowerON Frontend */
 	saa7146_setgpio(saa, 0, SAA7146_GPIO_OUTLO);
@@ -1207,16 +1211,16 @@ static void frontend_init(struct budget_av *budget_av)
 
 	/* additional setup necessary for the PLUS cards */
 	switch (saa->pci->subsystem_device) {
-		case SUBID_DVBS_KNC1_PLUS:
-		case SUBID_DVBC_KNC1_PLUS:
-		case SUBID_DVBT_KNC1_PLUS:
-		case SUBID_DVBC_EASYWATCH:
-		case SUBID_DVBC_KNC1_PLUS_MK3:
-		case SUBID_DVBS2_KNC1:
-		case SUBID_DVBS2_KNC1_OEM:
-		case SUBID_DVBS2_EASYWATCH:
-			saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTHI);
-			break;
+	case SUBID_DVBS_KNC1_PLUS:
+	case SUBID_DVBC_KNC1_PLUS:
+	case SUBID_DVBT_KNC1_PLUS:
+	case SUBID_DVBC_EASYWATCH:
+	case SUBID_DVBC_KNC1_PLUS_MK3:
+	case SUBID_DVBS2_KNC1:
+	case SUBID_DVBS2_KNC1_OEM:
+	case SUBID_DVBS2_EASYWATCH:
+		saa7146_setgpio(saa, 3, SAA7146_GPIO_OUTHI);
+		break;
 	}
 
 	switch (saa->pci->subsystem_device) {
@@ -1233,15 +1237,13 @@ static void frontend_init(struct budget_av *budget_av)
 		if (saa->pci->subsystem_vendor == 0x1894) {
 			fe = dvb_attach(stv0299_attach, &cinergy_1200s_1894_0010_config,
 					     &budget_av->budget.i2c_adap);
-			if (fe) {
+			if (fe)
 				dvb_attach(tua6100_attach, fe, 0x60, &budget_av->budget.i2c_adap);
-			}
 		} else {
 			fe = dvb_attach(stv0299_attach, &typhoon_config,
 					     &budget_av->budget.i2c_adap);
-			if (fe) {
+			if (fe)
 				fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
-			}
 		}
 		break;
 
@@ -1253,34 +1255,32 @@ static void frontend_init(struct budget_av *budget_av)
 	case SUBID_DVBS_EASYWATCH_2:
 		fe = dvb_attach(stv0299_attach, &philips_sd1878_config,
 				&budget_av->budget.i2c_adap);
-		if (fe) {
+		if (fe)
 			dvb_attach(dvb_pll_attach, fe, 0x60,
 				   &budget_av->budget.i2c_adap,
 				   DVB_PLL_PHILIPS_SD1878_TDA8261);
-		}
 		break;
 
 	case SUBID_DVBS_TYPHOON:
 		fe = dvb_attach(stv0299_attach, &typhoon_config,
 				    &budget_av->budget.i2c_adap);
-		if (fe) {
+		if (fe)
 			fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
-		}
 		break;
 	case SUBID_DVBS2_KNC1:
 	case SUBID_DVBS2_KNC1_OEM:
 	case SUBID_DVBS2_EASYWATCH:
 		budget_av->reinitialise_demod = 1;
-		if ((fe = dvb_attach(stb0899_attach, &knc1_dvbs2_config, &budget_av->budget.i2c_adap)))
+		fe = dvb_attach(stb0899_attach, &knc1_dvbs2_config, &budget_av->budget.i2c_adap);
+		if (fe)
 			dvb_attach(tda8261_attach, fe, &sd1878c_config, &budget_av->budget.i2c_adap);
 
 		break;
 	case SUBID_DVBS_CINERGY1200:
 		fe = dvb_attach(stv0299_attach, &cinergy_1200s_config,
 				    &budget_av->budget.i2c_adap);
-		if (fe) {
+		if (fe)
 			fe->ops.tuner_ops.set_params = philips_su1278_ty_ci_tuner_set_params;
-		}
 		break;
 
 	case SUBID_DVBC_KNC1:
@@ -1296,9 +1296,8 @@ static void frontend_init(struct budget_av *budget_av)
 			fe = dvb_attach(tda10021_attach, &philips_cu1216_config_altaddress,
 					     &budget_av->budget.i2c_adap,
 					     read_pwm(budget_av));
-		if (fe) {
+		if (fe)
 			fe->ops.tuner_ops.set_params = philips_cu1216_tuner_set_params;
-		}
 		break;
 
 	case SUBID_DVBC_EASYWATCH_MK3:
@@ -1312,9 +1311,8 @@ static void frontend_init(struct budget_av *budget_av)
 			&philips_cu1216_tda10023_config,
 			&budget_av->budget.i2c_adap,
 			read_pwm(budget_av));
-		if (fe) {
+		if (fe)
 			fe->ops.tuner_ops.set_params = philips_cu1216_tuner_set_params;
-		}
 		break;
 
 	case SUBID_DVBT_EASYWATCH:
@@ -1351,7 +1349,7 @@ static void frontend_init(struct budget_av *budget_av)
 }
 
 
-static void budget_av_irq(struct saa7146_dev *dev, u32 * isr)
+static void budget_av_irq(struct saa7146_dev *dev, u32 *isr)
 {
 	struct budget_av *budget_av = dev->ext_priv;
 
@@ -1368,7 +1366,7 @@ static int budget_av_detach(struct saa7146_dev *dev)
 
 	dprintk(2, "dev: %p\n", dev);
 
-	if (1 == budget_av->has_saa7113) {
+	if (budget_av->has_saa7113 == 1) {
 		saa7146_setgpio(dev, 0, SAA7146_GPIO_OUTLO);
 
 		msleep(200);
@@ -1439,7 +1437,8 @@ static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extensio
 
 	dprintk(2, "dev: %p\n", dev);
 
-	if (!(budget_av = kzalloc(sizeof(struct budget_av), GFP_KERNEL)))
+	budget_av = kzalloc(sizeof(struct budget_av), GFP_KERNEL);
+	if (!budget_av)
 		return -ENOMEM;
 
 	budget_av->has_saa7113 = 0;
@@ -1465,18 +1464,19 @@ static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extensio
 		if (err != 0) {
 			ttpci_budget_deinit(&budget_av->budget);
 			kfree(budget_av);
-			ERR("cannot init vv subsystem\n");
+			pr_err("cannot init vv subsystem\n");
 			return err;
 		}
 		vv_data.vid_ops.vidioc_enum_input = vidioc_enum_input;
 		vv_data.vid_ops.vidioc_g_input = vidioc_g_input;
 		vv_data.vid_ops.vidioc_s_input = vidioc_s_input;
 
-		if ((err = saa7146_register_device(&budget_av->vd, dev, "knc1", VFL_TYPE_VIDEO))) {
+		err = saa7146_register_device(&budget_av->vd, dev, "knc1", VFL_TYPE_VIDEO);
+		if (err) {
 			saa7146_vv_release(dev);
 			ttpci_budget_deinit(&budget_av->budget);
 			kfree(budget_av);
-			ERR("cannot register capture v4l2 device\n");
+			pr_err("cannot register capture v4l2 device\n");
 			return err;
 		}
 
@@ -1510,15 +1510,15 @@ static int budget_av_attach(struct saa7146_dev *dev, struct saa7146_pci_extensio
 }
 
 static struct saa7146_standard standard[] = {
-	{.name = "PAL",.id = V4L2_STD_PAL,
-	 .v_offset = 0x17,.v_field = 288,
-	 .h_offset = 0x14,.h_pixels = 680,
-	 .v_max_out = 576,.h_max_out = 768 },
-
-	{.name = "NTSC",.id = V4L2_STD_NTSC,
-	 .v_offset = 0x16,.v_field = 240,
-	 .h_offset = 0x06,.h_pixels = 708,
-	 .v_max_out = 480,.h_max_out = 640, },
+	{.name = "PAL", .id = V4L2_STD_PAL,
+	 .v_offset = 0x17, .v_field = 288,
+	 .h_offset = 0x14, .h_pixels = 680,
+	 .v_max_out = 576, .h_max_out = 768 },
+
+	{.name = "NTSC", .id = V4L2_STD_NTSC,
+	 .v_offset = 0x16, .v_field = 240,
+	 .h_offset = 0x06, .h_pixels = 708,
+	 .v_max_out = 480, .h_max_out = 640, },
 };
 
 static struct saa7146_ext_vv vv_data = {
@@ -1532,8 +1532,8 @@ static struct saa7146_ext_vv vv_data = {
 static struct saa7146_extension budget_extension;
 
 MAKE_BUDGET_INFO(knc1s, "KNC1 DVB-S", BUDGET_KNC1S);
-MAKE_BUDGET_INFO(knc1s2,"KNC1 DVB-S2", BUDGET_KNC1S2);
-MAKE_BUDGET_INFO(sates2,"Satelco EasyWatch DVB-S2", BUDGET_KNC1S2);
+MAKE_BUDGET_INFO(knc1s2, "KNC1 DVB-S2", BUDGET_KNC1S2);
+MAKE_BUDGET_INFO(sates2, "Satelco EasyWatch DVB-S2", BUDGET_KNC1S2);
 MAKE_BUDGET_INFO(knc1c, "KNC1 DVB-C", BUDGET_KNC1C);
 MAKE_BUDGET_INFO(knc1t, "KNC1 DVB-T", BUDGET_KNC1T);
 MAKE_BUDGET_INFO(kncxs, "KNC TV STAR DVB-S", BUDGET_TVSTAR);
diff --git a/drivers/media/pci/ttpci/budget-ci.c b/drivers/media/pci/ttpci/budget-ci.c
index 66e1a004ee43..33f08adf4feb 100644
--- a/drivers/media/pci/ttpci/budget-ci.c
+++ b/drivers/media/pci/ttpci/budget-ci.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 /*
- * budget-ci.c: driver for the SAA7146 based Budget DVB cards
+ * budget-ci.ko: driver for the SAA7146 based Budget DVB cards
+ *               with CI (but without analog video input)
  *
  * Compiled from various sources by Michael Hunold <michael@mihu.de>
  *
@@ -17,6 +18,7 @@
 #include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
+#include <linux/workqueue.h>
 #include <media/rc-core.h>
 
 #include "budget.h"
@@ -80,7 +82,7 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 struct budget_ci_ir {
 	struct rc_dev *dev;
-	struct tasklet_struct msp430_irq_tasklet;
+	struct work_struct msp430_irq_bh_work;
 	char name[72]; /* 40 + 32 for (struct saa7146_dev).name */
 	char phys[32];
 	int rc5_device;
@@ -91,7 +93,7 @@ struct budget_ci_ir {
 
 struct budget_ci {
 	struct budget budget;
-	struct tasklet_struct ciintf_irq_tasklet;
+	struct work_struct ciintf_irq_bh_work;
 	int slot_status;
 	int ci_irq;
 	struct dvb_ca_en50221 ca;
@@ -99,9 +101,9 @@ struct budget_ci {
 	u8 tuner_pll_address; /* used for philips_tdm1316l configs */
 };
 
-static void msp430_ir_interrupt(struct tasklet_struct *t)
+static void msp430_ir_interrupt(struct work_struct *t)
 {
-	struct budget_ci_ir *ir = from_tasklet(ir, t, msp430_irq_tasklet);
+	struct budget_ci_ir *ir = from_work(ir, t, msp430_irq_bh_work);
 	struct budget_ci *budget_ci = container_of(ir, typeof(*budget_ci), ir);
 	struct rc_dev *dev = budget_ci->ir.dev;
 	u32 command = ttpci_budget_debiread(&budget_ci->budget, DEBINOSWAP, DEBIADDR_IR, 2, 1, 0) >> 8;
@@ -123,7 +125,7 @@ static void msp430_ir_interrupt(struct tasklet_struct *t)
 	 */
 
 	if (ir_debug)
-		printk("budget_ci: received byte 0x%02x\n", command);
+		pr_info("received byte 0x%02x\n", command);
 
 	/* Remove repeat bit, we use every command */
 	command = command & 0x7f;
@@ -164,7 +166,7 @@ static int msp430_ir_init(struct budget_ci *budget_ci)
 
 	dev = rc_allocate_device(RC_DRIVER_SCANCODE);
 	if (!dev) {
-		printk(KERN_ERR "budget_ci: IR interface initialisation failed\n");
+		pr_err("IR interface initialisation failed\n");
 		return -ENOMEM;
 	}
 
@@ -223,14 +225,14 @@ static int msp430_ir_init(struct budget_ci *budget_ci)
 
 	error = rc_register_device(dev);
 	if (error) {
-		printk(KERN_ERR "budget_ci: could not init driver for IR device (code %d)\n", error);
+		pr_err("could not init driver for IR device (code %d)\n", error);
 		rc_free_device(dev);
 		return error;
 	}
 
 	budget_ci->ir.dev = dev;
 
-	tasklet_setup(&budget_ci->ir.msp430_irq_tasklet, msp430_ir_interrupt);
+	INIT_WORK(&budget_ci->ir.msp430_irq_bh_work, msp430_ir_interrupt);
 
 	SAA7146_IER_ENABLE(saa, MASK_06);
 	saa7146_setgpio(saa, 3, SAA7146_GPIO_IRQHI);
@@ -244,7 +246,7 @@ static void msp430_ir_deinit(struct budget_ci *budget_ci)
 
 	SAA7146_IER_DISABLE(saa, MASK_06);
 	saa7146_setgpio(saa, 3, SAA7146_GPIO_INPUT);
-	tasklet_kill(&budget_ci->ir.msp430_irq_tasklet);
+	cancel_work_sync(&budget_ci->ir.msp430_irq_bh_work);
 
 	rc_unregister_device(budget_ci->ir.dev);
 }
@@ -348,10 +350,10 @@ static int ciintf_slot_ts_enable(struct dvb_ca_en50221 *ca, int slot)
 	return 0;
 }
 
-static void ciintf_interrupt(struct tasklet_struct *t)
+static void ciintf_interrupt(struct work_struct *t)
 {
-	struct budget_ci *budget_ci = from_tasklet(budget_ci, t,
-						   ciintf_irq_tasklet);
+	struct budget_ci *budget_ci = from_work(budget_ci, t,
+						   ciintf_irq_bh_work);
 	struct saa7146_dev *saa = budget_ci->budget.dev;
 	unsigned int flags;
 
@@ -411,24 +413,21 @@ static int ciintf_poll_slot_status(struct dvb_ca_en50221 *ca, int slot, int open
 	flags = ttpci_budget_debiread(&budget_ci->budget, DEBICICTL, DEBIADDR_CICONTROL, 1, 1, 0);
 	if (flags & CICONTROL_CAMDETECT) {
 		// mark it as present if it wasn't before
-		if (budget_ci->slot_status & SLOTSTATUS_NONE) {
+		if (budget_ci->slot_status & SLOTSTATUS_NONE)
 			budget_ci->slot_status = SLOTSTATUS_PRESENT;
-		}
 
 		// during a RESET, we check if we can read from IO memory to see when CAM is ready
 		if (budget_ci->slot_status & SLOTSTATUS_RESET) {
-			if (ciintf_read_attribute_mem(ca, slot, 0) == 0x1d) {
+			if (ciintf_read_attribute_mem(ca, slot, 0) == 0x1d)
 				budget_ci->slot_status = SLOTSTATUS_READY;
-			}
 		}
 	} else {
 		budget_ci->slot_status = SLOTSTATUS_NONE;
 	}
 
 	if (budget_ci->slot_status != SLOTSTATUS_NONE) {
-		if (budget_ci->slot_status & SLOTSTATUS_READY) {
+		if (budget_ci->slot_status & SLOTSTATUS_READY)
 			return DVB_CA_EN50221_POLL_CAM_PRESENT | DVB_CA_EN50221_POLL_CAM_READY;
-		}
 		return DVB_CA_EN50221_POLL_CAM_PRESENT;
 	}
 
@@ -483,21 +482,21 @@ static int ciintf_init(struct budget_ci *budget_ci)
 	budget_ci->ca.slot_ts_enable = ciintf_slot_ts_enable;
 	budget_ci->ca.poll_slot_status = ciintf_poll_slot_status;
 	budget_ci->ca.data = budget_ci;
-	if ((result = dvb_ca_en50221_init(&budget_ci->budget.dvb_adapter,
-					  &budget_ci->ca,
-					  ca_flags, 1)) != 0) {
-		printk("budget_ci: CI interface detected, but initialisation failed.\n");
+
+	result = dvb_ca_en50221_init(&budget_ci->budget.dvb_adapter,
+				     &budget_ci->ca, ca_flags, 1);
+	if (result != 0) {
+		pr_err("CI interface detected, but initialisation failed.\n");
 		goto error;
 	}
 
 	// Setup CI slot IRQ
 	if (budget_ci->ci_irq) {
-		tasklet_setup(&budget_ci->ciintf_irq_tasklet, ciintf_interrupt);
-		if (budget_ci->slot_status != SLOTSTATUS_NONE) {
+		INIT_WORK(&budget_ci->ciintf_irq_bh_work, ciintf_interrupt);
+		if (budget_ci->slot_status != SLOTSTATUS_NONE)
 			saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQLO);
-		} else {
+		else
 			saa7146_setgpio(saa, 0, SAA7146_GPIO_IRQHI);
-		}
 		SAA7146_IER_ENABLE(saa, MASK_03);
 	}
 
@@ -506,7 +505,7 @@ static int ciintf_init(struct budget_ci *budget_ci)
 			       CICONTROL_RESET, 1, 0);
 
 	// success!
-	printk("budget_ci: CI interface initialised\n");
+	pr_info("CI interface initialised\n");
 	budget_ci->budget.ci_present = 1;
 
 	// forge a fake CI IRQ so the CAM state is setup correctly
@@ -532,7 +531,7 @@ static void ciintf_deinit(struct budget_ci *budget_ci)
 	if (budget_ci->ci_irq) {
 		SAA7146_IER_DISABLE(saa, MASK_03);
 		saa7146_setgpio(saa, 0, SAA7146_GPIO_INPUT);
-		tasklet_kill(&budget_ci->ciintf_irq_tasklet);
+		cancel_work_sync(&budget_ci->ciintf_irq_bh_work);
 	}
 
 	// reset interface
@@ -551,20 +550,20 @@ static void ciintf_deinit(struct budget_ci *budget_ci)
 	saa7146_write(saa, MC1, MASK_27);
 }
 
-static void budget_ci_irq(struct saa7146_dev *dev, u32 * isr)
+static void budget_ci_irq(struct saa7146_dev *dev, u32 *isr)
 {
 	struct budget_ci *budget_ci = dev->ext_priv;
 
 	dprintk(8, "dev: %p, budget_ci: %p\n", dev, budget_ci);
 
 	if (*isr & MASK_06)
-		tasklet_schedule(&budget_ci->ir.msp430_irq_tasklet);
+		queue_work(system_bh_wq, &budget_ci->ir.msp430_irq_bh_work);
 
 	if (*isr & MASK_10)
 		ttpci_budget_irq10_handler(dev, isr);
 
 	if ((*isr & MASK_03) && (budget_ci->budget.ci_present) && (budget_ci->ci_irq))
-		tasklet_schedule(&budget_ci->ciintf_irq_tasklet);
+		queue_work(system_bh_wq, &budget_ci->ciintf_irq_bh_work);
 }
 
 static u8 philips_su1278_tt_inittab[] = {
@@ -651,7 +650,7 @@ static int philips_su1278_tt_tuner_set_params(struct dvb_frontend *fe)
 	struct budget_ci *budget_ci = fe->dvb->priv;
 	u32 div;
 	u8 buf[4];
-	struct i2c_msg msg = {.addr = 0x60,.flags = 0,.buf = buf,.len = sizeof(buf) };
+	struct i2c_msg msg = {.addr = 0x60, .flags = 0, .buf = buf, .len = sizeof(buf) };
 
 	if ((p->frequency < 950000) || (p->frequency > 2150000))
 		return -EINVAL;
@@ -701,7 +700,7 @@ static int philips_tdm1316l_tuner_init(struct dvb_frontend *fe)
 	struct budget_ci *budget_ci = fe->dvb->priv;
 	static u8 td1316_init[] = { 0x0b, 0xf5, 0x85, 0xab };
 	static u8 disable_mc44BC374c[] = { 0x1d, 0x74, 0xa0, 0x68 };
-	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address,.flags = 0,.buf = td1316_init,.len =
+	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address, .flags = 0, .buf = td1316_init, .len =
 			sizeof(td1316_init) };
 
 	// setup PLL configuration
@@ -731,7 +730,7 @@ static int philips_tdm1316l_tuner_set_params(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct budget_ci *budget_ci = fe->dvb->priv;
 	u8 tuner_buf[4];
-	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address,.flags = 0,.buf = tuner_buf,.len = sizeof(tuner_buf) };
+	struct i2c_msg tuner_msg = {.addr = budget_ci->tuner_pll_address, .flags = 0, .buf = tuner_buf, .len = sizeof(tuner_buf) };
 	int tuner_frequency = 0;
 	u8 band, cp, filter;
 
@@ -856,9 +855,9 @@ static int dvbc_philips_tdm1316l_tuner_set_params(struct dvb_frontend *fe)
 
 	// determine charge pump
 	tuner_frequency = p->frequency + 36125000;
-	if (tuner_frequency < 87000000)
+	if (tuner_frequency < 87000000) {
 		return -EINVAL;
-	else if (tuner_frequency < 130000000) {
+	} else if (tuner_frequency < 130000000) {
 		cp = 3;
 		band = 1;
 	} else if (tuner_frequency < 160000000) {
@@ -885,8 +884,9 @@ static int dvbc_philips_tdm1316l_tuner_set_params(struct dvb_frontend *fe)
 	} else if (tuner_frequency < 895000000) {
 		cp = 7;
 		band = 4;
-	} else
+	} else {
 		return -EINVAL;
+	}
 
 	// assume PLL filter should always be 8MHz for the moment.
 	filter = 1;
@@ -1035,222 +1035,222 @@ static struct tda827x_config tda827x_config = {
 /* TT S2-3200 DVB-S (STB0899) Inittab */
 static const struct stb0899_s1_reg tt3200_stb0899_s1_init_1[] = {
 
-	{ STB0899_DEV_ID		, 0x81 },
-	{ STB0899_DISCNTRL1		, 0x32 },
-	{ STB0899_DISCNTRL2		, 0x80 },
-	{ STB0899_DISRX_ST0		, 0x04 },
-	{ STB0899_DISRX_ST1		, 0x00 },
-	{ STB0899_DISPARITY		, 0x00 },
-	{ STB0899_DISSTATUS		, 0x20 },
-	{ STB0899_DISF22		, 0x8c },
-	{ STB0899_DISF22RX		, 0x9a },
-	{ STB0899_SYSREG		, 0x0b },
-	{ STB0899_ACRPRESC		, 0x11 },
-	{ STB0899_ACRDIV1		, 0x0a },
-	{ STB0899_ACRDIV2		, 0x05 },
-	{ STB0899_DACR1			, 0x00 },
-	{ STB0899_DACR2			, 0x00 },
-	{ STB0899_OUTCFG		, 0x00 },
-	{ STB0899_MODECFG		, 0x00 },
-	{ STB0899_IRQSTATUS_3		, 0x30 },
-	{ STB0899_IRQSTATUS_2		, 0x00 },
-	{ STB0899_IRQSTATUS_1		, 0x00 },
-	{ STB0899_IRQSTATUS_0		, 0x00 },
-	{ STB0899_IRQMSK_3		, 0xf3 },
-	{ STB0899_IRQMSK_2		, 0xfc },
-	{ STB0899_IRQMSK_1		, 0xff },
-	{ STB0899_IRQMSK_0		, 0xff },
-	{ STB0899_IRQCFG		, 0x00 },
-	{ STB0899_I2CCFG		, 0x88 },
-	{ STB0899_I2CRPT		, 0x48 }, /* 12k Pullup, Repeater=16, Stop=disabled */
-	{ STB0899_IOPVALUE5		, 0x00 },
-	{ STB0899_IOPVALUE4		, 0x20 },
-	{ STB0899_IOPVALUE3		, 0xc9 },
-	{ STB0899_IOPVALUE2		, 0x90 },
-	{ STB0899_IOPVALUE1		, 0x40 },
-	{ STB0899_IOPVALUE0		, 0x00 },
-	{ STB0899_GPIO00CFG		, 0x82 },
-	{ STB0899_GPIO01CFG		, 0x82 },
-	{ STB0899_GPIO02CFG		, 0x82 },
-	{ STB0899_GPIO03CFG		, 0x82 },
-	{ STB0899_GPIO04CFG		, 0x82 },
-	{ STB0899_GPIO05CFG		, 0x82 },
-	{ STB0899_GPIO06CFG		, 0x82 },
-	{ STB0899_GPIO07CFG		, 0x82 },
-	{ STB0899_GPIO08CFG		, 0x82 },
-	{ STB0899_GPIO09CFG		, 0x82 },
-	{ STB0899_GPIO10CFG		, 0x82 },
-	{ STB0899_GPIO11CFG		, 0x82 },
-	{ STB0899_GPIO12CFG		, 0x82 },
-	{ STB0899_GPIO13CFG		, 0x82 },
-	{ STB0899_GPIO14CFG		, 0x82 },
-	{ STB0899_GPIO15CFG		, 0x82 },
-	{ STB0899_GPIO16CFG		, 0x82 },
-	{ STB0899_GPIO17CFG		, 0x82 },
-	{ STB0899_GPIO18CFG		, 0x82 },
-	{ STB0899_GPIO19CFG		, 0x82 },
-	{ STB0899_GPIO20CFG		, 0x82 },
-	{ STB0899_SDATCFG		, 0xb8 },
-	{ STB0899_SCLTCFG		, 0xba },
-	{ STB0899_AGCRFCFG		, 0x1c }, /* 0x11 */
-	{ STB0899_GPIO22		, 0x82 }, /* AGCBB2CFG */
-	{ STB0899_GPIO21		, 0x91 }, /* AGCBB1CFG */
-	{ STB0899_DIRCLKCFG		, 0x82 },
-	{ STB0899_CLKOUT27CFG		, 0x7e },
-	{ STB0899_STDBYCFG		, 0x82 },
-	{ STB0899_CS0CFG		, 0x82 },
-	{ STB0899_CS1CFG		, 0x82 },
-	{ STB0899_DISEQCOCFG		, 0x20 },
-	{ STB0899_GPIO32CFG		, 0x82 },
-	{ STB0899_GPIO33CFG		, 0x82 },
-	{ STB0899_GPIO34CFG		, 0x82 },
-	{ STB0899_GPIO35CFG		, 0x82 },
-	{ STB0899_GPIO36CFG		, 0x82 },
-	{ STB0899_GPIO37CFG		, 0x82 },
-	{ STB0899_GPIO38CFG		, 0x82 },
-	{ STB0899_GPIO39CFG		, 0x82 },
-	{ STB0899_NCOARSE		, 0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
-	{ STB0899_SYNTCTRL		, 0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
-	{ STB0899_FILTCTRL		, 0x00 },
-	{ STB0899_SYSCTRL		, 0x00 },
-	{ STB0899_STOPCLK1		, 0x20 },
-	{ STB0899_STOPCLK2		, 0x00 },
-	{ STB0899_INTBUFSTATUS		, 0x00 },
-	{ STB0899_INTBUFCTRL		, 0x0a },
-	{ 0xffff			, 0xff },
+	{ STB0899_DEV_ID,		0x81 },
+	{ STB0899_DISCNTRL1,		0x32 },
+	{ STB0899_DISCNTRL2,		0x80 },
+	{ STB0899_DISRX_ST0,		0x04 },
+	{ STB0899_DISRX_ST1,		0x00 },
+	{ STB0899_DISPARITY,		0x00 },
+	{ STB0899_DISSTATUS,		0x20 },
+	{ STB0899_DISF22,		0x8c },
+	{ STB0899_DISF22RX,		0x9a },
+	{ STB0899_SYSREG,		0x0b },
+	{ STB0899_ACRPRESC,		0x11 },
+	{ STB0899_ACRDIV1,		0x0a },
+	{ STB0899_ACRDIV2,		0x05 },
+	{ STB0899_DACR1,		0x00 },
+	{ STB0899_DACR2,		0x00 },
+	{ STB0899_OUTCFG,		0x00 },
+	{ STB0899_MODECFG,		0x00 },
+	{ STB0899_IRQSTATUS_3,		0x30 },
+	{ STB0899_IRQSTATUS_2,		0x00 },
+	{ STB0899_IRQSTATUS_1,		0x00 },
+	{ STB0899_IRQSTATUS_0,		0x00 },
+	{ STB0899_IRQMSK_3,		0xf3 },
+	{ STB0899_IRQMSK_2,		0xfc },
+	{ STB0899_IRQMSK_1,		0xff },
+	{ STB0899_IRQMSK_0,		0xff },
+	{ STB0899_IRQCFG,		0x00 },
+	{ STB0899_I2CCFG,		0x88 },
+	{ STB0899_I2CRPT,		0x48 }, /* 12k Pullup, Repeater=16, Stop=disabled */
+	{ STB0899_IOPVALUE5,		0x00 },
+	{ STB0899_IOPVALUE4,		0x20 },
+	{ STB0899_IOPVALUE3,		0xc9 },
+	{ STB0899_IOPVALUE2,		0x90 },
+	{ STB0899_IOPVALUE1,		0x40 },
+	{ STB0899_IOPVALUE0,		0x00 },
+	{ STB0899_GPIO00CFG,		0x82 },
+	{ STB0899_GPIO01CFG,		0x82 },
+	{ STB0899_GPIO02CFG,		0x82 },
+	{ STB0899_GPIO03CFG,		0x82 },
+	{ STB0899_GPIO04CFG,		0x82 },
+	{ STB0899_GPIO05CFG,		0x82 },
+	{ STB0899_GPIO06CFG,		0x82 },
+	{ STB0899_GPIO07CFG,		0x82 },
+	{ STB0899_GPIO08CFG,		0x82 },
+	{ STB0899_GPIO09CFG,		0x82 },
+	{ STB0899_GPIO10CFG,		0x82 },
+	{ STB0899_GPIO11CFG,		0x82 },
+	{ STB0899_GPIO12CFG,		0x82 },
+	{ STB0899_GPIO13CFG,		0x82 },
+	{ STB0899_GPIO14CFG,		0x82 },
+	{ STB0899_GPIO15CFG,		0x82 },
+	{ STB0899_GPIO16CFG,		0x82 },
+	{ STB0899_GPIO17CFG,		0x82 },
+	{ STB0899_GPIO18CFG,		0x82 },
+	{ STB0899_GPIO19CFG,		0x82 },
+	{ STB0899_GPIO20CFG,		0x82 },
+	{ STB0899_SDATCFG,		0xb8 },
+	{ STB0899_SCLTCFG,		0xba },
+	{ STB0899_AGCRFCFG,		0x1c }, /* 0x11 */
+	{ STB0899_GPIO22,		0x82 }, /* AGCBB2CFG */
+	{ STB0899_GPIO21,		0x91 }, /* AGCBB1CFG */
+	{ STB0899_DIRCLKCFG,		0x82 },
+	{ STB0899_CLKOUT27CFG,		0x7e },
+	{ STB0899_STDBYCFG,		0x82 },
+	{ STB0899_CS0CFG,		0x82 },
+	{ STB0899_CS1CFG,		0x82 },
+	{ STB0899_DISEQCOCFG,		0x20 },
+	{ STB0899_GPIO32CFG,		0x82 },
+	{ STB0899_GPIO33CFG,		0x82 },
+	{ STB0899_GPIO34CFG,		0x82 },
+	{ STB0899_GPIO35CFG,		0x82 },
+	{ STB0899_GPIO36CFG,		0x82 },
+	{ STB0899_GPIO37CFG,		0x82 },
+	{ STB0899_GPIO38CFG,		0x82 },
+	{ STB0899_GPIO39CFG,		0x82 },
+	{ STB0899_NCOARSE,		0x15 }, /* 0x15 = 27 Mhz Clock, F/3 = 198MHz, F/6 = 99MHz */
+	{ STB0899_SYNTCTRL,		0x02 }, /* 0x00 = CLK from CLKI, 0x02 = CLK from XTALI */
+	{ STB0899_FILTCTRL,		0x00 },
+	{ STB0899_SYSCTRL,		0x00 },
+	{ STB0899_STOPCLK1,		0x20 },
+	{ STB0899_STOPCLK2,		0x00 },
+	{ STB0899_INTBUFSTATUS,		0x00 },
+	{ STB0899_INTBUFCTRL,		0x0a },
+	{ 0xffff,			0xff },
 };
 
 static const struct stb0899_s1_reg tt3200_stb0899_s1_init_3[] = {
-	{ STB0899_DEMOD			, 0x00 },
-	{ STB0899_RCOMPC		, 0xc9 },
-	{ STB0899_AGC1CN		, 0x41 },
-	{ STB0899_AGC1REF		, 0x10 },
-	{ STB0899_RTC			, 0x7a },
-	{ STB0899_TMGCFG		, 0x4e },
-	{ STB0899_AGC2REF		, 0x34 },
-	{ STB0899_TLSR			, 0x84 },
-	{ STB0899_CFD			, 0xc7 },
-	{ STB0899_ACLC			, 0x87 },
-	{ STB0899_BCLC			, 0x94 },
-	{ STB0899_EQON			, 0x41 },
-	{ STB0899_LDT			, 0xdd },
-	{ STB0899_LDT2			, 0xc9 },
-	{ STB0899_EQUALREF		, 0xb4 },
-	{ STB0899_TMGRAMP		, 0x10 },
-	{ STB0899_TMGTHD		, 0x30 },
-	{ STB0899_IDCCOMP		, 0xfb },
-	{ STB0899_QDCCOMP		, 0x03 },
-	{ STB0899_POWERI		, 0x3b },
-	{ STB0899_POWERQ		, 0x3d },
-	{ STB0899_RCOMP			, 0x81 },
-	{ STB0899_AGCIQIN		, 0x80 },
-	{ STB0899_AGC2I1		, 0x04 },
-	{ STB0899_AGC2I2		, 0xf5 },
-	{ STB0899_TLIR			, 0x25 },
-	{ STB0899_RTF			, 0x80 },
-	{ STB0899_DSTATUS		, 0x00 },
-	{ STB0899_LDI			, 0xca },
-	{ STB0899_CFRM			, 0xf1 },
-	{ STB0899_CFRL			, 0xf3 },
-	{ STB0899_NIRM			, 0x2a },
-	{ STB0899_NIRL			, 0x05 },
-	{ STB0899_ISYMB			, 0x17 },
-	{ STB0899_QSYMB			, 0xfa },
-	{ STB0899_SFRH			, 0x2f },
-	{ STB0899_SFRM			, 0x68 },
-	{ STB0899_SFRL			, 0x40 },
-	{ STB0899_SFRUPH		, 0x2f },
-	{ STB0899_SFRUPM		, 0x68 },
-	{ STB0899_SFRUPL		, 0x40 },
-	{ STB0899_EQUAI1		, 0xfd },
-	{ STB0899_EQUAQ1		, 0x04 },
-	{ STB0899_EQUAI2		, 0x0f },
-	{ STB0899_EQUAQ2		, 0xff },
-	{ STB0899_EQUAI3		, 0xdf },
-	{ STB0899_EQUAQ3		, 0xfa },
-	{ STB0899_EQUAI4		, 0x37 },
-	{ STB0899_EQUAQ4		, 0x0d },
-	{ STB0899_EQUAI5		, 0xbd },
-	{ STB0899_EQUAQ5		, 0xf7 },
-	{ STB0899_DSTATUS2		, 0x00 },
-	{ STB0899_VSTATUS		, 0x00 },
-	{ STB0899_VERROR		, 0xff },
-	{ STB0899_IQSWAP		, 0x2a },
-	{ STB0899_ECNT1M		, 0x00 },
-	{ STB0899_ECNT1L		, 0x00 },
-	{ STB0899_ECNT2M		, 0x00 },
-	{ STB0899_ECNT2L		, 0x00 },
-	{ STB0899_ECNT3M		, 0x00 },
-	{ STB0899_ECNT3L		, 0x00 },
-	{ STB0899_FECAUTO1		, 0x06 },
-	{ STB0899_FECM			, 0x01 },
-	{ STB0899_VTH12			, 0xf0 },
-	{ STB0899_VTH23			, 0xa0 },
-	{ STB0899_VTH34			, 0x78 },
-	{ STB0899_VTH56			, 0x4e },
-	{ STB0899_VTH67			, 0x48 },
-	{ STB0899_VTH78			, 0x38 },
-	{ STB0899_PRVIT			, 0xff },
-	{ STB0899_VITSYNC		, 0x19 },
-	{ STB0899_RSULC			, 0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
-	{ STB0899_TSULC			, 0x42 },
-	{ STB0899_RSLLC			, 0x40 },
-	{ STB0899_TSLPL			, 0x12 },
-	{ STB0899_TSCFGH		, 0x0c },
-	{ STB0899_TSCFGM		, 0x00 },
-	{ STB0899_TSCFGL		, 0x0c },
-	{ STB0899_TSOUT			, 0x4d }, /* 0x0d for CAM */
-	{ STB0899_RSSYNCDEL		, 0x00 },
-	{ STB0899_TSINHDELH		, 0x02 },
-	{ STB0899_TSINHDELM		, 0x00 },
-	{ STB0899_TSINHDELL		, 0x00 },
-	{ STB0899_TSLLSTKM		, 0x00 },
-	{ STB0899_TSLLSTKL		, 0x00 },
-	{ STB0899_TSULSTKM		, 0x00 },
-	{ STB0899_TSULSTKL		, 0xab },
-	{ STB0899_PCKLENUL		, 0x00 },
-	{ STB0899_PCKLENLL		, 0xcc },
-	{ STB0899_RSPCKLEN		, 0xcc },
-	{ STB0899_TSSTATUS		, 0x80 },
-	{ STB0899_ERRCTRL1		, 0xb6 },
-	{ STB0899_ERRCTRL2		, 0x96 },
-	{ STB0899_ERRCTRL3		, 0x89 },
-	{ STB0899_DMONMSK1		, 0x27 },
-	{ STB0899_DMONMSK0		, 0x03 },
-	{ STB0899_DEMAPVIT		, 0x5c },
-	{ STB0899_PLPARM		, 0x1f },
-	{ STB0899_PDELCTRL		, 0x48 },
-	{ STB0899_PDELCTRL2		, 0x00 },
-	{ STB0899_BBHCTRL1		, 0x00 },
-	{ STB0899_BBHCTRL2		, 0x00 },
-	{ STB0899_HYSTTHRESH		, 0x77 },
-	{ STB0899_MATCSTM		, 0x00 },
-	{ STB0899_MATCSTL		, 0x00 },
-	{ STB0899_UPLCSTM		, 0x00 },
-	{ STB0899_UPLCSTL		, 0x00 },
-	{ STB0899_DFLCSTM		, 0x00 },
-	{ STB0899_DFLCSTL		, 0x00 },
-	{ STB0899_SYNCCST		, 0x00 },
-	{ STB0899_SYNCDCSTM		, 0x00 },
-	{ STB0899_SYNCDCSTL		, 0x00 },
-	{ STB0899_ISI_ENTRY		, 0x00 },
-	{ STB0899_ISI_BIT_EN		, 0x00 },
-	{ STB0899_MATSTRM		, 0x00 },
-	{ STB0899_MATSTRL		, 0x00 },
-	{ STB0899_UPLSTRM		, 0x00 },
-	{ STB0899_UPLSTRL		, 0x00 },
-	{ STB0899_DFLSTRM		, 0x00 },
-	{ STB0899_DFLSTRL		, 0x00 },
-	{ STB0899_SYNCSTR		, 0x00 },
-	{ STB0899_SYNCDSTRM		, 0x00 },
-	{ STB0899_SYNCDSTRL		, 0x00 },
-	{ STB0899_CFGPDELSTATUS1	, 0x10 },
-	{ STB0899_CFGPDELSTATUS2	, 0x00 },
-	{ STB0899_BBFERRORM		, 0x00 },
-	{ STB0899_BBFERRORL		, 0x00 },
-	{ STB0899_UPKTERRORM		, 0x00 },
-	{ STB0899_UPKTERRORL		, 0x00 },
-	{ 0xffff			, 0xff },
+	{ STB0899_DEMOD,		0x00 },
+	{ STB0899_RCOMPC,		0xc9 },
+	{ STB0899_AGC1CN,		0x41 },
+	{ STB0899_AGC1REF,		0x10 },
+	{ STB0899_RTC,			0x7a },
+	{ STB0899_TMGCFG,		0x4e },
+	{ STB0899_AGC2REF,		0x34 },
+	{ STB0899_TLSR,			0x84 },
+	{ STB0899_CFD,			0xc7 },
+	{ STB0899_ACLC,			0x87 },
+	{ STB0899_BCLC,			0x94 },
+	{ STB0899_EQON,			0x41 },
+	{ STB0899_LDT,			0xdd },
+	{ STB0899_LDT2,			0xc9 },
+	{ STB0899_EQUALREF,		0xb4 },
+	{ STB0899_TMGRAMP,		0x10 },
+	{ STB0899_TMGTHD,		0x30 },
+	{ STB0899_IDCCOMP,		0xfb },
+	{ STB0899_QDCCOMP,		0x03 },
+	{ STB0899_POWERI,		0x3b },
+	{ STB0899_POWERQ,		0x3d },
+	{ STB0899_RCOMP,		0x81 },
+	{ STB0899_AGCIQIN,		0x80 },
+	{ STB0899_AGC2I1,		0x04 },
+	{ STB0899_AGC2I2,		0xf5 },
+	{ STB0899_TLIR,			0x25 },
+	{ STB0899_RTF,			0x80 },
+	{ STB0899_DSTATUS,		0x00 },
+	{ STB0899_LDI,			0xca },
+	{ STB0899_CFRM,			0xf1 },
+	{ STB0899_CFRL,			0xf3 },
+	{ STB0899_NIRM,			0x2a },
+	{ STB0899_NIRL,			0x05 },
+	{ STB0899_ISYMB,		0x17 },
+	{ STB0899_QSYMB,		0xfa },
+	{ STB0899_SFRH,			0x2f },
+	{ STB0899_SFRM,			0x68 },
+	{ STB0899_SFRL,			0x40 },
+	{ STB0899_SFRUPH,		0x2f },
+	{ STB0899_SFRUPM,		0x68 },
+	{ STB0899_SFRUPL,		0x40 },
+	{ STB0899_EQUAI1,		0xfd },
+	{ STB0899_EQUAQ1,		0x04 },
+	{ STB0899_EQUAI2,		0x0f },
+	{ STB0899_EQUAQ2,		0xff },
+	{ STB0899_EQUAI3,		0xdf },
+	{ STB0899_EQUAQ3,		0xfa },
+	{ STB0899_EQUAI4,		0x37 },
+	{ STB0899_EQUAQ4,		0x0d },
+	{ STB0899_EQUAI5,		0xbd },
+	{ STB0899_EQUAQ5,		0xf7 },
+	{ STB0899_DSTATUS2,		0x00 },
+	{ STB0899_VSTATUS,		0x00 },
+	{ STB0899_VERROR,		0xff },
+	{ STB0899_IQSWAP,		0x2a },
+	{ STB0899_ECNT1M,		0x00 },
+	{ STB0899_ECNT1L,		0x00 },
+	{ STB0899_ECNT2M,		0x00 },
+	{ STB0899_ECNT2L,		0x00 },
+	{ STB0899_ECNT3M,		0x00 },
+	{ STB0899_ECNT3L,		0x00 },
+	{ STB0899_FECAUTO1,		0x06 },
+	{ STB0899_FECM,			0x01 },
+	{ STB0899_VTH12,		0xf0 },
+	{ STB0899_VTH23,		0xa0 },
+	{ STB0899_VTH34,		0x78 },
+	{ STB0899_VTH56,		0x4e },
+	{ STB0899_VTH67,		0x48 },
+	{ STB0899_VTH78,		0x38 },
+	{ STB0899_PRVIT,		0xff },
+	{ STB0899_VITSYNC,		0x19 },
+	{ STB0899_RSULC,		0xb1 }, /* DVB = 0xb1, DSS = 0xa1 */
+	{ STB0899_TSULC,		0x42 },
+	{ STB0899_RSLLC,		0x40 },
+	{ STB0899_TSLPL,		0x12 },
+	{ STB0899_TSCFGH,		0x0c },
+	{ STB0899_TSCFGM,		0x00 },
+	{ STB0899_TSCFGL,		0x0c },
+	{ STB0899_TSOUT,		0x4d }, /* 0x0d for CAM */
+	{ STB0899_RSSYNCDEL,		0x00 },
+	{ STB0899_TSINHDELH,		0x02 },
+	{ STB0899_TSINHDELM,		0x00 },
+	{ STB0899_TSINHDELL,		0x00 },
+	{ STB0899_TSLLSTKM,		0x00 },
+	{ STB0899_TSLLSTKL,		0x00 },
+	{ STB0899_TSULSTKM,		0x00 },
+	{ STB0899_TSULSTKL,		0xab },
+	{ STB0899_PCKLENUL,		0x00 },
+	{ STB0899_PCKLENLL,		0xcc },
+	{ STB0899_RSPCKLEN,		0xcc },
+	{ STB0899_TSSTATUS,		0x80 },
+	{ STB0899_ERRCTRL1,		0xb6 },
+	{ STB0899_ERRCTRL2,		0x96 },
+	{ STB0899_ERRCTRL3,		0x89 },
+	{ STB0899_DMONMSK1,		0x27 },
+	{ STB0899_DMONMSK0,		0x03 },
+	{ STB0899_DEMAPVIT,		0x5c },
+	{ STB0899_PLPARM,		0x1f },
+	{ STB0899_PDELCTRL,		0x48 },
+	{ STB0899_PDELCTRL2,		0x00 },
+	{ STB0899_BBHCTRL1,		0x00 },
+	{ STB0899_BBHCTRL2,		0x00 },
+	{ STB0899_HYSTTHRESH,		0x77 },
+	{ STB0899_MATCSTM,		0x00 },
+	{ STB0899_MATCSTL,		0x00 },
+	{ STB0899_UPLCSTM,		0x00 },
+	{ STB0899_UPLCSTL,		0x00 },
+	{ STB0899_DFLCSTM,		0x00 },
+	{ STB0899_DFLCSTL,		0x00 },
+	{ STB0899_SYNCCST,		0x00 },
+	{ STB0899_SYNCDCSTM,		0x00 },
+	{ STB0899_SYNCDCSTL,		0x00 },
+	{ STB0899_ISI_ENTRY,		0x00 },
+	{ STB0899_ISI_BIT_EN,		0x00 },
+	{ STB0899_MATSTRM,		0x00 },
+	{ STB0899_MATSTRL,		0x00 },
+	{ STB0899_UPLSTRM,		0x00 },
+	{ STB0899_UPLSTRL,		0x00 },
+	{ STB0899_DFLSTRM,		0x00 },
+	{ STB0899_DFLSTRL,		0x00 },
+	{ STB0899_SYNCSTR,		0x00 },
+	{ STB0899_SYNCDSTRM,		0x00 },
+	{ STB0899_SYNCDSTRL,		0x00 },
+	{ STB0899_CFGPDELSTATUS1,	0x10 },
+	{ STB0899_CFGPDELSTATUS2,	0x00 },
+	{ STB0899_BBFERRORM,		0x00 },
+	{ STB0899_BBFERRORL,		0x00 },
+	{ STB0899_UPKTERRORM,		0x00 },
+	{ STB0899_UPKTERRORL,		0x00 },
+	{ 0xffff,			0xff },
 };
 
 static struct stb0899_config tt3200_config = {
@@ -1359,7 +1359,7 @@ static void frontend_init(struct budget_ci *budget_ci)
 
 			budget_ci->budget.dvb_frontend->ops.dishnetwork_send_legacy_command = NULL;
 			if (dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, LNBP21_LLC, 0) == NULL) {
-				printk("%s: No LNBP21 found!\n", __func__);
+				pr_err("%s(): No LNBP21 found!\n", __func__);
 				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 				budget_ci->budget.dvb_frontend = NULL;
 			}
@@ -1370,7 +1370,7 @@ static void frontend_init(struct budget_ci *budget_ci)
 		budget_ci->budget.dvb_frontend = dvb_attach(tda10023_attach, &tda10023_config, &budget_ci->budget.i2c_adap, 0x48);
 		if (budget_ci->budget.dvb_frontend) {
 			if (dvb_attach(tda827x_attach, budget_ci->budget.dvb_frontend, 0x61, &budget_ci->budget.i2c_adap, &tda827x_config) == NULL) {
-				printk(KERN_ERR "%s: No tda827x found!\n", __func__);
+				pr_err("%s(): No tda827x found!\n", __func__);
 				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 				budget_ci->budget.dvb_frontend = NULL;
 			}
@@ -1382,12 +1382,12 @@ static void frontend_init(struct budget_ci *budget_ci)
 		if (budget_ci->budget.dvb_frontend) {
 			if (dvb_attach(stb6000_attach, budget_ci->budget.dvb_frontend, 0x63, &budget_ci->budget.i2c_adap)) {
 				if (!dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, 0, 0)) {
-					printk(KERN_ERR "%s: No LNBP21 found!\n", __func__);
+					pr_err("%s(): No LNBP21 found!\n", __func__);
 					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 					budget_ci->budget.dvb_frontend = NULL;
 				}
 			} else {
-				printk(KERN_ERR "%s: No STB6000 found!\n", __func__);
+				pr_err("%s(): No STB6000 found!\n", __func__);
 				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 				budget_ci->budget.dvb_frontend = NULL;
 			}
@@ -1422,13 +1422,13 @@ static void frontend_init(struct budget_ci *budget_ci)
 		if (budget_ci->budget.dvb_frontend) {
 			if (dvb_attach(stb6100_attach, budget_ci->budget.dvb_frontend, &tt3200_stb6100_config, &budget_ci->budget.i2c_adap)) {
 				if (!dvb_attach(lnbp21_attach, budget_ci->budget.dvb_frontend, &budget_ci->budget.i2c_adap, 0, 0)) {
-					printk("%s: No LNBP21 found!\n", __func__);
+					pr_err("%s(): No LNBP21 found!\n", __func__);
 					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 					budget_ci->budget.dvb_frontend = NULL;
 				}
 			} else {
-					dvb_frontend_detach(budget_ci->budget.dvb_frontend);
-					budget_ci->budget.dvb_frontend = NULL;
+				dvb_frontend_detach(budget_ci->budget.dvb_frontend);
+				budget_ci->budget.dvb_frontend = NULL;
 			}
 		}
 		break;
@@ -1436,7 +1436,7 @@ static void frontend_init(struct budget_ci *budget_ci)
 	}
 
 	if (budget_ci->budget.dvb_frontend == NULL) {
-		printk("budget-ci: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		pr_err("A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
 		       budget_ci->budget.dev->pci->vendor,
 		       budget_ci->budget.dev->pci->device,
 		       budget_ci->budget.dev->pci->subsystem_vendor,
@@ -1444,7 +1444,7 @@ static void frontend_init(struct budget_ci *budget_ci)
 	} else {
 		if (dvb_register_frontend
 		    (&budget_ci->budget.dvb_adapter, budget_ci->budget.dvb_frontend)) {
-			printk("budget-ci: Frontend registration failed!\n");
+			pr_err("Frontend registration failed!\n");
 			dvb_frontend_detach(budget_ci->budget.dvb_frontend);
 			budget_ci->budget.dvb_frontend = NULL;
 		}
@@ -1538,7 +1538,7 @@ static const struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(ttbs1500b, 0x13c2, 0x101b),
 	{
 	 .vendor = 0,
-	 }
+	}
 };
 
 MODULE_DEVICE_TABLE(pci, pci_tbl);
diff --git a/drivers/media/pci/ttpci/budget-core.c b/drivers/media/pci/ttpci/budget-core.c
index 25f44c3eebf3..29531d9c9db0 100644
--- a/drivers/media/pci/ttpci/budget-core.c
+++ b/drivers/media/pci/ttpci/budget-core.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 /*
- * budget-core.c: driver for the SAA7146 based Budget DVB cards
+ * budget-core.ko: base-driver for the SAA7146 based Budget DVB cards
  *
  * Compiled from various sources by Michael Hunold <michael@mihu.de>
  *
@@ -34,6 +34,7 @@
 #define BUFFER_WARNING_WAIT	(30*HZ)
 
 int budget_debug;
+EXPORT_SYMBOL_GPL(budget_debug);
 static int dma_buffer_size = TS_MIN_BUFSIZE_K;
 module_param_named(debug, budget_debug, int, 0644);
 module_param_named(bufsize, dma_buffer_size, int, 0444);
@@ -80,7 +81,7 @@ static int start_ts_capture(struct budget *budget)
 	 *      Pitch: 188, NumBytes3: 188, NumLines3: 1024
 	 */
 
-	switch(budget->card->type) {
+	switch (budget->card->type) {
 	case BUDGET_FS_ACTIVY:
 		saa7146_write(dev, DD1_INIT, 0x04000000);
 		saa7146_write(dev, MC2, (MASK_09 | MASK_25));
@@ -171,9 +172,9 @@ static int budget_read_fe_status(struct dvb_frontend *fe,
 	return ret;
 }
 
-static void vpeirq(struct tasklet_struct *t)
+static void vpeirq(struct work_struct *t)
 {
-	struct budget *budget = from_tasklet(budget, t, vpe_tasklet);
+	struct budget *budget = from_work(budget, t, vpe_bh_work);
 	u8 *mem = (u8 *) (budget->grabbing);
 	u32 olddma = budget->ttbp;
 	u32 newdma = saa7146_read(budget->dev, PCI_VDP3);
@@ -208,7 +209,7 @@ static void vpeirq(struct tasklet_struct *t)
 		budget->buffer_warnings++;
 
 	if (budget->buffer_warnings && time_after(jiffies, budget->buffer_warning_time)) {
-		printk("%s %s: used %d times >80%% of buffer (%u bytes now)\n",
+		pr_warn("%s %s: used %d times >80%% of buffer (%u bytes now)\n",
 			budget->dev->name, __func__, budget->buffer_warnings, count);
 		budget->buffer_warning_time = jiffies + BUFFER_WARNING_WAIT;
 		budget->buffer_warnings = 0;
@@ -259,6 +260,7 @@ int ttpci_budget_debiread(struct budget *budget, u32 config, int addr, int count
 	return ttpci_budget_debiread_nolock(budget, config, addr,
 					    count, nobusyloop);
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_debiread);
 
 static int ttpci_budget_debiwrite_nolock(struct budget *budget, u32 config,
 		int addr, int count, u32 value, int nobusyloop)
@@ -299,6 +301,7 @@ int ttpci_budget_debiwrite(struct budget *budget, u32 config, int addr,
 	return ttpci_budget_debiwrite_nolock(budget, config, addr,
 					     count, value, nobusyloop);
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_debiwrite);
 
 
 /****************************************************************************
@@ -423,7 +426,7 @@ int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 	budget->card = bi;
 	budget->dev = (struct saa7146_dev *) dev;
 
-	switch(budget->card->type) {
+	switch (budget->card->type) {
 	case BUDGET_FS_ACTIVY:
 		budget->buffer_width = TS_WIDTH_ACTIVY;
 		max_bufsize = TS_MAX_BUFSIZE_K_ACTIVY;
@@ -470,7 +473,7 @@ int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 		budget->dev->name,
 		budget->buffer_size > budget->buffer_width * budget->buffer_height ? "odd/even" : "single",
 		budget->buffer_width, budget->buffer_height);
-	printk("%s: dma buffer size %u\n", budget->dev->name, budget->buffer_size);
+	pr_info("%s: dma buffer size %u\n", budget->dev->name, budget->buffer_size);
 
 	ret = dvb_register_adapter(&budget->dvb_adapter, budget->card->name,
 				   owner, &budget->dev->pci->dev, adapter_nums);
@@ -491,8 +494,10 @@ int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 	spin_lock_init(&budget->feedlock);
 	spin_lock_init(&budget->debilock);
 
-	/* the Siemens DVB needs this if you want to have the i2c chips
-	   get recognized before the main driver is loaded */
+	/*
+	 * the Siemens DVB needs this if you want to have the i2c chips
+	 * get recognized before the main driver is loaded
+	 */
 	if (bi->type != BUDGET_FS_ACTIVY)
 		saa7146_write(dev, GPIO_CTRL, 0x500000);	/* GPIO 3 = 1 */
 
@@ -511,7 +516,7 @@ int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 	ttpci_eeprom_parse_mac(&budget->i2c_adap, budget->dvb_adapter.proposed_mac);
 
 	budget->grabbing = saa7146_vmalloc_build_pgtable(dev->pci, budget->buffer_size, &budget->pt);
-	if (NULL == budget->grabbing) {
+	if (budget->grabbing == NULL) {
 		ret = -ENOMEM;
 		goto err_del_i2c;
 	}
@@ -520,13 +525,14 @@ int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 	/* upload all */
 	saa7146_write(dev, GPIO_CTRL, 0x000000);
 
-	tasklet_setup(&budget->vpe_tasklet, vpeirq);
+	INIT_WORK(&budget->vpe_bh_work, vpeirq);
 
 	/* frontend power on */
 	if (bi->type != BUDGET_FS_ACTIVY)
 		saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
 
-	if ((ret = budget_register(budget)) == 0)
+	ret = budget_register(budget);
+	if (ret == 0)
 		return 0; /* Everything OK */
 
 	/* An error occurred, cleanup resources */
@@ -540,6 +546,7 @@ err_dvb_unregister:
 
 	return ret;
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_init);
 
 void ttpci_budget_init_hooks(struct budget *budget)
 {
@@ -548,6 +555,7 @@ void ttpci_budget_init_hooks(struct budget *budget)
 		budget->dvb_frontend->ops.read_status = budget_read_fe_status;
 	}
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_init_hooks);
 
 int ttpci_budget_deinit(struct budget *budget)
 {
@@ -557,7 +565,7 @@ int ttpci_budget_deinit(struct budget *budget)
 
 	budget_unregister(budget);
 
-	tasklet_kill(&budget->vpe_tasklet);
+	cancel_work_sync(&budget->vpe_bh_work);
 
 	saa7146_vfree_destroy_pgtable(dev->pci, budget->grabbing, &budget->pt);
 
@@ -567,16 +575,18 @@ int ttpci_budget_deinit(struct budget *budget)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_deinit);
 
-void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 * isr)
+void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 *isr)
 {
 	struct budget *budget = dev->ext_priv;
 
 	dprintk(8, "dev: %p, budget: %p\n", dev, budget);
 
 	if (*isr & MASK_10)
-		tasklet_schedule(&budget->vpe_tasklet);
+		queue_work(system_bh_wq, &budget->vpe_bh_work);
 }
+EXPORT_SYMBOL_GPL(ttpci_budget_irq10_handler);
 
 void ttpci_budget_set_video_port(struct saa7146_dev *dev, int video_port)
 {
@@ -590,14 +600,7 @@ void ttpci_budget_set_video_port(struct saa7146_dev *dev, int video_port)
 	}
 	spin_unlock(&budget->feedlock);
 }
-
-EXPORT_SYMBOL_GPL(ttpci_budget_debiread);
-EXPORT_SYMBOL_GPL(ttpci_budget_debiwrite);
-EXPORT_SYMBOL_GPL(ttpci_budget_init);
-EXPORT_SYMBOL_GPL(ttpci_budget_init_hooks);
-EXPORT_SYMBOL_GPL(ttpci_budget_deinit);
-EXPORT_SYMBOL_GPL(ttpci_budget_irq10_handler);
 EXPORT_SYMBOL_GPL(ttpci_budget_set_video_port);
-EXPORT_SYMBOL_GPL(budget_debug);
 
+MODULE_DESCRIPTION("base driver for the SAA7146 based Budget DVB cards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index b76a1b330b50..f623c250909b 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 /*
- * budget.c: driver for the SAA7146 based Budget DVB cards
+ * budget.ko: driver for the SAA7146 based Budget DVB cards
+ *            without analog video input or CI
  *
  * Compiled from various sources by Michael Hunold <michael@mihu.de>
  *
@@ -42,20 +43,24 @@ MODULE_PARM_DESC(diseqc_method, "Select DiSEqC method for subsystem id 13c2:1003
 
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
-static void Set22K (struct budget *budget, int state)
+static void Set22K(struct budget *budget, int state)
 {
-	struct saa7146_dev *dev=budget->dev;
+	struct saa7146_dev *dev = budget->dev;
+
 	dprintk(2, "budget: %p\n", budget);
 	saa7146_setgpio(dev, 3, (state ? SAA7146_GPIO_OUTHI : SAA7146_GPIO_OUTLO));
 }
 
-/* Diseqc functions only for TT Budget card */
-/* taken from the Skyvision DVB driver by
-   Ralph Metzler <rjkm@metzlerbros.de> */
+/*
+ * Diseqc functions only for TT Budget card
+ * taken from the Skyvision DVB driver by
+ * Ralph Metzler <rjkm@metzlerbros.de>
+ */
 
-static void DiseqcSendBit (struct budget *budget, int data)
+static void DiseqcSendBit(struct budget *budget, int data)
 {
-	struct saa7146_dev *dev=budget->dev;
+	struct saa7146_dev *dev = budget->dev;
+
 	dprintk(2, "budget: %p\n", budget);
 
 	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
@@ -64,13 +69,13 @@ static void DiseqcSendBit (struct budget *budget, int data)
 	udelay(data ? 1000 : 500);
 }
 
-static void DiseqcSendByte (struct budget *budget, int data)
+static void DiseqcSendByte(struct budget *budget, int data)
 {
-	int i, par=1, d;
+	int i, par = 1, d;
 
 	dprintk(2, "budget: %p\n", budget);
 
-	for (i=7; i>=0; i--) {
+	for (i = 7; i >= 0; i--) {
 		d = (data>>i)&1;
 		par ^= d;
 		DiseqcSendBit(budget, d);
@@ -79,9 +84,9 @@ static void DiseqcSendByte (struct budget *budget, int data)
 	DiseqcSendBit(budget, par);
 }
 
-static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long burst)
+static int SendDiSEqCMsg(struct budget *budget, int len, u8 *msg, unsigned long burst)
 {
-	struct saa7146_dev *dev=budget->dev;
+	struct saa7146_dev *dev = budget->dev;
 	int i;
 
 	dprintk(2, "budget: %p\n", budget);
@@ -89,15 +94,15 @@ static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long
 	saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTLO);
 	mdelay(16);
 
-	for (i=0; i<len; i++)
+	for (i = 0; i < len; i++)
 		DiseqcSendByte(budget, msg[i]);
 
 	mdelay(16);
 
-	if (burst!=-1) {
-		if (burst)
+	if (burst != -1) {
+		if (burst) {
 			DiseqcSendByte(budget, 0xff);
-		else {
+		} else {
 			saa7146_setgpio(dev, 3, SAA7146_GPIO_OUTHI);
 			mdelay(12);
 			udelay(500);
@@ -118,24 +123,24 @@ static int SendDiSEqCMsg (struct budget *budget, int len, u8 *msg, unsigned long
 static int SetVoltage_Activy(struct budget *budget,
 			     enum fe_sec_voltage voltage)
 {
-	struct saa7146_dev *dev=budget->dev;
+	struct saa7146_dev *dev = budget->dev;
 
 	dprintk(2, "budget: %p\n", budget);
 
 	switch (voltage) {
-		case SEC_VOLTAGE_13:
-			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
-			saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTLO);
-			break;
-		case SEC_VOLTAGE_18:
-			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
-			saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
-			break;
-		case SEC_VOLTAGE_OFF:
-			saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTLO);
-			break;
-		default:
-			return -EINVAL;
+	case SEC_VOLTAGE_13:
+		saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
+		saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTLO);
+		break;
+	case SEC_VOLTAGE_18:
+		saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTHI);
+		saa7146_setgpio(dev, 2, SAA7146_GPIO_OUTHI);
+		break;
+	case SEC_VOLTAGE_OFF:
+		saa7146_setgpio(dev, 1, SAA7146_GPIO_OUTLO);
+		break;
+	default:
+		return -EINVAL;
 	}
 
 	return 0;
@@ -146,7 +151,7 @@ static int siemens_budget_set_voltage(struct dvb_frontend *fe,
 {
 	struct budget *budget = fe->dvb->priv;
 
-	return SetVoltage_Activy (budget, voltage);
+	return SetVoltage_Activy(budget, voltage);
 }
 
 static int budget_set_tone(struct dvb_frontend *fe,
@@ -156,11 +161,11 @@ static int budget_set_tone(struct dvb_frontend *fe,
 
 	switch (tone) {
 	case SEC_TONE_ON:
-		Set22K (budget, 1);
+		Set22K(budget, 1);
 		break;
 
 	case SEC_TONE_OFF:
-		Set22K (budget, 0);
+		Set22K(budget, 0);
 		break;
 
 	default:
@@ -170,11 +175,11 @@ static int budget_set_tone(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int budget_diseqc_send_master_cmd(struct dvb_frontend* fe, struct dvb_diseqc_master_cmd* cmd)
+static int budget_diseqc_send_master_cmd(struct dvb_frontend *fe, struct dvb_diseqc_master_cmd *cmd)
 {
 	struct budget *budget = fe->dvb->priv;
 
-	SendDiSEqCMsg (budget, cmd->msg_len, cmd->msg, 0);
+	SendDiSEqCMsg(budget, cmd->msg_len, cmd->msg, 0);
 
 	return 0;
 }
@@ -184,7 +189,7 @@ static int budget_diseqc_send_burst(struct dvb_frontend *fe,
 {
 	struct budget *budget = fe->dvb->priv;
 
-	SendDiSEqCMsg (budget, 0, NULL, minicmd);
+	SendDiSEqCMsg(budget, 0, NULL, minicmd);
 
 	return 0;
 }
@@ -208,7 +213,8 @@ static int alps_bsrv2_tuner_set_params(struct dvb_frontend *fe)
 		pwr = 0;
 	else if (c->frequency >= 1100000)
 		pwr = 1;
-	else pwr = 2;
+	else
+		pwr = 2;
 
 	buf[0] = (div >> 8) & 0x7f;
 	buf[1] = div & 0xff;
@@ -220,12 +226,12 @@ static int alps_bsrv2_tuner_set_params(struct dvb_frontend *fe)
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
 	return 0;
 }
 
-static struct ves1x93_config alps_bsrv2_config =
-{
+static struct ves1x93_config alps_bsrv2_config = {
 	.demod_address = 0x08,
 	.xin = 90100000UL,
 	.invert_pwm = 0,
@@ -248,7 +254,8 @@ static int alps_tdbe2_tuner_set_params(struct dvb_frontend *fe)
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
 	return 0;
 }
 
@@ -303,7 +310,8 @@ static int grundig_29504_401_tuner_set_params(struct dvb_frontend *fe)
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
 	return 0;
 }
 
@@ -333,7 +341,8 @@ static int grundig_29504_451_tuner_set_params(struct dvb_frontend *fe)
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
 	return 0;
 }
 
@@ -365,7 +374,8 @@ static int s5h1420_tuner_set_params(struct dvb_frontend *fe)
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1);
-	if (i2c_transfer (&budget->i2c_adap, &msg, 1) != 1) return -EIO;
+	if (i2c_transfer(&budget->i2c_adap, &msg, 1) != 1)
+		return -EIO;
 
 	return 0;
 }
@@ -422,12 +432,12 @@ static int i2c_readreg(struct i2c_adapter *i2c, u8 adr, u8 reg)
 	return (i2c_transfer(i2c, msg, 2) != 2) ? -EIO : val;
 }
 
-static u8 read_pwm(struct budget* budget)
+static u8 read_pwm(struct budget *budget)
 {
 	u8 b = 0xff;
 	u8 pwm;
-	struct i2c_msg msg[] = { { .addr = 0x50,.flags = 0,.buf = &b,.len = 1 },
-				 { .addr = 0x50,.flags = I2C_M_RD,.buf = &pwm,.len = 1} };
+	struct i2c_msg msg[] = { { .addr = 0x50, .flags = 0, .buf = &b, .len = 1 },
+				 { .addr = 0x50, .flags = I2C_M_RD, .buf = &pwm, .len = 1} };
 
 	if ((i2c_transfer(&budget->i2c_adap, msg, 2) != 2) || (pwm == 0xff))
 		pwm = 0x48;
@@ -478,7 +488,7 @@ static void frontend_init(struct budget *budget)
 {
 	(void)alps_bsbe1_config; /* avoid warning */
 
-	switch(budget->dev->pci->subsystem_device) {
+	switch (budget->dev->pci->subsystem_device) {
 	case 0x1003: // Hauppauge/TT Nova budget (stv0299/ALPS BSRU6(tsa5059) OR ves1893/ALPS BSRV2(sp5659))
 	case 0x1013:
 		// try the ALPS BSRV2 first of all
@@ -527,7 +537,7 @@ static void frontend_init(struct budget *budget)
 	case 0x4f52: /* Cards based on Philips Semi Sylt PCI ref. design */
 		budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config, &budget->i2c_adap);
 		if (budget->dvb_frontend) {
-			printk(KERN_INFO "budget: tuner ALPS BSRU6 in Philips Semi. Sylt detected\n");
+			pr_info("tuner ALPS BSRU6 in Philips Semi. Sylt detected\n");
 			budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
 			budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
 			break;
@@ -545,7 +555,7 @@ static void frontend_init(struct budget *budget)
 			/* assume ALPS BSRU6 */
 			budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsru6_config_activy, &budget->i2c_adap);
 			if (budget->dvb_frontend) {
-				printk(KERN_INFO "budget: tuner ALPS BSRU6 detected\n");
+				pr_info("tuner ALPS BSRU6 detected\n");
 				budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsru6_tuner_set_params;
 				budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
 				budget->dvb_frontend->ops.set_voltage = siemens_budget_set_voltage;
@@ -561,7 +571,7 @@ static void frontend_init(struct budget *budget)
 			msleep(250);
 			budget->dvb_frontend = dvb_attach(stv0299_attach, &alps_bsbe1_config_activy, &budget->i2c_adap);
 			if (budget->dvb_frontend) {
-				printk(KERN_INFO "budget: tuner ALPS BSBE1 detected\n");
+				pr_info("tuner ALPS BSBE1 detected\n");
 				budget->dvb_frontend->ops.tuner_ops.set_params = alps_bsbe1_tuner_set_params;
 				budget->dvb_frontend->tuner_priv = &budget->i2c_adap;
 				budget->dvb_frontend->ops.set_voltage = siemens_budget_set_voltage;
@@ -607,7 +617,7 @@ static void frontend_init(struct budget *budget)
 			budget->dvb_frontend = fe;
 			if (dvb_attach(lnbp21_attach, fe, &budget->i2c_adap,
 				       0, 0) == NULL) {
-				printk("%s: No LNBP21 found!\n", __func__);
+				pr_err("%s(): No LNBP21 found!\n", __func__);
 				goto error_out;
 			}
 			break;
@@ -629,10 +639,10 @@ static void frontend_init(struct budget *budget)
 			budget->dvb_frontend = fe;
 			if (dvb_attach(tda826x_attach, fe, 0x60,
 				       &budget->i2c_adap, 0) == NULL)
-				printk("%s: No tda826x found!\n", __func__);
+				pr_err("%s(): No tda826x found!\n", __func__);
 			if (dvb_attach(lnbp21_attach, fe,
 				       &budget->i2c_adap, 0, 0) == NULL) {
-				printk("%s: No LNBP21 found!\n", __func__);
+				pr_err("%s(): No LNBP21 found!\n", __func__);
 				goto error_out;
 			}
 			break;
@@ -642,6 +652,7 @@ static void frontend_init(struct budget *budget)
 
 	case 0x101c: { /* TT S2-1600 */
 			const struct stv6110x_devctl *ctl;
+
 			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTLO);
 			msleep(50);
 			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTHI);
@@ -672,9 +683,11 @@ static void frontend_init(struct budget *budget)
 					tt1600_stv090x_config.tuner_set_refclk	  = ctl->tuner_set_refclk;
 					tt1600_stv090x_config.tuner_get_status	  = ctl->tuner_get_status;
 
-					/* call the init function once to initialize
-					   tuner's clock output divider and demod's
-					   master clock */
+					/*
+					 * call the init function once to initialize
+					 * tuner's clock output divider and demod's
+					 * master clock
+					 */
 					if (budget->dvb_frontend->ops.init)
 						budget->dvb_frontend->ops.init(budget->dvb_frontend);
 
@@ -682,11 +695,11 @@ static void frontend_init(struct budget *budget)
 						       budget->dvb_frontend,
 						       &budget->i2c_adap,
 						       &tt1600_isl6423_config) == NULL) {
-						printk(KERN_ERR "%s: No Intersil ISL6423 found!\n", __func__);
+						pr_err("%s(): No Intersil ISL6423 found!\n", __func__);
 						goto error_out;
 					}
 				} else {
-					printk(KERN_ERR "%s: No STV6110(A) Silicon Tuner found!\n", __func__);
+					pr_err("%s(): No STV6110(A) Silicon Tuner found!\n", __func__);
 					goto error_out;
 				}
 			}
@@ -695,6 +708,7 @@ static void frontend_init(struct budget *budget)
 
 	case 0x1020: { /* Omicom S2 */
 			const struct stv6110x_devctl *ctl;
+
 			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTLO);
 			msleep(50);
 			saa7146_setgpio(budget->dev, 2, SAA7146_GPIO_OUTHI);
@@ -706,7 +720,7 @@ static void frontend_init(struct budget *budget)
 							  STV090x_DEMODULATOR_0);
 
 			if (budget->dvb_frontend) {
-				printk(KERN_INFO "budget: Omicom S2 detected\n");
+				pr_info("Omicom S2 detected\n");
 
 				ctl = dvb_attach(stv6110x_attach,
 						 budget->dvb_frontend,
@@ -726,9 +740,11 @@ static void frontend_init(struct budget *budget)
 					tt1600_stv090x_config.tuner_set_refclk	  = ctl->tuner_set_refclk;
 					tt1600_stv090x_config.tuner_get_status	  = ctl->tuner_get_status;
 
-					/* call the init function once to initialize
-					   tuner's clock output divider and demod's
-					   master clock */
+					/*
+					 * call the init function once to initialize
+					 * tuner's clock output divider and demod's
+					 * master clock
+					 */
 					if (budget->dvb_frontend->ops.init)
 						budget->dvb_frontend->ops.init(budget->dvb_frontend);
 
@@ -737,12 +753,11 @@ static void frontend_init(struct budget *budget)
 							&budget->i2c_adap,
 							LNBH24_PCL | LNBH24_TTX,
 							LNBH24_TEN, 0x14>>1) == NULL) {
-						printk(KERN_ERR
-						"No LNBH24 found!\n");
+						pr_err("No LNBH24 found!\n");
 						goto error_out;
 					}
 				} else {
-					printk(KERN_ERR "%s: No STV6110(A) Silicon Tuner found!\n", __func__);
+					pr_err("%s(): No STV6110(A) Silicon Tuner found!\n", __func__);
 					goto error_out;
 				}
 			}
@@ -751,7 +766,7 @@ static void frontend_init(struct budget *budget)
 	}
 
 	if (budget->dvb_frontend == NULL) {
-		printk("budget: A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
+		pr_err("A frontend driver was not found for device [%04x:%04x] subsystem [%04x:%04x]\n",
 		       budget->dev->pci->vendor,
 		       budget->dev->pci->device,
 		       budget->dev->pci->subsystem_vendor,
@@ -763,21 +778,19 @@ static void frontend_init(struct budget *budget)
 	return;
 
 error_out:
-	printk("budget: Frontend registration failed!\n");
+	pr_err("Frontend registration failed!\n");
 	dvb_frontend_detach(budget->dvb_frontend);
 	budget->dvb_frontend = NULL;
-	return;
 }
 
-static int budget_attach (struct saa7146_dev* dev, struct saa7146_pci_extension_data *info)
+static int budget_attach(struct saa7146_dev *dev, struct saa7146_pci_extension_data *info)
 {
 	struct budget *budget = NULL;
 	int err;
 
 	budget = kmalloc(sizeof(struct budget), GFP_KERNEL);
-	if( NULL == budget ) {
+	if (budget == NULL)
 		return -ENOMEM;
-	}
 
 	dprintk(2, "dev:%p, info:%p, budget:%p\n", dev, info, budget);
 
@@ -785,8 +798,8 @@ static int budget_attach (struct saa7146_dev* dev, struct saa7146_pci_extension_
 
 	err = ttpci_budget_init(budget, dev, info, THIS_MODULE, adapter_nr);
 	if (err) {
-		printk("==> failed\n");
-		kfree (budget);
+		pr_err("==> failed\n");
+		kfree(budget);
 		return err;
 	}
 
@@ -798,7 +811,7 @@ static int budget_attach (struct saa7146_dev* dev, struct saa7146_pci_extension_
 	return 0;
 }
 
-static int budget_detach (struct saa7146_dev* dev)
+static int budget_detach(struct saa7146_dev *dev)
 {
 	struct budget *budget = dev->ext_priv;
 	int err;
@@ -808,9 +821,9 @@ static int budget_detach (struct saa7146_dev* dev)
 		dvb_frontend_detach(budget->dvb_frontend);
 	}
 
-	err = ttpci_budget_deinit (budget);
+	err = ttpci_budget_deinit(budget);
 
-	kfree (budget);
+	kfree(budget);
 	dev->ext_priv = NULL;
 
 	return err;
@@ -839,8 +852,8 @@ static const struct pci_device_id pci_tbl[] = {
 	MAKE_EXTENSION_PCI(ttbs,  0x13c2, 0x1016),
 	MAKE_EXTENSION_PCI(ttbs1401, 0x13c2, 0x1018),
 	MAKE_EXTENSION_PCI(tt1600, 0x13c2, 0x101c),
-	MAKE_EXTENSION_PCI(fsacs1,0x1131, 0x4f60),
-	MAKE_EXTENSION_PCI(fsacs0,0x1131, 0x4f61),
+	MAKE_EXTENSION_PCI(fsacs1, 0x1131, 0x4f60),
+	MAKE_EXTENSION_PCI(fsacs0, 0x1131, 0x4f61),
 	MAKE_EXTENSION_PCI(fsact1, 0x1131, 0x5f60),
 	MAKE_EXTENSION_PCI(fsact, 0x1131, 0x5f61),
 	MAKE_EXTENSION_PCI(omicom, 0x14c4, 0x1020),
diff --git a/drivers/media/pci/ttpci/budget.h b/drivers/media/pci/ttpci/budget.h
index bd87432e6cde..e933764ac4e3 100644
--- a/drivers/media/pci/ttpci/budget.h
+++ b/drivers/media/pci/ttpci/budget.h
@@ -3,6 +3,12 @@
 #ifndef __BUDGET_DVB__
 #define __BUDGET_DVB__
 
+#ifdef pr_fmt
+#undef pr_fmt
+#endif
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <media/dvb_frontend.h>
 #include <media/dvbdev.h>
 #include <media/demux.h>
@@ -12,6 +18,7 @@
 
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/workqueue.h>
 
 #include <media/drv-intf/saa7146.h>
 
@@ -22,9 +29,8 @@ extern int budget_debug;
 #endif
 
 #define dprintk(level, fmt, arg...) do {				\
-	if (level & budget_debug)					\
-		printk(KERN_DEBUG KBUILD_MODNAME ": %s(): " fmt,	\
-		       __func__, ##arg);				\
+	if ((level) & budget_debug)					\
+		pr_info("%s(): " fmt, __func__, ##arg);			\
 } while (0)
 
 #define TS_SIZE        188
@@ -49,8 +55,8 @@ struct budget {
 	unsigned char *grabbing;
 	struct saa7146_pgtable pt;
 
-	struct tasklet_struct fidb_tasklet;
-	struct tasklet_struct vpe_tasklet;
+	struct work_struct fidb_bh_work;
+	struct work_struct vpe_bh_work;
 
 	struct dmxdev dmxdev;
 	struct dvb_demux demux;
@@ -83,13 +89,13 @@ struct budget {
 	void *priv;
 };
 
-#define MAKE_BUDGET_INFO(x_var,x_name,x_type) \
+#define MAKE_BUDGET_INFO(x_var, x_name, x_type) \
 static struct budget_info x_var ## _info = { \
-	.name=x_name,	\
-	.type=x_type };	\
+	.name = x_name,	\
+	.type = x_type };	\
 static struct saa7146_pci_extension_data x_var = { \
 	.ext_priv = &x_var ## _info, \
-	.ext = &budget_extension };
+	.ext = &budget_extension }
 
 #define BUDGET_TT		   0
 #define BUDGET_TT_HW_DISEQC	   1
@@ -119,7 +125,7 @@ extern int ttpci_budget_init(struct budget *budget, struct saa7146_dev *dev,
 			     struct module *owner, short *adapter_nums);
 extern void ttpci_budget_init_hooks(struct budget *budget);
 extern int ttpci_budget_deinit(struct budget *budget);
-extern void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 * isr);
+extern void ttpci_budget_irq10_handler(struct saa7146_dev *dev, u32 *isr);
 extern void ttpci_budget_set_video_port(struct saa7146_dev *dev, int video_port);
 extern int ttpci_budget_debiread(struct budget *budget, u32 config, int addr, int count,
 				 int uselocks, int nobusyloop);
diff --git a/drivers/media/pci/tw5864/tw5864-core.c b/drivers/media/pci/tw5864/tw5864-core.c
index 560ff1ddcc83..832788603f88 100644
--- a/drivers/media/pci/tw5864/tw5864-core.c
+++ b/drivers/media/pci/tw5864/tw5864-core.c
@@ -24,6 +24,8 @@
 #include "tw5864.h"
 #include "tw5864-reg.h"
 
+#define DRIVER_NAME "tw5864"
+
 MODULE_DESCRIPTION("V4L2 driver module for tw5864-based multimedia capture & encoding devices");
 MODULE_AUTHOR("Bluecherry Maintainers <maintainers@bluecherrydvr.com>");
 MODULE_AUTHOR("Andrey Utkin <andrey.utkin@corp.bluecherry.net>");
@@ -144,7 +146,7 @@ static void tw5864_h264_isr(struct tw5864_dev *dev)
 		cur_frame->gop_seqno = input->frame_gop_seqno;
 
 		dev->h264_buf_w_index = next_frame_index;
-		tasklet_schedule(&dev->tasklet);
+		queue_work(system_bh_wq, &dev->bh_work);
 
 		cur_frame = next_frame;
 
@@ -246,7 +248,8 @@ static int tw5864_initdev(struct pci_dev *pci_dev,
 	if (!dev)
 		return -ENOMEM;
 
-	snprintf(dev->name, sizeof(dev->name), "tw5864:%s", pci_name(pci_dev));
+	snprintf(dev->name, sizeof(dev->name), "%s:%s", DRIVER_NAME,
+		 pci_name(pci_dev));
 
 	err = v4l2_device_register(&pci_dev->dev, &dev->v4l2_dev);
 	if (err)
@@ -269,12 +272,12 @@ static int tw5864_initdev(struct pci_dev *pci_dev,
 	}
 
 	/* get mmio */
-	err = pcim_iomap_regions(pci_dev, BIT(0), dev->name);
+	dev->mmio = pcim_iomap_region(pci_dev, 0, DRIVER_NAME);
+	err = PTR_ERR_OR_ZERO(dev->mmio);
 	if (err) {
 		dev_err(&dev->pci->dev, "Cannot request regions for MMIO\n");
 		goto unreg_v4l2;
 	}
-	dev->mmio = pcim_iomap_table(pci_dev)[0];
 
 	spin_lock_init(&dev->slock);
 
@@ -290,7 +293,7 @@ static int tw5864_initdev(struct pci_dev *pci_dev,
 
 	/* get irq */
 	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw5864_isr,
-			       IRQF_SHARED, "tw5864", dev);
+			       IRQF_SHARED, DRIVER_NAME, dev);
 	if (err < 0) {
 		dev_err(&dev->pci->dev, "can't get IRQ %d\n", pci_dev->irq);
 		goto fini_video;
@@ -324,7 +327,7 @@ static void tw5864_finidev(struct pci_dev *pci_dev)
 }
 
 static struct pci_driver tw5864_pci_driver = {
-	.name = "tw5864",
+	.name = DRIVER_NAME,
 	.id_table = tw5864_pci_tbl,
 	.probe = tw5864_initdev,
 	.remove = tw5864_finidev,
diff --git a/drivers/media/pci/tw5864/tw5864-video.c b/drivers/media/pci/tw5864/tw5864-video.c
index 8b1aae4b6319..0a08708e52b0 100644
--- a/drivers/media/pci/tw5864/tw5864-video.c
+++ b/drivers/media/pci/tw5864/tw5864-video.c
@@ -6,6 +6,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/workqueue.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-event.h>
 #include <media/videobuf2-dma-contig.h>
@@ -175,7 +176,7 @@ static const unsigned int intra4x4_lambda3[] = {
 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std);
 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std);
 
-static void tw5864_handle_frame_task(struct tasklet_struct *t);
+static void tw5864_handle_frame_work(struct work_struct *t);
 static void tw5864_handle_frame(struct tw5864_h264_frame *frame);
 static void tw5864_frame_interval_set(struct tw5864_input *input);
 
@@ -470,8 +471,6 @@ static const struct vb2_ops tw5864_video_qops = {
 	.buf_queue = tw5864_buf_queue,
 	.start_streaming = tw5864_start_streaming,
 	.stop_streaming = tw5864_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -1062,7 +1061,7 @@ int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
 	dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
 	tw5864_irqmask_apply(dev);
 
-	tasklet_setup(&dev->tasklet, tw5864_handle_frame_task);
+	INIT_WORK(&dev->bh_work, tw5864_handle_frame_work);
 
 	for (i = 0; i < TW5864_INPUTS; i++) {
 		dev->inputs[i].root = dev;
@@ -1079,7 +1078,7 @@ fini_video_inputs:
 	for (i = last_input_nr_registered; i >= 0; i--)
 		tw5864_video_input_fini(&dev->inputs[i]);
 
-	tasklet_kill(&dev->tasklet);
+	cancel_work_sync(&dev->bh_work);
 
 free_dma:
 	for (i = last_dma_allocated; i >= 0; i--) {
@@ -1198,7 +1197,7 @@ void tw5864_video_fini(struct tw5864_dev *dev)
 {
 	int i;
 
-	tasklet_kill(&dev->tasklet);
+	cancel_work_sync(&dev->bh_work);
 
 	for (i = 0; i < TW5864_INPUTS; i++)
 		tw5864_video_input_fini(&dev->inputs[i]);
@@ -1315,9 +1314,9 @@ static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
 	return detected;
 }
 
-static void tw5864_handle_frame_task(struct tasklet_struct *t)
+static void tw5864_handle_frame_work(struct work_struct *t)
 {
-	struct tw5864_dev *dev = from_tasklet(dev, t, tasklet);
+	struct tw5864_dev *dev = from_work(dev, t, bh_work);
 	unsigned long flags;
 	int batch_size = H264_BUF_CNT;
 
diff --git a/drivers/media/pci/tw5864/tw5864.h b/drivers/media/pci/tw5864/tw5864.h
index a8b6fbd5b710..2da5f4215fd9 100644
--- a/drivers/media/pci/tw5864/tw5864.h
+++ b/drivers/media/pci/tw5864/tw5864.h
@@ -12,6 +12,7 @@
 #include <linux/mutex.h>
 #include <linux/io.h>
 #include <linux/interrupt.h>
+#include <linux/workqueue.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
@@ -85,7 +86,7 @@ struct tw5864_input {
 	int nr; /* input number */
 	struct tw5864_dev *root;
 	struct mutex lock; /* used for vidq and vdev */
-	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	spinlock_t slock; /* used for sync between ISR, bh_work & V4L2 API */
 	struct video_device vdev;
 	struct v4l2_ctrl_handler hdl;
 	struct vb2_queue vidq;
@@ -142,7 +143,7 @@ struct tw5864_h264_frame {
 
 /* global device status */
 struct tw5864_dev {
-	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	spinlock_t slock; /* used for sync between ISR, bh_work & V4L2 API */
 	struct v4l2_device v4l2_dev;
 	struct tw5864_input inputs[TW5864_INPUTS];
 #define H264_BUF_CNT 4
@@ -150,7 +151,7 @@ struct tw5864_dev {
 	int h264_buf_r_index;
 	int h264_buf_w_index;
 
-	struct tasklet_struct tasklet;
+	struct work_struct bh_work;
 
 	int encoder_busy;
 	/* Input number to check next for ready raw picture (in RR fashion) */
diff --git a/drivers/media/pci/tw68/tw68-video.c b/drivers/media/pci/tw68/tw68-video.c
index cdf5d733b863..77773dec48b8 100644
--- a/drivers/media/pci/tw68/tw68-video.c
+++ b/drivers/media/pci/tw68/tw68-video.c
@@ -524,8 +524,6 @@ static const struct vb2_ops tw68_video_qops = {
 	.buf_finish	= tw68_buf_finish,
 	.start_streaming = tw68_start_streaming,
 	.stop_streaming = tw68_stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------ */
diff --git a/drivers/media/pci/tw686x/tw686x-core.c b/drivers/media/pci/tw686x/tw686x-core.c
index c53099c958ca..f39e0e34deb6 100644
--- a/drivers/media/pci/tw686x/tw686x-core.c
+++ b/drivers/media/pci/tw686x/tw686x-core.c
@@ -125,7 +125,7 @@ void tw686x_enable_channel(struct tw686x_dev *dev, unsigned int channel)
  */
 static void tw686x_dma_delay(struct timer_list *t)
 {
-	struct tw686x_dev *dev = from_timer(dev, t, dma_delay_timer);
+	struct tw686x_dev *dev = timer_container_of(dev, t, dma_delay_timer);
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->lock, flags);
@@ -373,7 +373,7 @@ static void tw686x_remove(struct pci_dev *pci_dev)
 
 	tw686x_video_free(dev);
 	tw686x_audio_free(dev);
-	del_timer_sync(&dev->dma_delay_timer);
+	timer_delete_sync(&dev->dma_delay_timer);
 
 	pci_iounmap(pci_dev, dev->mmio);
 	pci_release_regions(pci_dev);
diff --git a/drivers/media/pci/tw686x/tw686x-video.c b/drivers/media/pci/tw686x/tw686x-video.c
index 63be95fce83d..785dd797d921 100644
--- a/drivers/media/pci/tw686x/tw686x-video.c
+++ b/drivers/media/pci/tw686x/tw686x-video.c
@@ -579,8 +579,6 @@ static const struct vb2_ops tw686x_video_qops = {
 	.buf_prepare		= tw686x_buf_prepare,
 	.start_streaming	= tw686x_start_streaming,
 	.stop_streaming		= tw686x_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/pci/zoran/zoran_card.c b/drivers/media/pci/zoran/zoran_card.c
index 3975fc1b2ee3..e31f9f19a48a 100644
--- a/drivers/media/pci/zoran/zoran_card.c
+++ b/drivers/media/pci/zoran/zoran_card.c
@@ -1202,7 +1202,7 @@ static int zoran_debugfs_show(struct seq_file *seq, void *v)
 	seq_printf(seq, "JPG ver_dcm %u\n", zr->jpg_settings.ver_dcm);
 	seq_printf(seq, "JPG tmp_dcm %u\n", zr->jpg_settings.tmp_dcm);
 	seq_printf(seq, "JPG odd_even %u\n", zr->jpg_settings.odd_even);
-	seq_printf(seq, "JPG crop %dx%d %d %d\n",
+	seq_printf(seq, "JPG crop (%d,%d)/%dx%d\n",
 		   zr->jpg_settings.img_x,
 		   zr->jpg_settings.img_y,
 		   zr->jpg_settings.img_width,
diff --git a/drivers/media/pci/zoran/zoran_driver.c b/drivers/media/pci/zoran/zoran_driver.c
index 5c05e64c71a9..f42f596d3e62 100644
--- a/drivers/media/pci/zoran/zoran_driver.c
+++ b/drivers/media/pci/zoran/zoran_driver.c
@@ -950,8 +950,6 @@ static const struct vb2_ops zr_video_qops = {
 	.buf_prepare            = zr_vb2_prepare,
 	.start_streaming        = zr_vb2_start_streaming,
 	.stop_streaming         = zr_vb2_stop_streaming,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 int zoran_queue_init(struct zoran *zr, struct vb2_queue *vq, int dir)
diff --git a/drivers/media/pci/zoran/zr36016.c b/drivers/media/pci/zoran/zr36016.c
index 4b328ad6083f..d2e136c48a1b 100644
--- a/drivers/media/pci/zoran/zr36016.c
+++ b/drivers/media/pci/zoran/zr36016.c
@@ -216,7 +216,7 @@ static int zr36016_set_video(struct videocodec *codec, const struct tvnorm *norm
 	struct zr36016 *ptr = (struct zr36016 *)codec->data;
 	struct zoran *zr = videocodec_to_zoran(codec);
 
-	zrdev_dbg(zr, "%s: set_video %d.%d, %d/%d-%dx%d (0x%x) call\n",
+	zrdev_dbg(zr, "%s: set_video %d.%d, (%u,%u)/%ux%u (0x%x) call\n",
 		  ptr->name, norm->h_start, norm->v_start,
 		  cap->x, cap->y, cap->width, cap->height,
 		  cap->decimation);
diff --git a/drivers/media/pci/zoran/zr36050.c b/drivers/media/pci/zoran/zr36050.c
index b07d7e5c1b4a..c17965073557 100644
--- a/drivers/media/pci/zoran/zr36050.c
+++ b/drivers/media/pci/zoran/zr36050.c
@@ -547,7 +547,7 @@ static int zr36050_set_video(struct videocodec *codec, const struct tvnorm *norm
 	struct zoran *zr = videocodec_to_zoran(codec);
 	int size;
 
-	zrdev_dbg(zr, "%s: set_video %d.%d, %d/%d-%dx%d (0x%x) q%d call\n",
+	zrdev_dbg(zr, "%s: set_video %d.%d, (%u,%u)/%ux%u (0x%x) q%d call\n",
 		  ptr->name, norm->h_start, norm->v_start,
 		  cap->x, cap->y, cap->width, cap->height,
 		  cap->decimation, cap->quality);
diff --git a/drivers/media/pci/zoran/zr36060.c b/drivers/media/pci/zoran/zr36060.c
index 75fd167603dc..d6c12efc5bb6 100644
--- a/drivers/media/pci/zoran/zr36060.c
+++ b/drivers/media/pci/zoran/zr36060.c
@@ -488,7 +488,7 @@ static int zr36060_set_video(struct videocodec *codec, const struct tvnorm *norm
 	u32 reg;
 	int size;
 
-	zrdev_dbg(zr, "%s: set_video %d/%d-%dx%d (%%%d) call\n", ptr->name,
+	zrdev_dbg(zr, "%s: set_video (%u,%u)/%ux%u (%%%d) call\n", ptr->name,
 		  cap->x, cap->y, cap->width, cap->height, cap->decimation);
 
 	/* if () return -EINVAL;
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 91e54215de3a..9287faafdce5 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -67,8 +67,10 @@ source "drivers/media/platform/amlogic/Kconfig"
 source "drivers/media/platform/amphion/Kconfig"
 source "drivers/media/platform/aspeed/Kconfig"
 source "drivers/media/platform/atmel/Kconfig"
+source "drivers/media/platform/broadcom/Kconfig"
 source "drivers/media/platform/cadence/Kconfig"
 source "drivers/media/platform/chips-media/Kconfig"
+source "drivers/media/platform/imagination/Kconfig"
 source "drivers/media/platform/intel/Kconfig"
 source "drivers/media/platform/marvell/Kconfig"
 source "drivers/media/platform/mediatek/Kconfig"
@@ -77,11 +79,13 @@ source "drivers/media/platform/nuvoton/Kconfig"
 source "drivers/media/platform/nvidia/Kconfig"
 source "drivers/media/platform/nxp/Kconfig"
 source "drivers/media/platform/qcom/Kconfig"
+source "drivers/media/platform/raspberrypi/Kconfig"
 source "drivers/media/platform/renesas/Kconfig"
 source "drivers/media/platform/rockchip/Kconfig"
 source "drivers/media/platform/samsung/Kconfig"
 source "drivers/media/platform/st/Kconfig"
 source "drivers/media/platform/sunxi/Kconfig"
+source "drivers/media/platform/synopsys/Kconfig"
 source "drivers/media/platform/ti/Kconfig"
 source "drivers/media/platform/verisilicon/Kconfig"
 source "drivers/media/platform/via/Kconfig"
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index 3296ec1ebe16..6fd7db0541c7 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -10,8 +10,10 @@ obj-y += amlogic/
 obj-y += amphion/
 obj-y += aspeed/
 obj-y += atmel/
+obj-y += broadcom/
 obj-y += cadence/
 obj-y += chips-media/
+obj-y += imagination/
 obj-y += intel/
 obj-y += marvell/
 obj-y += mediatek/
@@ -20,11 +22,13 @@ obj-y += nuvoton/
 obj-y += nvidia/
 obj-y += nxp/
 obj-y += qcom/
+obj-y += raspberrypi/
 obj-y += renesas/
 obj-y += rockchip/
 obj-y += samsung/
 obj-y += st/
 obj-y += sunxi/
+obj-y += synopsys/
 obj-y += ti/
 obj-y += verisilicon/
 obj-y += via/
diff --git a/drivers/media/platform/allegro-dvt/allegro-core.c b/drivers/media/platform/allegro-dvt/allegro-core.c
index da61f9beb6b4..eb03df0d8652 100644
--- a/drivers/media/platform/allegro-dvt/allegro-core.c
+++ b/drivers/media/platform/allegro-dvt/allegro-core.c
@@ -179,7 +179,7 @@ struct allegro_dev {
 	struct list_head channels;
 };
 
-static struct regmap_config allegro_regmap_config = {
+static const struct regmap_config allegro_regmap_config = {
 	.name = "regmap",
 	.reg_bits = 32,
 	.val_bits = 32,
@@ -188,7 +188,7 @@ static struct regmap_config allegro_regmap_config = {
 	.cache_type = REGCACHE_NONE,
 };
 
-static struct regmap_config allegro_sram_config = {
+static const struct regmap_config allegro_sram_config = {
 	.name = "sram",
 	.reg_bits = 32,
 	.val_bits = 32,
@@ -1415,11 +1415,11 @@ static int allegro_mcu_send_encode_frame(struct allegro_dev *dev,
 static int allegro_mcu_wait_for_init_timeout(struct allegro_dev *dev,
 					     unsigned long timeout_ms)
 {
-	unsigned long tmo;
+	unsigned long time_left;
 
-	tmo = wait_for_completion_timeout(&dev->init_complete,
-					  msecs_to_jiffies(timeout_ms));
-	if (tmo == 0)
+	time_left = wait_for_completion_timeout(&dev->init_complete,
+						msecs_to_jiffies(timeout_ms));
+	if (time_left == 0)
 		return -ETIMEDOUT;
 
 	reinit_completion(&dev->init_complete);
@@ -1509,8 +1509,10 @@ static int allocate_buffers_internal(struct allegro_channel *channel,
 		INIT_LIST_HEAD(&buffer->head);
 
 		err = allegro_alloc_buffer(dev, buffer, size);
-		if (err)
+		if (err) {
+			kfree(buffer);
 			goto err;
+		}
 		list_add(&buffer->head, list);
 	}
 
@@ -2481,14 +2483,14 @@ static void allegro_mcu_interrupt(struct allegro_dev *dev)
 static void allegro_destroy_channel(struct allegro_channel *channel)
 {
 	struct allegro_dev *dev = channel->dev;
-	unsigned long timeout;
+	unsigned long time_left;
 
 	if (channel_exists(channel)) {
 		reinit_completion(&channel->completion);
 		allegro_mcu_send_destroy_channel(dev, channel);
-		timeout = wait_for_completion_timeout(&channel->completion,
-						      msecs_to_jiffies(5000));
-		if (timeout == 0)
+		time_left = wait_for_completion_timeout(&channel->completion,
+							msecs_to_jiffies(5000));
+		if (time_left == 0)
 			v4l2_warn(&dev->v4l2_dev,
 				  "channel %d: timeout while destroying\n",
 				  channel->mcu_channel_id);
@@ -2544,7 +2546,7 @@ static void allegro_destroy_channel(struct allegro_channel *channel)
 static int allegro_create_channel(struct allegro_channel *channel)
 {
 	struct allegro_dev *dev = channel->dev;
-	unsigned long timeout;
+	unsigned long time_left;
 
 	if (channel_exists(channel)) {
 		v4l2_warn(&dev->v4l2_dev,
@@ -2595,9 +2597,9 @@ static int allegro_create_channel(struct allegro_channel *channel)
 
 	reinit_completion(&channel->completion);
 	allegro_mcu_send_create_channel(dev, channel);
-	timeout = wait_for_completion_timeout(&channel->completion,
-					      msecs_to_jiffies(5000));
-	if (timeout == 0)
+	time_left = wait_for_completion_timeout(&channel->completion,
+						msecs_to_jiffies(5000));
+	if (time_left == 0)
 		channel->error = -ETIMEDOUT;
 	if (channel->error)
 		goto err;
@@ -2895,8 +2897,6 @@ static const struct vb2_ops allegro_queue_ops = {
 	.buf_queue = allegro_buf_queue,
 	.start_streaming = allegro_start_streaming,
 	.stop_streaming = allegro_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int allegro_queue_init(void *priv,
@@ -3912,6 +3912,7 @@ static int allegro_probe(struct platform_device *pdev)
 	if (ret < 0) {
 		v4l2_err(&dev->v4l2_dev,
 			 "failed to request firmware: %d\n", ret);
+		v4l2_device_unregister(&dev->v4l2_dev);
 		return ret;
 	}
 
@@ -4003,7 +4004,7 @@ static const struct dev_pm_ops allegro_pm_ops = {
 
 static struct platform_driver allegro_driver = {
 	.probe = allegro_probe,
-	.remove_new = allegro_remove,
+	.remove = allegro_remove,
 	.driver = {
 		.name = "allegro",
 		.of_match_table = allegro_dt_ids,
diff --git a/drivers/media/platform/allegro-dvt/nal-hevc.h b/drivers/media/platform/allegro-dvt/nal-hevc.h
index eb46f12aae80..361e2f55c254 100644
--- a/drivers/media/platform/allegro-dvt/nal-hevc.h
+++ b/drivers/media/platform/allegro-dvt/nal-hevc.h
@@ -96,10 +96,11 @@ struct nal_hevc_vps {
 	unsigned int extension_data_flag;
 };
 
+#define N_HRD_PARAMS 1
 struct nal_hevc_sub_layer_hrd_parameters {
-	unsigned int bit_rate_value_minus1[1];
-	unsigned int cpb_size_value_minus1[1];
-	unsigned int cbr_flag[1];
+	unsigned int bit_rate_value_minus1[N_HRD_PARAMS];
+	unsigned int cpb_size_value_minus1[N_HRD_PARAMS];
+	unsigned int cbr_flag[N_HRD_PARAMS];
 };
 
 struct nal_hevc_hrd_parameters {
diff --git a/drivers/media/platform/amlogic/Kconfig b/drivers/media/platform/amlogic/Kconfig
index 5014957404e9..458acf3d5fa8 100644
--- a/drivers/media/platform/amlogic/Kconfig
+++ b/drivers/media/platform/amlogic/Kconfig
@@ -2,4 +2,5 @@
 
 comment "Amlogic media platform drivers"
 
+source "drivers/media/platform/amlogic/c3/Kconfig"
 source "drivers/media/platform/amlogic/meson-ge2d/Kconfig"
diff --git a/drivers/media/platform/amlogic/Makefile b/drivers/media/platform/amlogic/Makefile
index d3cdb8fa4ddb..c744afcd1b9e 100644
--- a/drivers/media/platform/amlogic/Makefile
+++ b/drivers/media/platform/amlogic/Makefile
@@ -1,2 +1,4 @@
 # SPDX-License-Identifier: GPL-2.0-only
+
+obj-y += c3/
 obj-y += meson-ge2d/
diff --git a/drivers/media/platform/amlogic/c3/Kconfig b/drivers/media/platform/amlogic/c3/Kconfig
new file mode 100644
index 000000000000..d355d3a9358d
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/Kconfig
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+source "drivers/media/platform/amlogic/c3/isp/Kconfig"
+source "drivers/media/platform/amlogic/c3/mipi-adapter/Kconfig"
+source "drivers/media/platform/amlogic/c3/mipi-csi2/Kconfig"
diff --git a/drivers/media/platform/amlogic/c3/Makefile b/drivers/media/platform/amlogic/c3/Makefile
new file mode 100644
index 000000000000..14f305a493d2
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/Makefile
@@ -0,0 +1,5 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+obj-y += isp/
+obj-y += mipi-adapter/
+obj-y += mipi-csi2/
diff --git a/drivers/media/platform/amlogic/c3/isp/Kconfig b/drivers/media/platform/amlogic/c3/isp/Kconfig
new file mode 100644
index 000000000000..02c62a50a5e8
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+config VIDEO_C3_ISP
+	tristate "Amlogic C3 Image Signal Processor (ISP) driver"
+	depends on ARCH_MESON || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on OF
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_VMALLOC
+	help
+	  Video4Linux2 driver for Amlogic C3 ISP pipeline.
+	  The C3 ISP is used for processing raw images and
+	  outputing results to memory.
+
+	  To compile this driver as a module choose m here.
diff --git a/drivers/media/platform/amlogic/c3/isp/Makefile b/drivers/media/platform/amlogic/c3/isp/Makefile
new file mode 100644
index 000000000000..b1b064170b57
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/Makefile
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+c3-isp-objs := c3-isp-dev.o \
+	       c3-isp-params.o \
+	       c3-isp-stats.o \
+	       c3-isp-capture.o \
+	       c3-isp-core.o \
+	       c3-isp-resizer.o
+
+obj-$(CONFIG_VIDEO_C3_ISP) += c3-isp.o
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-capture.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-capture.c
new file mode 100644
index 000000000000..11d85f5342f0
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-capture.c
@@ -0,0 +1,804 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/cleanup.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+#define C3_ISP_WRMIFX3_REG(addr, id)	((addr) + (id) * 0x100)
+
+static const struct c3_isp_cap_format_info cap_formats[] = {
+	/* YUV formats */
+	{
+		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_Y_ONLY,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
+		.hdiv = 1,
+		.vdiv = 1,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
+		.fourcc = V4L2_PIX_FMT_NV12M,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
+		.hdiv = 2,
+		.vdiv = 2,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
+		.fourcc = V4L2_PIX_FMT_NV21M,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
+		.hdiv = 2,
+		.vdiv = 2,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
+		.fourcc = V4L2_PIX_FMT_NV16M,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
+		.hdiv = 1,
+		.vdiv = 2
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
+		.fourcc = V4L2_PIX_FMT_NV61M,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT,
+		.hdiv = 1,
+		.vdiv = 2,
+	},
+	/* RAW formats */
+	{
+		.mbus_code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.fourcc = V4L2_PIX_FMT_SRGGB12,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
+		.hdiv = 1,
+		.vdiv = 1,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.fourcc = V4L2_PIX_FMT_SBGGR12,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
+		.hdiv = 1,
+		.vdiv = 1,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.fourcc = V4L2_PIX_FMT_SGRBG12,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
+		.hdiv = 1,
+		.vdiv = 1,
+	}, {
+		.mbus_code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.fourcc = V4L2_PIX_FMT_SGBRG12,
+		.format = ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW,
+		.planes = ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1,
+		.ch0_pix_bits = ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS,
+		.uv_swap = ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU,
+		.in_bits = ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT,
+		.hdiv = 1,
+		.vdiv = 1,
+	},
+};
+
+/* Hardware configuration */
+
+/* Set the address of wrmifx3(write memory interface) */
+static void c3_isp_cap_wrmifx3_buff(struct c3_isp_capture *cap)
+{
+	dma_addr_t y_dma_addr;
+	dma_addr_t uv_dma_addr;
+
+	if (cap->buff) {
+		y_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_Y];
+		uv_dma_addr = cap->buff->dma_addr[C3_ISP_PLANE_UV];
+	} else {
+		y_dma_addr = cap->dummy_buff.dma_addr;
+		uv_dma_addr = cap->dummy_buff.dma_addr;
+	}
+
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_BADDR, cap->id),
+		     ISP_WRMIFX3_0_CH0_BASE_ADDR(y_dma_addr));
+
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_BADDR, cap->id),
+		     ISP_WRMIFX3_0_CH1_BASE_ADDR(uv_dma_addr));
+}
+
+static void c3_isp_cap_wrmifx3_format(struct c3_isp_capture *cap)
+{
+	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
+	const struct c3_isp_cap_format_info *info = cap->format.info;
+	u32 stride;
+	u32 chrom_h;
+	u32 chrom_v;
+
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_SIZE, cap->id),
+		     ISP_WRMIFX3_0_FMT_SIZE_HSIZE(pix_mp->width) |
+		     ISP_WRMIFX3_0_FMT_SIZE_VSIZE(pix_mp->height));
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
+			   ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_MASK, info->format);
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
+			   ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_MASK,
+			   info->in_bits);
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
+			   ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_MASK, info->planes);
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_FMT_CTRL, cap->id),
+			   ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_MASK,
+			   info->uv_swap);
+
+	stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_Y].bytesperline,
+			      C3_ISP_DMA_SIZE_ALIGN_BYTES);
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL0, cap->id),
+			   ISP_WRMIFX3_0_CH0_CTRL0_STRIDE_MASK,
+			   ISP_WRMIFX3_0_CH0_CTRL0_STRIDE(stride));
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH0_CTRL1, cap->id),
+			   ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_MODE_MASK,
+			   info->ch0_pix_bits);
+
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_H, cap->id),
+		     ISP_WRMIFX3_0_WIN_LUMA_H_LUMA_HEND(pix_mp->width));
+
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_LUMA_V, cap->id),
+		     ISP_WRMIFX3_0_WIN_LUMA_V_LUMA_VEND(pix_mp->height));
+
+	stride = DIV_ROUND_UP(pix_mp->plane_fmt[C3_ISP_PLANE_UV].bytesperline,
+			      C3_ISP_DMA_SIZE_ALIGN_BYTES);
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL0, cap->id),
+			   ISP_WRMIFX3_0_CH1_CTRL0_STRIDE_MASK,
+			   ISP_WRMIFX3_0_CH1_CTRL0_STRIDE(stride));
+
+	c3_isp_update_bits(cap->isp,
+			   C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_CH1_CTRL1, cap->id),
+			   ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_MODE_MASK,
+			   ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_16BITS);
+
+	chrom_h = DIV_ROUND_UP(pix_mp->width, info->hdiv);
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_H, cap->id),
+		     ISP_WRMIFX3_0_WIN_CHROM_H_CHROM_HEND(chrom_h));
+
+	chrom_v = DIV_ROUND_UP(pix_mp->height, info->vdiv);
+	c3_isp_write(cap->isp,
+		     C3_ISP_WRMIFX3_REG(ISP_WRMIFX3_0_WIN_CHROM_V, cap->id),
+		     ISP_WRMIFX3_0_WIN_CHROM_V_CHROM_VEND(chrom_v));
+}
+
+static int c3_isp_cap_dummy_buff_create(struct c3_isp_capture *cap)
+{
+	struct c3_isp_dummy_buffer *dummy_buff = &cap->dummy_buff;
+	struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
+
+	if (pix_mp->num_planes == 1)
+		dummy_buff->size = pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage;
+	else
+		dummy_buff->size =
+			max(pix_mp->plane_fmt[C3_ISP_PLANE_Y].sizeimage,
+			    pix_mp->plane_fmt[C3_ISP_PLANE_UV].sizeimage);
+
+	/* The driver never access vaddr, no mapping is required */
+	dummy_buff->vaddr = dma_alloc_attrs(cap->isp->dev, dummy_buff->size,
+					    &dummy_buff->dma_addr, GFP_KERNEL,
+					    DMA_ATTR_NO_KERNEL_MAPPING);
+	if (!dummy_buff->vaddr)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void c3_isp_cap_dummy_buff_destroy(struct c3_isp_capture *cap)
+{
+	dma_free_attrs(cap->isp->dev, cap->dummy_buff.size,
+		       cap->dummy_buff.vaddr, cap->dummy_buff.dma_addr,
+		       DMA_ATTR_NO_KERNEL_MAPPING);
+}
+
+static void c3_isp_cap_cfg_buff(struct c3_isp_capture *cap)
+{
+	cap->buff = list_first_entry_or_null(&cap->pending,
+					     struct c3_isp_cap_buffer, list);
+
+	c3_isp_cap_wrmifx3_buff(cap);
+
+	if (cap->buff)
+		list_del(&cap->buff->list);
+}
+
+static void c3_isp_cap_start(struct c3_isp_capture *cap)
+{
+	u32 mask;
+	u32 val;
+
+	scoped_guard(spinlock_irqsave, &cap->buff_lock)
+		c3_isp_cap_cfg_buff(cap);
+
+	c3_isp_cap_wrmifx3_format(cap);
+
+	if (cap->id == C3_ISP_CAP_DEV_0) {
+		mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF0_EN;
+	} else if (cap->id == C3_ISP_CAP_DEV_1) {
+		mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF1_EN;
+	} else {
+		mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF2_EN;
+	}
+
+	c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val);
+}
+
+static void c3_isp_cap_stop(struct c3_isp_capture *cap)
+{
+	u32 mask;
+	u32 val;
+
+	if (cap->id == C3_ISP_CAP_DEV_0) {
+		mask = ISP_TOP_PATH_EN_WRMIF0_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF0_DIS;
+	} else if (cap->id == C3_ISP_CAP_DEV_1) {
+		mask = ISP_TOP_PATH_EN_WRMIF1_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF1_DIS;
+	} else {
+		mask = ISP_TOP_PATH_EN_WRMIF2_EN_MASK;
+		val = ISP_TOP_PATH_EN_WRMIF2_DIS;
+	}
+
+	c3_isp_update_bits(cap->isp, ISP_TOP_PATH_EN, mask, val);
+}
+
+static void c3_isp_cap_done(struct c3_isp_capture *cap)
+{
+	struct c3_isp_cap_buffer *buff = cap->buff;
+
+	guard(spinlock_irqsave)(&cap->buff_lock);
+
+	if (buff) {
+		buff->vb.sequence = cap->isp->frm_sequence;
+		buff->vb.vb2_buf.timestamp = ktime_get();
+		buff->vb.field = V4L2_FIELD_NONE;
+		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	c3_isp_cap_cfg_buff(cap);
+}
+
+/* V4L2 video operations */
+
+static const struct c3_isp_cap_format_info *c3_cap_find_fmt(u32 fourcc)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(cap_formats); i++) {
+		if (cap_formats[i].fourcc == fourcc)
+			return &cap_formats[i];
+	}
+
+	return NULL;
+}
+
+static void c3_cap_try_fmt(struct v4l2_pix_format_mplane *pix_mp)
+{
+	const struct c3_isp_cap_format_info *fmt;
+	const struct v4l2_format_info *info;
+	struct v4l2_plane_pix_format *plane;
+
+	fmt = c3_cap_find_fmt(pix_mp->pixelformat);
+	if (!fmt)
+		fmt = &cap_formats[0];
+
+	pix_mp->width = clamp(pix_mp->width, C3_ISP_MIN_WIDTH,
+			      C3_ISP_MAX_WIDTH);
+	pix_mp->height = clamp(pix_mp->height, C3_ISP_MIN_HEIGHT,
+			       C3_ISP_MAX_HEIGHT);
+	pix_mp->pixelformat = fmt->fourcc;
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
+	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	pix_mp->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+
+	info = v4l2_format_info(fmt->fourcc);
+	pix_mp->num_planes = info->mem_planes;
+	memset(pix_mp->plane_fmt, 0, sizeof(pix_mp->plane_fmt));
+
+	for (unsigned int i = 0; i < info->comp_planes; i++) {
+		unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
+		unsigned int vdiv = (i == 0) ? 1 : info->vdiv;
+
+		plane = &pix_mp->plane_fmt[i];
+
+		plane->bytesperline = DIV_ROUND_UP(pix_mp->width, hdiv) *
+				      info->bpp[i] / info->bpp_div[i];
+		plane->bytesperline = ALIGN(plane->bytesperline,
+					    C3_ISP_DMA_SIZE_ALIGN_BYTES);
+		plane->sizeimage = plane->bytesperline *
+				   DIV_ROUND_UP(pix_mp->height, vdiv);
+	}
+}
+
+static void c3_isp_cap_return_buffers(struct c3_isp_capture *cap,
+				      enum vb2_buffer_state state)
+{
+	struct c3_isp_cap_buffer *buff;
+
+	guard(spinlock_irqsave)(&cap->buff_lock);
+
+	if (cap->buff) {
+		vb2_buffer_done(&cap->buff->vb.vb2_buf, state);
+		cap->buff = NULL;
+	}
+
+	while (!list_empty(&cap->pending)) {
+		buff = list_first_entry(&cap->pending,
+					struct c3_isp_cap_buffer, list);
+		list_del(&buff->list);
+		vb2_buffer_done(&buff->vb.vb2_buf, state);
+	}
+}
+
+static int c3_isp_cap_querycap(struct file *file, void *fh,
+			       struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, C3_ISP_DRIVER_NAME, sizeof(cap->driver));
+	strscpy(cap->card, "AML C3 ISP", sizeof(cap->card));
+
+	return 0;
+}
+
+static int c3_isp_cap_enum_fmt(struct file *file, void *fh,
+			       struct v4l2_fmtdesc *f)
+{
+	const struct c3_isp_cap_format_info *fmt;
+	unsigned int index = 0;
+	unsigned int i;
+
+	if (!f->mbus_code) {
+		if (f->index >= ARRAY_SIZE(cap_formats))
+			return -EINVAL;
+
+		fmt = &cap_formats[f->index];
+		f->pixelformat = fmt->fourcc;
+		return 0;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(cap_formats); i++) {
+		fmt = &cap_formats[i];
+		if (f->mbus_code != fmt->mbus_code)
+			continue;
+
+		if (index++ == f->index) {
+			f->pixelformat = cap_formats[i].fourcc;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int c3_isp_cap_g_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct c3_isp_capture *cap = video_drvdata(file);
+
+	f->fmt.pix_mp = cap->format.pix_mp;
+
+	return 0;
+}
+
+static int c3_isp_cap_s_fmt_mplane(struct file *file, void *fh,
+				   struct v4l2_format *f)
+{
+	struct c3_isp_capture *cap = video_drvdata(file);
+
+	c3_cap_try_fmt(&f->fmt.pix_mp);
+
+	cap->format.pix_mp = f->fmt.pix_mp;
+	cap->format.info = c3_cap_find_fmt(f->fmt.pix_mp.pixelformat);
+
+	return 0;
+}
+
+static int c3_isp_cap_try_fmt_mplane(struct file *file, void *fh,
+				     struct v4l2_format *f)
+{
+	c3_cap_try_fmt(&f->fmt.pix_mp);
+
+	return 0;
+}
+
+static int c3_isp_cap_enum_frmsize(struct file *file, void *fh,
+				   struct v4l2_frmsizeenum *fsize)
+{
+	const struct c3_isp_cap_format_info *fmt;
+
+	if (fsize->index)
+		return -EINVAL;
+
+	fmt = c3_cap_find_fmt(fsize->pixel_format);
+	if (!fmt)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = C3_ISP_MIN_WIDTH;
+	fsize->stepwise.min_height = C3_ISP_MIN_HEIGHT;
+	fsize->stepwise.max_width = C3_ISP_MAX_WIDTH;
+	fsize->stepwise.max_height = C3_ISP_MAX_HEIGHT;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.step_height = 2;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops isp_cap_v4l2_ioctl_ops = {
+	.vidioc_querycap		= c3_isp_cap_querycap,
+	.vidioc_enum_fmt_vid_cap	= c3_isp_cap_enum_fmt,
+	.vidioc_g_fmt_vid_cap_mplane	= c3_isp_cap_g_fmt_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= c3_isp_cap_s_fmt_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= c3_isp_cap_try_fmt_mplane,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_enum_framesizes         = c3_isp_cap_enum_frmsize,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_file_operations isp_cap_v4l2_fops = {
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+};
+
+static int c3_isp_cap_link_validate(struct media_link *link)
+{
+	struct video_device *vdev =
+		media_entity_to_video_device(link->sink->entity);
+	struct v4l2_subdev *sd =
+		media_entity_to_v4l2_subdev(link->source->entity);
+	struct c3_isp_capture *cap = video_get_drvdata(vdev);
+	struct v4l2_subdev_format src_fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.pad = link->source->index,
+	};
+	int ret;
+
+	ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &src_fmt);
+	if (ret)
+		return ret;
+
+	if (src_fmt.format.width != cap->format.pix_mp.width ||
+	    src_fmt.format.height != cap->format.pix_mp.height ||
+	    src_fmt.format.code != cap->format.info->mbus_code) {
+		dev_err(cap->isp->dev,
+			"link %s: %u -> %s: %u not valid: 0x%04x/%ux%u not match 0x%04x/%ux%u\n",
+			link->source->entity->name, link->source->index,
+			link->sink->entity->name, link->sink->index,
+			src_fmt.format.code, src_fmt.format.width,
+			src_fmt.format.height, cap->format.info->mbus_code,
+			cap->format.pix_mp.width, cap->format.pix_mp.height);
+
+		return -EPIPE;
+	}
+
+	return 0;
+}
+
+static const struct media_entity_operations isp_cap_entity_ops = {
+	.link_validate = c3_isp_cap_link_validate,
+};
+
+static int c3_isp_vb2_queue_setup(struct vb2_queue *q,
+				  unsigned int *num_buffers,
+				  unsigned int *num_planes,
+				  unsigned int sizes[],
+				  struct device *alloc_devs[])
+{
+	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
+	const struct v4l2_pix_format_mplane *pix_mp = &cap->format.pix_mp;
+	unsigned int i;
+
+	if (*num_planes) {
+		if (*num_planes != pix_mp->num_planes)
+			return -EINVAL;
+
+		for (i = 0; i < pix_mp->num_planes; i++)
+			if (sizes[i] < pix_mp->plane_fmt[i].sizeimage)
+				return -EINVAL;
+
+		return 0;
+	}
+
+	*num_planes = pix_mp->num_planes;
+	for (i = 0; i < pix_mp->num_planes; i++)
+		sizes[i] = pix_mp->plane_fmt[i].sizeimage;
+
+	return 0;
+}
+
+static void c3_isp_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_cap_buffer *buf =
+			container_of(v4l2_buf, struct c3_isp_cap_buffer, vb);
+	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
+
+	guard(spinlock_irqsave)(&cap->buff_lock);
+
+	list_add_tail(&buf->list, &cap->pending);
+}
+
+static int c3_isp_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+
+	for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++) {
+		size = cap->format.pix_mp.plane_fmt[i].sizeimage;
+		if (vb2_plane_size(vb, i) < size) {
+			dev_err(cap->isp->dev,
+				"User buffer too small (%ld < %lu)\n",
+				vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	return 0;
+}
+
+static int c3_isp_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct c3_isp_capture *cap = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_cap_buffer *buf =
+		container_of(v4l2_buf, struct c3_isp_cap_buffer, vb);
+
+	for (unsigned int i = 0; i < cap->format.pix_mp.num_planes; i++)
+		buf->dma_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+
+	return 0;
+}
+
+static int c3_isp_vb2_start_streaming(struct vb2_queue *q,
+				      unsigned int count)
+{
+	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
+	int ret;
+
+	ret = video_device_pipeline_start(&cap->vdev, &cap->isp->pipe);
+	if (ret) {
+		dev_err(cap->isp->dev,
+			"Failed to start cap%u pipeline: %d\n", cap->id, ret);
+		goto err_return_buffers;
+	}
+
+	ret = c3_isp_cap_dummy_buff_create(cap);
+	if (ret)
+		goto err_pipeline_stop;
+
+	ret = pm_runtime_resume_and_get(cap->isp->dev);
+	if (ret)
+		goto err_dummy_destroy;
+
+	c3_isp_cap_start(cap);
+
+	ret = v4l2_subdev_enable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE,
+					 BIT(0));
+	if (ret)
+		goto err_pm_put;
+
+	return 0;
+
+err_pm_put:
+	pm_runtime_put(cap->isp->dev);
+err_dummy_destroy:
+	c3_isp_cap_dummy_buff_destroy(cap);
+err_pipeline_stop:
+	video_device_pipeline_stop(&cap->vdev);
+err_return_buffers:
+	c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void c3_isp_vb2_stop_streaming(struct vb2_queue *q)
+{
+	struct c3_isp_capture *cap = vb2_get_drv_priv(q);
+
+	c3_isp_cap_stop(cap);
+
+	c3_isp_cap_return_buffers(cap, VB2_BUF_STATE_ERROR);
+
+	v4l2_subdev_disable_streams(&cap->rsz->sd, C3_ISP_RSZ_PAD_SOURCE,
+				    BIT(0));
+
+	pm_runtime_put(cap->isp->dev);
+
+	c3_isp_cap_dummy_buff_destroy(cap);
+
+	video_device_pipeline_stop(&cap->vdev);
+}
+
+static const struct vb2_ops isp_video_vb2_ops = {
+	.queue_setup = c3_isp_vb2_queue_setup,
+	.buf_queue = c3_isp_vb2_buf_queue,
+	.buf_prepare = c3_isp_vb2_buf_prepare,
+	.buf_init = c3_isp_vb2_buf_init,
+	.start_streaming = c3_isp_vb2_start_streaming,
+	.stop_streaming = c3_isp_vb2_stop_streaming,
+};
+
+static int c3_isp_register_capture(struct c3_isp_capture *cap)
+{
+	struct video_device *vdev = &cap->vdev;
+	struct vb2_queue *vb2_q = &cap->vb2_q;
+	int ret;
+
+	snprintf(vdev->name, sizeof(vdev->name), "c3-isp-cap%u", cap->id);
+	vdev->fops = &isp_cap_v4l2_fops;
+	vdev->ioctl_ops = &isp_cap_v4l2_ioctl_ops;
+	vdev->v4l2_dev = &cap->isp->v4l2_dev;
+	vdev->entity.ops = &isp_cap_entity_ops;
+	vdev->lock = &cap->lock;
+	vdev->minor = -1;
+	vdev->queue = vb2_q;
+	vdev->release = video_device_release_empty;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
+			    V4L2_CAP_STREAMING;
+	vdev->vfl_dir = VFL_DIR_RX;
+	video_set_drvdata(vdev, cap);
+
+	vb2_q->drv_priv = cap;
+	vb2_q->mem_ops = &vb2_dma_contig_memops;
+	vb2_q->ops = &isp_video_vb2_ops;
+	vb2_q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
+	vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vb2_q->buf_struct_size = sizeof(struct c3_isp_cap_buffer);
+	vb2_q->dev = cap->isp->dev;
+	vb2_q->lock = &cap->lock;
+
+	ret = vb2_queue_init(vb2_q);
+	if (ret)
+		goto err_destroy;
+
+	cap->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, &cap->pad);
+	if (ret)
+		goto err_queue_release;
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(cap->isp->dev,
+			"Failed to register %s: %d\n", vdev->name, ret);
+		goto err_entity_cleanup;
+	}
+
+	return 0;
+
+err_entity_cleanup:
+	media_entity_cleanup(&vdev->entity);
+err_queue_release:
+	vb2_queue_release(vb2_q);
+err_destroy:
+	mutex_destroy(&cap->lock);
+	return ret;
+}
+
+int c3_isp_captures_register(struct c3_isp_device *isp)
+{
+	int ret;
+	unsigned int i;
+	struct c3_isp_capture *cap;
+
+	for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) {
+		cap = &isp->caps[i];
+		memset(cap, 0, sizeof(*cap));
+
+		cap->format.pix_mp.width = C3_ISP_DEFAULT_WIDTH;
+		cap->format.pix_mp.height = C3_ISP_DEFAULT_HEIGHT;
+		cap->format.pix_mp.field = V4L2_FIELD_NONE;
+		cap->format.pix_mp.pixelformat = V4L2_PIX_FMT_NV12M;
+		cap->format.pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
+		cap->format.info =
+			c3_cap_find_fmt(cap->format.pix_mp.pixelformat);
+
+		c3_cap_try_fmt(&cap->format.pix_mp);
+
+		cap->id = i;
+		cap->rsz = &isp->resizers[i];
+		cap->isp = isp;
+		INIT_LIST_HEAD(&cap->pending);
+		spin_lock_init(&cap->buff_lock);
+		mutex_init(&cap->lock);
+
+		ret = c3_isp_register_capture(cap);
+		if (ret) {
+			cap->isp = NULL;
+			mutex_destroy(&cap->lock);
+			c3_isp_captures_unregister(isp);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+void c3_isp_captures_unregister(struct c3_isp_device *isp)
+{
+	unsigned int i;
+	struct c3_isp_capture *cap;
+
+	for (i = C3_ISP_CAP_DEV_0; i < C3_ISP_NUM_CAP_DEVS; i++) {
+		cap = &isp->caps[i];
+
+		if (!cap->isp)
+			continue;
+		vb2_queue_release(&cap->vb2_q);
+		media_entity_cleanup(&cap->vdev.entity);
+		video_unregister_device(&cap->vdev);
+		mutex_destroy(&cap->lock);
+	}
+}
+
+void c3_isp_captures_isr(struct c3_isp_device *isp)
+{
+	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_0]);
+	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_1]);
+	c3_isp_cap_done(&isp->caps[C3_ISP_CAP_DEV_2]);
+}
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-common.h b/drivers/media/platform/amlogic/c3/isp/c3-isp-common.h
new file mode 100644
index 000000000000..cb470802e61e
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-common.h
@@ -0,0 +1,340 @@
+/* SPDX-License-Identifier: (GPL-2.0-only OR MIT) */
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#ifndef __C3_ISP_COMMON_H__
+#define __C3_ISP_COMMON_H__
+
+#include <linux/clk.h>
+
+#include <media/media-device.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-v4l2.h>
+
+#define C3_ISP_DRIVER_NAME		"c3-isp"
+#define C3_ISP_CLOCK_NUM_MAX		3
+
+#define C3_ISP_DEFAULT_WIDTH		1920
+#define C3_ISP_DEFAULT_HEIGHT		1080
+#define C3_ISP_MAX_WIDTH		2888
+#define C3_ISP_MAX_HEIGHT		2240
+#define C3_ISP_MIN_WIDTH		160
+#define C3_ISP_MIN_HEIGHT		120
+
+#define C3_ISP_DMA_SIZE_ALIGN_BYTES	16
+
+enum c3_isp_core_pads {
+	C3_ISP_CORE_PAD_SINK_VIDEO,
+	C3_ISP_CORE_PAD_SINK_PARAMS,
+	C3_ISP_CORE_PAD_SOURCE_STATS,
+	C3_ISP_CORE_PAD_SOURCE_VIDEO_0,
+	C3_ISP_CORE_PAD_SOURCE_VIDEO_1,
+	C3_ISP_CORE_PAD_SOURCE_VIDEO_2,
+	C3_ISP_CORE_PAD_MAX
+};
+
+enum c3_isp_resizer_ids {
+	C3_ISP_RSZ_0,
+	C3_ISP_RSZ_1,
+	C3_ISP_RSZ_2,
+	C3_ISP_NUM_RSZ
+};
+
+enum c3_isp_resizer_pads {
+	C3_ISP_RSZ_PAD_SINK,
+	C3_ISP_RSZ_PAD_SOURCE,
+	C3_ISP_RSZ_PAD_MAX
+};
+
+enum c3_isp_cap_devs {
+	C3_ISP_CAP_DEV_0,
+	C3_ISP_CAP_DEV_1,
+	C3_ISP_CAP_DEV_2,
+	C3_ISP_NUM_CAP_DEVS
+};
+
+enum c3_isp_planes {
+	C3_ISP_PLANE_Y,
+	C3_ISP_PLANE_UV,
+	C3_ISP_NUM_PLANES
+};
+
+/*
+ * struct c3_isp_cap_format_info - The image format of capture device
+ *
+ * @mbus_code: the mbus code
+ * @fourcc: the pixel format
+ * @format: defines the output format of hardware
+ * @planes: defines the mutil plane of hardware
+ * @ch0_pix_bits: defines the channel 0 pixel bits mode of hardware
+ * @uv_swap: defines the uv swap flag of hardware
+ * @in_bits: defines the input bits of hardware
+ * @hdiv: horizontal chroma subsampling factor of hardware
+ * @vdiv: vertical chroma subsampling factor of hardware
+ */
+struct c3_isp_cap_format_info {
+	u32 mbus_code;
+	u32 fourcc;
+	u32 format;
+	u32 planes;
+	u32 ch0_pix_bits;
+	u8 uv_swap;
+	u8 in_bits;
+	u8 hdiv;
+	u8 vdiv;
+};
+
+/*
+ * struct c3_isp_cap_buffer - A container of vb2 buffer used by the video
+ *                            devices: capture video devices
+ *
+ * @vb: vb2 buffer
+ * @dma_addr: buffer physical address
+ * @list: entry of the buffer in the queue
+ */
+struct c3_isp_cap_buffer {
+	struct vb2_v4l2_buffer vb;
+	dma_addr_t dma_addr[C3_ISP_NUM_PLANES];
+	struct list_head list;
+};
+
+/*
+ * struct c3_isp_stats_dma_buffer - A container of vb2 buffer used by the video
+ *                                  devices: stats video devices
+ *
+ * @vb: vb2 buffer
+ * @dma_addr: buffer physical address
+ * @list: entry of the buffer in the queue
+ */
+struct c3_isp_stats_buffer {
+	struct vb2_v4l2_buffer vb;
+	dma_addr_t dma_addr;
+	struct list_head list;
+};
+
+/*
+ * struct c3_isp_params_buffer - A container of vb2 buffer used by the
+ *                               params video device
+ *
+ * @vb: vb2 buffer
+ * @cfg: scratch buffer used for caching the ISP configuration parameters
+ * @list: entry of the buffer in the queue
+ */
+struct c3_isp_params_buffer {
+	struct vb2_v4l2_buffer vb;
+	void *cfg;
+	struct list_head list;
+};
+
+/*
+ * struct c3_isp_dummy_buffer - A buffer to write the next frame to in case
+ *				there are no vb2 buffers available.
+ *
+ * @vaddr:	return value of call to dma_alloc_attrs
+ * @dma_addr:	dma address of the buffer
+ * @size:	size of the buffer
+ */
+struct c3_isp_dummy_buffer {
+	void *vaddr;
+	dma_addr_t dma_addr;
+	u32 size;
+};
+
+/*
+ * struct c3_isp_core - ISP core subdev
+ *
+ * @sd: ISP sub-device
+ * @pads: ISP sub-device pads
+ * @src_pad: source sub-device pad
+ * @isp: pointer to c3_isp_device
+ */
+struct c3_isp_core {
+	struct v4l2_subdev sd;
+	struct media_pad pads[C3_ISP_CORE_PAD_MAX];
+	struct media_pad *src_pad;
+	struct c3_isp_device *isp;
+};
+
+/*
+ * struct c3_isp_resizer - ISP resizer subdev
+ *
+ * @id: resizer id
+ * @sd: resizer sub-device
+ * @pads: resizer sub-device pads
+ * @src_sd: source sub-device
+ * @isp: pointer to c3_isp_device
+ * @src_pad: the pad of source sub-device
+ */
+struct c3_isp_resizer {
+	enum c3_isp_resizer_ids id;
+	struct v4l2_subdev sd;
+	struct media_pad pads[C3_ISP_RSZ_PAD_MAX];
+	struct v4l2_subdev *src_sd;
+	struct c3_isp_device *isp;
+	u32 src_pad;
+};
+
+/*
+ * struct c3_isp_stats - ISP statistics device
+ *
+ * @vb2_q: vb2 buffer queue
+ * @vdev: video node
+ * @vfmt: v4l2_format of the metadata format
+ * @pad: media pad
+ * @lock: protects vb2_q, vdev
+ * @isp: pointer to c3_isp_device
+ * @buff: in use buffer
+ * @buff_lock: protects stats buffer
+ * @pending: stats buffer list head
+ */
+struct c3_isp_stats {
+	struct vb2_queue vb2_q;
+	struct video_device vdev;
+	struct v4l2_format vfmt;
+	struct media_pad pad;
+
+	struct mutex lock; /* Protects vb2_q, vdev */
+	struct c3_isp_device *isp;
+
+	struct c3_isp_stats_buffer *buff;
+	spinlock_t buff_lock; /* Protects stats buffer */
+	struct list_head pending;
+};
+
+/*
+ * struct c3_isp_params - ISP parameters device
+ *
+ * @vb2_q: vb2 buffer queue
+ * @vdev: video node
+ * @vfmt: v4l2_format of the metadata format
+ * @pad: media pad
+ * @lock: protects vb2_q, vdev
+ * @isp: pointer to c3_isp_device
+ * @buff: in use buffer
+ * @buff_lock: protects stats buffer
+ * @pending: stats buffer list head
+ */
+struct c3_isp_params {
+	struct vb2_queue vb2_q;
+	struct video_device vdev;
+	struct v4l2_format vfmt;
+	struct media_pad pad;
+
+	struct mutex lock; /* Protects vb2_q, vdev */
+	struct c3_isp_device *isp;
+
+	struct c3_isp_params_buffer *buff;
+	spinlock_t buff_lock; /* Protects params buffer */
+	struct list_head pending;
+};
+
+/*
+ * struct c3_isp_capture - ISP capture device
+ *
+ * @id: capture device ID
+ * @vb2_q: vb2 buffer queue
+ * @vdev: video node
+ * @pad: media pad
+ * @lock: protects vb2_q, vdev
+ * @isp: pointer to c3_isp_device
+ * @rsz: pointer to c3_isp_resizer
+ * @buff: in use buffer
+ * @buff_lock: protects capture buffer
+ * @pending: capture buffer list head
+ * @format.info: a pointer to the c3_isp_capture_format of the pixel format
+ * @format.fmt: buffer format
+ */
+struct c3_isp_capture {
+	enum c3_isp_cap_devs id;
+	struct vb2_queue vb2_q;
+	struct video_device vdev;
+	struct media_pad pad;
+
+	struct mutex lock; /* Protects vb2_q, vdev */
+	struct c3_isp_device *isp;
+	struct c3_isp_resizer *rsz;
+
+	struct c3_isp_dummy_buffer dummy_buff;
+	struct c3_isp_cap_buffer *buff;
+	spinlock_t buff_lock; /* Protects stream buffer */
+	struct list_head pending;
+	struct {
+		const struct c3_isp_cap_format_info *info;
+		struct v4l2_pix_format_mplane pix_mp;
+	} format;
+};
+
+/**
+ * struct c3_isp_info - ISP information
+ *
+ * @clocks: array of ISP clock names
+ * @clock_num: actual clock number
+ */
+struct c3_isp_info {
+	char *clocks[C3_ISP_CLOCK_NUM_MAX];
+	u32 clock_num;
+};
+
+/**
+ * struct c3_isp_device - ISP platform device
+ *
+ * @dev: pointer to the struct device
+ * @base: base register address
+ * @clks: array of clocks
+ * @notifier: notifier to register on the v4l2-async API
+ * @v4l2_dev: v4l2_device variable
+ * @media_dev: media device variable
+ * @pipe: media pipeline
+ * @core: ISP core subdev
+ * @resizers: ISP resizer subdev
+ * @stats: ISP stats device
+ * @params: ISP params device
+ * @caps: array of ISP capture device
+ * @frm_sequence: used to record frame id
+ * @info: version-specific ISP information
+ */
+struct c3_isp_device {
+	struct device *dev;
+	void __iomem *base;
+	struct clk_bulk_data clks[C3_ISP_CLOCK_NUM_MAX];
+
+	struct v4l2_async_notifier notifier;
+	struct v4l2_device v4l2_dev;
+	struct media_device media_dev;
+	struct media_pipeline pipe;
+
+	struct c3_isp_core core;
+	struct c3_isp_resizer resizers[C3_ISP_NUM_RSZ];
+	struct c3_isp_stats stats;
+	struct c3_isp_params params;
+	struct c3_isp_capture caps[C3_ISP_NUM_CAP_DEVS];
+
+	u32 frm_sequence;
+	const struct c3_isp_info *info;
+};
+
+u32 c3_isp_read(struct c3_isp_device *isp, u32 reg);
+void c3_isp_write(struct c3_isp_device *isp, u32 reg, u32 val);
+void c3_isp_update_bits(struct c3_isp_device *isp, u32 reg, u32 mask, u32 val);
+
+void c3_isp_core_queue_sof(struct c3_isp_device *isp);
+int c3_isp_core_register(struct c3_isp_device *isp);
+void c3_isp_core_unregister(struct c3_isp_device *isp);
+int c3_isp_resizers_register(struct c3_isp_device *isp);
+void c3_isp_resizers_unregister(struct c3_isp_device *isp);
+int c3_isp_captures_register(struct c3_isp_device *isp);
+void c3_isp_captures_unregister(struct c3_isp_device *isp);
+void c3_isp_captures_isr(struct c3_isp_device *isp);
+void c3_isp_stats_pre_cfg(struct c3_isp_device *isp);
+int c3_isp_stats_register(struct c3_isp_device *isp);
+void c3_isp_stats_unregister(struct c3_isp_device *isp);
+void c3_isp_stats_isr(struct c3_isp_device *isp);
+void c3_isp_params_pre_cfg(struct c3_isp_device *isp);
+int c3_isp_params_register(struct c3_isp_device *isp);
+void c3_isp_params_unregister(struct c3_isp_device *isp);
+void c3_isp_params_isr(struct c3_isp_device *isp);
+
+#endif
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-core.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-core.c
new file mode 100644
index 000000000000..ff6413fff889
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-core.c
@@ -0,0 +1,641 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/media/amlogic/c3-isp-config.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-event.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+#define C3_ISP_CORE_SUBDEV_NAME		"c3-isp-core"
+
+#define C3_ISP_PHASE_OFFSET_0		0
+#define C3_ISP_PHASE_OFFSET_1		1
+#define C3_ISP_PHASE_OFFSET_NONE	0xff
+
+#define C3_ISP_CORE_DEF_SINK_PAD_FMT	MEDIA_BUS_FMT_SRGGB10_1X10
+#define C3_ISP_CORE_DEF_SRC_PAD_FMT	MEDIA_BUS_FMT_YUV10_1X30
+
+/*
+ * struct c3_isp_core_format_info - ISP core format information
+ *
+ * @mbus_code: the mbus code
+ * @pads: bitmask detailing valid pads for this mbus_code
+ * @xofst: horizontal phase offset of hardware
+ * @yofst: vertical phase offset of hardware
+ * @is_raw: the raw format flag of mbus code
+ */
+struct c3_isp_core_format_info {
+	u32 mbus_code;
+	u32 pads;
+	u8 xofst;
+	u8 yofst;
+	bool is_raw;
+};
+
+static const struct c3_isp_core_format_info c3_isp_core_fmts[] = {
+	/* RAW formats */
+	{
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_0,
+		.yofst		= C3_ISP_PHASE_OFFSET_1,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGBRG10_1X10,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_1,
+		.yofst		= C3_ISP_PHASE_OFFSET_1,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_0,
+		.yofst		= C3_ISP_PHASE_OFFSET_0,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SRGGB10_1X10,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_1,
+		.yofst		= C3_ISP_PHASE_OFFSET_0,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR12_1X12,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_0,
+		.yofst		= C3_ISP_PHASE_OFFSET_1,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGBRG12_1X12,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_1,
+		.yofst		= C3_ISP_PHASE_OFFSET_1,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG12_1X12,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_0,
+		.yofst		= C3_ISP_PHASE_OFFSET_0,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SRGGB12_1X12,
+		.pads		= BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
+		.xofst		= C3_ISP_PHASE_OFFSET_1,
+		.yofst		= C3_ISP_PHASE_OFFSET_0,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SRGGB16_1X16,
+		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
+		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR16_1X16,
+		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
+		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG16_1X16,
+		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
+		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.is_raw		= true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGBRG16_1X16,
+		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
+				| BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
+		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.is_raw		= true,
+	},
+	/* YUV formats */
+	{
+		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
+		.pads		= BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0) |
+				  BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1) |
+				  BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
+		.xofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.yofst		= C3_ISP_PHASE_OFFSET_NONE,
+		.is_raw		= false,
+	},
+};
+
+static const struct c3_isp_core_format_info
+*core_find_format_by_code(u32 code, u32 pad)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
+		const struct c3_isp_core_format_info *info =
+			&c3_isp_core_fmts[i];
+
+		if (info->mbus_code == code && info->pads & BIT(pad))
+			return info;
+	}
+
+	return NULL;
+}
+
+static const struct c3_isp_core_format_info
+*core_find_format_by_index(u32 index, u32 pad)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
+		const struct c3_isp_core_format_info *info =
+			&c3_isp_core_fmts[i];
+
+		if (!(info->pads & BIT(pad)))
+			continue;
+
+		if (!index)
+			return info;
+
+		index--;
+	}
+
+	return NULL;
+}
+
+static void c3_isp_core_enable(struct c3_isp_device *isp)
+{
+	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
+			   ISP_TOP_IRQ_EN_FRM_END_EN);
+	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
+			   ISP_TOP_IRQ_EN_FRM_RST_EN);
+
+	/* Enable image data to ISP core */
+	c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
+			   ISP_TOP_PATH_SEL_CORE_MIPI_CORE);
+}
+
+static void c3_isp_core_disable(struct c3_isp_device *isp)
+{
+	/* Disable image data to ISP core */
+	c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
+			   ISP_TOP_PATH_SEL_CORE_CORE_DIS);
+
+	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
+			   ISP_TOP_IRQ_EN_FRM_END_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
+			   ISP_TOP_IRQ_EN_FRM_RST_DIS);
+}
+
+/* Set the phase offset of blc, wb and lns */
+static void c3_isp_core_lswb_ofst(struct c3_isp_device *isp,
+				  u8 xofst, u8 yofst)
+{
+	c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
+			   ISP_LSWB_BLC_PHSOFST_HORIZ_OFST_MASK,
+			   ISP_LSWB_BLC_PHSOFST_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
+			   ISP_LSWB_BLC_PHSOFST_VERT_OFST_MASK,
+			   ISP_LSWB_BLC_PHSOFST_VERT_OFST(yofst));
+
+	c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
+			   ISP_LSWB_WB_PHSOFST_HORIZ_OFST_MASK,
+			   ISP_LSWB_WB_PHSOFST_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
+			   ISP_LSWB_WB_PHSOFST_VERT_OFST_MASK,
+			   ISP_LSWB_WB_PHSOFST_VERT_OFST(yofst));
+
+	c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
+			   ISP_LSWB_LNS_PHSOFST_HORIZ_OFST_MASK,
+			   ISP_LSWB_LNS_PHSOFST_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
+			   ISP_LSWB_LNS_PHSOFST_VERT_OFST_MASK,
+			   ISP_LSWB_LNS_PHSOFST_VERT_OFST(yofst));
+}
+
+/* Set the phase offset of af, ae and awb */
+static void c3_isp_core_3a_ofst(struct c3_isp_device *isp,
+				u8 xofst, u8 yofst)
+{
+	c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_HORIZ_OFST_MASK,
+			   ISP_AF_CTRL_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_VERT_OFST_MASK,
+			   ISP_AF_CTRL_VERT_OFST(yofst));
+
+	c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_HORIZ_OFST_MASK,
+			   ISP_AE_CTRL_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_VERT_OFST_MASK,
+			   ISP_AE_CTRL_VERT_OFST(yofst));
+
+	c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_HORIZ_OFST_MASK,
+			   ISP_AWB_CTRL_HORIZ_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_VERT_OFST_MASK,
+			   ISP_AWB_CTRL_VERT_OFST(yofst));
+}
+
+/* Set the phase offset of demosaic */
+static void c3_isp_core_dms_ofst(struct c3_isp_device *isp,
+				 u8 xofst, u8 yofst)
+{
+	c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
+			   ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST_MASK,
+			   ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST(xofst));
+	c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
+			   ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST_MASK,
+			   ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST(yofst));
+}
+
+static void c3_isp_core_cfg_format(struct c3_isp_device *isp,
+				   struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	const struct c3_isp_core_format_info *isp_fmt;
+
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
+	isp_fmt = core_find_format_by_code(fmt->code,
+					   C3_ISP_CORE_PAD_SINK_VIDEO);
+
+	c3_isp_write(isp, ISP_TOP_INPUT_SIZE,
+		     ISP_TOP_INPUT_SIZE_HORIZ_SIZE(fmt->width) |
+		     ISP_TOP_INPUT_SIZE_VERT_SIZE(fmt->height));
+	c3_isp_write(isp, ISP_TOP_FRM_SIZE,
+		     ISP_TOP_FRM_SIZE_CORE_HORIZ_SIZE(fmt->width) |
+		     ISP_TOP_FRM_SIZE_CORE_VERT_SIZE(fmt->height));
+
+	c3_isp_update_bits(isp, ISP_TOP_HOLD_SIZE,
+			   ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE_MASK,
+			   ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE(fmt->width));
+
+	c3_isp_write(isp, ISP_AF_HV_SIZE,
+		     ISP_AF_HV_SIZE_GLB_WIN_XSIZE(fmt->width) |
+		     ISP_AF_HV_SIZE_GLB_WIN_YSIZE(fmt->height));
+	c3_isp_write(isp, ISP_AE_HV_SIZE,
+		     ISP_AE_HV_SIZE_HORIZ_SIZE(fmt->width) |
+		     ISP_AE_HV_SIZE_VERT_SIZE(fmt->height));
+	c3_isp_write(isp, ISP_AWB_HV_SIZE,
+		     ISP_AWB_HV_SIZE_HORIZ_SIZE(fmt->width) |
+		     ISP_AWB_HV_SIZE_VERT_SIZE(fmt->height));
+
+	c3_isp_core_lswb_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
+	c3_isp_core_3a_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
+	c3_isp_core_dms_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
+}
+
+static bool c3_isp_core_streams_ready(struct c3_isp_core *core)
+{
+	unsigned int n_links = 0;
+	struct media_link *link;
+
+	for_each_media_entity_data_link(&core->sd.entity, link) {
+		if ((link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
+		     link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
+		     link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_2) &&
+		    link->flags == MEDIA_LNK_FL_ENABLED)
+			n_links++;
+	}
+
+	return n_links == core->isp->pipe.start_count;
+}
+
+static int c3_isp_core_enable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct c3_isp_core *core = v4l2_get_subdevdata(sd);
+	struct media_pad *sink_pad;
+	struct v4l2_subdev *src_sd;
+	int ret;
+
+	if (!c3_isp_core_streams_ready(core))
+		return 0;
+
+	core->isp->frm_sequence = 0;
+	c3_isp_core_cfg_format(core->isp, state);
+	c3_isp_core_enable(core->isp);
+
+	sink_pad = &core->pads[C3_ISP_CORE_PAD_SINK_VIDEO];
+	core->src_pad = media_pad_remote_pad_unique(sink_pad);
+	if (IS_ERR(core->src_pad)) {
+		dev_dbg(core->isp->dev,
+			"Failed to get source pad for ISP core\n");
+		return -EPIPE;
+	}
+
+	src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);
+
+	ret = v4l2_subdev_enable_streams(src_sd, core->src_pad->index, BIT(0));
+	if (ret) {
+		c3_isp_core_disable(core->isp);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int c3_isp_core_disable_streams(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       u32 pad, u64 streams_mask)
+{
+	struct c3_isp_core *core = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *src_sd;
+
+	if (core->isp->pipe.start_count != 1)
+		return 0;
+
+	if (core->src_pad) {
+		src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);
+		v4l2_subdev_disable_streams(src_sd, core->src_pad->index,
+					    BIT(0));
+	}
+	core->src_pad = NULL;
+
+	c3_isp_core_disable(core->isp);
+
+	return 0;
+}
+
+static int c3_isp_core_enum_mbus_code(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct c3_isp_core_format_info *info;
+
+	switch (code->pad) {
+	case C3_ISP_CORE_PAD_SINK_VIDEO:
+	case C3_ISP_CORE_PAD_SOURCE_VIDEO_0:
+	case C3_ISP_CORE_PAD_SOURCE_VIDEO_1:
+	case C3_ISP_CORE_PAD_SOURCE_VIDEO_2:
+		info = core_find_format_by_index(code->index, code->pad);
+		if (!info)
+			return -EINVAL;
+
+		code->code = info->mbus_code;
+
+		break;
+	case C3_ISP_CORE_PAD_SINK_PARAMS:
+	case C3_ISP_CORE_PAD_SOURCE_STATS:
+		if (code->index)
+			return -EINVAL;
+
+		code->code = MEDIA_BUS_FMT_METADATA_FIXED;
+
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void c3_isp_core_set_sink_fmt(struct v4l2_subdev_state *state,
+				     struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+	const struct c3_isp_core_format_info *isp_fmt;
+
+	sink_fmt = v4l2_subdev_state_get_format(state, format->pad);
+
+	isp_fmt = core_find_format_by_code(format->format.code, format->pad);
+	if (!isp_fmt)
+		sink_fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
+	else
+		sink_fmt->code = format->format.code;
+
+	sink_fmt->width = clamp_t(u32, format->format.width,
+				  C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
+	sink_fmt->height = clamp_t(u32, format->format.height,
+				   C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
+	sink_fmt->field = V4L2_FIELD_NONE;
+	sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
+	sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
+	     i < C3_ISP_CORE_PAD_MAX; i++) {
+		src_fmt = v4l2_subdev_state_get_format(state, i);
+
+		src_fmt->width  = sink_fmt->width;
+		src_fmt->height = sink_fmt->height;
+	}
+
+	format->format = *sink_fmt;
+}
+
+static void c3_isp_core_set_source_fmt(struct v4l2_subdev_state *state,
+				       struct v4l2_subdev_format *format)
+{
+	const struct c3_isp_core_format_info *isp_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+	struct v4l2_mbus_framefmt *sink_fmt;
+
+	sink_fmt = v4l2_subdev_state_get_format(state,
+						C3_ISP_CORE_PAD_SINK_VIDEO);
+	src_fmt = v4l2_subdev_state_get_format(state, format->pad);
+
+	isp_fmt = core_find_format_by_code(format->format.code, format->pad);
+	if (!isp_fmt)
+		src_fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
+	else
+		src_fmt->code = format->format.code;
+
+	src_fmt->width = sink_fmt->width;
+	src_fmt->height = sink_fmt->height;
+	src_fmt->field = V4L2_FIELD_NONE;
+	src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+
+	if (isp_fmt && isp_fmt->is_raw) {
+		src_fmt->colorspace = V4L2_COLORSPACE_RAW;
+		src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+		src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	} else {
+		src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+		src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	}
+
+	format->format = *src_fmt;
+}
+
+static int c3_isp_core_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
+			       struct v4l2_subdev_format *format)
+{
+	if (format->pad == C3_ISP_CORE_PAD_SINK_VIDEO)
+		c3_isp_core_set_sink_fmt(state, format);
+	else if (format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
+		 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
+		 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_2)
+		c3_isp_core_set_source_fmt(state, format);
+	else
+		format->format =
+			*v4l2_subdev_state_get_format(state, format->pad);
+
+	return 0;
+}
+
+static int c3_isp_core_init_state(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	/* Video sink pad */
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
+	fmt->width = C3_ISP_DEFAULT_WIDTH;
+	fmt->height = C3_ISP_DEFAULT_HEIGHT;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	/* Video source pad */
+	for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
+	     i < C3_ISP_CORE_PAD_MAX; i++) {
+		fmt = v4l2_subdev_state_get_format(state, i);
+		fmt->width = C3_ISP_DEFAULT_WIDTH;
+		fmt->height = C3_ISP_DEFAULT_HEIGHT;
+		fmt->field = V4L2_FIELD_NONE;
+		fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
+		fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+		fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+		fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	}
+
+	/* Parameters pad */
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_PARAMS);
+	fmt->width = 0;
+	fmt->height = 0;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;
+
+	/* Statistics pad */
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SOURCE_STATS);
+	fmt->width = 0;
+	fmt->height = 0;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;
+
+	return 0;
+}
+
+static int c3_isp_core_subscribe_event(struct v4l2_subdev *sd,
+				       struct v4l2_fh *fh,
+				       struct v4l2_event_subscription *sub)
+{
+	if (sub->type != V4L2_EVENT_FRAME_SYNC)
+		return -EINVAL;
+
+	/* V4L2_EVENT_FRAME_SYNC doesn't need id, so should set 0 */
+	if (sub->id != 0)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, 0, NULL);
+}
+
+static const struct v4l2_subdev_pad_ops c3_isp_core_pad_ops = {
+	.enum_mbus_code = c3_isp_core_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = c3_isp_core_set_fmt,
+	.enable_streams = c3_isp_core_enable_streams,
+	.disable_streams = c3_isp_core_disable_streams,
+};
+
+static const struct v4l2_subdev_core_ops c3_isp_core_core_ops = {
+	.subscribe_event = c3_isp_core_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_ops c3_isp_core_subdev_ops = {
+	.core = &c3_isp_core_core_ops,
+	.pad = &c3_isp_core_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops c3_isp_core_internal_ops = {
+	.init_state = c3_isp_core_init_state,
+};
+
+static int c3_isp_core_link_validate(struct media_link *link)
+{
+	if (link->sink->index == C3_ISP_CORE_PAD_SINK_PARAMS)
+		return 0;
+
+	return v4l2_subdev_link_validate(link);
+}
+
+/* Media entity operations */
+static const struct media_entity_operations c3_isp_core_entity_ops = {
+	.link_validate = c3_isp_core_link_validate,
+};
+
+void c3_isp_core_queue_sof(struct c3_isp_device *isp)
+{
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+	};
+
+	event.u.frame_sync.frame_sequence = isp->frm_sequence;
+	v4l2_event_queue(isp->core.sd.devnode, &event);
+}
+
+int c3_isp_core_register(struct c3_isp_device *isp)
+{
+	struct c3_isp_core *core = &isp->core;
+	struct v4l2_subdev *sd = &core->sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &c3_isp_core_subdev_ops);
+	sd->owner = THIS_MODULE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
+	sd->internal_ops = &c3_isp_core_internal_ops;
+	snprintf(sd->name, sizeof(sd->name), "%s", C3_ISP_CORE_SUBDEV_NAME);
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->entity.ops = &c3_isp_core_entity_ops;
+
+	core->isp = isp;
+	sd->dev = isp->dev;
+	v4l2_set_subdevdata(sd, core);
+
+	core->pads[C3_ISP_CORE_PAD_SINK_VIDEO].flags = MEDIA_PAD_FL_SINK;
+	core->pads[C3_ISP_CORE_PAD_SINK_PARAMS].flags = MEDIA_PAD_FL_SINK;
+	core->pads[C3_ISP_CORE_PAD_SOURCE_STATS].flags = MEDIA_PAD_FL_SOURCE;
+	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_0].flags = MEDIA_PAD_FL_SOURCE;
+	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_1].flags = MEDIA_PAD_FL_SOURCE;
+	core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_2].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, C3_ISP_CORE_PAD_MAX,
+				     core->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(&isp->v4l2_dev, sd);
+	if (ret)
+		goto err_subdev_cleanup;
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(sd);
+err_entity_cleanup:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+void c3_isp_core_unregister(struct c3_isp_device *isp)
+{
+	struct c3_isp_core *core = &isp->core;
+	struct v4l2_subdev *sd = &core->sd;
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+}
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-dev.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-dev.c
new file mode 100644
index 000000000000..c3b779f63088
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-dev.c
@@ -0,0 +1,421 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+u32 c3_isp_read(struct c3_isp_device *isp, u32 reg)
+{
+	return readl(isp->base + reg);
+}
+
+void c3_isp_write(struct c3_isp_device *isp, u32 reg, u32 val)
+{
+	writel(val, isp->base + reg);
+}
+
+void c3_isp_update_bits(struct c3_isp_device *isp, u32 reg, u32 mask, u32 val)
+{
+	u32 orig, tmp;
+
+	orig = c3_isp_read(isp, reg);
+
+	tmp = orig & ~mask;
+	tmp |= val & mask;
+
+	if (tmp != orig)
+		c3_isp_write(isp, reg, tmp);
+}
+
+/* PM runtime suspend */
+static int c3_isp_runtime_suspend(struct device *dev)
+{
+	struct c3_isp_device *isp = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(isp->info->clock_num, isp->clks);
+
+	return 0;
+}
+
+/* PM runtime resume */
+static int c3_isp_runtime_resume(struct device *dev)
+{
+	struct c3_isp_device *isp = dev_get_drvdata(dev);
+
+	return clk_bulk_prepare_enable(isp->info->clock_num, isp->clks);
+}
+
+static const struct dev_pm_ops c3_isp_pm_ops = {
+	SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+			    pm_runtime_force_resume)
+	RUNTIME_PM_OPS(c3_isp_runtime_suspend,
+		       c3_isp_runtime_resume, NULL)
+};
+
+/* IRQ handling */
+static irqreturn_t c3_isp_irq_handler(int irq, void *dev)
+{
+	struct c3_isp_device *isp = dev;
+	u32 status;
+
+	/* Get irq status and clear irq status */
+	status = c3_isp_read(isp, ISP_TOP_RO_IRQ_STAT);
+	c3_isp_write(isp, ISP_TOP_IRQ_CLR, status);
+
+	if (status & ISP_TOP_RO_IRQ_STAT_FRM_END_MASK) {
+		c3_isp_stats_isr(isp);
+		c3_isp_params_isr(isp);
+		c3_isp_captures_isr(isp);
+		isp->frm_sequence++;
+	}
+
+	if (status & ISP_TOP_RO_IRQ_STAT_FRM_RST_MASK)
+		c3_isp_core_queue_sof(isp);
+
+	return IRQ_HANDLED;
+}
+
+/* Subdev notifier register */
+static int c3_isp_notify_bound(struct v4l2_async_notifier *notifier,
+			       struct v4l2_subdev *sd,
+			       struct v4l2_async_connection *asc)
+{
+	struct c3_isp_device *isp =
+		container_of(notifier, struct c3_isp_device, notifier);
+	struct media_pad *sink =
+		&isp->core.sd.entity.pads[C3_ISP_CORE_PAD_SINK_VIDEO];
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
+					       MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static int c3_isp_notify_complete(struct v4l2_async_notifier *notifier)
+{
+	struct c3_isp_device *isp =
+		container_of(notifier, struct c3_isp_device, notifier);
+
+	return v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+}
+
+static const struct v4l2_async_notifier_operations c3_isp_notify_ops = {
+	.bound = c3_isp_notify_bound,
+	.complete = c3_isp_notify_complete,
+};
+
+static int c3_isp_async_nf_register(struct c3_isp_device *isp)
+{
+	struct v4l2_async_connection *asc;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_nf_init(&isp->notifier, &isp->v4l2_dev);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(isp->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENOTCONN;
+
+	asc = v4l2_async_nf_add_fwnode_remote(&isp->notifier, ep,
+					      struct v4l2_async_connection);
+	fwnode_handle_put(ep);
+
+	if (IS_ERR(asc))
+		return PTR_ERR(asc);
+
+	isp->notifier.ops = &c3_isp_notify_ops;
+	ret = v4l2_async_nf_register(&isp->notifier);
+	if (ret)
+		v4l2_async_nf_cleanup(&isp->notifier);
+
+	return ret;
+}
+
+static void c3_isp_async_nf_unregister(struct c3_isp_device *isp)
+{
+	v4l2_async_nf_unregister(&isp->notifier);
+	v4l2_async_nf_cleanup(&isp->notifier);
+}
+
+static int c3_isp_media_register(struct c3_isp_device *isp)
+{
+	struct media_device *media_dev = &isp->media_dev;
+	struct v4l2_device *v4l2_dev = &isp->v4l2_dev;
+	int ret;
+
+	/* Initialize media device */
+	strscpy(media_dev->model, C3_ISP_DRIVER_NAME, sizeof(media_dev->model));
+	media_dev->dev = isp->dev;
+
+	media_device_init(media_dev);
+
+	/* Initialize v4l2 device */
+	v4l2_dev->mdev = media_dev;
+	strscpy(v4l2_dev->name, C3_ISP_DRIVER_NAME, sizeof(v4l2_dev->name));
+
+	ret = v4l2_device_register(isp->dev, v4l2_dev);
+	if (ret)
+		goto err_media_dev_cleanup;
+
+	ret = media_device_register(&isp->media_dev);
+	if (ret) {
+		dev_err(isp->dev, "Failed to register media device: %d\n", ret);
+		goto err_unreg_v4l2_dev;
+	}
+
+	return 0;
+
+err_unreg_v4l2_dev:
+	v4l2_device_unregister(&isp->v4l2_dev);
+err_media_dev_cleanup:
+	media_device_cleanup(media_dev);
+	return ret;
+}
+
+static void c3_isp_media_unregister(struct c3_isp_device *isp)
+{
+	media_device_unregister(&isp->media_dev);
+	v4l2_device_unregister(&isp->v4l2_dev);
+	media_device_cleanup(&isp->media_dev);
+}
+
+static void c3_isp_remove_links(struct c3_isp_device *isp)
+{
+	unsigned int i;
+
+	media_entity_remove_links(&isp->core.sd.entity);
+
+	for (i = 0; i < C3_ISP_NUM_RSZ; i++)
+		media_entity_remove_links(&isp->resizers[i].sd.entity);
+
+	for (i = 0; i < C3_ISP_NUM_CAP_DEVS; i++)
+		media_entity_remove_links(&isp->caps[i].vdev.entity);
+}
+
+static int c3_isp_create_links(struct c3_isp_device *isp)
+{
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < C3_ISP_NUM_RSZ; i++) {
+		ret = media_create_pad_link(&isp->resizers[i].sd.entity,
+					    C3_ISP_RSZ_PAD_SOURCE,
+					    &isp->caps[i].vdev.entity, 0,
+					    MEDIA_LNK_FL_ENABLED |
+					    MEDIA_LNK_FL_IMMUTABLE);
+		if (ret) {
+			dev_err(isp->dev,
+				"Failed to link rsz %u and cap %u\n", i, i);
+			goto err_remove_links;
+		}
+
+		ret = media_create_pad_link(&isp->core.sd.entity,
+					    C3_ISP_CORE_PAD_SOURCE_VIDEO_0 + i,
+					    &isp->resizers[i].sd.entity,
+					    C3_ISP_RSZ_PAD_SINK,
+					    MEDIA_LNK_FL_ENABLED);
+		if (ret) {
+			dev_err(isp->dev,
+				"Failed to link core and rsz %u\n", i);
+			goto err_remove_links;
+		}
+	}
+
+	ret = media_create_pad_link(&isp->core.sd.entity,
+				    C3_ISP_CORE_PAD_SOURCE_STATS,
+				    &isp->stats.vdev.entity,
+				    0, MEDIA_LNK_FL_ENABLED);
+	if (ret) {
+		dev_err(isp->dev, "Failed to link core and stats\n");
+		goto err_remove_links;
+	}
+
+	ret = media_create_pad_link(&isp->params.vdev.entity, 0,
+				    &isp->core.sd.entity,
+				    C3_ISP_CORE_PAD_SINK_PARAMS,
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret) {
+		dev_err(isp->dev, "Failed to link params and core\n");
+		goto err_remove_links;
+	}
+
+	return 0;
+
+err_remove_links:
+	c3_isp_remove_links(isp);
+	return ret;
+}
+
+static int c3_isp_videos_register(struct c3_isp_device *isp)
+{
+	int ret;
+
+	ret = c3_isp_captures_register(isp);
+	if (ret)
+		return ret;
+
+	ret = c3_isp_stats_register(isp);
+	if (ret)
+		goto err_captures_unregister;
+
+	ret = c3_isp_params_register(isp);
+	if (ret)
+		goto err_stats_unregister;
+
+	ret = c3_isp_create_links(isp);
+	if (ret)
+		goto err_params_unregister;
+
+	return 0;
+
+err_params_unregister:
+	c3_isp_params_unregister(isp);
+err_stats_unregister:
+	c3_isp_stats_unregister(isp);
+err_captures_unregister:
+	c3_isp_captures_unregister(isp);
+	return ret;
+}
+
+static void c3_isp_videos_unregister(struct c3_isp_device *isp)
+{
+	c3_isp_remove_links(isp);
+	c3_isp_params_unregister(isp);
+	c3_isp_stats_unregister(isp);
+	c3_isp_captures_unregister(isp);
+}
+
+static int c3_isp_get_clocks(struct c3_isp_device *isp)
+{
+	const struct c3_isp_info *info = isp->info;
+
+	for (unsigned int i = 0; i < info->clock_num; i++)
+		isp->clks[i].id = info->clocks[i];
+
+	return devm_clk_bulk_get(isp->dev, info->clock_num, isp->clks);
+}
+
+static int c3_isp_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct c3_isp_device *isp;
+	int irq;
+	int ret;
+
+	isp = devm_kzalloc(dev, sizeof(*isp), GFP_KERNEL);
+	if (!isp)
+		return -ENOMEM;
+
+	isp->info = of_device_get_match_data(dev);
+	isp->dev = dev;
+
+	isp->base = devm_platform_ioremap_resource_byname(pdev, "isp");
+	if (IS_ERR(isp->base))
+		return dev_err_probe(dev, PTR_ERR(isp->base),
+				     "Failed to ioremap resource\n");
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	ret = c3_isp_get_clocks(isp);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get clocks\n");
+
+	platform_set_drvdata(pdev, isp);
+
+	pm_runtime_enable(dev);
+
+	ret = c3_isp_media_register(isp);
+	if (ret)
+		goto err_runtime_disable;
+
+	ret = c3_isp_core_register(isp);
+	if (ret)
+		goto err_v4l2_unregister;
+
+	ret = c3_isp_resizers_register(isp);
+	if (ret)
+		goto err_core_unregister;
+
+	ret = c3_isp_async_nf_register(isp);
+	if (ret)
+		goto err_resizers_unregister;
+
+	ret = devm_request_irq(dev, irq,
+			       c3_isp_irq_handler, IRQF_SHARED,
+			       dev_driver_string(dev), isp);
+	if (ret)
+		goto err_nf_unregister;
+
+	ret = c3_isp_videos_register(isp);
+	if (ret)
+		goto err_nf_unregister;
+
+	return 0;
+
+err_nf_unregister:
+	c3_isp_async_nf_unregister(isp);
+err_resizers_unregister:
+	c3_isp_resizers_unregister(isp);
+err_core_unregister:
+	c3_isp_core_unregister(isp);
+err_v4l2_unregister:
+	c3_isp_media_unregister(isp);
+err_runtime_disable:
+	pm_runtime_disable(dev);
+	return ret;
+};
+
+static void c3_isp_remove(struct platform_device *pdev)
+{
+	struct c3_isp_device *isp = platform_get_drvdata(pdev);
+
+	c3_isp_videos_unregister(isp);
+	c3_isp_async_nf_unregister(isp);
+	c3_isp_core_unregister(isp);
+	c3_isp_resizers_unregister(isp);
+	c3_isp_media_unregister(isp);
+	pm_runtime_disable(isp->dev);
+};
+
+static const struct c3_isp_info isp_info = {
+	.clocks = {"vapb", "isp0"},
+	.clock_num = 2
+};
+
+static const struct of_device_id c3_isp_of_match[] = {
+	{ .compatible = "amlogic,c3-isp",
+	  .data = &isp_info },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, c3_isp_of_match);
+
+static struct platform_driver c3_isp_driver = {
+	.probe = c3_isp_probe,
+	.remove = c3_isp_remove,
+	.driver = {
+		.name = "c3-isp",
+		.of_match_table = c3_isp_of_match,
+		.pm = pm_ptr(&c3_isp_pm_ops),
+	},
+};
+
+module_platform_driver(c3_isp_driver);
+
+MODULE_AUTHOR("Keke Li <keke.li@amlogic.com>");
+MODULE_DESCRIPTION("Amlogic C3 ISP pipeline");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-params.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-params.c
new file mode 100644
index 000000000000..c80667dd7662
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-params.c
@@ -0,0 +1,1008 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/cleanup.h>
+#include <linux/media/amlogic/c3-isp-config.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+/*
+ * union c3_isp_params_block - Generalisation of a parameter block
+ *
+ * This union allows the driver to treat a block as a generic struct to this
+ * union and safely access the header and block-specific struct without having
+ * to resort to casting. The header member is accessed first, and the type field
+ * checked which allows the driver to determine which of the other members
+ * should be used.
+ *
+ * @header:		The shared header struct embedded as the first member
+ *			of all the possible other members. This member would be
+ *			accessed first and the type field checked to determine
+ *			which of the other members should be accessed.
+ * @awb_gains:		For header.type == C3_ISP_PARAMS_BLOCK_AWB_GAINS
+ * @awb_cfg:		For header.type == C3_ISP_PARAMS_BLOCK_AWB_CONFIG
+ * @ae_cfg:		For header.type == C3_ISP_PARAMS_BLOCK_AE_CONFIG
+ * @af_cfg:		For header.type == C3_ISP_PARAMS_BLOCK_AF_CONFIG
+ * @pst_gamma:		For header.type == C3_ISP_PARAMS_BLOCK_PST_GAMMA
+ * @ccm:		For header.type == C3_ISP_PARAMS_BLOCK_CCM
+ * @csc:		For header.type == C3_ISP_PARAMS_BLOCK_CSC
+ * @blc:		For header.type == C3_ISP_PARAMS_BLOCK_BLC
+ */
+union c3_isp_params_block {
+	struct c3_isp_params_block_header header;
+	struct c3_isp_params_awb_gains awb_gains;
+	struct c3_isp_params_awb_config awb_cfg;
+	struct c3_isp_params_ae_config ae_cfg;
+	struct c3_isp_params_af_config af_cfg;
+	struct c3_isp_params_pst_gamma pst_gamma;
+	struct c3_isp_params_ccm ccm;
+	struct c3_isp_params_csc csc;
+	struct c3_isp_params_blc blc;
+};
+
+typedef void (*c3_isp_block_handler)(struct c3_isp_device *isp,
+				     const union c3_isp_params_block *block);
+
+struct c3_isp_params_handler {
+	size_t size;
+	c3_isp_block_handler handler;
+};
+
+#define to_c3_isp_params_buffer(vbuf) \
+	container_of(vbuf, struct c3_isp_params_buffer, vb)
+
+/* Hardware configuration */
+
+static void c3_isp_params_cfg_awb_gains(struct c3_isp_device *isp,
+					const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_awb_gains *awb_gains = &block->awb_gains;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL,
+				   ISP_TOP_BEO_CTRL_WB_EN_MASK,
+				   ISP_TOP_BEO_CTRL_WB_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_LSWB_WB_GAIN0,
+			   ISP_LSWB_WB_GAIN0_GR_GAIN_MASK,
+			   ISP_LSWB_WB_GAIN0_GR_GAIN(awb_gains->gr_gain));
+	c3_isp_update_bits(isp, ISP_LSWB_WB_GAIN0,
+			   ISP_LSWB_WB_GAIN0_R_GAIN_MASK,
+			   ISP_LSWB_WB_GAIN0_R_GAIN(awb_gains->r_gain));
+	c3_isp_update_bits(isp, ISP_LSWB_WB_GAIN1,
+			   ISP_LSWB_WB_GAIN1_B_GAIN_MASK,
+			   ISP_LSWB_WB_GAIN1_B_GAIN(awb_gains->b_gain));
+	c3_isp_update_bits(isp, ISP_LSWB_WB_GAIN1,
+			   ISP_LSWB_WB_GAIN1_GB_GAIN_MASK,
+			   ISP_LSWB_WB_GAIN1_GB_GAIN(awb_gains->gb_gain));
+	c3_isp_update_bits(isp, ISP_LSWB_WB_GAIN2,
+			   ISP_LSWB_WB_GAIN2_IR_GAIN_MASK,
+			   ISP_LSWB_WB_GAIN2_IR_GAIN(awb_gains->gb_gain));
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL,
+				   ISP_TOP_BEO_CTRL_WB_EN_MASK,
+				   ISP_TOP_BEO_CTRL_WB_EN);
+}
+
+static void c3_isp_params_awb_wt(struct c3_isp_device *isp,
+				 const struct c3_isp_params_awb_config *cfg)
+{
+	unsigned int zones_num;
+	unsigned int base;
+	unsigned int data;
+	unsigned int i;
+
+	/* Set the weight address to 0 position */
+	c3_isp_write(isp, ISP_AWB_BLK_WT_ADDR, 0);
+
+	zones_num = cfg->horiz_zones_num * cfg->vert_zones_num;
+
+	/* Need to write 8 weights at once */
+	for (i = 0; i < zones_num / 8; i++) {
+		base = i * 8;
+		data = ISP_AWB_BLK_WT_DATA_WT(0, cfg->zone_weight[base + 0]) |
+		       ISP_AWB_BLK_WT_DATA_WT(1, cfg->zone_weight[base + 1]) |
+		       ISP_AWB_BLK_WT_DATA_WT(2, cfg->zone_weight[base + 2]) |
+		       ISP_AWB_BLK_WT_DATA_WT(3, cfg->zone_weight[base + 3]) |
+		       ISP_AWB_BLK_WT_DATA_WT(4, cfg->zone_weight[base + 4]) |
+		       ISP_AWB_BLK_WT_DATA_WT(5, cfg->zone_weight[base + 5]) |
+		       ISP_AWB_BLK_WT_DATA_WT(6, cfg->zone_weight[base + 6]) |
+		       ISP_AWB_BLK_WT_DATA_WT(7, cfg->zone_weight[base + 7]);
+		c3_isp_write(isp, ISP_AWB_BLK_WT_DATA, data);
+	}
+
+	if (zones_num % 8 == 0)
+		return;
+
+	data = 0;
+	base = i * 8;
+
+	for (i = 0; i < zones_num % 8; i++)
+		data |= ISP_AWB_BLK_WT_DATA_WT(i, cfg->zone_weight[base + i]);
+
+	c3_isp_write(isp, ISP_AWB_BLK_WT_DATA, data);
+}
+
+static void c3_isp_params_awb_cood(struct c3_isp_device *isp,
+				   const struct c3_isp_params_awb_config *cfg)
+{
+	unsigned int max_point_num;
+
+	/* The number of points is one more than the number of edges */
+	max_point_num = max(cfg->horiz_zones_num, cfg->vert_zones_num) + 1;
+
+	/* Set the index address to 0 position */
+	c3_isp_write(isp, ISP_AWB_IDX_ADDR, 0);
+
+	for (unsigned int i = 0; i < max_point_num; i++)
+		c3_isp_write(isp, ISP_AWB_IDX_DATA,
+			     ISP_AWB_IDX_DATA_HIDX_DATA(cfg->horiz_coord[i]) |
+			     ISP_AWB_IDX_DATA_VIDX_DATA(cfg->vert_coord[i]));
+}
+
+static void c3_isp_params_cfg_awb_config(struct c3_isp_device *isp,
+					 const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_awb_config *awb_cfg = &block->awb_cfg;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AWB_STAT_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AWB_POINT_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AWB_POINT(awb_cfg->tap_point));
+
+	c3_isp_update_bits(isp, ISP_AWB_STAT_CTRL2,
+			   ISP_AWB_STAT_CTRL2_SATUR_CTRL_MASK,
+			   ISP_AWB_STAT_CTRL2_SATUR_CTRL(awb_cfg->satur_vald));
+
+	c3_isp_update_bits(isp, ISP_AWB_HV_BLKNUM,
+			   ISP_AWB_HV_BLKNUM_H_NUM_MASK,
+			   ISP_AWB_HV_BLKNUM_H_NUM(awb_cfg->horiz_zones_num));
+	c3_isp_update_bits(isp, ISP_AWB_HV_BLKNUM,
+			   ISP_AWB_HV_BLKNUM_V_NUM_MASK,
+			   ISP_AWB_HV_BLKNUM_V_NUM(awb_cfg->vert_zones_num));
+
+	c3_isp_update_bits(isp, ISP_AWB_STAT_RG, ISP_AWB_STAT_RG_MIN_VALUE_MASK,
+			   ISP_AWB_STAT_RG_MIN_VALUE(awb_cfg->rg_min));
+	c3_isp_update_bits(isp, ISP_AWB_STAT_RG, ISP_AWB_STAT_RG_MAX_VALUE_MASK,
+			   ISP_AWB_STAT_RG_MAX_VALUE(awb_cfg->rg_max));
+
+	c3_isp_update_bits(isp, ISP_AWB_STAT_BG, ISP_AWB_STAT_BG_MIN_VALUE_MASK,
+			   ISP_AWB_STAT_BG_MIN_VALUE(awb_cfg->bg_min));
+	c3_isp_update_bits(isp, ISP_AWB_STAT_BG, ISP_AWB_STAT_BG_MAX_VALUE_MASK,
+			   ISP_AWB_STAT_BG_MAX_VALUE(awb_cfg->bg_max));
+
+	c3_isp_update_bits(isp, ISP_AWB_STAT_RG_HL,
+			   ISP_AWB_STAT_RG_HL_LOW_VALUE_MASK,
+			   ISP_AWB_STAT_RG_HL_LOW_VALUE(awb_cfg->rg_low));
+	c3_isp_update_bits(isp, ISP_AWB_STAT_RG_HL,
+			   ISP_AWB_STAT_RG_HL_HIGH_VALUE_MASK,
+			   ISP_AWB_STAT_RG_HL_HIGH_VALUE(awb_cfg->rg_high));
+
+	c3_isp_update_bits(isp, ISP_AWB_STAT_BG_HL,
+			   ISP_AWB_STAT_BG_HL_LOW_VALUE_MASK,
+			   ISP_AWB_STAT_BG_HL_LOW_VALUE(awb_cfg->bg_low));
+	c3_isp_update_bits(isp, ISP_AWB_STAT_BG_HL,
+			   ISP_AWB_STAT_BG_HL_HIGH_VALUE_MASK,
+			   ISP_AWB_STAT_BG_HL_HIGH_VALUE(awb_cfg->bg_high));
+
+	c3_isp_params_awb_wt(isp, awb_cfg);
+	c3_isp_params_awb_cood(isp, awb_cfg);
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN);
+}
+
+static void c3_isp_params_ae_wt(struct c3_isp_device *isp,
+				const struct c3_isp_params_ae_config *cfg)
+{
+	unsigned int zones_num;
+	unsigned int base;
+	unsigned int data;
+	unsigned int i;
+
+	/* Set the weight address to 0 position */
+	c3_isp_write(isp, ISP_AE_BLK_WT_ADDR, 0);
+
+	zones_num = cfg->horiz_zones_num * cfg->vert_zones_num;
+
+	/* Need to write 8 weights at once */
+	for (i = 0; i < zones_num / 8; i++) {
+		base = i * 8;
+		data = ISP_AE_BLK_WT_DATA_WT(0, cfg->zone_weight[base + 0]) |
+		       ISP_AE_BLK_WT_DATA_WT(1, cfg->zone_weight[base + 1]) |
+		       ISP_AE_BLK_WT_DATA_WT(2, cfg->zone_weight[base + 2]) |
+		       ISP_AE_BLK_WT_DATA_WT(3, cfg->zone_weight[base + 3]) |
+		       ISP_AE_BLK_WT_DATA_WT(4, cfg->zone_weight[base + 4]) |
+		       ISP_AE_BLK_WT_DATA_WT(5, cfg->zone_weight[base + 5]) |
+		       ISP_AE_BLK_WT_DATA_WT(6, cfg->zone_weight[base + 6]) |
+		       ISP_AE_BLK_WT_DATA_WT(7, cfg->zone_weight[base + 7]);
+		c3_isp_write(isp, ISP_AE_BLK_WT_DATA, data);
+	}
+
+	if (zones_num % 8 == 0)
+		return;
+
+	data = 0;
+	base = i * 8;
+
+	/* Write the last weights data */
+	for (i = 0; i < zones_num % 8; i++)
+		data |= ISP_AE_BLK_WT_DATA_WT(i, cfg->zone_weight[base + i]);
+
+	c3_isp_write(isp, ISP_AE_BLK_WT_DATA, data);
+}
+
+static void c3_isp_params_ae_cood(struct c3_isp_device *isp,
+				  const struct c3_isp_params_ae_config *cfg)
+{
+	unsigned int max_point_num;
+
+	/* The number of points is one more than the number of edges */
+	max_point_num = max(cfg->horiz_zones_num, cfg->vert_zones_num) + 1;
+
+	/* Set the index address to 0 position */
+	c3_isp_write(isp, ISP_AE_IDX_ADDR, 0);
+
+	for (unsigned int i = 0; i < max_point_num; i++)
+		c3_isp_write(isp, ISP_AE_IDX_DATA,
+			     ISP_AE_IDX_DATA_HIDX_DATA(cfg->horiz_coord[i]) |
+			     ISP_AE_IDX_DATA_VIDX_DATA(cfg->vert_coord[i]));
+}
+
+static void c3_isp_params_cfg_ae_config(struct c3_isp_device *isp,
+					const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_ae_config *ae_cfg = &block->ae_cfg;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AE_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AE_STAT_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AE_POINT_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AE_POINT(ae_cfg->tap_point));
+
+	if (ae_cfg->tap_point == C3_ISP_AE_STATS_TAP_GE)
+		c3_isp_update_bits(isp, ISP_AE_CTRL,
+				   ISP_AE_CTRL_INPUT_2LINE_MASK,
+				   ISP_AE_CTRL_INPUT_2LINE_EN);
+	else
+		c3_isp_update_bits(isp, ISP_AE_CTRL,
+				   ISP_AE_CTRL_INPUT_2LINE_MASK,
+				   ISP_AE_CTRL_INPUT_2LINE_DIS);
+
+	c3_isp_update_bits(isp, ISP_AE_HV_BLKNUM,
+			   ISP_AE_HV_BLKNUM_H_NUM_MASK,
+			   ISP_AE_HV_BLKNUM_H_NUM(ae_cfg->horiz_zones_num));
+	c3_isp_update_bits(isp, ISP_AE_HV_BLKNUM,
+			   ISP_AE_HV_BLKNUM_V_NUM_MASK,
+			   ISP_AE_HV_BLKNUM_V_NUM(ae_cfg->vert_zones_num));
+
+	c3_isp_params_ae_wt(isp, ae_cfg);
+	c3_isp_params_ae_cood(isp, ae_cfg);
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AE_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AE_STAT_EN);
+}
+
+static void c3_isp_params_af_cood(struct c3_isp_device *isp,
+				  const struct c3_isp_params_af_config *cfg)
+{
+	unsigned int max_point_num;
+
+	/* The number of points is one more than the number of edges */
+	max_point_num = max(cfg->horiz_zones_num, cfg->vert_zones_num) + 1;
+
+	/* Set the index address to 0 position */
+	c3_isp_write(isp, ISP_AF_IDX_ADDR, 0);
+
+	for (unsigned int i = 0; i < max_point_num; i++)
+		c3_isp_write(isp, ISP_AF_IDX_DATA,
+			     ISP_AF_IDX_DATA_HIDX_DATA(cfg->horiz_coord[i]) |
+			     ISP_AF_IDX_DATA_VIDX_DATA(cfg->vert_coord[i]));
+}
+
+static void c3_isp_params_cfg_af_config(struct c3_isp_device *isp,
+					const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_af_config *af_cfg = &block->af_cfg;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AF_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AF_STAT_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AF_POINT_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AF_POINT(af_cfg->tap_point));
+
+	c3_isp_update_bits(isp, ISP_AF_HV_BLKNUM,
+			   ISP_AF_HV_BLKNUM_H_NUM_MASK,
+			   ISP_AF_HV_BLKNUM_H_NUM(af_cfg->horiz_zones_num));
+	c3_isp_update_bits(isp, ISP_AF_HV_BLKNUM,
+			   ISP_AF_HV_BLKNUM_V_NUM_MASK,
+			   ISP_AF_HV_BLKNUM_V_NUM(af_cfg->vert_zones_num));
+
+	c3_isp_params_af_cood(isp, af_cfg);
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+				   ISP_TOP_3A_STAT_CRTL_AF_STAT_EN_MASK,
+				   ISP_TOP_3A_STAT_CRTL_AF_STAT_EN);
+}
+
+static void c3_isp_params_cfg_pst_gamma(struct c3_isp_device *isp,
+					const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_pst_gamma *gm = &block->pst_gamma;
+	unsigned int base;
+	unsigned int i;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_PST_GAMMA_EN_MASK,
+				   ISP_TOP_BED_CTRL_PST_GAMMA_DIS);
+		return;
+	}
+
+	/* R, G and B channels use the same gamma lut */
+	for (unsigned int j = 0; j < 3; j++) {
+		/* Set the channel lut address */
+		c3_isp_write(isp, ISP_PST_GAMMA_LUT_ADDR,
+			     ISP_PST_GAMMA_LUT_ADDR_IDX_ADDR(j));
+
+		/* Need to write 2 lut values at once */
+		for (i = 0; i < ARRAY_SIZE(gm->lut) / 2; i++) {
+			base = i * 2;
+			c3_isp_write(isp, ISP_PST_GAMMA_LUT_DATA,
+				     ISP_PST_GM_LUT_DATA0(gm->lut[base]) |
+				     ISP_PST_GM_LUT_DATA1(gm->lut[base + 1]));
+		}
+
+		/* Write the last one */
+		if (ARRAY_SIZE(gm->lut) % 2) {
+			base = i * 2;
+			c3_isp_write(isp, ISP_PST_GAMMA_LUT_DATA,
+				     ISP_PST_GM_LUT_DATA0(gm->lut[base]));
+		}
+	}
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_PST_GAMMA_EN_MASK,
+				   ISP_TOP_BED_CTRL_PST_GAMMA_EN);
+}
+
+/* Configure 3 x 3 ccm matrix */
+static void c3_isp_params_cfg_ccm(struct c3_isp_device *isp,
+				  const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_ccm *ccm = &block->ccm;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_CCM_EN_MASK,
+				   ISP_TOP_BED_CTRL_CCM_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_CCM_MTX_00_01,
+			   ISP_CCM_MTX_00_01_MTX_00_MASK,
+			   ISP_CCM_MTX_00_01_MTX_00(ccm->matrix[0][0]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_00_01,
+			   ISP_CCM_MTX_00_01_MTX_01_MASK,
+			   ISP_CCM_MTX_00_01_MTX_01(ccm->matrix[0][1]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_02_03,
+			   ISP_CCM_MTX_02_03_MTX_02_MASK,
+			   ISP_CCM_MTX_02_03_MTX_02(ccm->matrix[0][2]));
+
+	c3_isp_update_bits(isp, ISP_CCM_MTX_10_11,
+			   ISP_CCM_MTX_10_11_MTX_10_MASK,
+			   ISP_CCM_MTX_10_11_MTX_10(ccm->matrix[1][0]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_10_11,
+			   ISP_CCM_MTX_10_11_MTX_11_MASK,
+			   ISP_CCM_MTX_10_11_MTX_11(ccm->matrix[1][1]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_12_13,
+			   ISP_CCM_MTX_12_13_MTX_12_MASK,
+			   ISP_CCM_MTX_12_13_MTX_12(ccm->matrix[1][2]));
+
+	c3_isp_update_bits(isp, ISP_CCM_MTX_20_21,
+			   ISP_CCM_MTX_20_21_MTX_20_MASK,
+			   ISP_CCM_MTX_20_21_MTX_20(ccm->matrix[2][0]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_20_21,
+			   ISP_CCM_MTX_20_21_MTX_21_MASK,
+			   ISP_CCM_MTX_20_21_MTX_21(ccm->matrix[2][1]));
+	c3_isp_update_bits(isp, ISP_CCM_MTX_22_23_RS,
+			   ISP_CCM_MTX_22_23_RS_MTX_22_MASK,
+			   ISP_CCM_MTX_22_23_RS_MTX_22(ccm->matrix[2][2]));
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_CCM_EN_MASK,
+				   ISP_TOP_BED_CTRL_CCM_EN);
+}
+
+/* Configure color space conversion matrix parameters */
+static void c3_isp_params_cfg_csc(struct c3_isp_device *isp,
+				  const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_csc *csc = &block->csc;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_CM0_EN_MASK,
+				   ISP_TOP_BED_CTRL_CM0_DIS);
+		return;
+	}
+
+	c3_isp_update_bits(isp, ISP_CM0_COEF00_01,
+			   ISP_CM0_COEF00_01_MTX_00_MASK,
+			   ISP_CM0_COEF00_01_MTX_00(csc->matrix[0][0]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF00_01,
+			   ISP_CM0_COEF00_01_MTX_01_MASK,
+			   ISP_CM0_COEF00_01_MTX_01(csc->matrix[0][1]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF02_10,
+			   ISP_CM0_COEF02_10_MTX_02_MASK,
+			   ISP_CM0_COEF02_10_MTX_02(csc->matrix[0][2]));
+
+	c3_isp_update_bits(isp, ISP_CM0_COEF02_10,
+			   ISP_CM0_COEF02_10_MTX_10_MASK,
+			   ISP_CM0_COEF02_10_MTX_10(csc->matrix[1][0]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF11_12,
+			   ISP_CM0_COEF11_12_MTX_11_MASK,
+			   ISP_CM0_COEF11_12_MTX_11(csc->matrix[1][1]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF11_12,
+			   ISP_CM0_COEF11_12_MTX_12_MASK,
+			   ISP_CM0_COEF11_12_MTX_12(csc->matrix[1][2]));
+
+	c3_isp_update_bits(isp, ISP_CM0_COEF20_21,
+			   ISP_CM0_COEF20_21_MTX_20_MASK,
+			   ISP_CM0_COEF20_21_MTX_20(csc->matrix[2][0]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF20_21,
+			   ISP_CM0_COEF20_21_MTX_21_MASK,
+			   ISP_CM0_COEF20_21_MTX_21(csc->matrix[2][1]));
+	c3_isp_update_bits(isp, ISP_CM0_COEF22_OUP_OFST0,
+			   ISP_CM0_COEF22_OUP_OFST0_MTX_22_MASK,
+			   ISP_CM0_COEF22_OUP_OFST0_MTX_22(csc->matrix[2][2]));
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+				   ISP_TOP_BED_CTRL_CM0_EN_MASK,
+				   ISP_TOP_BED_CTRL_CM0_EN);
+}
+
+/* Set blc offset of each color channel */
+static void c3_isp_params_cfg_blc(struct c3_isp_device *isp,
+				  const union c3_isp_params_block *block)
+{
+	const struct c3_isp_params_blc *blc = &block->blc;
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_DISABLE) {
+		c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL,
+				   ISP_TOP_BEO_CTRL_BLC_EN_MASK,
+				   ISP_TOP_BEO_CTRL_BLC_DIS);
+		return;
+	}
+
+	c3_isp_write(isp, ISP_LSWB_BLC_OFST0,
+		     ISP_LSWB_BLC_OFST0_R_OFST(blc->r_ofst) |
+		     ISP_LSWB_BLC_OFST0_GR_OFST(blc->gr_ofst));
+	c3_isp_write(isp, ISP_LSWB_BLC_OFST1,
+		     ISP_LSWB_BLC_OFST1_GB_OFST(blc->gb_ofst) |
+		     ISP_LSWB_BLC_OFST1_B_OFST(blc->b_ofst));
+
+	if (block->header.flags & C3_ISP_PARAMS_BLOCK_FL_ENABLE)
+		c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL,
+				   ISP_TOP_BEO_CTRL_BLC_EN_MASK,
+				   ISP_TOP_BEO_CTRL_BLC_EN);
+}
+
+static const struct c3_isp_params_handler c3_isp_params_handlers[] = {
+	[C3_ISP_PARAMS_BLOCK_AWB_GAINS] = {
+		.size = sizeof(struct c3_isp_params_awb_gains),
+		.handler = c3_isp_params_cfg_awb_gains,
+	},
+	[C3_ISP_PARAMS_BLOCK_AWB_CONFIG] = {
+		.size = sizeof(struct c3_isp_params_awb_config),
+		.handler = c3_isp_params_cfg_awb_config,
+	},
+	[C3_ISP_PARAMS_BLOCK_AE_CONFIG] = {
+		.size = sizeof(struct c3_isp_params_ae_config),
+		.handler = c3_isp_params_cfg_ae_config,
+	},
+	[C3_ISP_PARAMS_BLOCK_AF_CONFIG] = {
+		.size = sizeof(struct c3_isp_params_af_config),
+		.handler = c3_isp_params_cfg_af_config,
+	},
+	[C3_ISP_PARAMS_BLOCK_PST_GAMMA] = {
+		.size = sizeof(struct c3_isp_params_pst_gamma),
+		.handler = c3_isp_params_cfg_pst_gamma,
+	},
+	[C3_ISP_PARAMS_BLOCK_CCM] = {
+		.size = sizeof(struct c3_isp_params_ccm),
+		.handler = c3_isp_params_cfg_ccm,
+	},
+	[C3_ISP_PARAMS_BLOCK_CSC] = {
+		.size = sizeof(struct c3_isp_params_csc),
+		.handler = c3_isp_params_cfg_csc,
+	},
+	[C3_ISP_PARAMS_BLOCK_BLC] = {
+		.size = sizeof(struct c3_isp_params_blc),
+		.handler = c3_isp_params_cfg_blc,
+	},
+};
+
+static void c3_isp_params_cfg_blocks(struct c3_isp_params *params)
+{
+	struct c3_isp_params_cfg *config = params->buff->cfg;
+	size_t block_offset = 0;
+
+	if (WARN_ON(!config))
+		return;
+
+	/* Walk the list of parameter blocks and process them */
+	while (block_offset < config->data_size) {
+		const struct c3_isp_params_handler *block_handler;
+		const union c3_isp_params_block *block;
+
+		block = (const union c3_isp_params_block *)
+			 &config->data[block_offset];
+
+		block_handler = &c3_isp_params_handlers[block->header.type];
+		block_handler->handler(params->isp, block);
+
+		block_offset += block->header.size;
+	}
+}
+
+void c3_isp_params_pre_cfg(struct c3_isp_device *isp)
+{
+	struct c3_isp_params *params = &isp->params;
+
+	/* Disable some unused modules */
+	c3_isp_update_bits(isp, ISP_TOP_FEO_CTRL0,
+			   ISP_TOP_FEO_CTRL0_INPUT_FMT_EN_MASK,
+			   ISP_TOP_FEO_CTRL0_INPUT_FMT_DIS);
+
+	c3_isp_update_bits(isp, ISP_TOP_FEO_CTRL1_0,
+			   ISP_TOP_FEO_CTRL1_0_DPC_EN_MASK,
+			   ISP_TOP_FEO_CTRL1_0_DPC_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FEO_CTRL1_0,
+			   ISP_TOP_FEO_CTRL1_0_OG_EN_MASK,
+			   ISP_TOP_FEO_CTRL1_0_OG_DIS);
+
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL, ISP_TOP_FED_CTRL_PDPC_EN_MASK,
+			   ISP_TOP_FED_CTRL_PDPC_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL,
+			   ISP_TOP_FED_CTRL_RAWCNR_EN_MASK,
+			   ISP_TOP_FED_CTRL_RAWCNR_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL, ISP_TOP_FED_CTRL_SNR1_EN_MASK,
+			   ISP_TOP_FED_CTRL_SNR1_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL, ISP_TOP_FED_CTRL_TNR0_EN_MASK,
+			   ISP_TOP_FED_CTRL_TNR0_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL,
+			   ISP_TOP_FED_CTRL_CUBIC_CS_EN_MASK,
+			   ISP_TOP_FED_CTRL_CUBIC_CS_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL, ISP_TOP_FED_CTRL_SQRT_EN_MASK,
+			   ISP_TOP_FED_CTRL_SQRT_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_FED_CTRL,
+			   ISP_TOP_FED_CTRL_DGAIN_EN_MASK,
+			   ISP_TOP_FED_CTRL_DGAIN_DIS);
+
+	c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL,
+			   ISP_TOP_BEO_CTRL_INV_DGAIN_EN_MASK,
+			   ISP_TOP_BEO_CTRL_INV_DGAIN_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BEO_CTRL, ISP_TOP_BEO_CTRL_EOTF_EN_MASK,
+			   ISP_TOP_BEO_CTRL_EOTF_DIS);
+
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+			   ISP_TOP_BED_CTRL_YHS_STAT_EN_MASK,
+			   ISP_TOP_BED_CTRL_YHS_STAT_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+			   ISP_TOP_BED_CTRL_GRPH_STAT_EN_MASK,
+			   ISP_TOP_BED_CTRL_GRPH_STAT_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+			   ISP_TOP_BED_CTRL_FMETER_EN_MASK,
+			   ISP_TOP_BED_CTRL_FMETER_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL, ISP_TOP_BED_CTRL_BSC_EN_MASK,
+			   ISP_TOP_BED_CTRL_BSC_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL, ISP_TOP_BED_CTRL_CNR2_EN_MASK,
+			   ISP_TOP_BED_CTRL_CNR2_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL, ISP_TOP_BED_CTRL_CM1_EN_MASK,
+			   ISP_TOP_BED_CTRL_CM1_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+			   ISP_TOP_BED_CTRL_LUT3D_EN_MASK,
+			   ISP_TOP_BED_CTRL_LUT3D_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL,
+			   ISP_TOP_BED_CTRL_PST_TNR_LITE_EN_MASK,
+			   ISP_TOP_BED_CTRL_PST_TNR_LITE_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_BED_CTRL, ISP_TOP_BED_CTRL_AMCM_EN_MASK,
+			   ISP_TOP_BED_CTRL_AMCM_DIS);
+
+	/*
+	 * Disable AE, AF and AWB stat module. Please configure the parameters
+	 * in userspace algorithm if need to enable these switch.
+	 */
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AE_STAT_EN_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AE_STAT_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AWB_STAT_DIS);
+	c3_isp_update_bits(isp, ISP_TOP_3A_STAT_CRTL,
+			   ISP_TOP_3A_STAT_CRTL_AF_STAT_EN_MASK,
+			   ISP_TOP_3A_STAT_CRTL_AF_STAT_DIS);
+
+	c3_isp_write(isp, ISP_LSWB_WB_LIMIT0,
+		     ISP_LSWB_WB_LIMIT0_WB_LIMIT_R_MAX |
+		     ISP_LSWB_WB_LIMIT0_WB_LIMIT_GR_MAX);
+	c3_isp_write(isp, ISP_LSWB_WB_LIMIT1,
+		     ISP_LSWB_WB_LIMIT1_WB_LIMIT_GB_MAX |
+		     ISP_LSWB_WB_LIMIT1_WB_LIMIT_B_MAX);
+
+	guard(spinlock_irqsave)(&params->buff_lock);
+
+	/* Only use the first buffer to initialize ISP */
+	params->buff =
+		list_first_entry_or_null(&params->pending,
+					 struct c3_isp_params_buffer, list);
+	if (params->buff)
+		c3_isp_params_cfg_blocks(params);
+}
+
+/* V4L2 video operations */
+
+static int c3_isp_params_querycap(struct file *file, void *fh,
+				  struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, C3_ISP_DRIVER_NAME, sizeof(cap->driver));
+	strscpy(cap->card, "AML C3 ISP", sizeof(cap->card));
+
+	return 0;
+}
+
+static int c3_isp_params_enum_fmt(struct file *file, void *fh,
+				  struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+
+	f->pixelformat = V4L2_META_FMT_C3ISP_PARAMS;
+
+	return 0;
+}
+
+static int c3_isp_params_g_fmt(struct file *file, void *fh,
+			       struct v4l2_format *f)
+{
+	struct c3_isp_params *params = video_drvdata(file);
+
+	f->fmt.meta = params->vfmt.fmt.meta;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops isp_params_v4l2_ioctl_ops = {
+	.vidioc_querycap                = c3_isp_params_querycap,
+	.vidioc_enum_fmt_meta_out       = c3_isp_params_enum_fmt,
+	.vidioc_g_fmt_meta_out          = c3_isp_params_g_fmt,
+	.vidioc_s_fmt_meta_out          = c3_isp_params_g_fmt,
+	.vidioc_try_fmt_meta_out        = c3_isp_params_g_fmt,
+	.vidioc_reqbufs                 = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+	.vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs             = vb2_ioctl_create_bufs,
+	.vidioc_streamon                = vb2_ioctl_streamon,
+	.vidioc_streamoff               = vb2_ioctl_streamoff,
+};
+
+static const struct v4l2_file_operations isp_params_v4l2_fops = {
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+};
+
+static int c3_isp_params_vb2_queue_setup(struct vb2_queue *q,
+					 unsigned int *num_buffers,
+					 unsigned int *num_planes,
+					 unsigned int sizes[],
+					 struct device *alloc_devs[])
+{
+	if (*num_planes) {
+		if (*num_planes != 1)
+			return -EINVAL;
+
+		if (sizes[0] < sizeof(struct c3_isp_params_cfg))
+			return -EINVAL;
+
+		return 0;
+	}
+
+	*num_planes = 1;
+	sizes[0] = sizeof(struct c3_isp_params_cfg);
+
+	return 0;
+}
+
+static void c3_isp_params_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_params_buffer *buf = to_c3_isp_params_buffer(v4l2_buf);
+	struct c3_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);
+
+	guard(spinlock_irqsave)(&params->buff_lock);
+
+	list_add_tail(&buf->list, &params->pending);
+}
+
+static int c3_isp_params_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_params_buffer *buf = to_c3_isp_params_buffer(vbuf);
+	struct c3_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);
+	struct c3_isp_params_cfg *cfg = buf->cfg;
+	struct c3_isp_params_cfg *usr_cfg = vb2_plane_vaddr(vb, 0);
+	size_t payload_size = vb2_get_plane_payload(vb, 0);
+	size_t header_size = offsetof(struct c3_isp_params_cfg, data);
+	size_t block_offset = 0;
+	size_t cfg_size;
+
+	/* Payload size can't be greater than the destination buffer size */
+	if (payload_size > params->vfmt.fmt.meta.buffersize) {
+		dev_dbg(params->isp->dev,
+			"Payload size is too large: %zu\n", payload_size);
+		return -EINVAL;
+	}
+
+	/* Payload size can't be smaller than the header size */
+	if (payload_size < header_size) {
+		dev_dbg(params->isp->dev,
+			"Payload size is too small: %zu\n", payload_size);
+		return -EINVAL;
+	}
+
+	/*
+	 * Use the internal scratch buffer to avoid userspace modifying
+	 * the buffer content while the driver is processing it.
+	 */
+	memcpy(cfg, usr_cfg, payload_size);
+
+	/* Only v0 is supported at the moment */
+	if (cfg->version != C3_ISP_PARAMS_BUFFER_V0) {
+		dev_dbg(params->isp->dev,
+			"Invalid params buffer version: %u\n", cfg->version);
+		return -EINVAL;
+	}
+
+	/* Validate the size reported in the parameter buffer header */
+	cfg_size = header_size + cfg->data_size;
+	if (cfg_size != payload_size) {
+		dev_dbg(params->isp->dev,
+			"Data size %zu and payload size %zu are different\n",
+			cfg_size, payload_size);
+		return -EINVAL;
+	}
+
+	/* Walk the list of parameter blocks and validate them */
+	cfg_size = cfg->data_size;
+	while (cfg_size >= sizeof(struct c3_isp_params_block_header)) {
+		const struct c3_isp_params_block_header *block;
+		const struct c3_isp_params_handler *handler;
+
+		block = (struct c3_isp_params_block_header *)
+			&cfg->data[block_offset];
+
+		if (block->type >= ARRAY_SIZE(c3_isp_params_handlers)) {
+			dev_dbg(params->isp->dev,
+				"Invalid params block type\n");
+			return -EINVAL;
+		}
+
+		if (block->size > cfg_size) {
+			dev_dbg(params->isp->dev,
+				"Block size is greater than cfg size\n");
+			return -EINVAL;
+		}
+
+		if ((block->flags & (C3_ISP_PARAMS_BLOCK_FL_ENABLE |
+				     C3_ISP_PARAMS_BLOCK_FL_DISABLE)) ==
+		    (C3_ISP_PARAMS_BLOCK_FL_ENABLE |
+		     C3_ISP_PARAMS_BLOCK_FL_DISABLE)) {
+			dev_dbg(params->isp->dev,
+				"Invalid parameters block flags\n");
+			return -EINVAL;
+		}
+
+		handler = &c3_isp_params_handlers[block->type];
+		if (block->size != handler->size) {
+			dev_dbg(params->isp->dev,
+				"Invalid params block size\n");
+			return -EINVAL;
+		}
+
+		block_offset += block->size;
+		cfg_size -= block->size;
+	}
+
+	if (cfg_size) {
+		dev_dbg(params->isp->dev,
+			"Unexpected data after the params buffer end\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int c3_isp_params_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_params *params = vb2_get_drv_priv(vb->vb2_queue);
+	struct c3_isp_params_buffer *buf = to_c3_isp_params_buffer(v4l2_buf);
+
+	buf->cfg = kvmalloc(params->vfmt.fmt.meta.buffersize, GFP_KERNEL);
+	if (!buf->cfg)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void c3_isp_params_vb2_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_params_buffer *buf = to_c3_isp_params_buffer(v4l2_buf);
+
+	kvfree(buf->cfg);
+	buf->cfg = NULL;
+}
+
+static void c3_isp_params_vb2_stop_streaming(struct vb2_queue *q)
+{
+	struct c3_isp_params *params = vb2_get_drv_priv(q);
+	struct c3_isp_params_buffer *buff;
+
+	guard(spinlock_irqsave)(&params->buff_lock);
+
+	while (!list_empty(&params->pending)) {
+		buff = list_first_entry(&params->pending,
+					struct c3_isp_params_buffer, list);
+		list_del(&buff->list);
+		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static const struct vb2_ops isp_params_vb2_ops = {
+	.queue_setup = c3_isp_params_vb2_queue_setup,
+	.buf_queue = c3_isp_params_vb2_buf_queue,
+	.buf_prepare = c3_isp_params_vb2_buf_prepare,
+	.buf_init = c3_isp_params_vb2_buf_init,
+	.buf_cleanup = c3_isp_params_vb2_buf_cleanup,
+	.stop_streaming = c3_isp_params_vb2_stop_streaming,
+};
+
+int c3_isp_params_register(struct c3_isp_device *isp)
+{
+	struct c3_isp_params *params = &isp->params;
+	struct video_device *vdev = &params->vdev;
+	struct vb2_queue *vb2_q = &params->vb2_q;
+	int ret;
+
+	memset(params, 0, sizeof(*params));
+	params->vfmt.fmt.meta.dataformat = V4L2_META_FMT_C3ISP_PARAMS;
+	params->vfmt.fmt.meta.buffersize = sizeof(struct c3_isp_params_cfg);
+	params->isp = isp;
+	INIT_LIST_HEAD(&params->pending);
+	spin_lock_init(&params->buff_lock);
+	mutex_init(&params->lock);
+
+	snprintf(vdev->name, sizeof(vdev->name), "c3-isp-params");
+	vdev->fops = &isp_params_v4l2_fops;
+	vdev->ioctl_ops = &isp_params_v4l2_ioctl_ops;
+	vdev->v4l2_dev = &isp->v4l2_dev;
+	vdev->lock = &params->lock;
+	vdev->minor = -1;
+	vdev->queue = vb2_q;
+	vdev->release = video_device_release_empty;
+	vdev->device_caps = V4L2_CAP_META_OUTPUT | V4L2_CAP_STREAMING;
+	vdev->vfl_dir = VFL_DIR_TX;
+	video_set_drvdata(vdev, params);
+
+	vb2_q->drv_priv = params;
+	vb2_q->mem_ops = &vb2_vmalloc_memops;
+	vb2_q->ops = &isp_params_vb2_ops;
+	vb2_q->type = V4L2_BUF_TYPE_META_OUTPUT;
+	vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
+	vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vb2_q->buf_struct_size = sizeof(struct c3_isp_params_buffer);
+	vb2_q->dev = isp->dev;
+	vb2_q->lock = &params->lock;
+	vb2_q->min_queued_buffers = 1;
+
+	ret = vb2_queue_init(vb2_q);
+	if (ret)
+		goto err_detroy;
+
+	params->pad.flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&vdev->entity, 1, &params->pad);
+	if (ret)
+		goto err_queue_release;
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		dev_err(isp->dev,
+			"Failed to register %s: %d\n", vdev->name, ret);
+		goto err_entity_cleanup;
+	}
+
+	return 0;
+
+err_entity_cleanup:
+	media_entity_cleanup(&vdev->entity);
+err_queue_release:
+	vb2_queue_release(vb2_q);
+err_detroy:
+	mutex_destroy(&params->lock);
+	return ret;
+}
+
+void c3_isp_params_unregister(struct c3_isp_device *isp)
+{
+	struct c3_isp_params *params = &isp->params;
+
+	vb2_queue_release(&params->vb2_q);
+	media_entity_cleanup(&params->vdev.entity);
+	video_unregister_device(&params->vdev);
+	mutex_destroy(&params->lock);
+}
+
+void c3_isp_params_isr(struct c3_isp_device *isp)
+{
+	struct c3_isp_params *params = &isp->params;
+
+	guard(spinlock_irqsave)(&params->buff_lock);
+
+	params->buff =
+		list_first_entry_or_null(&params->pending,
+					 struct c3_isp_params_buffer, list);
+	if (!params->buff)
+		return;
+
+	list_del(&params->buff->list);
+
+	c3_isp_params_cfg_blocks(params);
+
+	params->buff->vb.sequence = params->isp->frm_sequence;
+	params->buff->vb.vb2_buf.timestamp = ktime_get();
+	params->buff->vb.field = V4L2_FIELD_NONE;
+	vb2_buffer_done(&params->buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
+}
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-regs.h b/drivers/media/platform/amlogic/c3/isp/c3-isp-regs.h
new file mode 100644
index 000000000000..fa249985a771
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-regs.h
@@ -0,0 +1,618 @@
+/* SPDX-License-Identifier: (GPL-2.0-only OR MIT) */
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#ifndef __C3_ISP_REGS_H__
+#define __C3_ISP_REGS_H__
+
+#define ISP_TOP_INPUT_SIZE				0x0000
+#define ISP_TOP_INPUT_SIZE_VERT_SIZE_MASK		GENMASK(15, 0)
+#define ISP_TOP_INPUT_SIZE_VERT_SIZE(x)			((x) << 0)
+#define ISP_TOP_INPUT_SIZE_HORIZ_SIZE_MASK		GENMASK(31, 16)
+#define ISP_TOP_INPUT_SIZE_HORIZ_SIZE(x)		((x) << 16)
+
+#define ISP_TOP_FRM_SIZE				0x0004
+#define ISP_TOP_FRM_SIZE_CORE_VERT_SIZE_MASK		GENMASK(15, 0)
+#define ISP_TOP_FRM_SIZE_CORE_VERT_SIZE(x)		((x) << 0)
+#define ISP_TOP_FRM_SIZE_CORE_HORIZ_SIZE_MASK		GENMASK(31, 16)
+#define ISP_TOP_FRM_SIZE_CORE_HORIZ_SIZE(x)		((x) << 16)
+
+#define ISP_TOP_HOLD_SIZE				0x0008
+#define ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE_MASK		GENMASK(31, 16)
+#define ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE(x)		((x) << 16)
+
+#define ISP_TOP_PATH_EN					0x0010
+#define ISP_TOP_PATH_EN_DISP0_EN_MASK			BIT(0)
+#define ISP_TOP_PATH_EN_DISP0_EN			BIT(0)
+#define ISP_TOP_PATH_EN_DISP0_DIS			(0 << 0)
+#define ISP_TOP_PATH_EN_DISP1_EN_MASK			BIT(1)
+#define ISP_TOP_PATH_EN_DISP1_EN			BIT(1)
+#define ISP_TOP_PATH_EN_DISP1_DIS			(0 << 1)
+#define ISP_TOP_PATH_EN_DISP2_EN_MASK			BIT(2)
+#define ISP_TOP_PATH_EN_DISP2_EN			BIT(2)
+#define ISP_TOP_PATH_EN_DISP2_DIS			(0 << 2)
+#define ISP_TOP_PATH_EN_WRMIF0_EN_MASK			BIT(8)
+#define ISP_TOP_PATH_EN_WRMIF0_EN			BIT(8)
+#define ISP_TOP_PATH_EN_WRMIF0_DIS			(0 << 8)
+#define ISP_TOP_PATH_EN_WRMIF1_EN_MASK			BIT(9)
+#define ISP_TOP_PATH_EN_WRMIF1_EN			BIT(9)
+#define ISP_TOP_PATH_EN_WRMIF1_DIS			(0 << 9)
+#define ISP_TOP_PATH_EN_WRMIF2_EN_MASK			BIT(10)
+#define ISP_TOP_PATH_EN_WRMIF2_EN			BIT(10)
+#define ISP_TOP_PATH_EN_WRMIF2_DIS			(0 << 10)
+
+#define ISP_TOP_PATH_SEL				0x0014
+#define ISP_TOP_PATH_SEL_CORE_MASK			GENMASK(18, 16)
+#define ISP_TOP_PATH_SEL_CORE_CORE_DIS			(0 << 16)
+#define ISP_TOP_PATH_SEL_CORE_MIPI_CORE			BIT(16)
+
+#define ISP_TOP_DISPIN_SEL				0x0018
+#define ISP_TOP_DISPIN_SEL_DISP0_MASK			GENMASK(3, 0)
+#define ISP_TOP_DISPIN_SEL_DISP0_CORE_OUT		(0 << 0)
+#define ISP_TOP_DISPIN_SEL_DISP0_MIPI_OUT		(2 << 0)
+#define ISP_TOP_DISPIN_SEL_DISP1_MASK			GENMASK(7, 4)
+#define ISP_TOP_DISPIN_SEL_DISP1_CORE_OUT		(0 << 4)
+#define ISP_TOP_DISPIN_SEL_DISP1_MIPI_OUT		(2 << 4)
+#define ISP_TOP_DISPIN_SEL_DISP2_MASK			GENMASK(11, 8)
+#define ISP_TOP_DISPIN_SEL_DISP2_CORE_OUT		(0 << 8)
+#define ISP_TOP_DISPIN_SEL_DISP2_MIPI_OUT		(2 << 8)
+
+#define ISP_TOP_IRQ_EN					0x0080
+#define ISP_TOP_IRQ_EN_FRM_END_MASK			BIT(0)
+#define ISP_TOP_IRQ_EN_FRM_END_EN			BIT(0)
+#define ISP_TOP_IRQ_EN_FRM_END_DIS			(0 << 0)
+#define ISP_TOP_IRQ_EN_FRM_RST_MASK			BIT(1)
+#define ISP_TOP_IRQ_EN_FRM_RST_EN			BIT(1)
+#define ISP_TOP_IRQ_EN_FRM_RST_DIS			(0 << 1)
+#define ISP_TOP_IRQ_EN_3A_DMA_ERR_MASK			BIT(5)
+#define ISP_TOP_IRQ_EN_3A_DMA_ERR_EN			BIT(5)
+#define ISP_TOP_IRQ_EN_3A_DMA_ERR_DIS			(0 << 5)
+
+#define ISP_TOP_IRQ_CLR					0x0084
+#define ISP_TOP_RO_IRQ_STAT				0x01c4
+#define ISP_TOP_RO_IRQ_STAT_FRM_END_MASK		BIT(0)
+#define ISP_TOP_RO_IRQ_STAT_FRM_RST_MASK		BIT(1)
+#define ISP_TOP_RO_IRQ_STAT_3A_DMA_ERR_MASK		BIT(5)
+
+#define ISP_TOP_MODE_CTRL				0x0400
+#define ISP_TOP_FEO_CTRL0				0x040c
+#define ISP_TOP_FEO_CTRL0_INPUT_FMT_EN_MASK		BIT(8)
+#define ISP_TOP_FEO_CTRL0_INPUT_FMT_DIS			(0 << 8)
+#define ISP_TOP_FEO_CTRL0_INPUT_FMT_EN			BIT(8)
+
+#define ISP_TOP_FEO_CTRL1_0				0x0410
+#define ISP_TOP_FEO_CTRL1_0_DPC_EN_MASK			BIT(3)
+#define ISP_TOP_FEO_CTRL1_0_DPC_DIS			(0 << 3)
+#define ISP_TOP_FEO_CTRL1_0_DPC_EN			BIT(3)
+#define ISP_TOP_FEO_CTRL1_0_OG_EN_MASK			BIT(5)
+#define ISP_TOP_FEO_CTRL1_0_OG_DIS			(0 << 5)
+#define ISP_TOP_FEO_CTRL1_0_OG_EN			BIT(5)
+
+#define ISP_TOP_FED_CTRL				0x0418
+#define ISP_TOP_FED_CTRL_PDPC_EN_MASK			BIT(1)
+#define ISP_TOP_FED_CTRL_PDPC_DIS			(0 << 1)
+#define ISP_TOP_FED_CTRL_PDPC_EN			BIT(1)
+#define ISP_TOP_FED_CTRL_RAWCNR_EN_MASK			GENMASK(6, 5)
+#define ISP_TOP_FED_CTRL_RAWCNR_DIS			(0 << 5)
+#define ISP_TOP_FED_CTRL_RAWCNR_EN			BIT(5)
+#define ISP_TOP_FED_CTRL_SNR1_EN_MASK			BIT(9)
+#define ISP_TOP_FED_CTRL_SNR1_DIS			(0 << 9)
+#define ISP_TOP_FED_CTRL_SNR1_EN			BIT(9)
+#define ISP_TOP_FED_CTRL_TNR0_EN_MASK			BIT(11)
+#define ISP_TOP_FED_CTRL_TNR0_DIS			(0 << 11)
+#define ISP_TOP_FED_CTRL_TNR0_EN			BIT(11)
+#define ISP_TOP_FED_CTRL_CUBIC_CS_EN_MASK		BIT(12)
+#define ISP_TOP_FED_CTRL_CUBIC_CS_DIS			(0 << 12)
+#define ISP_TOP_FED_CTRL_CUBIC_CS_EN			BIT(12)
+#define ISP_TOP_FED_CTRL_SQRT_EN_MASK			BIT(14)
+#define ISP_TOP_FED_CTRL_SQRT_DIS			(0 << 14)
+#define ISP_TOP_FED_CTRL_SQRT_EN			BIT(14)
+#define ISP_TOP_FED_CTRL_DGAIN_EN_MASK			BIT(16)
+#define ISP_TOP_FED_CTRL_DGAIN_DIS			(0 << 16)
+#define ISP_TOP_FED_CTRL_DGAIN_EN			BIT(16)
+
+#define ISP_TOP_BEO_CTRL				0x041c
+#define ISP_TOP_BEO_CTRL_WB_EN_MASK			BIT(6)
+#define ISP_TOP_BEO_CTRL_WB_DIS				(0 << 6)
+#define ISP_TOP_BEO_CTRL_WB_EN				BIT(6)
+#define ISP_TOP_BEO_CTRL_BLC_EN_MASK			BIT(7)
+#define ISP_TOP_BEO_CTRL_BLC_DIS			(0 << 7)
+#define ISP_TOP_BEO_CTRL_BLC_EN				BIT(7)
+#define ISP_TOP_BEO_CTRL_INV_DGAIN_EN_MASK		BIT(8)
+#define ISP_TOP_BEO_CTRL_INV_DGAIN_DIS			(0 << 8)
+#define ISP_TOP_BEO_CTRL_INV_DGAIN_EN			BIT(8)
+#define ISP_TOP_BEO_CTRL_EOTF_EN_MASK			BIT(9)
+#define ISP_TOP_BEO_CTRL_EOTF_DIS			(0 << 9)
+#define ISP_TOP_BEO_CTRL_EOTF_EN			BIT(9)
+
+#define ISP_TOP_BED_CTRL				0x0420
+#define ISP_TOP_BED_CTRL_YHS_STAT_EN_MASK		GENMASK(1, 0)
+#define ISP_TOP_BED_CTRL_YHS_STAT_DIS			(0 << 0)
+#define ISP_TOP_BED_CTRL_YHS_STAT_EN			BIT(0)
+#define ISP_TOP_BED_CTRL_GRPH_STAT_EN_MASK		BIT(2)
+#define ISP_TOP_BED_CTRL_GRPH_STAT_DIS			(0 << 2)
+#define ISP_TOP_BED_CTRL_GRPH_STAT_EN			BIT(2)
+#define ISP_TOP_BED_CTRL_FMETER_EN_MASK			BIT(3)
+#define ISP_TOP_BED_CTRL_FMETER_DIS			(0 << 3)
+#define ISP_TOP_BED_CTRL_FMETER_EN			BIT(3)
+#define ISP_TOP_BED_CTRL_BSC_EN_MASK			BIT(10)
+#define ISP_TOP_BED_CTRL_BSC_DIS			(0 << 10)
+#define ISP_TOP_BED_CTRL_BSC_EN				BIT(10)
+#define ISP_TOP_BED_CTRL_CNR2_EN_MASK			BIT(11)
+#define ISP_TOP_BED_CTRL_CNR2_DIS			(0 << 11)
+#define ISP_TOP_BED_CTRL_CNR2_EN			BIT(11)
+#define ISP_TOP_BED_CTRL_CM1_EN_MASK			BIT(13)
+#define ISP_TOP_BED_CTRL_CM1_DIS			(0 << 13)
+#define ISP_TOP_BED_CTRL_CM1_EN				BIT(13)
+#define ISP_TOP_BED_CTRL_CM0_EN_MASK			BIT(14)
+#define ISP_TOP_BED_CTRL_CM0_DIS			(0 << 14)
+#define ISP_TOP_BED_CTRL_CM0_EN				BIT(14)
+#define ISP_TOP_BED_CTRL_PST_GAMMA_EN_MASK		BIT(16)
+#define ISP_TOP_BED_CTRL_PST_GAMMA_DIS			(0 << 16)
+#define ISP_TOP_BED_CTRL_PST_GAMMA_EN			BIT(16)
+#define ISP_TOP_BED_CTRL_LUT3D_EN_MASK			BIT(17)
+#define ISP_TOP_BED_CTRL_LUT3D_DIS			(0 << 17)
+#define ISP_TOP_BED_CTRL_LUT3D_EN			BIT(17)
+#define ISP_TOP_BED_CTRL_CCM_EN_MASK			BIT(18)
+#define ISP_TOP_BED_CTRL_CCM_DIS			(0 << 18)
+#define ISP_TOP_BED_CTRL_CCM_EN				BIT(18)
+#define ISP_TOP_BED_CTRL_PST_TNR_LITE_EN_MASK		BIT(21)
+#define ISP_TOP_BED_CTRL_PST_TNR_LITE_DIS		(0 << 21)
+#define ISP_TOP_BED_CTRL_PST_TNR_LITE_EN		BIT(21)
+#define ISP_TOP_BED_CTRL_AMCM_EN_MASK			BIT(25)
+#define ISP_TOP_BED_CTRL_AMCM_DIS			(0 << 25)
+#define ISP_TOP_BED_CTRL_AMCM_EN			BIT(25)
+
+#define ISP_TOP_3A_STAT_CRTL				0x0424
+#define ISP_TOP_3A_STAT_CRTL_AE_STAT_EN_MASK		BIT(0)
+#define ISP_TOP_3A_STAT_CRTL_AE_STAT_DIS		(0 << 0)
+#define ISP_TOP_3A_STAT_CRTL_AE_STAT_EN			BIT(0)
+#define ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN_MASK		BIT(1)
+#define ISP_TOP_3A_STAT_CRTL_AWB_STAT_DIS		(0 << 1)
+#define ISP_TOP_3A_STAT_CRTL_AWB_STAT_EN		BIT(1)
+#define ISP_TOP_3A_STAT_CRTL_AF_STAT_EN_MASK		BIT(2)
+#define ISP_TOP_3A_STAT_CRTL_AF_STAT_DIS		(0 << 2)
+#define ISP_TOP_3A_STAT_CRTL_AF_STAT_EN			BIT(2)
+#define ISP_TOP_3A_STAT_CRTL_AWB_POINT_MASK		GENMASK(6, 4)
+#define ISP_TOP_3A_STAT_CRTL_AWB_POINT(x)		((x) << 4)
+#define ISP_TOP_3A_STAT_CRTL_AE_POINT_MASK		GENMASK(9, 8)
+#define ISP_TOP_3A_STAT_CRTL_AE_POINT(x)		((x) << 8)
+#define ISP_TOP_3A_STAT_CRTL_AF_POINT_MASK		GENMASK(13, 12)
+#define ISP_TOP_3A_STAT_CRTL_AF_POINT(x)		((x) << 12)
+
+#define ISP_LSWB_BLC_OFST0				0x4028
+#define ISP_LSWB_BLC_OFST0_R_OFST_MASK			GENMASK(15, 0)
+#define ISP_LSWB_BLC_OFST0_R_OFST(x)			((x) << 0)
+#define ISP_LSWB_BLC_OFST0_GR_OFST_MASK			GENMASK(31, 16)
+#define ISP_LSWB_BLC_OFST0_GR_OFST(x)			((x) << 16)
+
+#define ISP_LSWB_BLC_OFST1				0x402c
+#define ISP_LSWB_BLC_OFST1_GB_OFST_MASK			GENMASK(15, 0)
+#define ISP_LSWB_BLC_OFST1_GB_OFST(x)			((x) << 0)
+#define ISP_LSWB_BLC_OFST1_B_OFST_MASK			GENMASK(31, 16)
+#define ISP_LSWB_BLC_OFST1_B_OFST(x)			((x) << 16)
+
+#define ISP_LSWB_BLC_PHSOFST				0x4034
+#define ISP_LSWB_BLC_PHSOFST_VERT_OFST_MASK		GENMASK(1, 0)
+#define ISP_LSWB_BLC_PHSOFST_VERT_OFST(x)		((x) << 0)
+#define ISP_LSWB_BLC_PHSOFST_HORIZ_OFST_MASK		GENMASK(3, 2)
+#define ISP_LSWB_BLC_PHSOFST_HORIZ_OFST(x)		((x) << 2)
+
+#define ISP_LSWB_WB_GAIN0				0x4038
+#define ISP_LSWB_WB_GAIN0_R_GAIN_MASK			GENMASK(11, 0)
+#define ISP_LSWB_WB_GAIN0_R_GAIN(x)			((x) << 0)
+#define ISP_LSWB_WB_GAIN0_GR_GAIN_MASK			GENMASK(27, 16)
+#define ISP_LSWB_WB_GAIN0_GR_GAIN(x)			((x) << 16)
+
+#define ISP_LSWB_WB_GAIN1				0x403c
+#define ISP_LSWB_WB_GAIN1_GB_GAIN_MASK			GENMASK(11, 0)
+#define ISP_LSWB_WB_GAIN1_GB_GAIN(x)			((x) << 0)
+#define ISP_LSWB_WB_GAIN1_B_GAIN_MASK			GENMASK(27, 16)
+#define ISP_LSWB_WB_GAIN1_B_GAIN(x)			((x) << 16)
+
+#define ISP_LSWB_WB_GAIN2				0x4040
+#define ISP_LSWB_WB_GAIN2_IR_GAIN_MASK			GENMASK(11, 0)
+#define ISP_LSWB_WB_GAIN2_IR_GAIN(x)			((x) << 0)
+
+#define ISP_LSWB_WB_LIMIT0				0x4044
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_R_MASK		GENMASK(15, 0)
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_R(x)		((x) << 0)
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_R_MAX		(0x8fff << 0)
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_GR_MASK		GENMASK(31, 16)
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_GR(x)		((x) << 16)
+#define ISP_LSWB_WB_LIMIT0_WB_LIMIT_GR_MAX		(0x8fff << 16)
+
+#define ISP_LSWB_WB_LIMIT1				0x4048
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_GB_MASK		GENMASK(15, 0)
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_GB(x)		((x) << 0)
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_GB_MAX		(0x8fff << 0)
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_B_MASK		GENMASK(31, 16)
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_B(x)		((x) << 16)
+#define ISP_LSWB_WB_LIMIT1_WB_LIMIT_B_MAX		(0x8fff << 16)
+
+#define ISP_LSWB_WB_PHSOFST				0x4050
+#define ISP_LSWB_WB_PHSOFST_VERT_OFST_MASK		GENMASK(1, 0)
+#define ISP_LSWB_WB_PHSOFST_VERT_OFST(x)		((x) << 0)
+#define ISP_LSWB_WB_PHSOFST_HORIZ_OFST_MASK		GENMASK(3, 2)
+#define ISP_LSWB_WB_PHSOFST_HORIZ_OFST(x)		((x) << 2)
+
+#define ISP_LSWB_LNS_PHSOFST				0x4054
+#define ISP_LSWB_LNS_PHSOFST_VERT_OFST_MASK		GENMASK(1, 0)
+#define ISP_LSWB_LNS_PHSOFST_VERT_OFST(x)		((x) << 0)
+#define ISP_LSWB_LNS_PHSOFST_HORIZ_OFST_MASK		GENMASK(3, 2)
+#define ISP_LSWB_LNS_PHSOFST_HORIZ_OFST(x)		((x) << 2)
+
+#define ISP_DMS_COMMON_PARAM0				0x5000
+#define ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST_MASK	GENMASK(1, 0)
+#define ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST(x)		((x) << 0)
+#define ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST_MASK	GENMASK(3, 2)
+#define ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST(x)		((x) << 2)
+
+#define ISP_CM0_COEF00_01				0x6048
+#define ISP_CM0_COEF00_01_MTX_00_MASK			GENMASK(12, 0)
+#define ISP_CM0_COEF00_01_MTX_00(x)			((x) << 0)
+#define ISP_CM0_COEF00_01_MTX_01_MASK			GENMASK(28, 16)
+#define ISP_CM0_COEF00_01_MTX_01(x)			((x) << 16)
+
+#define ISP_CM0_COEF02_10				0x604c
+#define ISP_CM0_COEF02_10_MTX_02_MASK			GENMASK(12, 0)
+#define ISP_CM0_COEF02_10_MTX_02(x)			((x) << 0)
+#define ISP_CM0_COEF02_10_MTX_10_MASK			GENMASK(28, 16)
+#define ISP_CM0_COEF02_10_MTX_10(x)			((x) << 16)
+
+#define ISP_CM0_COEF11_12				0x6050
+#define ISP_CM0_COEF11_12_MTX_11_MASK			GENMASK(12, 0)
+#define ISP_CM0_COEF11_12_MTX_11(x)			((x) << 0)
+#define ISP_CM0_COEF11_12_MTX_12_MASK			GENMASK(28, 16)
+#define ISP_CM0_COEF11_12_MTX_12(x)			((x) << 16)
+
+#define ISP_CM0_COEF20_21				0x6054
+#define ISP_CM0_COEF20_21_MTX_20_MASK			GENMASK(12, 0)
+#define ISP_CM0_COEF20_21_MTX_20(x)			((x) << 0)
+#define ISP_CM0_COEF20_21_MTX_21_MASK			GENMASK(28, 16)
+#define ISP_CM0_COEF20_21_MTX_21(x)			((x) << 16)
+
+#define ISP_CM0_COEF22_OUP_OFST0			0x6058
+#define ISP_CM0_COEF22_OUP_OFST0_MTX_22_MASK		GENMASK(12, 0)
+#define ISP_CM0_COEF22_OUP_OFST0_MTX_22(x)		((x) << 0)
+
+#define ISP_CCM_MTX_00_01				0x6098
+#define ISP_CCM_MTX_00_01_MTX_00_MASK			GENMASK(12, 0)
+#define ISP_CCM_MTX_00_01_MTX_00(x)			((x) << 0)
+#define ISP_CCM_MTX_00_01_MTX_01_MASK			GENMASK(28, 16)
+#define ISP_CCM_MTX_00_01_MTX_01(x)			((x) << 16)
+
+#define ISP_CCM_MTX_02_03				0x609c
+#define ISP_CCM_MTX_02_03_MTX_02_MASK			GENMASK(12, 0)
+#define ISP_CCM_MTX_02_03_MTX_02(x)			((x) << 0)
+
+#define ISP_CCM_MTX_10_11				0x60A0
+#define ISP_CCM_MTX_10_11_MTX_10_MASK			GENMASK(12, 0)
+#define ISP_CCM_MTX_10_11_MTX_10(x)			((x) << 0)
+#define ISP_CCM_MTX_10_11_MTX_11_MASK			GENMASK(28, 16)
+#define ISP_CCM_MTX_10_11_MTX_11(x)			((x) << 16)
+
+#define ISP_CCM_MTX_12_13				0x60A4
+#define ISP_CCM_MTX_12_13_MTX_12_MASK			GENMASK(12, 0)
+#define ISP_CCM_MTX_12_13_MTX_12(x)			((x) << 0)
+
+#define ISP_CCM_MTX_20_21				0x60A8
+#define ISP_CCM_MTX_20_21_MTX_20_MASK			GENMASK(12, 0)
+#define ISP_CCM_MTX_20_21_MTX_20(x)			((x) << 0)
+#define ISP_CCM_MTX_20_21_MTX_21_MASK			GENMASK(28, 16)
+#define ISP_CCM_MTX_20_21_MTX_21(x)			((x) << 16)
+
+#define ISP_CCM_MTX_22_23_RS				0x60Ac
+#define ISP_CCM_MTX_22_23_RS_MTX_22_MASK		GENMASK(12, 0)
+#define ISP_CCM_MTX_22_23_RS_MTX_22(x)			((x) << 0)
+
+#define ISP_PST_GAMMA_LUT_ADDR				0x60cc
+#define ISP_PST_GAMMA_LUT_ADDR_IDX_ADDR(x)		((x) << 7)
+
+#define ISP_PST_GAMMA_LUT_DATA				0x60d0
+#define ISP_PST_GM_LUT_DATA0(x)		(((x) & GENMASK(15, 0)) << 0)
+#define ISP_PST_GM_LUT_DATA1(x)		(((x) & GENMASK(15, 0)) << 16)
+
+#define DISP0_TOP_TOP_CTRL				0x8000
+#define DISP0_TOP_TOP_CTRL_CROP2_EN_MASK		BIT(5)
+#define DISP0_TOP_TOP_CTRL_CROP2_EN			BIT(5)
+#define DISP0_TOP_TOP_CTRL_CROP2_DIS			(0 << 5)
+
+#define DISP0_TOP_CRP2_START				0x8004
+#define DISP0_TOP_CRP2_START_V_START_MASK		GENMASK(15, 0)
+#define DISP0_TOP_CRP2_START_V_START(x)			((x) << 0)
+#define DISP0_TOP_CRP2_START_H_START_MASK		GENMASK(31, 16)
+#define DISP0_TOP_CRP2_START_H_START(x)			((x) << 16)
+
+#define DISP0_TOP_CRP2_SIZE				0x8008
+#define DISP0_TOP_CRP2_SIZE_V_SIZE_MASK			GENMASK(15, 0)
+#define DISP0_TOP_CRP2_SIZE_V_SIZE(x)			((x) << 0)
+#define DISP0_TOP_CRP2_SIZE_H_SIZE_MASK			GENMASK(31, 16)
+#define DISP0_TOP_CRP2_SIZE_H_SIZE(x)			((x) << 16)
+
+#define DISP0_TOP_OUT_SIZE				0x800c
+#define DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT_MASK		GENMASK(12, 0)
+#define DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT(x)		((x) << 0)
+#define DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH_MASK		GENMASK(28, 16)
+#define DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH(x)		((x) << 16)
+
+#define ISP_DISP0_TOP_IN_SIZE				0x804c
+#define ISP_DISP0_TOP_IN_SIZE_VSIZE_MASK		GENMASK(12, 0)
+#define ISP_DISP0_TOP_IN_SIZE_VSIZE(x)			((x) << 0)
+#define ISP_DISP0_TOP_IN_SIZE_HSIZE_MASK		GENMASK(28, 16)
+#define ISP_DISP0_TOP_IN_SIZE_HSIZE(x)			((x) << 16)
+
+#define DISP0_PPS_SCALE_EN				0x8200
+#define DISP0_PPS_SCALE_EN_VSC_TAP_NUM_MASK		GENMASK(3, 0)
+#define DISP0_PPS_SCALE_EN_VSC_TAP_NUM(x)		((x) << 0)
+#define DISP0_PPS_SCALE_EN_HSC_TAP_NUM_MASK		GENMASK(7, 4)
+#define DISP0_PPS_SCALE_EN_HSC_TAP_NUM(x)		((x) << 4)
+#define DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM_MASK		GENMASK(11, 8)
+#define DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM(x)		((x) << 8)
+#define DISP0_PPS_SCALE_EN_PREHSC_FLT_NUM_MASK		GENMASK(15, 12)
+#define DISP0_PPS_SCALE_EN_PREHSC_FLT_NUM(x)		((x) << 12)
+#define DISP0_PPS_SCALE_EN_PREVSC_RATE_MASK		GENMASK(17, 16)
+#define DISP0_PPS_SCALE_EN_PREVSC_RATE(x)		((x) << 16)
+#define DISP0_PPS_SCALE_EN_PREHSC_RATE_MASK		GENMASK(19, 18)
+#define DISP0_PPS_SCALE_EN_PREHSC_RATE(x)		((x) << 18)
+#define DISP0_PPS_SCALE_EN_HSC_EN_MASK			BIT(20)
+#define DISP0_PPS_SCALE_EN_HSC_EN(x)			((x) << 20)
+#define DISP0_PPS_SCALE_EN_HSC_DIS			(0 << 20)
+#define DISP0_PPS_SCALE_EN_VSC_EN_MASK			BIT(21)
+#define DISP0_PPS_SCALE_EN_VSC_EN(x)			((x) << 21)
+#define DISP0_PPS_SCALE_EN_VSC_DIS			(0 << 21)
+#define DISP0_PPS_SCALE_EN_PREVSC_EN_MASK		BIT(22)
+#define DISP0_PPS_SCALE_EN_PREVSC_EN(x)			((x) << 22)
+#define DISP0_PPS_SCALE_EN_PREVSC_DIS			(0 << 22)
+#define DISP0_PPS_SCALE_EN_PREHSC_EN_MASK		BIT(23)
+#define DISP0_PPS_SCALE_EN_PREHSC_EN(x)			((x) << 23)
+#define DISP0_PPS_SCALE_EN_PREHSC_DIS			(0 << 23)
+#define DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS_MASK		GENMASK(27, 24)
+#define DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS(x)		((x) << 24)
+#define DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS_MASK		GENMASK(31, 28)
+#define DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS(x)		((x) << 28)
+
+#define DISP0_PPS_VSC_START_PHASE_STEP			0x8224
+#define DISP0_PPS_VSC_START_PHASE_STEP_VERT_FRAC_MASK	GENMASK(23, 0)
+#define DISP0_PPS_VSC_START_PHASE_STEP_VERT_FRAC(x)	((x) << 0)
+#define DISP0_PPS_VSC_START_PHASE_STEP_VERT_INTE_MASK	GENMASK(27, 24)
+#define DISP0_PPS_VSC_START_PHASE_STEP_VERT_INTE(x)	((x) << 24)
+
+#define DISP0_PPS_HSC_START_PHASE_STEP			0x8230
+#define DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_FRAC_MASK	GENMASK(23, 0)
+#define DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_FRAC(x)	((x) << 0)
+#define DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_INTE_MASK	GENMASK(27, 24)
+#define DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_INTE(x)	((x) << 24)
+
+#define DISP0_PPS_444TO422				0x823c
+#define DISP0_PPS_444TO422_EN_MASK			BIT(0)
+#define DISP0_PPS_444TO422_EN(x)			((x) << 0)
+
+#define ISP_SCALE0_COEF_IDX_LUMA			0x8240
+#define ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_MASK	BIT(9)
+#define ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_EN	BIT(9)
+#define ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_DIS	(0 << 9)
+#define ISP_SCALE0_COEF_IDX_LUMA_CTYPE_MASK		GENMASK(12, 10)
+#define ISP_SCALE0_COEF_IDX_LUMA_CTYPE(x)		((x) << 10)
+
+#define ISP_SCALE0_COEF_LUMA				0x8244
+#define ISP_SCALE0_COEF_LUMA_DATA1(x)		(((x) & GENMASK(10, 0)) << 0)
+#define ISP_SCALE0_COEF_LUMA_DATA0(x)		(((x) & GENMASK(10, 0)) << 16)
+
+#define ISP_SCALE0_COEF_IDX_CHRO			0x8248
+#define ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_MASK	BIT(9)
+#define ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_EN	BIT(9)
+#define ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_DIS	(0 << 9)
+#define ISP_SCALE0_COEF_IDX_CHRO_CTYPE_MASK		GENMASK(12, 10)
+#define ISP_SCALE0_COEF_IDX_CHRO_CTYPE(x)		((x) << 10)
+
+#define ISP_SCALE0_COEF_CHRO				0x824c
+#define ISP_SCALE0_COEF_CHRO_DATA1(x)		(((x) & GENMASK(10, 0)) << 0)
+#define ISP_SCALE0_COEF_CHRO_DATA0(x)		(((x) & GENMASK(10, 0)) << 16)
+
+#define ISP_AF_CTRL					0xa044
+#define ISP_AF_CTRL_VERT_OFST_MASK			GENMASK(15, 14)
+#define ISP_AF_CTRL_VERT_OFST(x)			((x) << 14)
+#define ISP_AF_CTRL_HORIZ_OFST_MASK			GENMASK(17, 16)
+#define ISP_AF_CTRL_HORIZ_OFST(x)			((x) << 16)
+
+#define ISP_AF_HV_SIZE					0xa04c
+#define ISP_AF_HV_SIZE_GLB_WIN_YSIZE_MASK		GENMASK(15, 0)
+#define ISP_AF_HV_SIZE_GLB_WIN_YSIZE(x)			((x) << 0)
+#define ISP_AF_HV_SIZE_GLB_WIN_XSIZE_MASK		GENMASK(31, 16)
+#define ISP_AF_HV_SIZE_GLB_WIN_XSIZE(x)			((x) << 16)
+
+#define ISP_AF_HV_BLKNUM				0xa050
+#define ISP_AF_HV_BLKNUM_V_NUM_MASK			GENMASK(5, 0)
+#define ISP_AF_HV_BLKNUM_V_NUM(x)			((x) << 0)
+#define ISP_AF_HV_BLKNUM_H_NUM_MASK			GENMASK(21, 16)
+#define ISP_AF_HV_BLKNUM_H_NUM(x)			((x) << 16)
+
+#define ISP_AF_EN_CTRL					0xa054
+#define ISP_AF_EN_CTRL_STAT_SEL_MASK			BIT(21)
+#define ISP_AF_EN_CTRL_STAT_SEL_OLD			(0 << 21)
+#define ISP_AF_EN_CTRL_STAT_SEL_NEW			BIT(21)
+
+#define ISP_AF_IDX_ADDR					0xa1c0
+#define ISP_AF_IDX_DATA					0xa1c4
+#define ISP_AF_IDX_DATA_VIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 0)
+#define ISP_AF_IDX_DATA_HIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 16)
+
+#define ISP_AE_CTRL					0xa448
+#define ISP_AE_CTRL_INPUT_2LINE_MASK			BIT(7)
+#define ISP_AE_CTRL_INPUT_2LINE_EN			BIT(7)
+#define ISP_AE_CTRL_INPUT_2LINE_DIS			(0 << 7)
+#define ISP_AE_CTRL_LUMA_MODE_MASK			GENMASK(9, 8)
+#define ISP_AE_CTRL_LUMA_MODE_CUR			(0 << 8)
+#define ISP_AE_CTRL_LUMA_MODE_MAX			BIT(8)
+#define ISP_AE_CTRL_LUMA_MODE_FILTER			(2 << 8)
+#define ISP_AE_CTRL_VERT_OFST_MASK			GENMASK(25, 24)
+#define ISP_AE_CTRL_VERT_OFST(x)			((x) << 24)
+#define ISP_AE_CTRL_HORIZ_OFST_MASK			GENMASK(27, 26)
+#define ISP_AE_CTRL_HORIZ_OFST(x)			((x) << 26)
+
+#define ISP_AE_HV_SIZE					0xa464
+#define ISP_AE_HV_SIZE_VERT_SIZE_MASK			GENMASK(15, 0)
+#define ISP_AE_HV_SIZE_VERT_SIZE(x)			((x) << 0)
+#define ISP_AE_HV_SIZE_HORIZ_SIZE_MASK			GENMASK(31, 16)
+#define ISP_AE_HV_SIZE_HORIZ_SIZE(x)			((x) << 16)
+
+#define ISP_AE_HV_BLKNUM				0xa468
+#define ISP_AE_HV_BLKNUM_V_NUM_MASK			GENMASK(6, 0)
+#define ISP_AE_HV_BLKNUM_V_NUM(x)			((x) << 0)
+#define ISP_AE_HV_BLKNUM_H_NUM_MASK			GENMASK(22, 16)
+#define ISP_AE_HV_BLKNUM_H_NUM(x)			((x) << 16)
+
+#define ISP_AE_IDX_ADDR					0xa600
+#define ISP_AE_IDX_DATA					0xa604
+#define ISP_AE_IDX_DATA_VIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 0)
+#define ISP_AE_IDX_DATA_HIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 16)
+
+#define ISP_AE_BLK_WT_ADDR				0xa608
+#define ISP_AE_BLK_WT_DATA				0xa60c
+#define ISP_AE_BLK_WT_DATA_WT(i, x)	(((x) & GENMASK(3, 0)) << ((i) * 4))
+
+#define ISP_AWB_CTRL					0xa834
+#define ISP_AWB_CTRL_VERT_OFST_MASK			GENMASK(1, 0)
+#define ISP_AWB_CTRL_VERT_OFST(x)			((x) << 0)
+#define ISP_AWB_CTRL_HORIZ_OFST_MASK			GENMASK(3, 2)
+#define ISP_AWB_CTRL_HORIZ_OFST(x)			((x) << 2)
+
+#define ISP_AWB_HV_SIZE					0xa83c
+#define ISP_AWB_HV_SIZE_VERT_SIZE_MASK			GENMASK(15, 0)
+#define ISP_AWB_HV_SIZE_VERT_SIZE(x)			((x) << 0)
+#define ISP_AWB_HV_SIZE_HORIZ_SIZE_MASK			GENMASK(31, 16)
+#define ISP_AWB_HV_SIZE_HORIZ_SIZE(x)			((x) << 16)
+
+#define ISP_AWB_HV_BLKNUM				0xa840
+#define ISP_AWB_HV_BLKNUM_V_NUM_MASK			GENMASK(5, 0)
+#define ISP_AWB_HV_BLKNUM_V_NUM(x)			((x) << 0)
+#define ISP_AWB_HV_BLKNUM_H_NUM_MASK			GENMASK(21, 16)
+#define ISP_AWB_HV_BLKNUM_H_NUM(x)			((x) << 16)
+
+#define ISP_AWB_STAT_RG					0xa848
+#define ISP_AWB_STAT_RG_MIN_VALUE_MASK			GENMASK(11, 0)
+#define ISP_AWB_STAT_RG_MIN_VALUE(x)			((x) << 0)
+#define ISP_AWB_STAT_RG_MAX_VALUE_MASK			GENMASK(27, 16)
+#define ISP_AWB_STAT_RG_MAX_VALUE(x)			((x) << 16)
+
+#define ISP_AWB_STAT_BG					0xa84c
+#define ISP_AWB_STAT_BG_MIN_VALUE_MASK			GENMASK(11, 0)
+#define ISP_AWB_STAT_BG_MIN_VALUE(x)			((x) << 0)
+#define ISP_AWB_STAT_BG_MAX_VALUE_MASK			GENMASK(27, 16)
+#define ISP_AWB_STAT_BG_MAX_VALUE(x)			((x) << 16)
+
+#define ISP_AWB_STAT_RG_HL				0xa850
+#define ISP_AWB_STAT_RG_HL_LOW_VALUE_MASK		GENMASK(11, 0)
+#define ISP_AWB_STAT_RG_HL_LOW_VALUE(x)			((x) << 0)
+#define ISP_AWB_STAT_RG_HL_HIGH_VALUE_MASK		GENMASK(27, 16)
+#define ISP_AWB_STAT_RG_HL_HIGH_VALUE(x)		((x) << 16)
+
+#define ISP_AWB_STAT_BG_HL				0xa854
+#define ISP_AWB_STAT_BG_HL_LOW_VALUE_MASK		GENMASK(11, 0)
+#define ISP_AWB_STAT_BG_HL_LOW_VALUE(x)			((x) << 0)
+#define ISP_AWB_STAT_BG_HL_HIGH_VALUE_MASK		GENMASK(27, 16)
+#define ISP_AWB_STAT_BG_HL_HIGH_VALUE(x)		((x) << 16)
+
+#define ISP_AWB_STAT_CTRL2				0xa858
+#define ISP_AWB_STAT_CTRL2_SATUR_CTRL_MASK		BIT(0)
+#define ISP_AWB_STAT_CTRL2_SATUR_CTRL(x)		((x) << 0)
+
+#define ISP_AWB_IDX_ADDR				0xaa00
+#define ISP_AWB_IDX_DATA				0xaa04
+#define ISP_AWB_IDX_DATA_VIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 0)
+#define ISP_AWB_IDX_DATA_HIDX_DATA(x)		(((x) & GENMASK(15, 0)) << 16)
+
+#define ISP_AWB_BLK_WT_ADDR				0xaa08
+#define ISP_AWB_BLK_WT_DATA				0xaa0c
+#define ISP_AWB_BLK_WT_DATA_WT(i, x)	(((x) & GENMASK(3, 0)) << ((i) * 4))
+
+#define ISP_WRMIFX3_0_CH0_CTRL0				0xc400
+#define ISP_WRMIFX3_0_CH0_CTRL0_STRIDE_MASK		GENMASK(28, 16)
+#define ISP_WRMIFX3_0_CH0_CTRL0_STRIDE(x)		((x) << 16)
+
+#define ISP_WRMIFX3_0_CH0_CTRL1				0xc404
+#define ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_MODE_MASK	GENMASK(30, 27)
+#define ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_8BITS		BIT(27)
+#define ISP_WRMIFX3_0_CH0_CTRL1_PIX_BITS_16BITS		(2 << 27)
+
+#define ISP_WRMIFX3_0_CH1_CTRL0				0xc408
+#define ISP_WRMIFX3_0_CH1_CTRL0_STRIDE_MASK		GENMASK(28, 16)
+#define ISP_WRMIFX3_0_CH1_CTRL0_STRIDE(x)		((x) << 16)
+
+#define ISP_WRMIFX3_0_CH1_CTRL1				0xc40c
+#define ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_MODE_MASK	GENMASK(30, 27)
+#define ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_8BITS		BIT(27)
+#define ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_16BITS		(2 << 27)
+#define ISP_WRMIFX3_0_CH1_CTRL1_PIX_BITS_32BITS		(3 << 27)
+
+#define ISP_WRMIFX3_0_WIN_LUMA_H			0xc420
+#define ISP_WRMIFX3_0_WIN_LUMA_H_LUMA_HEND_MASK		GENMASK(28, 16)
+#define ISP_WRMIFX3_0_WIN_LUMA_H_LUMA_HEND(x)		(((x) - 1) << 16)
+
+#define ISP_WRMIFX3_0_WIN_LUMA_V			0xc424
+#define ISP_WRMIFX3_0_WIN_LUMA_V_LUMA_VEND_MASK		GENMASK(28, 16)
+#define ISP_WRMIFX3_0_WIN_LUMA_V_LUMA_VEND(x)		(((x) - 1) << 16)
+
+#define ISP_WRMIFX3_0_WIN_CHROM_H			0xc428
+#define ISP_WRMIFX3_0_WIN_CHROM_H_CHROM_HEND_MASK	GENMASK(28, 16)
+#define ISP_WRMIFX3_0_WIN_CHROM_H_CHROM_HEND(x)		(((x) - 1) << 16)
+
+#define ISP_WRMIFX3_0_WIN_CHROM_V			0xc42c
+#define ISP_WRMIFX3_0_WIN_CHROM_V_CHROM_VEND_MASK	GENMASK(28, 16)
+#define ISP_WRMIFX3_0_WIN_CHROM_V_CHROM_VEND(x)		(((x) - 1) << 16)
+
+#define ISP_WRMIFX3_0_CH0_BADDR				0xc440
+#define ISP_WRMIFX3_0_CH0_BASE_ADDR(x)			((x) >> 4)
+
+#define ISP_WRMIFX3_0_CH1_BADDR				0xc444
+#define ISP_WRMIFX3_0_CH1_BASE_ADDR(x)			((x) >> 4)
+
+#define ISP_WRMIFX3_0_FMT_SIZE				0xc464
+#define ISP_WRMIFX3_0_FMT_SIZE_HSIZE_MASK		GENMASK(15, 0)
+#define ISP_WRMIFX3_0_FMT_SIZE_HSIZE(x)			((x) << 0)
+#define ISP_WRMIFX3_0_FMT_SIZE_VSIZE_MASK		GENMASK(31, 16)
+#define ISP_WRMIFX3_0_FMT_SIZE_VSIZE(x)			((x) << 16)
+
+#define ISP_WRMIFX3_0_FMT_CTRL				0xc468
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_MASK		GENMASK(1, 0)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_8BIT		(0 << 0)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_10BIT		BIT(0)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_12BIT		(2 << 0)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_IBITS_16BIT		(3 << 0)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_MASK		BIT(2)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_VU		(0 << 2)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_UV_SWAP_UV		BIT(2)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_MASK		GENMASK(5, 4)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X1		(0 << 4)
+#define ISP_WRMIFX3_0_FMT_CTRL_MTX_PLANE_X2		BIT(4)
+#define ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_MASK		GENMASK(18, 16)
+#define ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV422		BIT(16)
+#define ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_YUV420		(2 << 16)
+#define ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_Y_ONLY		(3 << 16)
+#define ISP_WRMIFX3_0_FMT_CTRL_MODE_OUT_RAW		(4 << 16)
+
+#define VIU_DMAWR_BADDR0				0xc840
+#define VIU_DMAWR_BADDR0_AF_STATS_BASE_ADDR_MASK	GENMASK(27, 0)
+#define VIU_DMAWR_BADDR0_AF_STATS_BASE_ADDR(x)		((x) >> 4)
+
+#define VIU_DMAWR_BADDR1				0xc844
+#define VIU_DMAWR_BADDR1_AWB_STATS_BASE_ADDR_MASK	GENMASK(27, 0)
+#define VIU_DMAWR_BADDR1_AWB_STATS_BASE_ADDR(x)		((x) >> 4)
+
+#define VIU_DMAWR_BADDR2				0xc848
+#define VIU_DMAWR_BADDR2_AE_STATS_BASE_ADDR_MASK	GENMASK(27, 0)
+#define VIU_DMAWR_BADDR2_AE_STATS_BASE_ADDR(x)		((x) >> 4)
+
+#define VIU_DMAWR_SIZE0					0xc854
+#define VIU_DMAWR_SIZE0_AF_STATS_SIZE_MASK		GENMASK(15, 0)
+#define VIU_DMAWR_SIZE0_AF_STATS_SIZE(x)		((x) << 0)
+#define VIU_DMAWR_SIZE0_AWB_STATS_SIZE_MASK		GENMASK(31, 16)
+#define VIU_DMAWR_SIZE0_AWB_STATS_SIZE(x)		((x) << 16)
+
+#define VIU_DMAWR_SIZE1					0xc858
+#define VIU_DMAWR_SIZE1_AE_STATS_SIZE_MASK		GENMASK(15, 0)
+#define VIU_DMAWR_SIZE1_AE_STATS_SIZE(x)		((x) << 0)
+
+#endif
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-resizer.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-resizer.c
new file mode 100644
index 000000000000..453a889e0b27
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-resizer.c
@@ -0,0 +1,892 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/pm_runtime.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+#define C3_ISP_RSZ_DEF_PAD_FMT		MEDIA_BUS_FMT_YUV10_1X30
+#define C3_ISP_DISP_REG(base, id)	((base) + (id) * 0x400)
+#define C3_ISP_PPS_LUT_H_NUM		33
+#define C3_ISP_PPS_LUT_CTYPE_0		0
+#define C3_ISP_PPS_LUT_CTYPE_2		2
+#define C3_ISP_SCL_EN			1
+#define C3_ISP_SCL_DIS			0
+
+/*
+ * struct c3_isp_rsz_format_info - ISP resizer format information
+ *
+ * @mbus_code: the mbus code
+ * @pads: bitmask detailing valid pads for this mbus_code
+ * @is_raw: the raw format flag of mbus code
+ */
+struct c3_isp_rsz_format_info {
+	u32 mbus_code;
+	u32 pads;
+	bool is_raw;
+};
+
+static const struct c3_isp_rsz_format_info c3_isp_rsz_fmts[] = {
+	/* RAW formats */
+	{
+		.mbus_code	= MEDIA_BUS_FMT_SBGGR16_1X16,
+		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
+				| BIT(C3_ISP_RSZ_PAD_SOURCE),
+		.is_raw = true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGBRG16_1X16,
+		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
+				| BIT(C3_ISP_RSZ_PAD_SOURCE),
+		.is_raw = true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SGRBG16_1X16,
+		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
+				| BIT(C3_ISP_RSZ_PAD_SOURCE),
+		.is_raw = true,
+	}, {
+		.mbus_code	= MEDIA_BUS_FMT_SRGGB16_1X16,
+		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
+				| BIT(C3_ISP_RSZ_PAD_SOURCE),
+		.is_raw = true,
+	},
+	/* YUV formats */
+	{
+		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
+		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
+				| BIT(C3_ISP_RSZ_PAD_SOURCE),
+		.is_raw = false,
+	},
+};
+
+/*
+ * struct c3_isp_pps_io_size - ISP scaler input and output size
+ *
+ * @thsize: input horizontal size of after preprocessing
+ * @tvsize: input vertical size of after preprocessing
+ * @ohsize: output horizontal size
+ * @ovsize: output vertical size
+ * @ihsize: input horizontal size
+ * @max_hsize: maximum horizontal size
+ */
+struct c3_isp_pps_io_size {
+	u32 thsize;
+	u32 tvsize;
+	u32 ohsize;
+	u32 ovsize;
+	u32 ihsize;
+	u32 max_hsize;
+};
+
+/* The normal parameters of pps module */
+static const int c3_isp_pps_lut[C3_ISP_PPS_LUT_H_NUM][4] =  {
+	{  0, 511,   0,   0}, { -5, 511,   5,   0}, {-10, 511,  11,   0},
+	{-14, 510,  17,  -1}, {-18, 508,  23,  -1}, {-22, 506,  29,  -1},
+	{-25, 503,  36,  -2}, {-28, 500,  43,  -3}, {-32, 496,  51,  -3},
+	{-34, 491,  59,  -4}, {-37, 487,  67,  -5}, {-39, 482,  75,  -6},
+	{-41, 476,  84,  -7}, {-42, 470,  92,  -8}, {-44, 463, 102,  -9},
+	{-45, 456, 111, -10}, {-45, 449, 120, -12}, {-47, 442, 130, -13},
+	{-47, 434, 140, -15}, {-47, 425, 151, -17}, {-47, 416, 161, -18},
+	{-47, 407, 172, -20}, {-47, 398, 182, -21}, {-47, 389, 193, -23},
+	{-46, 379, 204, -25}, {-45, 369, 215, -27}, {-44, 358, 226, -28},
+	{-43, 348, 237, -30}, {-43, 337, 249, -31}, {-41, 326, 260, -33},
+	{-40, 316, 271, -35}, {-39, 305, 282, -36}, {-37, 293, 293, -37}
+};
+
+static const struct c3_isp_rsz_format_info
+*rsz_find_format_by_code(u32 code, u32 pad)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
+		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];
+
+		if (info->mbus_code == code && info->pads & BIT(pad))
+			return info;
+	}
+
+	return NULL;
+}
+
+static const struct c3_isp_rsz_format_info
+*rsz_find_format_by_index(u32 index, u32 pad)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
+		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];
+
+		if (!(info->pads & BIT(pad)))
+			continue;
+
+		if (!index)
+			return info;
+
+		index--;
+	}
+
+	return NULL;
+}
+
+static void c3_isp_rsz_pps_size(struct c3_isp_resizer *rsz,
+				struct c3_isp_pps_io_size *io_size)
+{
+	int thsize = io_size->thsize;
+	int tvsize = io_size->tvsize;
+	u32 ohsize = io_size->ohsize;
+	u32 ovsize = io_size->ovsize;
+	u32 ihsize = io_size->ihsize;
+	u32 max_hsize = io_size->max_hsize;
+	int h_int;
+	int v_int;
+	int h_fract;
+	int v_fract;
+	int yuv444to422_en;
+
+	/* Calculate the integer part of horizonal scaler step */
+	h_int = thsize / ohsize;
+
+	/* Calculate the vertical part of horizonal scaler step */
+	v_int = tvsize / ovsize;
+
+	/*
+	 * Calculate the fraction part of horizonal scaler step.
+	 * step_h_fraction = (source / dest) * 2^24,
+	 * so step_h_fraction = ((source << 12) / dest) << 12.
+	 */
+	h_fract = ((thsize << 12) / ohsize) << 12;
+
+	/*
+	 * Calculate the fraction part of vertical scaler step
+	 * step_v_fraction = (source / dest) * 2^24,
+	 * so step_v_fraction = ((source << 12) / dest) << 12.
+	 */
+	v_fract = ((tvsize << 12) / ovsize) << 12;
+
+	yuv444to422_en = ihsize > (max_hsize / 2) ? 1 : 0;
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_444TO422, rsz->id),
+			   DISP0_PPS_444TO422_EN_MASK,
+			   DISP0_PPS_444TO422_EN(yuv444to422_en));
+
+	c3_isp_write(rsz->isp,
+		     C3_ISP_DISP_REG(DISP0_PPS_VSC_START_PHASE_STEP, rsz->id),
+		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_FRAC(v_fract) |
+		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_INTE(v_int));
+
+	c3_isp_write(rsz->isp,
+		     C3_ISP_DISP_REG(DISP0_PPS_HSC_START_PHASE_STEP, rsz->id),
+		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_FRAC(h_fract) |
+		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_INTE(h_int));
+}
+
+static void c3_isp_rsz_pps_lut(struct c3_isp_resizer *rsz, u32 ctype)
+{
+	unsigned int i;
+
+	/*
+	 * Default value of this register is 0, so only need to set
+	 * SCALE_LUMA_COEF_S11_MODE and SCALE_LUMA_CTYPE. This register needs
+	 * to be written in one time.
+	 */
+	c3_isp_write(rsz->isp,
+		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_LUMA, rsz->id),
+		     ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_EN |
+		     ISP_SCALE0_COEF_IDX_LUMA_CTYPE(ctype));
+
+	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
+		c3_isp_write(rsz->isp,
+			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
+			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][0]) |
+			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][1]));
+		c3_isp_write(rsz->isp,
+			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
+			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][2]) |
+			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][3]));
+	}
+
+	/*
+	 * Default value of this register is 0, so only need to set
+	 * SCALE_CHRO_COEF_S11_MODE and SCALE_CHRO_CTYPE. This register needs
+	 * to be written in one time.
+	 */
+	c3_isp_write(rsz->isp,
+		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_CHRO, rsz->id),
+		     ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_EN |
+		     ISP_SCALE0_COEF_IDX_CHRO_CTYPE(ctype));
+
+	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
+		c3_isp_write(rsz->isp,
+			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
+			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][0]) |
+			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][1]));
+		c3_isp_write(rsz->isp,
+			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
+			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][2]) |
+			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][3]));
+	}
+}
+
+static void c3_isp_rsz_pps_disable(struct c3_isp_resizer *rsz)
+{
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_HSC_DIS);
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_VSC_DIS);
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_PREVSC_DIS);
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_PREHSC_DIS);
+}
+
+static int c3_isp_rsz_pps_enable(struct c3_isp_resizer *rsz,
+				 struct v4l2_subdev_state *state)
+{
+	struct v4l2_rect *crop;
+	struct v4l2_rect *cmps;
+	int max_hsize;
+	int hsc_en, vsc_en;
+	int preh_en, prev_en;
+	u32 prehsc_rate;
+	u32 prevsc_flt_num;
+	int pre_vscale_max_hsize;
+	u32 ihsize_after_pre_hsc;
+	u32 ihsize_after_pre_hsc_alt;
+	u32 vsc_tap_num_alt;
+	u32 ihsize;
+	u32 ivsize;
+	struct c3_isp_pps_io_size io_size;
+
+	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
+	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
+
+	ihsize = crop->width;
+	ivsize = crop->height;
+
+	hsc_en = (ihsize == cmps->width) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;
+	vsc_en = (ivsize == cmps->height) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;
+
+	/* Disable pps when there no need to use pps */
+	if (!hsc_en && !vsc_en) {
+		c3_isp_rsz_pps_disable(rsz);
+		return 0;
+	}
+
+	/* Pre-scale needs to be enable if the down scaling factor exceeds 4 */
+	preh_en = (ihsize > cmps->width * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;
+	prev_en = (ivsize > cmps->height * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;
+
+	if (rsz->id == C3_ISP_RSZ_2) {
+		max_hsize = C3_ISP_MAX_WIDTH;
+
+		/* Set vertical tap number */
+		prevsc_flt_num = 4;
+
+		/* Set the max hsize of pre-vertical scale */
+		pre_vscale_max_hsize = max_hsize / 2;
+	} else {
+		max_hsize = C3_ISP_DEFAULT_WIDTH;
+
+		/* Set vertical tap number and the max hsize of pre-vertical */
+		if (ihsize > (max_hsize / 2) &&
+		    ihsize <= max_hsize && prev_en) {
+			prevsc_flt_num = 2;
+			pre_vscale_max_hsize = max_hsize;
+		} else {
+			prevsc_flt_num = 4;
+			pre_vscale_max_hsize = max_hsize / 2;
+		}
+	}
+
+	/*
+	 * Set pre-horizonal scale rate and the hsize of after
+	 * pre-horizonal scale.
+	 */
+	if (preh_en) {
+		prehsc_rate = 1;
+		ihsize_after_pre_hsc = DIV_ROUND_UP(ihsize, 2);
+	} else {
+		prehsc_rate = 0;
+		ihsize_after_pre_hsc = ihsize;
+	}
+
+	/* Change pre-horizonal scale rate */
+	if (prev_en && ihsize_after_pre_hsc >= pre_vscale_max_hsize)
+		prehsc_rate += 1;
+
+	/* Set the actual hsize of after pre-horizonal scale */
+	if (preh_en)
+		ihsize_after_pre_hsc_alt =
+			DIV_ROUND_UP(ihsize, 1 << prehsc_rate);
+	else
+		ihsize_after_pre_hsc_alt = ihsize;
+
+	/* Set vertical scaler bank length */
+	if (ihsize_after_pre_hsc_alt <= (max_hsize / 2))
+		vsc_tap_num_alt = 4;
+	else if (ihsize_after_pre_hsc_alt <= max_hsize)
+		vsc_tap_num_alt = prev_en ? 2 : 4;
+	else
+		vsc_tap_num_alt = prev_en ? 4 : 2;
+
+	io_size.thsize = ihsize_after_pre_hsc_alt;
+	io_size.tvsize = prev_en ? DIV_ROUND_UP(ivsize, 2) : ivsize;
+	io_size.ohsize = cmps->width;
+	io_size.ovsize = cmps->height;
+	io_size.ihsize = ihsize;
+	io_size.max_hsize = max_hsize;
+
+	c3_isp_rsz_pps_size(rsz, &io_size);
+	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_0);
+	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_2);
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM_MASK,
+			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM(vsc_tap_num_alt));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM_MASK,
+			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM(prevsc_flt_num));
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREVSC_RATE_MASK,
+			   DISP0_PPS_SCALE_EN_PREVSC_RATE(prev_en));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREHSC_RATE_MASK,
+			   DISP0_PPS_SCALE_EN_PREHSC_RATE(prehsc_rate));
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_HSC_EN(hsc_en));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_VSC_EN(vsc_en));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_PREVSC_EN(prev_en));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
+			   DISP0_PPS_SCALE_EN_PREHSC_EN(preh_en));
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS_MASK,
+			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS(9));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
+			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS_MASK,
+			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS(9));
+
+	return 0;
+}
+
+static void c3_isp_rsz_start(struct c3_isp_resizer *rsz,
+			     struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+	const struct c3_isp_rsz_format_info *rsz_fmt;
+	struct v4l2_rect *sink_crop;
+	u32 mask;
+	u32 val;
+
+	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
+	sink_crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
+	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
+	rsz_fmt = rsz_find_format_by_code(sink_fmt->code, C3_ISP_RSZ_PAD_SINK);
+
+	if (rsz->id == C3_ISP_RSZ_0) {
+		mask = ISP_TOP_DISPIN_SEL_DISP0_MASK;
+		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP0_MIPI_OUT
+				      : ISP_TOP_DISPIN_SEL_DISP0_CORE_OUT;
+	} else if (rsz->id == C3_ISP_RSZ_1) {
+		mask = ISP_TOP_DISPIN_SEL_DISP1_MASK;
+		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP1_MIPI_OUT
+				      : ISP_TOP_DISPIN_SEL_DISP1_CORE_OUT;
+	} else {
+		mask = ISP_TOP_DISPIN_SEL_DISP2_MASK;
+		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP2_MIPI_OUT
+				      : ISP_TOP_DISPIN_SEL_DISP2_CORE_OUT;
+	}
+
+	c3_isp_update_bits(rsz->isp, ISP_TOP_DISPIN_SEL, mask, val);
+
+	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(ISP_DISP0_TOP_IN_SIZE, rsz->id),
+		     ISP_DISP0_TOP_IN_SIZE_HSIZE(sink_fmt->width) |
+		     ISP_DISP0_TOP_IN_SIZE_VSIZE(sink_fmt->height));
+
+	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_START, rsz->id),
+		     DISP0_TOP_CRP2_START_V_START(sink_crop->top) |
+		     DISP0_TOP_CRP2_START_H_START(sink_crop->left));
+
+	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_SIZE, rsz->id),
+		     DISP0_TOP_CRP2_SIZE_V_SIZE(sink_crop->height) |
+		     DISP0_TOP_CRP2_SIZE_H_SIZE(sink_crop->width));
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
+			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
+			   DISP0_TOP_TOP_CTRL_CROP2_EN);
+
+	if (!rsz_fmt->is_raw)
+		c3_isp_rsz_pps_enable(rsz, state);
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
+			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT_MASK,
+			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT(src_fmt->height));
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
+			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH_MASK,
+			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH(src_fmt->width));
+
+	if (rsz->id == C3_ISP_RSZ_0) {
+		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP0_EN;
+	} else if (rsz->id == C3_ISP_RSZ_1) {
+		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP1_EN;
+	} else {
+		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP2_EN;
+	}
+
+	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);
+}
+
+static void c3_isp_rsz_stop(struct c3_isp_resizer *rsz)
+{
+	u32 mask;
+	u32 val;
+
+	if (rsz->id == C3_ISP_RSZ_0) {
+		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP0_DIS;
+	} else if (rsz->id == C3_ISP_RSZ_1) {
+		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP1_DIS;
+	} else {
+		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
+		val = ISP_TOP_PATH_EN_DISP2_DIS;
+	}
+
+	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);
+
+	c3_isp_update_bits(rsz->isp,
+			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
+			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
+			   DISP0_TOP_TOP_CTRL_CROP2_DIS);
+
+	c3_isp_rsz_pps_disable(rsz);
+}
+
+static int c3_isp_rsz_enable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     u32 pad, u64 streams_mask)
+{
+	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);
+
+	c3_isp_rsz_start(rsz, state);
+
+	c3_isp_params_pre_cfg(rsz->isp);
+	c3_isp_stats_pre_cfg(rsz->isp);
+
+	return v4l2_subdev_enable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
+}
+
+static int c3_isp_rsz_disable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);
+
+	c3_isp_rsz_stop(rsz);
+
+	return v4l2_subdev_disable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
+}
+
+static int c3_isp_rsz_enum_mbus_code(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct c3_isp_rsz_format_info *info;
+
+	info = rsz_find_format_by_index(code->index, code->pad);
+	if (!info)
+		return -EINVAL;
+
+	code->code = info->mbus_code;
+
+	return 0;
+}
+
+static void c3_isp_rsz_set_sink_fmt(struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+	struct v4l2_rect *sink_crop;
+	struct v4l2_rect *sink_cmps;
+	const struct c3_isp_rsz_format_info *rsz_fmt;
+
+	sink_fmt = v4l2_subdev_state_get_format(state, format->pad);
+	sink_crop = v4l2_subdev_state_get_crop(state, format->pad);
+	sink_cmps = v4l2_subdev_state_get_compose(state, format->pad);
+	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
+
+	rsz_fmt = rsz_find_format_by_code(format->format.code, format->pad);
+	if (rsz_fmt)
+		sink_fmt->code = format->format.code;
+	else
+		sink_fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
+
+	sink_fmt->width = clamp_t(u32, format->format.width,
+				  C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
+	sink_fmt->height = clamp_t(u32, format->format.height,
+				   C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
+	sink_fmt->field = V4L2_FIELD_NONE;
+	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+
+	if (rsz_fmt && rsz_fmt->is_raw) {
+		sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
+		sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+		sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	} else {
+		sink_fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		sink_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+		sink_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	}
+
+	sink_crop->width = sink_fmt->width;
+	sink_crop->height = sink_fmt->height;
+	sink_crop->left = 0;
+	sink_crop->top = 0;
+
+	sink_cmps->width = sink_crop->width;
+	sink_cmps->height = sink_crop->height;
+	sink_cmps->left = 0;
+	sink_cmps->top = 0;
+
+	src_fmt->code = sink_fmt->code;
+	src_fmt->width = sink_cmps->width;
+	src_fmt->height = sink_cmps->height;
+
+	format->format = *sink_fmt;
+}
+
+static void c3_isp_rsz_set_source_fmt(struct v4l2_subdev_state *state,
+				      struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *src_fmt;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_rect *sink_cmps;
+	const struct c3_isp_rsz_format_info *rsz_fmt;
+
+	src_fmt = v4l2_subdev_state_get_format(state, format->pad);
+	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
+	sink_cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
+
+	src_fmt->code = sink_fmt->code;
+	src_fmt->width = sink_cmps->width;
+	src_fmt->height = sink_cmps->height;
+	src_fmt->field = V4L2_FIELD_NONE;
+	src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+
+	rsz_fmt = rsz_find_format_by_code(src_fmt->code, format->pad);
+	if (rsz_fmt->is_raw) {
+		src_fmt->colorspace = V4L2_COLORSPACE_RAW;
+		src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+		src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+	} else {
+		src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
+		src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+		src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	}
+
+	format->format = *src_fmt;
+}
+
+static int c3_isp_rsz_set_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state,
+			      struct v4l2_subdev_format *format)
+{
+	if (format->pad == C3_ISP_RSZ_PAD_SINK)
+		c3_isp_rsz_set_sink_fmt(state, format);
+	else
+		c3_isp_rsz_set_source_fmt(state, format);
+
+	return 0;
+}
+
+static int c3_isp_rsz_get_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_rect *crop;
+	struct v4l2_rect *cmps;
+
+	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		fmt = v4l2_subdev_state_get_format(state, sel->pad);
+		sel->r.width = fmt->width;
+		sel->r.height = fmt->height;
+		sel->r.left = 0;
+		sel->r.top = 0;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		crop = v4l2_subdev_state_get_crop(state, sel->pad);
+		sel->r.width = crop->width;
+		sel->r.height = crop->height;
+		sel->r.left = 0;
+		sel->r.top = 0;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		crop = v4l2_subdev_state_get_crop(state, sel->pad);
+		sel->r = *crop;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
+		sel->r = *cmps;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int c3_isp_rsz_set_selection(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_selection *sel)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+	struct v4l2_rect *crop;
+	struct v4l2_rect *cmps;
+
+	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
+		return -EINVAL;
+
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		fmt = v4l2_subdev_state_get_format(state, sel->pad);
+		crop = v4l2_subdev_state_get_crop(state, sel->pad);
+		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
+		src_fmt = v4l2_subdev_state_get_format(state,
+						       C3_ISP_RSZ_PAD_SOURCE);
+
+		sel->r.left = clamp_t(s32, sel->r.left, 0, fmt->width - 1);
+		sel->r.top = clamp_t(s32, sel->r.top, 0, fmt->height - 1);
+		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
+				     fmt->width - sel->r.left);
+		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
+				      fmt->height - sel->r.top);
+
+		crop->width = ALIGN(sel->r.width, 2);
+		crop->height = ALIGN(sel->r.height, 2);
+		crop->left = sel->r.left;
+		crop->top = sel->r.top;
+
+		*cmps = *crop;
+
+		src_fmt->code = fmt->code;
+		src_fmt->width = cmps->width;
+		src_fmt->height = cmps->height;
+
+		sel->r = *crop;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		crop = v4l2_subdev_state_get_crop(state, sel->pad);
+		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
+
+		sel->r.left = 0;
+		sel->r.top = 0;
+		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
+				     crop->width);
+		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
+				      crop->height);
+
+		cmps->width = ALIGN(sel->r.width, 2);
+		cmps->height = ALIGN(sel->r.height, 2);
+		cmps->left = sel->r.left;
+		cmps->top = sel->r.top;
+
+		sel->r = *cmps;
+
+		fmt = v4l2_subdev_state_get_format(state,
+						   C3_ISP_RSZ_PAD_SOURCE);
+		fmt->width = cmps->width;
+		fmt->height = cmps->height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int c3_isp_rsz_init_state(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	struct v4l2_rect *crop;
+	struct v4l2_rect *cmps;
+
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
+	fmt->width = C3_ISP_DEFAULT_WIDTH;
+	fmt->height = C3_ISP_DEFAULT_HEIGHT;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+
+	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
+	crop->width = C3_ISP_DEFAULT_WIDTH;
+	crop->height = C3_ISP_DEFAULT_HEIGHT;
+	crop->left = 0;
+	crop->top = 0;
+
+	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
+	cmps->width = C3_ISP_DEFAULT_WIDTH;
+	cmps->height = C3_ISP_DEFAULT_HEIGHT;
+	cmps->left = 0;
+	cmps->top = 0;
+
+	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
+	fmt->width = cmps->width;
+	fmt->height = cmps->height;
+	fmt->field = V4L2_FIELD_NONE;
+	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
+	fmt->colorspace = V4L2_COLORSPACE_SRGB;
+	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops c3_isp_rsz_pad_ops = {
+	.enum_mbus_code = c3_isp_rsz_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = c3_isp_rsz_set_fmt,
+	.get_selection = c3_isp_rsz_get_selection,
+	.set_selection = c3_isp_rsz_set_selection,
+	.enable_streams = c3_isp_rsz_enable_streams,
+	.disable_streams = c3_isp_rsz_disable_streams,
+};
+
+static const struct v4l2_subdev_ops c3_isp_rsz_subdev_ops = {
+	.pad = &c3_isp_rsz_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops c3_isp_rsz_internal_ops = {
+	.init_state = c3_isp_rsz_init_state,
+};
+
+/* Media entity operations */
+static const struct media_entity_operations c3_isp_rsz_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+static int c3_isp_rsz_register(struct c3_isp_resizer *rsz)
+{
+	struct v4l2_subdev *sd = &rsz->sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &c3_isp_rsz_subdev_ops);
+	sd->owner = THIS_MODULE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->internal_ops = &c3_isp_rsz_internal_ops;
+	snprintf(sd->name, sizeof(sd->name), "c3-isp-resizer%u", rsz->id);
+
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+	sd->entity.ops = &c3_isp_rsz_entity_ops;
+
+	sd->dev = rsz->isp->dev;
+	v4l2_set_subdevdata(sd, rsz);
+
+	rsz->pads[C3_ISP_RSZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	rsz->pads[C3_ISP_RSZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, C3_ISP_RSZ_PAD_MAX,
+				     rsz->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(&rsz->isp->v4l2_dev, sd);
+	if (ret)
+		goto err_subdev_cleanup;
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(sd);
+err_entity_cleanup:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+static void c3_isp_rsz_unregister(struct c3_isp_resizer *rsz)
+{
+	struct v4l2_subdev *sd = &rsz->sd;
+
+	v4l2_device_unregister_subdev(sd);
+	v4l2_subdev_cleanup(sd);
+	media_entity_cleanup(&sd->entity);
+}
+
+int c3_isp_resizers_register(struct c3_isp_device *isp)
+{
+	int ret;
+
+	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
+		struct c3_isp_resizer *rsz = &isp->resizers[i];
+
+		rsz->id = i;
+		rsz->isp = isp;
+		rsz->src_sd = &isp->core.sd;
+		rsz->src_pad = C3_ISP_CORE_PAD_SOURCE_VIDEO_0 + i;
+
+		ret = c3_isp_rsz_register(rsz);
+		if (ret) {
+			rsz->isp = NULL;
+			c3_isp_resizers_unregister(isp);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+void c3_isp_resizers_unregister(struct c3_isp_device *isp)
+{
+	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
+		struct c3_isp_resizer *rsz = &isp->resizers[i];
+
+		if (rsz->isp)
+			c3_isp_rsz_unregister(rsz);
+	}
+}
diff --git a/drivers/media/platform/amlogic/c3/isp/c3-isp-stats.c b/drivers/media/platform/amlogic/c3/isp/c3-isp-stats.c
new file mode 100644
index 000000000000..8a5d7e1a30c9
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/isp/c3-isp-stats.c
@@ -0,0 +1,326 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/cleanup.h>
+#include <linux/media/amlogic/c3-isp-config.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "c3-isp-common.h"
+#include "c3-isp-regs.h"
+
+/* Hardware configuration */
+
+static void c3_isp_stats_cfg_dmawr_addr(struct c3_isp_stats *stats)
+{
+	u32 awb_dma_size = sizeof(struct c3_isp_awb_stats);
+	u32 ae_dma_size = sizeof(struct c3_isp_ae_stats);
+	u32 awb_dma_addr = stats->buff->dma_addr;
+	u32 af_dma_addr;
+	u32 ae_dma_addr;
+
+	ae_dma_addr = awb_dma_addr + awb_dma_size;
+	af_dma_addr = ae_dma_addr + ae_dma_size;
+
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_BADDR0,
+			   VIU_DMAWR_BADDR0_AF_STATS_BASE_ADDR_MASK,
+			   VIU_DMAWR_BADDR0_AF_STATS_BASE_ADDR(af_dma_addr));
+
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_BADDR1,
+			   VIU_DMAWR_BADDR1_AWB_STATS_BASE_ADDR_MASK,
+			   VIU_DMAWR_BADDR1_AWB_STATS_BASE_ADDR(awb_dma_addr));
+
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_BADDR2,
+			   VIU_DMAWR_BADDR2_AE_STATS_BASE_ADDR_MASK,
+			   VIU_DMAWR_BADDR2_AE_STATS_BASE_ADDR(ae_dma_addr));
+}
+
+static void c3_isp_stats_cfg_buff(struct c3_isp_stats *stats)
+{
+	stats->buff =
+		list_first_entry_or_null(&stats->pending,
+					 struct c3_isp_stats_buffer, list);
+	if (stats->buff) {
+		c3_isp_stats_cfg_dmawr_addr(stats);
+		list_del(&stats->buff->list);
+	}
+}
+
+void c3_isp_stats_pre_cfg(struct c3_isp_device *isp)
+{
+	struct c3_isp_stats *stats = &isp->stats;
+	u32 dma_size;
+
+	c3_isp_update_bits(stats->isp, ISP_AF_EN_CTRL,
+			   ISP_AF_EN_CTRL_STAT_SEL_MASK,
+			   ISP_AF_EN_CTRL_STAT_SEL_NEW);
+	c3_isp_update_bits(stats->isp, ISP_AE_CTRL,
+			   ISP_AE_CTRL_LUMA_MODE_MASK,
+			   ISP_AE_CTRL_LUMA_MODE_FILTER);
+
+	/* The unit of dma_size is 16 bytes */
+	dma_size = sizeof(struct c3_isp_af_stats) / C3_ISP_DMA_SIZE_ALIGN_BYTES;
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_SIZE0,
+			   VIU_DMAWR_SIZE0_AF_STATS_SIZE_MASK,
+			   VIU_DMAWR_SIZE0_AF_STATS_SIZE(dma_size));
+
+	dma_size = sizeof(struct c3_isp_awb_stats) /
+		   C3_ISP_DMA_SIZE_ALIGN_BYTES;
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_SIZE0,
+			   VIU_DMAWR_SIZE0_AWB_STATS_SIZE_MASK,
+			   VIU_DMAWR_SIZE0_AWB_STATS_SIZE(dma_size));
+
+	dma_size = sizeof(struct c3_isp_ae_stats) / C3_ISP_DMA_SIZE_ALIGN_BYTES;
+	c3_isp_update_bits(stats->isp, VIU_DMAWR_SIZE1,
+			   VIU_DMAWR_SIZE1_AE_STATS_SIZE_MASK,
+			   VIU_DMAWR_SIZE1_AE_STATS_SIZE(dma_size));
+
+	guard(spinlock_irqsave)(&stats->buff_lock);
+
+	c3_isp_stats_cfg_buff(stats);
+}
+
+static int c3_isp_stats_querycap(struct file *file, void *fh,
+				 struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, C3_ISP_DRIVER_NAME, sizeof(cap->driver));
+	strscpy(cap->card, "AML C3 ISP", sizeof(cap->card));
+
+	return 0;
+}
+
+static int c3_isp_stats_enum_fmt(struct file *file, void *fh,
+				 struct v4l2_fmtdesc *f)
+{
+	struct c3_isp_stats *stats = video_drvdata(file);
+
+	if (f->index > 0 || f->type != stats->vb2_q.type)
+		return -EINVAL;
+
+	f->pixelformat = V4L2_META_FMT_C3ISP_STATS;
+
+	return 0;
+}
+
+static int c3_isp_stats_g_fmt(struct file *file, void *fh,
+			      struct v4l2_format *f)
+{
+	struct c3_isp_stats *stats = video_drvdata(file);
+
+	f->fmt.meta = stats->vfmt.fmt.meta;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops isp_stats_v4l2_ioctl_ops = {
+	.vidioc_querycap                = c3_isp_stats_querycap,
+	.vidioc_enum_fmt_meta_cap       = c3_isp_stats_enum_fmt,
+	.vidioc_g_fmt_meta_cap          = c3_isp_stats_g_fmt,
+	.vidioc_s_fmt_meta_cap          = c3_isp_stats_g_fmt,
+	.vidioc_try_fmt_meta_cap        = c3_isp_stats_g_fmt,
+	.vidioc_reqbufs	                = vb2_ioctl_reqbufs,
+	.vidioc_querybuf                = vb2_ioctl_querybuf,
+	.vidioc_qbuf                    = vb2_ioctl_qbuf,
+	.vidioc_expbuf                  = vb2_ioctl_expbuf,
+	.vidioc_dqbuf                   = vb2_ioctl_dqbuf,
+	.vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
+	.vidioc_create_bufs             = vb2_ioctl_create_bufs,
+	.vidioc_streamon                = vb2_ioctl_streamon,
+	.vidioc_streamoff               = vb2_ioctl_streamoff,
+};
+
+static const struct v4l2_file_operations isp_stats_v4l2_fops = {
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+};
+
+static int c3_isp_stats_vb2_queue_setup(struct vb2_queue *q,
+					unsigned int *num_buffers,
+					unsigned int *num_planes,
+					unsigned int sizes[],
+					struct device *alloc_devs[])
+{
+	if (*num_planes) {
+		if (*num_planes != 1)
+			return -EINVAL;
+
+		if (sizes[0] < sizeof(struct c3_isp_stats_info))
+			return -EINVAL;
+
+		return 0;
+	}
+
+	*num_planes = 1;
+	sizes[0] = sizeof(struct c3_isp_stats_info);
+
+	return 0;
+}
+
+static void c3_isp_stats_vb2_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_stats_buffer *buf =
+			container_of(v4l2_buf, struct c3_isp_stats_buffer, vb);
+	struct c3_isp_stats *stats = vb2_get_drv_priv(vb->vb2_queue);
+
+	guard(spinlock_irqsave)(&stats->buff_lock);
+
+	list_add_tail(&buf->list, &stats->pending);
+}
+
+static int c3_isp_stats_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct c3_isp_stats *stats = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned int size = stats->vfmt.fmt.meta.buffersize;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_err(stats->isp->dev,
+			"User buffer too small (%ld < %u)\n",
+			vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static int c3_isp_stats_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+	struct c3_isp_stats_buffer *buf =
+			container_of(v4l2_buf, struct c3_isp_stats_buffer, vb);
+
+	buf->dma_addr = vb2_dma_contig_plane_dma_addr(vb, 0);
+
+	return 0;
+}
+
+static void c3_isp_stats_vb2_stop_streaming(struct vb2_queue *q)
+{
+	struct c3_isp_stats *stats = vb2_get_drv_priv(q);
+
+	guard(spinlock_irqsave)(&stats->buff_lock);
+
+	if (stats->buff) {
+		vb2_buffer_done(&stats->buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		stats->buff = NULL;
+	}
+
+	while (!list_empty(&stats->pending)) {
+		struct c3_isp_stats_buffer *buff;
+
+		buff = list_first_entry(&stats->pending,
+					struct c3_isp_stats_buffer, list);
+		list_del(&buff->list);
+		vb2_buffer_done(&buff->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static const struct vb2_ops isp_stats_vb2_ops = {
+	.queue_setup = c3_isp_stats_vb2_queue_setup,
+	.buf_queue = c3_isp_stats_vb2_buf_queue,
+	.buf_prepare = c3_isp_stats_vb2_buf_prepare,
+	.buf_init = c3_isp_stats_vb2_buf_init,
+	.stop_streaming = c3_isp_stats_vb2_stop_streaming,
+};
+
+int c3_isp_stats_register(struct c3_isp_device *isp)
+{
+	struct c3_isp_stats *stats = &isp->stats;
+	struct video_device *vdev = &stats->vdev;
+	struct vb2_queue *vb2_q = &stats->vb2_q;
+	int ret;
+
+	memset(stats, 0, sizeof(*stats));
+	stats->vfmt.fmt.meta.dataformat = V4L2_META_FMT_C3ISP_STATS;
+	stats->vfmt.fmt.meta.buffersize = sizeof(struct c3_isp_stats_info);
+	stats->isp = isp;
+	INIT_LIST_HEAD(&stats->pending);
+	spin_lock_init(&stats->buff_lock);
+
+	mutex_init(&stats->lock);
+
+	snprintf(vdev->name, sizeof(vdev->name), "c3-isp-stats");
+	vdev->fops = &isp_stats_v4l2_fops;
+	vdev->ioctl_ops = &isp_stats_v4l2_ioctl_ops;
+	vdev->v4l2_dev = &isp->v4l2_dev;
+	vdev->lock = &stats->lock;
+	vdev->minor = -1;
+	vdev->queue = vb2_q;
+	vdev->release = video_device_release_empty;
+	vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING;
+	vdev->vfl_dir = VFL_DIR_RX;
+	video_set_drvdata(vdev, stats);
+
+	vb2_q->drv_priv = stats;
+	vb2_q->mem_ops = &vb2_dma_contig_memops;
+	vb2_q->ops = &isp_stats_vb2_ops;
+	vb2_q->type = V4L2_BUF_TYPE_META_CAPTURE;
+	vb2_q->io_modes = VB2_DMABUF | VB2_MMAP;
+	vb2_q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	vb2_q->buf_struct_size = sizeof(struct c3_isp_stats_buffer);
+	vb2_q->dev = isp->dev;
+	vb2_q->lock = &stats->lock;
+	vb2_q->min_queued_buffers = 2;
+
+	ret = vb2_queue_init(vb2_q);
+	if (ret)
+		goto err_destroy;
+
+	stats->pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&vdev->entity, 1, &stats->pad);
+	if (ret)
+		goto err_queue_release;
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(isp->dev,
+			"Failed to register %s: %d\n", vdev->name, ret);
+		goto err_entity_cleanup;
+	}
+
+	return 0;
+
+err_entity_cleanup:
+	media_entity_cleanup(&vdev->entity);
+err_queue_release:
+	vb2_queue_release(vb2_q);
+err_destroy:
+	mutex_destroy(&stats->lock);
+	return ret;
+}
+
+void c3_isp_stats_unregister(struct c3_isp_device *isp)
+{
+	struct c3_isp_stats *stats = &isp->stats;
+
+	vb2_queue_release(&stats->vb2_q);
+	media_entity_cleanup(&stats->vdev.entity);
+	video_unregister_device(&stats->vdev);
+	mutex_destroy(&stats->lock);
+}
+
+void c3_isp_stats_isr(struct c3_isp_device *isp)
+{
+	struct c3_isp_stats *stats = &isp->stats;
+
+	guard(spinlock_irqsave)(&stats->buff_lock);
+
+	if (stats->buff) {
+		stats->buff->vb.sequence = stats->isp->frm_sequence;
+		stats->buff->vb.vb2_buf.timestamp = ktime_get();
+		stats->buff->vb.field = V4L2_FIELD_NONE;
+		vb2_buffer_done(&stats->buff->vb.vb2_buf, VB2_BUF_STATE_DONE);
+	}
+
+	c3_isp_stats_cfg_buff(stats);
+}
diff --git a/drivers/media/platform/amlogic/c3/mipi-adapter/Kconfig b/drivers/media/platform/amlogic/c3/mipi-adapter/Kconfig
new file mode 100644
index 000000000000..bf19059b3543
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-adapter/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+config VIDEO_C3_MIPI_ADAPTER
+	tristate "Amlogic C3 MIPI adapter"
+	depends on ARCH_MESON || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on OF
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Video4Linux2 driver for Amlogic C3 MIPI adapter.
+	  C3 MIPI adapter mainly responsible for organizing
+	  MIPI data and sending raw data to ISP pipeline.
+
+	  To compile this driver as a module choose m here.
diff --git a/drivers/media/platform/amlogic/c3/mipi-adapter/Makefile b/drivers/media/platform/amlogic/c3/mipi-adapter/Makefile
new file mode 100644
index 000000000000..216fc310c5b4
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-adapter/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+obj-$(CONFIG_VIDEO_C3_MIPI_ADAPTER) += c3-mipi-adap.o
diff --git a/drivers/media/platform/amlogic/c3/mipi-adapter/c3-mipi-adap.c b/drivers/media/platform/amlogic/c3/mipi-adapter/c3-mipi-adap.c
new file mode 100644
index 000000000000..4bd98fb9c7e9
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-adapter/c3-mipi-adap.c
@@ -0,0 +1,842 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+
+/*
+ * Adapter Block Diagram
+ * ---------------------
+ *
+ *                   +--------------------------------------------+
+ *                   |                   Adapter                  |
+ *                   |--------------------------------------------|
+ * +------------+    |          |          |         |            |    +-----+
+ * | MIPI CSI-2 |--->| Frontend -> DDR_RD0 -> PIXEL0 -> ALIGNMENT |--->| ISP |
+ * +------------+    |          |          |         |            |    +-----+
+ *                   +--------------------------------------------+
+ *
+ */
+
+/* C3 adapter submodule definition */
+enum {
+	SUBMD_TOP,
+	SUBMD_FD,
+	SUBMD_RD,
+};
+
+#define ADAP_SUBMD_MASK		GENMASK(17, 16)
+#define ADAP_SUBMD_SHIFT	16
+#define ADAP_SUBMD(x)	(((x) & (ADAP_SUBMD_MASK)) >> (ADAP_SUBMD_SHIFT))
+#define ADAP_REG_ADDR_MASK	GENMASK(15, 0)
+#define ADAP_REG_ADDR(x)	((x) & (ADAP_REG_ADDR_MASK))
+#define ADAP_REG_T(x)		((SUBMD_TOP << ADAP_SUBMD_SHIFT) | (x))
+#define ADAP_REG_F(x)		((SUBMD_FD << ADAP_SUBMD_SHIFT) | (x))
+#define ADAP_REG_R(x)		((SUBMD_RD << ADAP_SUBMD_SHIFT) | (x))
+
+#define MIPI_ADAP_CLOCK_NUM_MAX				3
+#define MIPI_ADAP_SUBDEV_NAME				"c3-mipi-adapter"
+
+/* C3 MIPI adapter TOP register */
+#define MIPI_TOP_CTRL0					ADAP_REG_T(0x00)
+#define MIPI_TOP_CTRL0_RST_ADAPTER_MASK			BIT(1)
+#define MIPI_TOP_CTRL0_RST_ADAPTER_APPLY		BIT(1)
+#define MIPI_TOP_CTRL0_RST_ADAPTER_EXIT			(0 << 1)
+
+#define MIPI_ADAPT_DE_CTRL0				ADAP_REG_T(0x40)
+#define MIPI_ADAPT_DE_CTRL0_RD_BUS_BYPASS_MASK		BIT(3)
+#define MIPI_ADAPT_DE_CTRL0_RD_BUS_BYPASS_EN		BIT(3)
+#define MIPI_ADAPT_DE_CTRL0_RD_BUS_BYPASS_DIS		(0 << 3)
+#define MIPI_ADAPT_DE_CTRL0_WR_BUS_BYPASS_MASK		BIT(7)
+#define MIPI_ADAPT_DE_CTRL0_WR_BUS_BYPASS_EN		BIT(7)
+#define MIPI_ADAPT_DE_CTRL0_WR_BUS_BYPASS_DIS		(0 << 7)
+
+/* C3 MIPI adapter FRONTEND register */
+#define CSI2_CLK_RESET					ADAP_REG_F(0x00)
+#define CSI2_CLK_RESET_SW_RESET_MASK			BIT(0)
+#define CSI2_CLK_RESET_SW_RESET_APPLY			BIT(0)
+#define CSI2_CLK_RESET_SW_RESET_RELEASE			(0 << 0)
+#define CSI2_CLK_RESET_CLK_ENABLE_MASK			BIT(1)
+#define CSI2_CLK_RESET_CLK_ENABLE_EN			BIT(1)
+#define CSI2_CLK_RESET_CLK_ENABLE_DIS			(0 << 1)
+
+#define CSI2_GEN_CTRL0					ADAP_REG_F(0x04)
+#define CSI2_GEN_CTRL0_VC0_MASK				BIT(0)
+#define CSI2_GEN_CTRL0_VC0_EN				BIT(0)
+#define CSI2_GEN_CTRL0_VC0_DIS				(0 << 0)
+#define CSI2_GEN_CTRL0_ENABLE_PACKETS_MASK		GENMASK(20, 16)
+#define CSI2_GEN_CTRL0_ENABLE_PACKETS_RAW		BIT(16)
+#define CSI2_GEN_CTRL0_ENABLE_PACKETS_YUV		(2 << 16)
+
+#define CSI2_X_START_END_ISP				ADAP_REG_F(0x0c)
+#define CSI2_X_START_END_ISP_X_START_MASK		GENMASK(15, 0)
+#define CSI2_X_START_END_ISP_X_START(x)			((x) << 0)
+#define CSI2_X_START_END_ISP_X_END_MASK			GENMASK(31, 16)
+#define CSI2_X_START_END_ISP_X_END(x)			(((x) - 1) << 16)
+
+#define CSI2_Y_START_END_ISP				ADAP_REG_F(0x10)
+#define CSI2_Y_START_END_ISP_Y_START_MASK		GENMASK(15, 0)
+#define CSI2_Y_START_END_ISP_Y_START(x)			((x) << 0)
+#define CSI2_Y_START_END_ISP_Y_END_MASK			GENMASK(31, 16)
+#define CSI2_Y_START_END_ISP_Y_END(x)			(((x) - 1) << 16)
+
+#define CSI2_VC_MODE					ADAP_REG_F(0x1c)
+#define CSI2_VC_MODE_VS_ISP_SEL_VC_MASK			GENMASK(19, 16)
+#define CSI2_VC_MODE_VS_ISP_SEL_VC_0			BIT(16)
+#define CSI2_VC_MODE_VS_ISP_SEL_VC_1			(2 << 16)
+#define CSI2_VC_MODE_VS_ISP_SEL_VC_2			(4 << 16)
+#define CSI2_VC_MODE_VS_ISP_SEL_VC_3			(8 << 16)
+#define CSI2_VC_MODE_HS_ISP_SEL_VC_MASK			GENMASK(23, 20)
+#define CSI2_VC_MODE_HS_ISP_SEL_VC_0			BIT(20)
+#define CSI2_VC_MODE_HS_ISP_SEL_VC_1			(2 << 20)
+#define CSI2_VC_MODE_HS_ISP_SEL_VC_2			(4 << 20)
+#define CSI2_VC_MODE_HS_ISP_SEL_VC_3			(8 << 20)
+
+/* C3 MIPI adapter READER register */
+#define MIPI_ADAPT_DDR_RD0_CNTL0			ADAP_REG_R(0x00)
+#define MIPI_ADAPT_DDR_RD0_CNTL0_MODULE_EN_MASK		BIT(0)
+#define MIPI_ADAPT_DDR_RD0_CNTL0_MODULE_EN		BIT(0)
+#define MIPI_ADAPT_DDR_RD0_CNTL0_MODULE_DIS		(0 << 0)
+
+#define MIPI_ADAPT_DDR_RD0_CNTL1			ADAP_REG_R(0x04)
+#define MIPI_ADAPT_DDR_RD0_CNTL1_PORT_SEL_MASK		GENMASK(31, 30)
+#define MIPI_ADAPT_DDR_RD0_CNTL1_PORT_SEL_DIRECT_MODE	(0 << 30)
+#define MIPI_ADAPT_DDR_RD0_CNTL1_PORT_SEL_DDR_MODE	BIT(30)
+
+#define MIPI_ADAPT_PIXEL0_CNTL0				ADAP_REG_R(0x80)
+#define MIPI_ADAPT_PIXEL0_CNTL0_WORK_MODE_MASK		GENMASK(17, 16)
+#define MIPI_ADAPT_PIXEL0_CNTL0_WORK_MODE_RAW_DDR	(0 << 16)
+#define MIPI_ADAPT_PIXEL0_CNTL0_WORK_MODE_RAW_DIRECT	BIT(16)
+#define MIPI_ADAPT_PIXEL0_CNTL0_DATA_TYPE_MASK		GENMASK(25, 20)
+#define MIPI_ADAPT_PIXEL0_CNTL0_DATA_TYPE(x)		((x) << 20)
+#define MIPI_ADAPT_PIXEL0_CNTL0_START_EN_MASK		BIT(31)
+#define MIPI_ADAPT_PIXEL0_CNTL0_START_EN		BIT(31)
+
+#define MIPI_ADAPT_ALIG_CNTL0				ADAP_REG_R(0x100)
+#define MIPI_ADAPT_ALIG_CNTL0_H_NUM_MASK		GENMASK(15, 0)
+#define MIPI_ADAPT_ALIG_CNTL0_H_NUM(x)			((x) << 0)
+#define MIPI_ADAPT_ALIG_CNTL0_V_NUM_MASK		GENMASK(31, 16)
+#define MIPI_ADAPT_ALIG_CNTL0_V_NUM(x)			((x) << 16)
+
+#define MIPI_ADAPT_ALIG_CNTL1				ADAP_REG_R(0x104)
+#define MIPI_ADAPT_ALIG_CNTL1_HPE_NUM_MASK		GENMASK(31, 16)
+#define MIPI_ADAPT_ALIG_CNTL1_HPE_NUM(x)		((x) << 16)
+
+#define MIPI_ADAPT_ALIG_CNTL2				ADAP_REG_R(0x108)
+#define MIPI_ADAPT_ALIG_CNTL2_VPE_NUM_MASK		GENMASK(31, 16)
+#define MIPI_ADAPT_ALIG_CNTL2_VPE_NUM(x)		((x) << 16)
+
+#define MIPI_ADAPT_ALIG_CNTL6				ADAP_REG_R(0x118)
+#define MIPI_ADAPT_ALIG_CNTL6_PATH0_EN_MASK		BIT(0)
+#define MIPI_ADAPT_ALIG_CNTL6_PATH0_EN			BIT(0)
+#define MIPI_ADAPT_ALIG_CNTL6_PATH0_DIS			(0 << 0)
+#define MIPI_ADAPT_ALIG_CNTL6_PIX0_DATA_MODE_MASK	BIT(4)
+#define MIPI_ADAPT_ALIG_CNTL6_PIX0_DATA_MODE_DDR	(0 << 4)
+#define MIPI_ADAPT_ALIG_CNTL6_PIX0_DATA_MODE_DIRECT	BIT(4)
+#define MIPI_ADAPT_ALIG_CNTL6_DATA0_EN_MASK		BIT(12)
+#define MIPI_ADAPT_ALIG_CNTL6_DATA0_EN			BIT(12)
+#define MIPI_ADAPT_ALIG_CNTL6_DATA0_DIS			(0 << 12)
+
+#define MIPI_ADAPT_ALIG_CNTL8				ADAP_REG_R(0x120)
+#define MIPI_ADAPT_ALIG_CNTL8_FRMAE_CONTINUE_MASK	BIT(5)
+#define MIPI_ADAPT_ALIG_CNTL8_FRMAE_CONTINUE_EN		BIT(5)
+#define MIPI_ADAPT_ALIG_CNTL8_FRMAE_CONTINUE_DIS	(0 << 5)
+#define MIPI_ADAPT_ALIG_CNTL8_EXCEED_DIS_MASK		BIT(12)
+#define MIPI_ADAPT_ALIG_CNTL8_EXCEED_HOLD		(0 << 12)
+#define MIPI_ADAPT_ALIG_CNTL8_EXCEED_NOT_HOLD		BIT(12)
+#define MIPI_ADAPT_ALIG_CNTL8_START_EN_MASK		BIT(31)
+#define MIPI_ADAPT_ALIG_CNTL8_START_EN			BIT(31)
+
+#define MIPI_ADAP_MAX_WIDTH		2888
+#define MIPI_ADAP_MIN_WIDTH		160
+#define MIPI_ADAP_MAX_HEIGHT		2240
+#define MIPI_ADAP_MIN_HEIGHT		120
+#define MIPI_ADAP_DEFAULT_WIDTH		1920
+#define MIPI_ADAP_DEFAULT_HEIGHT	1080
+#define MIPI_ADAP_DEFAULT_FMT		MEDIA_BUS_FMT_SRGGB10_1X10
+
+/* C3 MIPI adapter pad list */
+enum {
+	C3_MIPI_ADAP_PAD_SINK,
+	C3_MIPI_ADAP_PAD_SRC,
+	C3_MIPI_ADAP_PAD_MAX
+};
+
+/*
+ * struct c3_adap_info - mipi adapter information
+ *
+ * @clocks: array of mipi adapter clock names
+ * @clock_num: actual clock number
+ */
+struct c3_adap_info {
+	char *clocks[MIPI_ADAP_CLOCK_NUM_MAX];
+	u32 clock_num;
+};
+
+/*
+ * struct c3_adap_device - mipi adapter platform device
+ *
+ * @dev: pointer to the struct device
+ * @top: mipi adapter top register address
+ * @fd: mipi adapter frontend register address
+ * @rd: mipi adapter reader register address
+ * @clks: array of MIPI adapter clocks
+ * @sd: mipi adapter sub-device
+ * @pads: mipi adapter sub-device pads
+ * @notifier: notifier to register on the v4l2-async API
+ * @src_sd: source sub-device pad
+ * @info: version-specific MIPI adapter information
+ */
+struct c3_adap_device {
+	struct device *dev;
+	void __iomem *top;
+	void __iomem *fd;
+	void __iomem *rd;
+	struct clk_bulk_data clks[MIPI_ADAP_CLOCK_NUM_MAX];
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[C3_MIPI_ADAP_PAD_MAX];
+	struct v4l2_async_notifier notifier;
+	struct media_pad *src_pad;
+
+	const struct c3_adap_info *info;
+};
+
+/* Format helpers */
+
+struct c3_adap_pix_format {
+	u32 code;
+	u8 type;
+};
+
+static const struct c3_adap_pix_format c3_mipi_adap_formats[] = {
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, MIPI_CSI2_DT_RAW10 },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, MIPI_CSI2_DT_RAW10 },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, MIPI_CSI2_DT_RAW10 },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, MIPI_CSI2_DT_RAW10 },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, MIPI_CSI2_DT_RAW12 },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, MIPI_CSI2_DT_RAW12 },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, MIPI_CSI2_DT_RAW12 },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, MIPI_CSI2_DT_RAW12 },
+};
+
+static const struct c3_adap_pix_format *c3_mipi_adap_find_format(u32 code)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(c3_mipi_adap_formats); i++)
+		if (code == c3_mipi_adap_formats[i].code)
+			return &c3_mipi_adap_formats[i];
+
+	return NULL;
+}
+
+/* Hardware configuration */
+
+static void c3_mipi_adap_update_bits(struct c3_adap_device *adap, u32 reg,
+				     u32 mask, u32 val)
+{
+	void __iomem *addr;
+	u32 orig, tmp;
+
+	switch (ADAP_SUBMD(reg)) {
+	case SUBMD_TOP:
+		addr = adap->top + ADAP_REG_ADDR(reg);
+		break;
+	case SUBMD_FD:
+		addr = adap->fd + ADAP_REG_ADDR(reg);
+		break;
+	case SUBMD_RD:
+		addr = adap->rd + ADAP_REG_ADDR(reg);
+		break;
+	default:
+		dev_err(adap->dev,
+			"Invalid sub-module: %lu\n", ADAP_SUBMD(reg));
+		return;
+	}
+
+	orig = readl(addr);
+	tmp = orig & ~mask;
+	tmp |= val & mask;
+
+	if (tmp != orig)
+		writel(tmp, addr);
+}
+
+/* Configure adapter top sub module */
+static void c3_mipi_adap_cfg_top(struct c3_adap_device *adap)
+{
+	/* Reset adapter */
+	c3_mipi_adap_update_bits(adap, MIPI_TOP_CTRL0,
+				 MIPI_TOP_CTRL0_RST_ADAPTER_MASK,
+				 MIPI_TOP_CTRL0_RST_ADAPTER_APPLY);
+	c3_mipi_adap_update_bits(adap, MIPI_TOP_CTRL0,
+				 MIPI_TOP_CTRL0_RST_ADAPTER_MASK,
+				 MIPI_TOP_CTRL0_RST_ADAPTER_EXIT);
+
+	/* Bypass decompress */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_DE_CTRL0,
+				 MIPI_ADAPT_DE_CTRL0_RD_BUS_BYPASS_MASK,
+				 MIPI_ADAPT_DE_CTRL0_RD_BUS_BYPASS_EN);
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_DE_CTRL0,
+				 MIPI_ADAPT_DE_CTRL0_WR_BUS_BYPASS_MASK,
+				 MIPI_ADAPT_DE_CTRL0_WR_BUS_BYPASS_EN);
+}
+
+/* Configure adapter frontend sub module */
+static void c3_mipi_adap_cfg_frontend(struct c3_adap_device *adap,
+				      struct v4l2_mbus_framefmt *fmt)
+{
+	/* Reset frontend module */
+	c3_mipi_adap_update_bits(adap, CSI2_CLK_RESET,
+				 CSI2_CLK_RESET_SW_RESET_MASK,
+				 CSI2_CLK_RESET_SW_RESET_APPLY);
+	c3_mipi_adap_update_bits(adap, CSI2_CLK_RESET,
+				 CSI2_CLK_RESET_SW_RESET_MASK,
+				 CSI2_CLK_RESET_SW_RESET_RELEASE);
+	c3_mipi_adap_update_bits(adap, CSI2_CLK_RESET,
+				 CSI2_CLK_RESET_CLK_ENABLE_MASK,
+				 CSI2_CLK_RESET_CLK_ENABLE_EN);
+
+	c3_mipi_adap_update_bits(adap, CSI2_X_START_END_ISP,
+				 CSI2_X_START_END_ISP_X_START_MASK,
+				 CSI2_X_START_END_ISP_X_START(0));
+	c3_mipi_adap_update_bits(adap, CSI2_X_START_END_ISP,
+				 CSI2_X_START_END_ISP_X_END_MASK,
+				 CSI2_X_START_END_ISP_X_END(fmt->width));
+
+	c3_mipi_adap_update_bits(adap, CSI2_Y_START_END_ISP,
+				 CSI2_Y_START_END_ISP_Y_START_MASK,
+				 CSI2_Y_START_END_ISP_Y_START(0));
+	c3_mipi_adap_update_bits(adap, CSI2_Y_START_END_ISP,
+				 CSI2_Y_START_END_ISP_Y_END_MASK,
+				 CSI2_Y_START_END_ISP_Y_END(fmt->height));
+
+	/* Select VS and HS signal for direct path */
+	c3_mipi_adap_update_bits(adap, CSI2_VC_MODE,
+				 CSI2_VC_MODE_VS_ISP_SEL_VC_MASK,
+				 CSI2_VC_MODE_VS_ISP_SEL_VC_0);
+	c3_mipi_adap_update_bits(adap, CSI2_VC_MODE,
+				 CSI2_VC_MODE_HS_ISP_SEL_VC_MASK,
+				 CSI2_VC_MODE_HS_ISP_SEL_VC_0);
+
+	/* Enable to receive RAW packet */
+	c3_mipi_adap_update_bits(adap, CSI2_GEN_CTRL0,
+				 CSI2_GEN_CTRL0_ENABLE_PACKETS_MASK,
+				 CSI2_GEN_CTRL0_ENABLE_PACKETS_RAW);
+
+	/* Enable virtual channel 0 */
+	c3_mipi_adap_update_bits(adap, CSI2_GEN_CTRL0,
+				 CSI2_GEN_CTRL0_VC0_MASK,
+				 CSI2_GEN_CTRL0_VC0_EN);
+}
+
+static void c3_mipi_adap_cfg_rd0(struct c3_adap_device *adap)
+{
+	/* Select direct mode for DDR_RD0 mode */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_DDR_RD0_CNTL1,
+				 MIPI_ADAPT_DDR_RD0_CNTL1_PORT_SEL_MASK,
+				 MIPI_ADAPT_DDR_RD0_CNTL1_PORT_SEL_DIRECT_MODE);
+
+	/* Data can't bypass DDR_RD0 in direct mode, so enable DDR_RD0 here */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_DDR_RD0_CNTL0,
+				 MIPI_ADAPT_DDR_RD0_CNTL0_MODULE_EN_MASK,
+				 MIPI_ADAPT_DDR_RD0_CNTL0_MODULE_EN);
+}
+
+static void c3_mipi_adap_cfg_pixel0(struct c3_adap_device *adap,
+				    struct v4l2_mbus_framefmt *fmt)
+{
+	const struct c3_adap_pix_format *pix;
+
+	pix = c3_mipi_adap_find_format(fmt->code);
+
+	/* Set work mode and data type for PIXEL0 module */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_PIXEL0_CNTL0,
+				 MIPI_ADAPT_PIXEL0_CNTL0_WORK_MODE_MASK,
+				 MIPI_ADAPT_PIXEL0_CNTL0_WORK_MODE_RAW_DIRECT);
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_PIXEL0_CNTL0,
+				 MIPI_ADAPT_PIXEL0_CNTL0_DATA_TYPE_MASK,
+				 MIPI_ADAPT_PIXEL0_CNTL0_DATA_TYPE(pix->type));
+
+	/* Start PIXEL0 module */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_PIXEL0_CNTL0,
+				 MIPI_ADAPT_PIXEL0_CNTL0_START_EN_MASK,
+				 MIPI_ADAPT_PIXEL0_CNTL0_START_EN);
+}
+
+static void c3_mipi_adap_cfg_alig(struct c3_adap_device *adap,
+				  struct v4l2_mbus_framefmt *fmt)
+{
+	/*
+	 * ISP hardware requires the number of horizonal blanks greater than
+	 * 64 cycles, so adding 64 here.
+	 */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL0,
+				 MIPI_ADAPT_ALIG_CNTL0_H_NUM_MASK,
+				 MIPI_ADAPT_ALIG_CNTL0_H_NUM(fmt->width + 64));
+
+	/*
+	 * ISP hardware requires the number of vertical blanks greater than
+	 * 40 lines, so adding 40 here.
+	 */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL0,
+				 MIPI_ADAPT_ALIG_CNTL0_V_NUM_MASK,
+				 MIPI_ADAPT_ALIG_CNTL0_V_NUM(fmt->height + 40));
+
+	/* End pixel in a line */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL1,
+				 MIPI_ADAPT_ALIG_CNTL1_HPE_NUM_MASK,
+				 MIPI_ADAPT_ALIG_CNTL1_HPE_NUM(fmt->width));
+
+	/* End line in a frame */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL2,
+				 MIPI_ADAPT_ALIG_CNTL2_VPE_NUM_MASK,
+				 MIPI_ADAPT_ALIG_CNTL2_VPE_NUM(fmt->height));
+
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL6,
+				 MIPI_ADAPT_ALIG_CNTL6_PATH0_EN_MASK,
+				 MIPI_ADAPT_ALIG_CNTL6_PATH0_EN);
+
+	/* Select direct mode for ALIG module */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL6,
+				 MIPI_ADAPT_ALIG_CNTL6_PIX0_DATA_MODE_MASK,
+				 MIPI_ADAPT_ALIG_CNTL6_PIX0_DATA_MODE_DIRECT);
+
+	/* Enable to send raw data */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL6,
+				 MIPI_ADAPT_ALIG_CNTL6_DATA0_EN_MASK,
+				 MIPI_ADAPT_ALIG_CNTL6_DATA0_EN);
+
+	/* Set continue mode and disable hold counter */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL8,
+				 MIPI_ADAPT_ALIG_CNTL8_FRMAE_CONTINUE_MASK,
+				 MIPI_ADAPT_ALIG_CNTL8_FRMAE_CONTINUE_EN);
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL8,
+				 MIPI_ADAPT_ALIG_CNTL8_EXCEED_DIS_MASK,
+				 MIPI_ADAPT_ALIG_CNTL8_EXCEED_NOT_HOLD);
+
+	/* Start ALIG module */
+	c3_mipi_adap_update_bits(adap, MIPI_ADAPT_ALIG_CNTL8,
+				 MIPI_ADAPT_ALIG_CNTL8_START_EN_MASK,
+				 MIPI_ADAPT_ALIG_CNTL8_START_EN);
+}
+
+/* V4L2 subdev operations */
+
+static int c3_mipi_adap_enable_streams(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       u32 pad, u64 streams_mask)
+{
+	struct c3_adap_device *adap = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *fmt;
+	struct media_pad *sink_pad;
+	struct v4l2_subdev *src_sd;
+	int ret;
+
+	sink_pad = &adap->pads[C3_MIPI_ADAP_PAD_SINK];
+	adap->src_pad = media_pad_remote_pad_unique(sink_pad);
+	if (IS_ERR(adap->src_pad)) {
+		dev_dbg(adap->dev, "Failed to get source pad for MIPI adap\n");
+		return -EPIPE;
+	}
+
+	src_sd = media_entity_to_v4l2_subdev(adap->src_pad->entity);
+
+	pm_runtime_resume_and_get(adap->dev);
+
+	fmt = v4l2_subdev_state_get_format(state, C3_MIPI_ADAP_PAD_SINK);
+
+	c3_mipi_adap_cfg_top(adap);
+	c3_mipi_adap_cfg_frontend(adap, fmt);
+	c3_mipi_adap_cfg_rd0(adap);
+	c3_mipi_adap_cfg_pixel0(adap, fmt);
+	c3_mipi_adap_cfg_alig(adap, fmt);
+
+	ret = v4l2_subdev_enable_streams(src_sd, adap->src_pad->index, BIT(0));
+	if (ret) {
+		pm_runtime_put(adap->dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int c3_mipi_adap_disable_streams(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *state,
+					u32 pad, u64 streams_mask)
+{
+	struct c3_adap_device *adap = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *src_sd;
+
+	if (adap->src_pad) {
+		src_sd = media_entity_to_v4l2_subdev(adap->src_pad->entity);
+		v4l2_subdev_disable_streams(src_sd, adap->src_pad->index,
+					    BIT(0));
+	}
+	adap->src_pad = NULL;
+
+	pm_runtime_put(adap->dev);
+
+	return 0;
+}
+
+static int c3_mipi_adap_enum_mbus_code(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	switch (code->pad) {
+	case C3_MIPI_ADAP_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(c3_mipi_adap_formats))
+			return -EINVAL;
+
+		code->code = c3_mipi_adap_formats[code->index].code;
+		break;
+	case C3_MIPI_ADAP_PAD_SRC:
+		if (code->index)
+			return -EINVAL;
+
+		fmt = v4l2_subdev_state_get_format(state, code->pad);
+		code->code = fmt->code;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int c3_mipi_adap_set_fmt(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
+				struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	const struct c3_adap_pix_format *pix_format;
+
+	if (format->pad != C3_MIPI_ADAP_PAD_SINK)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	pix_format = c3_mipi_adap_find_format(format->format.code);
+	if (!pix_format)
+		pix_format = &c3_mipi_adap_formats[0];
+
+	fmt = v4l2_subdev_state_get_format(state, format->pad);
+	fmt->code = pix_format->code;
+	fmt->width = clamp_t(u32, format->format.width,
+			     MIPI_ADAP_MIN_WIDTH, MIPI_ADAP_MAX_WIDTH);
+	fmt->height = clamp_t(u32, format->format.height,
+			      MIPI_ADAP_MIN_HEIGHT, MIPI_ADAP_MAX_HEIGHT);
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	format->format = *fmt;
+
+	/* Synchronize the format to source pad */
+	fmt = v4l2_subdev_state_get_format(state, C3_MIPI_ADAP_PAD_SRC);
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int c3_mipi_adap_init_state(struct v4l2_subdev *sd,
+				   struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+
+	sink_fmt = v4l2_subdev_state_get_format(state, C3_MIPI_ADAP_PAD_SINK);
+	src_fmt = v4l2_subdev_state_get_format(state, C3_MIPI_ADAP_PAD_SRC);
+
+	sink_fmt->width = MIPI_ADAP_DEFAULT_WIDTH;
+	sink_fmt->height = MIPI_ADAP_DEFAULT_HEIGHT;
+	sink_fmt->field = V4L2_FIELD_NONE;
+	sink_fmt->code = MIPI_ADAP_DEFAULT_FMT;
+	sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
+	sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	*src_fmt = *sink_fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops c3_mipi_adap_pad_ops = {
+	.enum_mbus_code = c3_mipi_adap_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = c3_mipi_adap_set_fmt,
+	.enable_streams = c3_mipi_adap_enable_streams,
+	.disable_streams = c3_mipi_adap_disable_streams,
+};
+
+static const struct v4l2_subdev_ops c3_mipi_adap_subdev_ops = {
+	.pad = &c3_mipi_adap_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops c3_mipi_adap_internal_ops = {
+	.init_state = c3_mipi_adap_init_state,
+};
+
+/* Media entity operations */
+static const struct media_entity_operations c3_mipi_adap_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/* PM runtime */
+
+static int c3_mipi_adap_runtime_suspend(struct device *dev)
+{
+	struct c3_adap_device *adap = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(adap->info->clock_num, adap->clks);
+
+	return 0;
+}
+
+static int c3_mipi_adap_runtime_resume(struct device *dev)
+{
+	struct c3_adap_device *adap = dev_get_drvdata(dev);
+
+	return clk_bulk_prepare_enable(adap->info->clock_num, adap->clks);
+}
+
+static const struct dev_pm_ops c3_mipi_adap_pm_ops = {
+	SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+			    pm_runtime_force_resume)
+	RUNTIME_PM_OPS(c3_mipi_adap_runtime_suspend,
+		       c3_mipi_adap_runtime_resume, NULL)
+};
+
+/* Probe/remove & platform driver */
+
+static int c3_mipi_adap_subdev_init(struct c3_adap_device *adap)
+{
+	struct v4l2_subdev *sd = &adap->sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &c3_mipi_adap_subdev_ops);
+	sd->owner = THIS_MODULE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->internal_ops = &c3_mipi_adap_internal_ops;
+	snprintf(sd->name, sizeof(sd->name), "%s", MIPI_ADAP_SUBDEV_NAME);
+
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->entity.ops = &c3_mipi_adap_entity_ops;
+
+	sd->dev = adap->dev;
+	v4l2_set_subdevdata(sd, adap);
+
+	adap->pads[C3_MIPI_ADAP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	adap->pads[C3_MIPI_ADAP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, C3_MIPI_ADAP_PAD_MAX,
+				     adap->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret) {
+		media_entity_cleanup(&sd->entity);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void c3_mipi_adap_subdev_deinit(struct c3_adap_device *adap)
+{
+	v4l2_subdev_cleanup(&adap->sd);
+	media_entity_cleanup(&adap->sd.entity);
+}
+
+/* Subdev notifier register */
+static int c3_mipi_adap_notify_bound(struct v4l2_async_notifier *notifier,
+				     struct v4l2_subdev *sd,
+				     struct v4l2_async_connection *asc)
+{
+	struct c3_adap_device *adap = v4l2_get_subdevdata(notifier->sd);
+	struct media_pad *sink = &adap->sd.entity.pads[C3_MIPI_ADAP_PAD_SINK];
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
+					       MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static const struct v4l2_async_notifier_operations c3_mipi_adap_notify_ops = {
+	.bound = c3_mipi_adap_notify_bound,
+};
+
+static int c3_mipi_adap_async_register(struct c3_adap_device *adap)
+{
+	struct v4l2_async_connection *asc;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_subdev_nf_init(&adap->notifier, &adap->sd);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(adap->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENOTCONN;
+
+	asc = v4l2_async_nf_add_fwnode_remote(&adap->notifier, ep,
+					      struct v4l2_async_connection);
+	if (IS_ERR(asc)) {
+		ret = PTR_ERR(asc);
+		goto err_put_handle;
+	}
+
+	adap->notifier.ops = &c3_mipi_adap_notify_ops;
+	ret = v4l2_async_nf_register(&adap->notifier);
+	if (ret)
+		goto err_cleanup_nf;
+
+	ret = v4l2_async_register_subdev(&adap->sd);
+	if (ret)
+		goto err_unregister_nf;
+
+	fwnode_handle_put(ep);
+
+	return 0;
+
+err_unregister_nf:
+	v4l2_async_nf_unregister(&adap->notifier);
+err_cleanup_nf:
+	v4l2_async_nf_cleanup(&adap->notifier);
+err_put_handle:
+	fwnode_handle_put(ep);
+	return ret;
+}
+
+static void c3_mipi_adap_async_unregister(struct c3_adap_device *adap)
+{
+	v4l2_async_unregister_subdev(&adap->sd);
+	v4l2_async_nf_unregister(&adap->notifier);
+	v4l2_async_nf_cleanup(&adap->notifier);
+}
+
+static int c3_mipi_adap_ioremap_resource(struct c3_adap_device *adap)
+{
+	struct device *dev = adap->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	adap->top = devm_platform_ioremap_resource_byname(pdev, "top");
+	if (IS_ERR(adap->top))
+		return PTR_ERR(adap->top);
+
+	adap->fd = devm_platform_ioremap_resource_byname(pdev, "fd");
+	if (IS_ERR(adap->fd))
+		return PTR_ERR(adap->fd);
+
+	adap->rd = devm_platform_ioremap_resource_byname(pdev, "rd");
+	if (IS_ERR(adap->rd))
+		return PTR_ERR(adap->rd);
+
+	return 0;
+}
+
+static int c3_mipi_adap_get_clocks(struct c3_adap_device *adap)
+{
+	const struct c3_adap_info *info = adap->info;
+
+	for (unsigned int i = 0; i < info->clock_num; i++)
+		adap->clks[i].id = info->clocks[i];
+
+	return devm_clk_bulk_get(adap->dev, info->clock_num, adap->clks);
+}
+
+static int c3_mipi_adap_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct c3_adap_device *adap;
+	int ret;
+
+	adap = devm_kzalloc(dev, sizeof(*adap), GFP_KERNEL);
+	if (!adap)
+		return -ENOMEM;
+
+	adap->info = of_device_get_match_data(dev);
+	adap->dev = dev;
+
+	ret = c3_mipi_adap_ioremap_resource(adap);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to ioremap resource\n");
+
+	ret = c3_mipi_adap_get_clocks(adap);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get clocks\n");
+
+	platform_set_drvdata(pdev, adap);
+
+	pm_runtime_enable(dev);
+
+	ret = c3_mipi_adap_subdev_init(adap);
+	if (ret)
+		goto err_disable_runtime_pm;
+
+	ret = c3_mipi_adap_async_register(adap);
+	if (ret)
+		goto err_deinit_subdev;
+
+	return 0;
+
+err_deinit_subdev:
+	c3_mipi_adap_subdev_deinit(adap);
+err_disable_runtime_pm:
+	pm_runtime_disable(dev);
+	return ret;
+};
+
+static void c3_mipi_adap_remove(struct platform_device *pdev)
+{
+	struct c3_adap_device *adap = platform_get_drvdata(pdev);
+
+	c3_mipi_adap_async_unregister(adap);
+	c3_mipi_adap_subdev_deinit(adap);
+
+	pm_runtime_disable(&pdev->dev);
+};
+
+static const struct c3_adap_info c3_mipi_adap_info = {
+	.clocks = {"vapb", "isp0"},
+	.clock_num = 2
+};
+
+static const struct of_device_id c3_mipi_adap_of_match[] = {
+	{
+		.compatible = "amlogic,c3-mipi-adapter",
+		.data = &c3_mipi_adap_info
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, c3_mipi_adap_of_match);
+
+static struct platform_driver c3_mipi_adap_driver = {
+	.probe = c3_mipi_adap_probe,
+	.remove = c3_mipi_adap_remove,
+	.driver = {
+		.name = "c3-mipi-adapter",
+		.of_match_table = c3_mipi_adap_of_match,
+		.pm = pm_ptr(&c3_mipi_adap_pm_ops),
+	},
+};
+
+module_platform_driver(c3_mipi_adap_driver);
+
+MODULE_AUTHOR("Keke Li <keke.li@amlogic.com>");
+MODULE_DESCRIPTION("Amlogic C3 MIPI adapter");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/amlogic/c3/mipi-csi2/Kconfig b/drivers/media/platform/amlogic/c3/mipi-csi2/Kconfig
new file mode 100644
index 000000000000..0d7b2e203273
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-csi2/Kconfig
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+config VIDEO_C3_MIPI_CSI2
+	tristate "Amlogic C3 MIPI CSI-2 receiver"
+	depends on ARCH_MESON || COMPILE_TEST
+	depends on VIDEO_DEV
+	depends on OF
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	help
+	  Video4Linux2 driver for Amlogic C3 MIPI CSI-2 receiver.
+	  C3 MIPI CSI-2 receiver is used to receive MIPI data from
+	  image sensor.
+
+	  To compile this driver as a module choose m here.
diff --git a/drivers/media/platform/amlogic/c3/mipi-csi2/Makefile b/drivers/media/platform/amlogic/c3/mipi-csi2/Makefile
new file mode 100644
index 000000000000..cc08fc722bfd
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-csi2/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+obj-$(CONFIG_VIDEO_C3_MIPI_CSI2) += c3-mipi-csi2.o
diff --git a/drivers/media/platform/amlogic/c3/mipi-csi2/c3-mipi-csi2.c b/drivers/media/platform/amlogic/c3/mipi-csi2/c3-mipi-csi2.c
new file mode 100644
index 000000000000..1011ab3ebac7
--- /dev/null
+++ b/drivers/media/platform/amlogic/c3/mipi-csi2/c3-mipi-csi2.c
@@ -0,0 +1,828 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
+/*
+ * Copyright (C) 2024 Amlogic, Inc. All rights reserved
+ */
+
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/math.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+
+/* C3 CSI-2 submodule definition */
+enum {
+	SUBMD_APHY,
+	SUBMD_DPHY,
+	SUBMD_HOST,
+};
+
+#define CSI2_SUBMD_MASK		GENMASK(17, 16)
+#define CSI2_SUBMD_SHIFT	16
+#define CSI2_SUBMD(x)	(((x) & (CSI2_SUBMD_MASK)) >> (CSI2_SUBMD_SHIFT))
+#define CSI2_REG_ADDR_MASK	GENMASK(15, 0)
+#define CSI2_REG_ADDR(x)	((x) & (CSI2_REG_ADDR_MASK))
+#define CSI2_REG_A(x)		((SUBMD_APHY << CSI2_SUBMD_SHIFT) | (x))
+#define CSI2_REG_D(x)		((SUBMD_DPHY << CSI2_SUBMD_SHIFT) | (x))
+#define CSI2_REG_H(x)		((SUBMD_HOST << CSI2_SUBMD_SHIFT) | (x))
+
+#define MIPI_CSI2_CLOCK_NUM_MAX			3
+#define MIPI_CSI2_SUBDEV_NAME			"c3-mipi-csi2"
+
+/* C3 CSI-2 APHY register */
+#define CSI_PHY_CNTL0				CSI2_REG_A(0x44)
+#define CSI_PHY_CNTL0_HS_LP_BIAS_EN		BIT(10)
+#define CSI_PHY_CNTL0_HS_RX_TRIM_11		(11 << 11)
+#define CSI_PHY_CNTL0_LP_LOW_VTH_2		(2 << 16)
+#define CSI_PHY_CNTL0_LP_HIGH_VTH_4		(4 << 20)
+#define CSI_PHY_CNTL0_DATA_LANE0_HS_DIG_EN	BIT(24)
+#define CSI_PHY_CNTL0_DATA_LANE1_HS_DIG_EN	BIT(25)
+#define CSI_PHY_CNTL0_CLK0_LANE_HS_DIG_EN	BIT(26)
+#define CSI_PHY_CNTL0_DATA_LANE2_HS_DIG_EN	BIT(27)
+#define CSI_PHY_CNTL0_DATA_LANE3_HS_DIG_EN	BIT(28)
+
+#define CSI_PHY_CNTL1				CSI2_REG_A(0x48)
+#define CSI_PHY_CNTL1_HS_EQ_CAP_SMALL		(2 << 16)
+#define CSI_PHY_CNTL1_HS_EQ_CAP_BIG		(3 << 16)
+#define CSI_PHY_CNTL1_HS_EQ_RES_MIN		(3 << 18)
+#define CSI_PHY_CNTL1_HS_EQ_RES_MED		(2 << 18)
+#define CSI_PHY_CNTL1_HS_EQ_RES_MAX		BIT(18)
+#define CSI_PHY_CNTL1_CLK_CHN_EQ_MAX_GAIN	BIT(20)
+#define CSI_PHY_CNTL1_DATA_CHN_EQ_MAX_GAIN	BIT(21)
+#define CSI_PHY_CNTL1_COM_BG_EN			BIT(24)
+#define CSI_PHY_CNTL1_HS_SYNC_EN		BIT(25)
+
+/* C3 CSI-2 DPHY register */
+#define MIPI_PHY_CTRL				CSI2_REG_D(0x00)
+#define MIPI_PHY_CTRL_DATA_LANE0_EN		(0 << 0)
+#define MIPI_PHY_CTRL_DATA_LANE0_DIS		BIT(0)
+#define MIPI_PHY_CTRL_DATA_LANE1_EN		(0 << 1)
+#define MIPI_PHY_CTRL_DATA_LANE1_DIS		BIT(1)
+#define MIPI_PHY_CTRL_DATA_LANE2_EN		(0 << 2)
+#define MIPI_PHY_CTRL_DATA_LANE2_DIS		BIT(2)
+#define MIPI_PHY_CTRL_DATA_LANE3_EN		(0 << 3)
+#define MIPI_PHY_CTRL_DATA_LANE3_DIS		BIT(3)
+#define MIPI_PHY_CTRL_CLOCK_LANE_EN		(0 << 4)
+#define MIPI_PHY_CTRL_CLOCK_LANE_DIS		BIT(4)
+
+#define MIPI_PHY_CLK_LANE_CTRL			CSI2_REG_D(0x04)
+#define MIPI_PHY_CLK_LANE_CTRL_FORCE_ULPS_ENTER	BIT(0)
+#define MIPI_PHY_CLK_LANE_CTRL_FORCE_ULPS_EXIT	BIT(1)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS	(0 << 3)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS_2	BIT(3)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS_4	(2 << 3)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS_8	(3 << 3)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS_16	(4 << 3)
+#define MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_EN	BIT(6)
+#define MIPI_PHY_CLK_LANE_CTRL_LPEN_DIS		BIT(7)
+#define MIPI_PHY_CLK_LANE_CTRL_END_EN		BIT(8)
+#define MIPI_PHY_CLK_LANE_CTRL_HS_RX_EN		BIT(9)
+
+#define MIPI_PHY_DATA_LANE_CTRL1		CSI2_REG_D(0x0c)
+#define MIPI_PHY_DATA_LANE_CTRL1_INSERT_ERRESC	BIT(0)
+#define MIPI_PHY_DATA_LANE_CTRL1_HS_SYNC_CHK_EN	BIT(1)
+#define MIPI_PHY_DATA_LANE_CTRL1_PIPE_MASK	GENMASK(6, 2)
+#define MIPI_PHY_DATA_LANE_CTRL1_PIPE_ALL_EN	(0x1f << 2)
+#define MIPI_PHY_DATA_LANE_CTRL1_PIPE_DELAY_MASK	GENMASK(9, 7)
+#define MIPI_PHY_DATA_LANE_CTRL1_PIPE_DELAY_3	(3 << 7)
+
+#define MIPI_PHY_TCLK_MISS			CSI2_REG_D(0x10)
+#define MIPI_PHY_TCLK_MISS_CYCLES_MASK		GENMASK(7, 0)
+#define MIPI_PHY_TCLK_MISS_CYCLES_9		(9 << 0)
+
+#define MIPI_PHY_TCLK_SETTLE			CSI2_REG_D(0x14)
+#define MIPI_PHY_TCLK_SETTLE_CYCLES_MASK	GENMASK(7, 0)
+#define MIPI_PHY_TCLK_SETTLE_CYCLES_31		(31 << 0)
+
+#define MIPI_PHY_THS_EXIT			CSI2_REG_D(0x18)
+#define MIPI_PHY_THS_EXIT_CYCLES_MASK		GENMASK(7, 0)
+#define MIPI_PHY_THS_EXIT_CYCLES_8		(8 << 0)
+
+#define MIPI_PHY_THS_SKIP			CSI2_REG_D(0x1c)
+#define MIPI_PHY_THS_SKIP_CYCLES_MASK		GENMASK(7, 0)
+#define MIPI_PHY_THS_SKIP_CYCLES_10		(10 << 0)
+
+#define MIPI_PHY_THS_SETTLE			CSI2_REG_D(0x20)
+#define MIPI_PHY_THS_SETTLE_CYCLES_MASK		GENMASK(7, 0)
+
+#define MIPI_PHY_TINIT				CSI2_REG_D(0x24)
+#define MIPI_PHY_TINIT_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TINIT_CYCLES_20000		(20000 << 0)
+
+#define MIPI_PHY_TULPS_C			CSI2_REG_D(0x28)
+#define MIPI_PHY_TULPS_C_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TULPS_C_CYCLES_4096		(4096 << 0)
+
+#define MIPI_PHY_TULPS_S			CSI2_REG_D(0x2c)
+#define MIPI_PHY_TULPS_S_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TULPS_S_CYCLES_256		(256 << 0)
+
+#define MIPI_PHY_TMBIAS				CSI2_REG_D(0x30)
+#define MIPI_PHY_TMBIAS_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TMBIAS_CYCLES_256		(256 << 0)
+
+#define MIPI_PHY_TLP_EN_W			CSI2_REG_D(0x34)
+#define MIPI_PHY_TLP_EN_W_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TLP_EN_W_CYCLES_12		(12 << 0)
+
+#define MIPI_PHY_TLPOK				CSI2_REG_D(0x38)
+#define MIPI_PHY_TLPOK_CYCLES_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TLPOK_CYCLES_256		(256 << 0)
+
+#define MIPI_PHY_TWD_INIT			CSI2_REG_D(0x3c)
+#define MIPI_PHY_TWD_INIT_DOG_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TWD_INIT_DOG_0X400000		(0x400000 << 0)
+
+#define MIPI_PHY_TWD_HS				CSI2_REG_D(0x40)
+#define MIPI_PHY_TWD_HS_DOG_MASK		GENMASK(31, 0)
+#define MIPI_PHY_TWD_HS_DOG_0X400000		(0x400000 << 0)
+
+#define MIPI_PHY_MUX_CTRL0			CSI2_REG_D(0x284)
+#define MIPI_PHY_MUX_CTRL0_SFEN3_SRC_MASK	GENMASK(3, 0)
+#define MIPI_PHY_MUX_CTRL0_SFEN3_SRC_LANE0	(0 << 0)
+#define MIPI_PHY_MUX_CTRL0_SFEN3_SRC_LANE1	BIT(0)
+#define MIPI_PHY_MUX_CTRL0_SFEN3_SRC_LANE2	(2 << 0)
+#define MIPI_PHY_MUX_CTRL0_SFEN3_SRC_LANE3	(3 << 0)
+#define MIPI_PHY_MUX_CTRL0_SFEN2_SRC_MASK	GENMASK(7, 4)
+#define MIPI_PHY_MUX_CTRL0_SFEN2_SRC_LANE0	(0 << 4)
+#define MIPI_PHY_MUX_CTRL0_SFEN2_SRC_LANE1	BIT(4)
+#define MIPI_PHY_MUX_CTRL0_SFEN2_SRC_LANE2	(2 << 4)
+#define MIPI_PHY_MUX_CTRL0_SFEN2_SRC_LANE3	(3 << 4)
+#define MIPI_PHY_MUX_CTRL0_SFEN1_SRC_MASK	GENMASK(11, 8)
+#define MIPI_PHY_MUX_CTRL0_SFEN1_SRC_LANE0	(0 << 8)
+#define MIPI_PHY_MUX_CTRL0_SFEN1_SRC_LANE1	BIT(8)
+#define MIPI_PHY_MUX_CTRL0_SFEN1_SRC_LANE2	(2 << 8)
+#define MIPI_PHY_MUX_CTRL0_SFEN1_SRC_LANE3	(3 << 8)
+#define MIPI_PHY_MUX_CTRL0_SFEN0_SRC_MASK	GENMASK(14, 12)
+#define MIPI_PHY_MUX_CTRL0_SFEN0_SRC_LANE0	(0 << 12)
+#define MIPI_PHY_MUX_CTRL0_SFEN0_SRC_LANE1	BIT(12)
+#define MIPI_PHY_MUX_CTRL0_SFEN0_SRC_LANE2	(2 << 12)
+#define MIPI_PHY_MUX_CTRL0_SFEN0_SRC_LANE3	(3 << 12)
+
+#define MIPI_PHY_MUX_CTRL1			CSI2_REG_D(0x288)
+#define MIPI_PHY_MUX_CTRL1_LANE3_SRC_MASK	GENMASK(3, 0)
+#define MIPI_PHY_MUX_CTRL1_LANE3_SRC_SFEN0	(0 << 0)
+#define MIPI_PHY_MUX_CTRL1_LANE3_SRC_SFEN1	BIT(0)
+#define MIPI_PHY_MUX_CTRL1_LANE3_SRC_SFEN2	(2 << 0)
+#define MIPI_PHY_MUX_CTRL1_LANE3_SRC_SFEN3	(3 << 0)
+#define MIPI_PHY_MUX_CTRL1_LANE2_SRC_MASK	GENMASK(7, 4)
+#define MIPI_PHY_MUX_CTRL1_LANE2_SRC_SFEN0	(0 << 4)
+#define MIPI_PHY_MUX_CTRL1_LANE2_SRC_SFEN1	BIT(4)
+#define MIPI_PHY_MUX_CTRL1_LANE2_SRC_SFEN2	(2 << 4)
+#define MIPI_PHY_MUX_CTRL1_LANE2_SRC_SFEN3	(3 << 4)
+#define MIPI_PHY_MUX_CTRL1_LANE1_SRC_MASK	GENMASK(11, 8)
+#define MIPI_PHY_MUX_CTRL1_LANE1_SRC_SFEN0	(0 << 8)
+#define MIPI_PHY_MUX_CTRL1_LANE1_SRC_SFEN1	BIT(8)
+#define MIPI_PHY_MUX_CTRL1_LANE1_SRC_SFEN2	(2 << 8)
+#define MIPI_PHY_MUX_CTRL1_LANE1_SRC_SFEN3	(3 << 8)
+#define MIPI_PHY_MUX_CTRL1_LANE0_SRC_MASK	GENMASK(14, 12)
+#define MIPI_PHY_MUX_CTRL1_LANE0_SRC_SFEN0	(0 << 12)
+#define MIPI_PHY_MUX_CTRL1_LANE0_SRC_SFEN1	BIT(12)
+#define MIPI_PHY_MUX_CTRL1_LANE0_SRC_SFEN2	(2 << 12)
+#define MIPI_PHY_MUX_CTRL1_LANE0_SRC_SFEN3	(3 << 12)
+
+/* C3 CSI-2 HOST register */
+#define CSI2_HOST_N_LANES			CSI2_REG_H(0x04)
+#define CSI2_HOST_N_LANES_MASK			GENMASK(1, 0)
+#define CSI2_HOST_N_LANES_1			(0 << 0)
+#define CSI2_HOST_N_LANES_2			BIT(0)
+#define CSI2_HOST_N_LANES_3			(2 << 0)
+#define CSI2_HOST_N_LANES_4			(3 << 0)
+
+#define CSI2_HOST_CSI2_RESETN			CSI2_REG_H(0x10)
+#define CSI2_HOST_CSI2_RESETN_MASK		BIT(0)
+#define CSI2_HOST_CSI2_RESETN_ACTIVE		(0 << 0)
+#define CSI2_HOST_CSI2_RESETN_EXIT		BIT(0)
+
+#define C3_MIPI_CSI2_MAX_WIDTH			2888
+#define C3_MIPI_CSI2_MIN_WIDTH			160
+#define C3_MIPI_CSI2_MAX_HEIGHT			2240
+#define C3_MIPI_CSI2_MIN_HEIGHT			120
+#define C3_MIPI_CSI2_DEFAULT_WIDTH		1920
+#define C3_MIPI_CSI2_DEFAULT_HEIGHT		1080
+#define C3_MIPI_CSI2_DEFAULT_FMT		MEDIA_BUS_FMT_SRGGB10_1X10
+
+/* C3 CSI-2 pad list */
+enum {
+	C3_MIPI_CSI2_PAD_SINK,
+	C3_MIPI_CSI2_PAD_SRC,
+	C3_MIPI_CSI2_PAD_MAX
+};
+
+/*
+ * struct c3_csi_info - MIPI CSI2 information
+ *
+ * @clocks: array of MIPI CSI2 clock names
+ * @clock_num: actual clock number
+ */
+struct c3_csi_info {
+	char *clocks[MIPI_CSI2_CLOCK_NUM_MAX];
+	u32 clock_num;
+};
+
+/*
+ * struct c3_csi_device - MIPI CSI2 platform device
+ *
+ * @dev: pointer to the struct device
+ * @aphy: MIPI CSI2 aphy register address
+ * @dphy: MIPI CSI2 dphy register address
+ * @host: MIPI CSI2 host register address
+ * @clks: array of MIPI CSI2 clocks
+ * @sd: MIPI CSI2 sub-device
+ * @pads: MIPI CSI2 sub-device pads
+ * @notifier: notifier to register on the v4l2-async API
+ * @src_pad: source sub-device pad
+ * @bus: MIPI CSI2 bus information
+ * @info: version-specific MIPI CSI2 information
+ */
+struct c3_csi_device {
+	struct device *dev;
+	void __iomem *aphy;
+	void __iomem *dphy;
+	void __iomem *host;
+	struct clk_bulk_data clks[MIPI_CSI2_CLOCK_NUM_MAX];
+
+	struct v4l2_subdev sd;
+	struct media_pad pads[C3_MIPI_CSI2_PAD_MAX];
+	struct v4l2_async_notifier notifier;
+	struct media_pad *src_pad;
+	struct v4l2_mbus_config_mipi_csi2 bus;
+
+	const struct c3_csi_info *info;
+};
+
+static const u32 c3_mipi_csi_formats[] = {
+	MEDIA_BUS_FMT_SBGGR10_1X10,
+	MEDIA_BUS_FMT_SGBRG10_1X10,
+	MEDIA_BUS_FMT_SGRBG10_1X10,
+	MEDIA_BUS_FMT_SRGGB10_1X10,
+	MEDIA_BUS_FMT_SBGGR12_1X12,
+	MEDIA_BUS_FMT_SGBRG12_1X12,
+	MEDIA_BUS_FMT_SGRBG12_1X12,
+	MEDIA_BUS_FMT_SRGGB12_1X12,
+};
+
+/* Hardware configuration */
+
+static void c3_mipi_csi_write(struct c3_csi_device *csi, u32 reg, u32 val)
+{
+	void __iomem *addr;
+
+	switch (CSI2_SUBMD(reg)) {
+	case SUBMD_APHY:
+		addr = csi->aphy + CSI2_REG_ADDR(reg);
+		break;
+	case SUBMD_DPHY:
+		addr = csi->dphy + CSI2_REG_ADDR(reg);
+		break;
+	case SUBMD_HOST:
+		addr = csi->host + CSI2_REG_ADDR(reg);
+		break;
+	default:
+		dev_err(csi->dev, "Invalid sub-module: %lu\n", CSI2_SUBMD(reg));
+		return;
+	}
+
+	writel(val, addr);
+}
+
+static void c3_mipi_csi_cfg_aphy(struct c3_csi_device *csi)
+{
+	c3_mipi_csi_write(csi, CSI_PHY_CNTL0,
+			  CSI_PHY_CNTL0_HS_LP_BIAS_EN |
+			  CSI_PHY_CNTL0_HS_RX_TRIM_11 |
+			  CSI_PHY_CNTL0_LP_LOW_VTH_2 |
+			  CSI_PHY_CNTL0_LP_HIGH_VTH_4 |
+			  CSI_PHY_CNTL0_DATA_LANE0_HS_DIG_EN |
+			  CSI_PHY_CNTL0_DATA_LANE1_HS_DIG_EN |
+			  CSI_PHY_CNTL0_CLK0_LANE_HS_DIG_EN |
+			  CSI_PHY_CNTL0_DATA_LANE2_HS_DIG_EN |
+			  CSI_PHY_CNTL0_DATA_LANE3_HS_DIG_EN);
+
+	c3_mipi_csi_write(csi, CSI_PHY_CNTL1,
+			  CSI_PHY_CNTL1_HS_EQ_CAP_SMALL |
+			  CSI_PHY_CNTL1_HS_EQ_RES_MED |
+			  CSI_PHY_CNTL1_CLK_CHN_EQ_MAX_GAIN |
+			  CSI_PHY_CNTL1_DATA_CHN_EQ_MAX_GAIN |
+			  CSI_PHY_CNTL1_COM_BG_EN |
+			  CSI_PHY_CNTL1_HS_SYNC_EN);
+}
+
+static void c3_mipi_csi_cfg_dphy(struct c3_csi_device *csi, s64 rate)
+{
+	u32 val;
+	u32 settle;
+
+	/* Calculate the high speed settle */
+	val = DIV_ROUND_UP_ULL(1000000000, rate);
+	settle = (16 * val + 230) / 10;
+
+	c3_mipi_csi_write(csi, MIPI_PHY_CLK_LANE_CTRL,
+			  MIPI_PHY_CLK_LANE_CTRL_HS_RX_EN |
+			  MIPI_PHY_CLK_LANE_CTRL_END_EN |
+			  MIPI_PHY_CLK_LANE_CTRL_LPEN_DIS |
+			  MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_EN |
+			  MIPI_PHY_CLK_LANE_CTRL_TCLK_ZERO_HS_8);
+
+	c3_mipi_csi_write(csi, MIPI_PHY_TCLK_MISS, MIPI_PHY_TCLK_MISS_CYCLES_9);
+	c3_mipi_csi_write(csi, MIPI_PHY_TCLK_SETTLE,
+			  MIPI_PHY_TCLK_SETTLE_CYCLES_31);
+	c3_mipi_csi_write(csi, MIPI_PHY_THS_EXIT, MIPI_PHY_THS_EXIT_CYCLES_8);
+	c3_mipi_csi_write(csi, MIPI_PHY_THS_SKIP, MIPI_PHY_THS_SKIP_CYCLES_10);
+	c3_mipi_csi_write(csi, MIPI_PHY_THS_SETTLE, settle);
+	c3_mipi_csi_write(csi, MIPI_PHY_TINIT, MIPI_PHY_TINIT_CYCLES_20000);
+	c3_mipi_csi_write(csi, MIPI_PHY_TMBIAS, MIPI_PHY_TMBIAS_CYCLES_256);
+	c3_mipi_csi_write(csi, MIPI_PHY_TULPS_C, MIPI_PHY_TULPS_C_CYCLES_4096);
+	c3_mipi_csi_write(csi, MIPI_PHY_TULPS_S, MIPI_PHY_TULPS_S_CYCLES_256);
+	c3_mipi_csi_write(csi, MIPI_PHY_TLP_EN_W, MIPI_PHY_TLP_EN_W_CYCLES_12);
+	c3_mipi_csi_write(csi, MIPI_PHY_TLPOK, MIPI_PHY_TLPOK_CYCLES_256);
+	c3_mipi_csi_write(csi, MIPI_PHY_TWD_INIT,
+			  MIPI_PHY_TWD_INIT_DOG_0X400000);
+	c3_mipi_csi_write(csi, MIPI_PHY_TWD_HS, MIPI_PHY_TWD_HS_DOG_0X400000);
+
+	c3_mipi_csi_write(csi, MIPI_PHY_DATA_LANE_CTRL1,
+			  MIPI_PHY_DATA_LANE_CTRL1_INSERT_ERRESC |
+			  MIPI_PHY_DATA_LANE_CTRL1_HS_SYNC_CHK_EN |
+			  MIPI_PHY_DATA_LANE_CTRL1_PIPE_ALL_EN |
+			  MIPI_PHY_DATA_LANE_CTRL1_PIPE_DELAY_3);
+
+	/* Set the order of lanes */
+	c3_mipi_csi_write(csi, MIPI_PHY_MUX_CTRL0,
+			  MIPI_PHY_MUX_CTRL0_SFEN3_SRC_LANE3 |
+			  MIPI_PHY_MUX_CTRL0_SFEN2_SRC_LANE2 |
+			  MIPI_PHY_MUX_CTRL0_SFEN1_SRC_LANE1 |
+			  MIPI_PHY_MUX_CTRL0_SFEN0_SRC_LANE0);
+
+	c3_mipi_csi_write(csi, MIPI_PHY_MUX_CTRL1,
+			  MIPI_PHY_MUX_CTRL1_LANE3_SRC_SFEN3 |
+			  MIPI_PHY_MUX_CTRL1_LANE2_SRC_SFEN2 |
+			  MIPI_PHY_MUX_CTRL1_LANE1_SRC_SFEN1 |
+			  MIPI_PHY_MUX_CTRL1_LANE0_SRC_SFEN0);
+
+	/* Enable digital data and clock lanes */
+	c3_mipi_csi_write(csi, MIPI_PHY_CTRL,
+			  MIPI_PHY_CTRL_DATA_LANE0_EN |
+			  MIPI_PHY_CTRL_DATA_LANE1_EN |
+			  MIPI_PHY_CTRL_DATA_LANE2_EN |
+			  MIPI_PHY_CTRL_DATA_LANE3_EN |
+			  MIPI_PHY_CTRL_CLOCK_LANE_EN);
+}
+
+static void c3_mipi_csi_cfg_host(struct c3_csi_device *csi)
+{
+	/* Reset CSI-2 controller output */
+	c3_mipi_csi_write(csi, CSI2_HOST_CSI2_RESETN,
+			  CSI2_HOST_CSI2_RESETN_ACTIVE);
+	c3_mipi_csi_write(csi, CSI2_HOST_CSI2_RESETN,
+			  CSI2_HOST_CSI2_RESETN_EXIT);
+
+	/* Set data lane number */
+	c3_mipi_csi_write(csi, CSI2_HOST_N_LANES, csi->bus.num_data_lanes - 1);
+}
+
+static int c3_mipi_csi_start_stream(struct c3_csi_device *csi,
+				    struct v4l2_subdev *src_sd)
+{
+	s64 link_freq;
+	s64 lane_rate;
+
+	link_freq = v4l2_get_link_freq(src_sd->ctrl_handler, 0, 0);
+	if (link_freq < 0) {
+		dev_err(csi->dev,
+			"Unable to obtain link frequency: %lld\n", link_freq);
+		return link_freq;
+	}
+
+	lane_rate = link_freq * 2;
+	if (lane_rate > 1500000000) {
+		dev_err(csi->dev, "Invalid lane rate: %lld\n", lane_rate);
+		return -EINVAL;
+	}
+
+	c3_mipi_csi_cfg_aphy(csi);
+	c3_mipi_csi_cfg_dphy(csi, lane_rate);
+	c3_mipi_csi_cfg_host(csi);
+
+	return 0;
+}
+
+static int c3_mipi_csi_enable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct c3_csi_device *csi = v4l2_get_subdevdata(sd);
+	struct media_pad *sink_pad;
+	struct v4l2_subdev *src_sd;
+	int ret;
+
+	sink_pad = &csi->pads[C3_MIPI_CSI2_PAD_SINK];
+	csi->src_pad = media_pad_remote_pad_unique(sink_pad);
+	if (IS_ERR(csi->src_pad)) {
+		dev_dbg(csi->dev, "Failed to get source pad for MIPI CSI-2\n");
+		return -EPIPE;
+	}
+
+	src_sd = media_entity_to_v4l2_subdev(csi->src_pad->entity);
+
+	pm_runtime_resume_and_get(csi->dev);
+
+	c3_mipi_csi_start_stream(csi, src_sd);
+
+	ret = v4l2_subdev_enable_streams(src_sd, csi->src_pad->index, BIT(0));
+	if (ret) {
+		pm_runtime_put(csi->dev);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int c3_mipi_csi_disable_streams(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       u32 pad, u64 streams_mask)
+{
+	struct c3_csi_device *csi = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *src_sd;
+
+	if (csi->src_pad) {
+		src_sd = media_entity_to_v4l2_subdev(csi->src_pad->entity);
+		v4l2_subdev_disable_streams(src_sd, csi->src_pad->index,
+					    BIT(0));
+	}
+	csi->src_pad = NULL;
+
+	pm_runtime_put(csi->dev);
+
+	return 0;
+}
+
+static int c3_mipi_csi_enum_mbus_code(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      struct v4l2_subdev_mbus_code_enum *code)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	switch (code->pad) {
+	case C3_MIPI_CSI2_PAD_SINK:
+		if (code->index >= ARRAY_SIZE(c3_mipi_csi_formats))
+			return -EINVAL;
+
+		code->code = c3_mipi_csi_formats[code->index];
+		break;
+	case C3_MIPI_CSI2_PAD_SRC:
+		if (code->index)
+			return -EINVAL;
+
+		fmt = v4l2_subdev_state_get_format(state, code->pad);
+		code->code = fmt->code;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int c3_mipi_csi_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
+			       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	unsigned int i;
+
+	if (format->pad != C3_MIPI_CSI2_PAD_SINK)
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	fmt = v4l2_subdev_state_get_format(state, format->pad);
+
+	for (i = 0; i < ARRAY_SIZE(c3_mipi_csi_formats); i++) {
+		if (format->format.code == c3_mipi_csi_formats[i]) {
+			fmt->code = c3_mipi_csi_formats[i];
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(c3_mipi_csi_formats))
+		fmt->code = c3_mipi_csi_formats[0];
+
+	fmt->width = clamp_t(u32, format->format.width,
+			     C3_MIPI_CSI2_MIN_WIDTH, C3_MIPI_CSI2_MAX_WIDTH);
+	fmt->height = clamp_t(u32, format->format.height,
+			      C3_MIPI_CSI2_MIN_HEIGHT, C3_MIPI_CSI2_MAX_HEIGHT);
+	fmt->colorspace = V4L2_COLORSPACE_RAW;
+	fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	format->format = *fmt;
+
+	/* Synchronize the format to source pad */
+	fmt = v4l2_subdev_state_get_format(state, C3_MIPI_CSI2_PAD_SRC);
+	*fmt = format->format;
+
+	return 0;
+}
+
+static int c3_mipi_csi_init_state(struct v4l2_subdev *sd,
+				  struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+
+	sink_fmt = v4l2_subdev_state_get_format(state, C3_MIPI_CSI2_PAD_SINK);
+	src_fmt = v4l2_subdev_state_get_format(state, C3_MIPI_CSI2_PAD_SRC);
+
+	sink_fmt->width = C3_MIPI_CSI2_DEFAULT_WIDTH;
+	sink_fmt->height = C3_MIPI_CSI2_DEFAULT_HEIGHT;
+	sink_fmt->field = V4L2_FIELD_NONE;
+	sink_fmt->code = C3_MIPI_CSI2_DEFAULT_FMT;
+	sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
+	sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
+	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
+
+	*src_fmt = *sink_fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops c3_mipi_csi_pad_ops = {
+	.enum_mbus_code = c3_mipi_csi_enum_mbus_code,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = c3_mipi_csi_set_fmt,
+	.enable_streams = c3_mipi_csi_enable_streams,
+	.disable_streams = c3_mipi_csi_disable_streams,
+};
+
+static const struct v4l2_subdev_ops c3_mipi_csi_subdev_ops = {
+	.pad = &c3_mipi_csi_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops c3_mipi_csi_internal_ops = {
+	.init_state = c3_mipi_csi_init_state,
+};
+
+/* Media entity operations */
+static const struct media_entity_operations c3_mipi_csi_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+};
+
+/* PM runtime */
+
+static int c3_mipi_csi_runtime_suspend(struct device *dev)
+{
+	struct c3_csi_device *csi = dev_get_drvdata(dev);
+
+	clk_bulk_disable_unprepare(csi->info->clock_num, csi->clks);
+
+	return 0;
+}
+
+static int c3_mipi_csi_runtime_resume(struct device *dev)
+{
+	struct c3_csi_device *csi = dev_get_drvdata(dev);
+
+	return clk_bulk_prepare_enable(csi->info->clock_num, csi->clks);
+}
+
+static const struct dev_pm_ops c3_mipi_csi_pm_ops = {
+	SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+			    pm_runtime_force_resume)
+	RUNTIME_PM_OPS(c3_mipi_csi_runtime_suspend,
+		       c3_mipi_csi_runtime_resume, NULL)
+};
+
+/* Probe/remove & platform driver */
+
+static int c3_mipi_csi_subdev_init(struct c3_csi_device *csi)
+{
+	struct v4l2_subdev *sd = &csi->sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &c3_mipi_csi_subdev_ops);
+	sd->owner = THIS_MODULE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->internal_ops = &c3_mipi_csi_internal_ops;
+	snprintf(sd->name, sizeof(sd->name), "%s", MIPI_CSI2_SUBDEV_NAME);
+
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->entity.ops = &c3_mipi_csi_entity_ops;
+
+	sd->dev = csi->dev;
+	v4l2_set_subdevdata(sd, csi);
+
+	csi->pads[C3_MIPI_CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	csi->pads[C3_MIPI_CSI2_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
+	ret = media_entity_pads_init(&sd->entity, C3_MIPI_CSI2_PAD_MAX,
+				     csi->pads);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret) {
+		media_entity_cleanup(&sd->entity);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void c3_mipi_csi_subdev_deinit(struct c3_csi_device *csi)
+{
+	v4l2_subdev_cleanup(&csi->sd);
+	media_entity_cleanup(&csi->sd.entity);
+}
+
+/* Subdev notifier register */
+static int c3_mipi_csi_notify_bound(struct v4l2_async_notifier *notifier,
+				    struct v4l2_subdev *sd,
+				    struct v4l2_async_connection *asc)
+{
+	struct c3_csi_device *csi = v4l2_get_subdevdata(notifier->sd);
+	struct media_pad *sink = &csi->sd.entity.pads[C3_MIPI_CSI2_PAD_SINK];
+
+	return v4l2_create_fwnode_links_to_pad(sd, sink, MEDIA_LNK_FL_ENABLED |
+					       MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static const struct v4l2_async_notifier_operations c3_mipi_csi_notify_ops = {
+	.bound = c3_mipi_csi_notify_bound,
+};
+
+static int c3_mipi_csi_async_register(struct c3_csi_device *csi)
+{
+	struct v4l2_fwnode_endpoint vep = {
+		.bus_type = V4L2_MBUS_CSI2_DPHY,
+	};
+	struct v4l2_async_connection *asc;
+	struct fwnode_handle *ep;
+	int ret;
+
+	v4l2_async_subdev_nf_init(&csi->notifier, &csi->sd);
+
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep)
+		return -ENOTCONN;
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &vep);
+	if (ret)
+		goto err_put_handle;
+
+	csi->bus = vep.bus.mipi_csi2;
+
+	asc = v4l2_async_nf_add_fwnode_remote(&csi->notifier, ep,
+					      struct v4l2_async_connection);
+	if (IS_ERR(asc)) {
+		ret = PTR_ERR(asc);
+		goto err_put_handle;
+	}
+
+	csi->notifier.ops = &c3_mipi_csi_notify_ops;
+	ret = v4l2_async_nf_register(&csi->notifier);
+	if (ret)
+		goto err_cleanup_nf;
+
+	ret = v4l2_async_register_subdev(&csi->sd);
+	if (ret)
+		goto err_unregister_nf;
+
+	fwnode_handle_put(ep);
+
+	return 0;
+
+err_unregister_nf:
+	v4l2_async_nf_unregister(&csi->notifier);
+err_cleanup_nf:
+	v4l2_async_nf_cleanup(&csi->notifier);
+err_put_handle:
+	fwnode_handle_put(ep);
+	return ret;
+}
+
+static void c3_mipi_csi_async_unregister(struct c3_csi_device *csi)
+{
+	v4l2_async_unregister_subdev(&csi->sd);
+	v4l2_async_nf_unregister(&csi->notifier);
+	v4l2_async_nf_cleanup(&csi->notifier);
+}
+
+static int c3_mipi_csi_ioremap_resource(struct c3_csi_device *csi)
+{
+	struct device *dev = csi->dev;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	csi->aphy = devm_platform_ioremap_resource_byname(pdev, "aphy");
+	if (IS_ERR(csi->aphy))
+		return PTR_ERR(csi->aphy);
+
+	csi->dphy = devm_platform_ioremap_resource_byname(pdev, "dphy");
+	if (IS_ERR(csi->dphy))
+		return PTR_ERR(csi->dphy);
+
+	csi->host = devm_platform_ioremap_resource_byname(pdev, "host");
+	if (IS_ERR(csi->host))
+		return PTR_ERR(csi->host);
+
+	return 0;
+}
+
+static int c3_mipi_csi_get_clocks(struct c3_csi_device *csi)
+{
+	const struct c3_csi_info *info = csi->info;
+
+	for (unsigned int i = 0; i < info->clock_num; i++)
+		csi->clks[i].id = info->clocks[i];
+
+	return devm_clk_bulk_get(csi->dev, info->clock_num, csi->clks);
+}
+
+static int c3_mipi_csi_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct c3_csi_device *csi;
+	int ret;
+
+	csi = devm_kzalloc(dev, sizeof(*csi), GFP_KERNEL);
+	if (!csi)
+		return -ENOMEM;
+
+	csi->info = of_device_get_match_data(dev);
+	csi->dev = dev;
+
+	ret = c3_mipi_csi_ioremap_resource(csi);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to ioremap resource\n");
+
+	ret = c3_mipi_csi_get_clocks(csi);
+	if (ret)
+		return dev_err_probe(dev, ret, "Failed to get clocks\n");
+
+	platform_set_drvdata(pdev, csi);
+
+	pm_runtime_enable(dev);
+
+	ret = c3_mipi_csi_subdev_init(csi);
+	if (ret)
+		goto err_disable_runtime_pm;
+
+	ret = c3_mipi_csi_async_register(csi);
+	if (ret)
+		goto err_deinit_subdev;
+
+	return 0;
+
+err_deinit_subdev:
+	c3_mipi_csi_subdev_deinit(csi);
+err_disable_runtime_pm:
+	pm_runtime_disable(dev);
+	return ret;
+};
+
+static void c3_mipi_csi_remove(struct platform_device *pdev)
+{
+	struct c3_csi_device *csi = platform_get_drvdata(pdev);
+
+	c3_mipi_csi_async_unregister(csi);
+	c3_mipi_csi_subdev_deinit(csi);
+
+	pm_runtime_disable(&pdev->dev);
+};
+
+static const struct c3_csi_info c3_mipi_csi_info = {
+	.clocks = {"vapb", "phy0"},
+	.clock_num = 2
+};
+
+static const struct of_device_id c3_mipi_csi_of_match[] = {
+	{
+		.compatible = "amlogic,c3-mipi-csi2",
+		.data = &c3_mipi_csi_info,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, c3_mipi_csi_of_match);
+
+static struct platform_driver c3_mipi_csi_driver = {
+	.probe = c3_mipi_csi_probe,
+	.remove = c3_mipi_csi_remove,
+	.driver = {
+		.name = "c3-mipi-csi2",
+		.of_match_table = c3_mipi_csi_of_match,
+		.pm = pm_ptr(&c3_mipi_csi_pm_ops),
+	},
+};
+
+module_platform_driver(c3_mipi_csi_driver);
+
+MODULE_AUTHOR("Keke Li <keke.li@amlogic.com>");
+MODULE_DESCRIPTION("Amlogic C3 MIPI CSI-2 receiver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/amlogic/meson-ge2d/ge2d.c b/drivers/media/platform/amlogic/meson-ge2d/ge2d.c
index 09409908ba5d..0c004bb8ba05 100644
--- a/drivers/media/platform/amlogic/meson-ge2d/ge2d.c
+++ b/drivers/media/platform/amlogic/meson-ge2d/ge2d.c
@@ -391,8 +391,6 @@ static const struct vb2_ops ge2d_qops = {
 	.buf_queue = ge2d_buf_queue,
 	.start_streaming = ge2d_start_streaming,
 	.stop_streaming = ge2d_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int
@@ -1045,7 +1043,7 @@ MODULE_DEVICE_TABLE(of, meson_ge2d_match);
 
 static struct platform_driver ge2d_drv = {
 	.probe = ge2d_probe,
-	.remove_new = ge2d_remove,
+	.remove = ge2d_remove,
 	.driver = {
 		.name = "meson-ge2d",
 		.of_match_table = meson_ge2d_match,
diff --git a/drivers/media/platform/amphion/vdec.c b/drivers/media/platform/amphion/vdec.c
index a57f9f4f3b87..85d518823159 100644
--- a/drivers/media/platform/amphion/vdec.c
+++ b/drivers/media/platform/amphion/vdec.c
@@ -195,7 +195,6 @@ static int vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct vdec_t *vdec = inst->priv;
 	int ret = 0;
 
-	vpu_inst_lock(inst);
 	switch (ctrl->id) {
 	case V4L2_CID_MPEG_VIDEO_DEC_DISPLAY_DELAY_ENABLE:
 		vdec->params.display_delay_enable = ctrl->val;
@@ -207,7 +206,6 @@ static int vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = -EINVAL;
 		break;
 	}
-	vpu_inst_unlock(inst);
 
 	return ret;
 }
@@ -807,7 +805,7 @@ static void vdec_buf_done(struct vpu_inst *inst, struct vpu_frame_info *frame)
 	cur_fmt = vpu_get_format(inst, inst->cap_format.type);
 	vbuf = &vpu_buf->m2m_buf.vb;
 	if (vbuf->vb2_buf.index != frame->id)
-		dev_err(inst->dev, "[%d] buffer id(%d, %d) dismatch\n",
+		dev_err(inst->dev, "[%d] buffer id(%d, %d) mismatch\n",
 			inst->id, vbuf->vb2_buf.index, frame->id);
 
 	if (vpu_get_buffer_state(vbuf) == VPU_BUF_STATE_READY && vdec->params.display_delay_enable)
diff --git a/drivers/media/platform/amphion/venc.c b/drivers/media/platform/amphion/venc.c
index 4eb57d793a9c..c5c1f1fbaa80 100644
--- a/drivers/media/platform/amphion/venc.c
+++ b/drivers/media/platform/amphion/venc.c
@@ -52,6 +52,7 @@ struct venc_t {
 	u32 ready_count;
 	u32 enable;
 	u32 stopped;
+	u32 memory_resource_configured;
 
 	u32 skipped_count;
 	u32 skipped_bytes;
@@ -518,7 +519,6 @@ static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct venc_t *venc = inst->priv;
 	int ret = 0;
 
-	vpu_inst_lock(inst);
 	switch (ctrl->id) {
 	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
 		venc->params.profile = ctrl->val;
@@ -579,7 +579,6 @@ static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = -EINVAL;
 		break;
 	}
-	vpu_inst_unlock(inst);
 
 	return ret;
 }
@@ -680,6 +679,9 @@ static int venc_ctrl_init(struct vpu_inst *inst)
 			       ~(1 << V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME),
 			       V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME);
 
+	v4l2_ctrl_new_std(&inst->ctrl_handler, NULL,
+			  V4L2_CID_MPEG_VIDEO_AVERAGE_QP, 0, 51, 1, 0);
+
 	if (inst->ctrl_handler.error) {
 		ret = inst->ctrl_handler.error;
 		v4l2_ctrl_handler_free(&inst->ctrl_handler);
@@ -819,6 +821,7 @@ static int venc_get_one_encoded_frame(struct vpu_inst *inst,
 	vbuf->field = inst->cap_format.field;
 	vbuf->flags |= frame->info.pic_type;
 	vpu_set_buffer_state(vbuf, VPU_BUF_STATE_IDLE);
+	vpu_set_buffer_average_qp(vbuf, frame->info.average_qp);
 	dev_dbg(inst->dev, "[%d][OUTPUT TS]%32lld\n", inst->id, vbuf->vb2_buf.timestamp);
 	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
 	venc->ready_count++;
@@ -941,10 +944,19 @@ static int venc_start_session(struct vpu_inst *inst, u32 type)
 	ret = vpu_iface_set_encode_params(inst, &venc->params, 0);
 	if (ret)
 		goto error;
+
+	venc->memory_resource_configured = false;
 	ret = vpu_session_configure_codec(inst);
 	if (ret)
 		goto error;
 
+	if (!venc->memory_resource_configured) {
+		vb2_queue_error(v4l2_m2m_get_src_vq(inst->fh.m2m_ctx));
+		vb2_queue_error(v4l2_m2m_get_dst_vq(inst->fh.m2m_ctx));
+		ret = -ENOMEM;
+		goto error;
+	}
+
 	inst->state = VPU_CODEC_STATE_CONFIGURED;
 	/*vpu_iface_config_memory_resource*/
 
@@ -983,6 +995,7 @@ static void venc_cleanup_mem_resource(struct vpu_inst *inst)
 	u32 i;
 
 	venc = inst->priv;
+	venc->memory_resource_configured = false;
 
 	for (i = 0; i < ARRAY_SIZE(venc->enc); i++)
 		vpu_free_dma(&venc->enc[i]);
@@ -1046,6 +1059,7 @@ static void venc_request_mem_resource(struct vpu_inst *inst,
 		vpu_iface_config_memory_resource(inst, MEM_RES_REF, i, &venc->ref[i]);
 	for (i = 0; i < act_frame_num; i++)
 		vpu_iface_config_memory_resource(inst, MEM_RES_ACT, i, &venc->act[i]);
+	venc->memory_resource_configured = true;
 }
 
 static void venc_cleanup_frames(struct venc_t *venc)
diff --git a/drivers/media/platform/amphion/vpu.h b/drivers/media/platform/amphion/vpu.h
index 0246cf0ac3a8..1451549c9dd2 100644
--- a/drivers/media/platform/amphion/vpu.h
+++ b/drivers/media/platform/amphion/vpu.h
@@ -162,7 +162,6 @@ struct vpu_core {
 	struct delayed_work msg_delayed_work;
 	struct kfifo msg_fifo;
 	void *msg_buffer;
-	unsigned int msg_buffer_size;
 
 	struct vpu_dev *vpu;
 	void *iface;
@@ -306,6 +305,7 @@ struct vpu_vb2_buffer {
 	dma_addr_t chroma_v;
 	unsigned int state;
 	u32 tag;
+	u32 average_qp;
 };
 
 void vpu_writel(struct vpu_dev *vpu, u32 reg, u32 val);
diff --git a/drivers/media/platform/amphion/vpu_core.c b/drivers/media/platform/amphion/vpu_core.c
index 3a2030d02e45..da00f5fc0e5d 100644
--- a/drivers/media/platform/amphion/vpu_core.c
+++ b/drivers/media/platform/amphion/vpu_core.c
@@ -250,6 +250,7 @@ static void vpu_core_get_vpu(struct vpu_core *core)
 static int vpu_core_register(struct device *dev, struct vpu_core *core)
 {
 	struct vpu_dev *vpu = dev_get_drvdata(dev);
+	unsigned int buffer_size;
 	int ret = 0;
 
 	dev_dbg(core->dev, "register core %s\n", vpu_core_type_desc(core->type));
@@ -263,14 +264,14 @@ static int vpu_core_register(struct device *dev, struct vpu_core *core)
 	}
 	INIT_WORK(&core->msg_work, vpu_msg_run_work);
 	INIT_DELAYED_WORK(&core->msg_delayed_work, vpu_msg_delayed_work);
-	core->msg_buffer_size = roundup_pow_of_two(VPU_MSG_BUFFER_SIZE);
-	core->msg_buffer = vzalloc(core->msg_buffer_size);
+	buffer_size = roundup_pow_of_two(VPU_MSG_BUFFER_SIZE);
+	core->msg_buffer = vzalloc(buffer_size);
 	if (!core->msg_buffer) {
 		dev_err(core->dev, "failed allocate buffer for fifo\n");
 		ret = -ENOMEM;
 		goto error;
 	}
-	ret = kfifo_init(&core->msg_fifo, core->msg_buffer, core->msg_buffer_size);
+	ret = kfifo_init(&core->msg_fifo, core->msg_buffer, buffer_size);
 	if (ret) {
 		dev_err(core->dev, "failed init kfifo\n");
 		goto error;
@@ -864,7 +865,7 @@ MODULE_DEVICE_TABLE(of, vpu_core_dt_match);
 
 static struct platform_driver amphion_vpu_core_driver = {
 	.probe = vpu_core_probe,
-	.remove_new = vpu_core_remove,
+	.remove = vpu_core_remove,
 	.driver = {
 		.name = "amphion-vpu-core",
 		.of_match_table = vpu_core_dt_match,
diff --git a/drivers/media/platform/amphion/vpu_defs.h b/drivers/media/platform/amphion/vpu_defs.h
index 7320852668d6..428d988cf2f7 100644
--- a/drivers/media/platform/amphion/vpu_defs.h
+++ b/drivers/media/platform/amphion/vpu_defs.h
@@ -114,6 +114,7 @@ struct vpu_enc_pic_info {
 	u32 wptr;
 	u32 crc;
 	s64 timestamp;
+	u32 average_qp;
 };
 
 struct vpu_dec_codec_info {
diff --git a/drivers/media/platform/amphion/vpu_drv.c b/drivers/media/platform/amphion/vpu_drv.c
index 2bf70aafd2ba..efbfd2652721 100644
--- a/drivers/media/platform/amphion/vpu_drv.c
+++ b/drivers/media/platform/amphion/vpu_drv.c
@@ -151,8 +151,8 @@ err_add_decoder:
 	media_device_cleanup(&vpu->mdev);
 	v4l2_device_unregister(&vpu->v4l2_dev);
 err_vpu_deinit:
-	pm_runtime_set_suspended(dev);
 	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
 
 	return ret;
 }
@@ -227,7 +227,7 @@ MODULE_DEVICE_TABLE(of, vpu_dt_match);
 
 static struct platform_driver amphion_vpu_driver = {
 	.probe = vpu_probe,
-	.remove_new = vpu_remove,
+	.remove = vpu_remove,
 	.driver = {
 		.name = "amphion-vpu",
 		.of_match_table = vpu_dt_match,
diff --git a/drivers/media/platform/amphion/vpu_malone.c b/drivers/media/platform/amphion/vpu_malone.c
index d3425de7bccd..feca7d4220ed 100644
--- a/drivers/media/platform/amphion/vpu_malone.c
+++ b/drivers/media/platform/amphion/vpu_malone.c
@@ -3,6 +3,7 @@
  * Copyright 2020-2021 NXP
  */
 
+#include <linux/bitfield.h>
 #include <linux/init.h>
 #include <linux/interconnect.h>
 #include <linux/ioctl.h>
@@ -25,6 +26,10 @@
 #include "vpu_imx8q.h"
 #include "vpu_malone.h"
 
+static bool low_latency;
+module_param(low_latency, bool, 0644);
+MODULE_PARM_DESC(low_latency, "Set low latency frame flush mode: 0 (disable) or 1 (enable)");
+
 #define CMD_SIZE			25600
 #define MSG_SIZE			25600
 #define CODEC_SIZE			0x1000
@@ -68,6 +73,12 @@
 
 #define MALONE_DEC_FMT_RV_MASK			BIT(21)
 
+#define MALONE_VERSION_MASK			0xFFFFF
+#define MALONE_VERSION(maj, min, inc)		\
+		(FIELD_PREP(0xF0000, maj) | FIELD_PREP(0xFF00, min) | FIELD_PREP(0xFF, inc))
+#define CHECK_VERSION(iface, maj, min)		\
+		(FIELD_GET(MALONE_VERSION_MASK, (iface)->fw_version) >= MALONE_VERSION(maj, min, 0))
+
 enum vpu_malone_stream_input_mode {
 	INVALID_MODE = 0,
 	FRAME_LVL,
@@ -207,11 +218,6 @@ struct vpu_malone_dbglog_desc {
 	u32 reserved;
 };
 
-struct vpu_malone_frame_buffer {
-	u32 addr;
-	u32 size;
-};
-
 struct vpu_malone_udata {
 	u32 base;
 	u32 total_size;
@@ -337,6 +343,8 @@ struct vpu_dec_ctrl {
 	u32 buf_addr[VID_API_NUM_STREAMS];
 };
 
+static const struct malone_padding_scode *get_padding_scode(u32 type, u32 fmt);
+
 u32 vpu_malone_get_data_size(void)
 {
 	return sizeof(struct vpu_dec_ctrl);
@@ -659,9 +667,15 @@ static int vpu_malone_set_params(struct vpu_shared_addr *shared,
 		hc->jpg[instance].jpg_mjpeg_interlaced = 0;
 	}
 
-	hc->codec_param[instance].disp_imm = params->display_delay_enable ? 1 : 0;
-	if (malone_format != MALONE_FMT_AVC)
+	if (params->display_delay_enable &&
+	    get_padding_scode(SCODE_PADDING_BUFFLUSH, params->codec_format))
+		hc->codec_param[instance].disp_imm = 1;
+	else
+		hc->codec_param[instance].disp_imm = 0;
+
+	if (params->codec_format == V4L2_PIX_FMT_HEVC && !CHECK_VERSION(iface, 1, 9))
 		hc->codec_param[instance].disp_imm = 0;
+
 	hc->codec_param[instance].dbglog_enable = 0;
 	iface->dbglog_desc.level = 0;
 
@@ -1028,6 +1042,7 @@ static const struct malone_padding_scode padding_scodes[] = {
 	{SCODE_PADDING_EOS,      V4L2_PIX_FMT_JPEG,        {0x0, 0x0}},
 	{SCODE_PADDING_BUFFLUSH, V4L2_PIX_FMT_H264,        {0x15010000, 0x0}},
 	{SCODE_PADDING_BUFFLUSH, V4L2_PIX_FMT_H264_MVC,    {0x15010000, 0x0}},
+	{SCODE_PADDING_BUFFLUSH, V4L2_PIX_FMT_HEVC,        {0x3e010000, 0x20}},
 };
 
 static const struct malone_padding_scode padding_scode_dft = {0x0, 0x0};
@@ -1062,8 +1077,11 @@ static int vpu_malone_add_padding_scode(struct vpu_buffer *stream_buffer,
 	int ret;
 
 	ps = get_padding_scode(scode_type, pixelformat);
-	if (!ps)
+	if (!ps) {
+		if (scode_type == SCODE_PADDING_BUFFLUSH)
+			return 0;
 		return -EINVAL;
+	}
 
 	wptr = readl(&str_buf->wptr);
 	if (wptr < stream_buffer->phys || wptr > stream_buffer->phys + stream_buffer->length)
@@ -1567,7 +1585,15 @@ static int vpu_malone_input_frame_data(struct vpu_malone_str_buffer __iomem *str
 
 	vpu_malone_update_wptr(str_buf, wptr);
 
-	if (disp_imm && !vpu_vb_is_codecconfig(vbuf)) {
+	/*
+	 * Enable the low latency flush mode if display delay is set to 0
+	 * or the low latency frame flush mode if it is set to 1.
+	 * The low latency flush mode requires some padding data to be appended to each frame,
+	 * but there must not be any padding data between the sequence header and the frame.
+	 * This module is currently only supported for the H264 and HEVC formats,
+	 * for other formats, vpu_malone_add_scode() will return 0.
+	 */
+	if ((disp_imm || low_latency) && !vpu_vb_is_codecconfig(vbuf)) {
 		ret = vpu_malone_add_scode(inst->core->iface,
 					   inst->id,
 					   &inst->stream_buffer,
diff --git a/drivers/media/platform/amphion/vpu_v4l2.c b/drivers/media/platform/amphion/vpu_v4l2.c
index c88738e8fff7..45707931bc4f 100644
--- a/drivers/media/platform/amphion/vpu_v4l2.c
+++ b/drivers/media/platform/amphion/vpu_v4l2.c
@@ -63,6 +63,13 @@ unsigned int vpu_get_buffer_state(struct vb2_v4l2_buffer *vbuf)
 	return vpu_buf->state;
 }
 
+void vpu_set_buffer_average_qp(struct vb2_v4l2_buffer *vbuf, u32 qp)
+{
+	struct vpu_vb2_buffer *vpu_buf = to_vpu_vb2_buffer(vbuf);
+
+	vpu_buf->average_qp = qp;
+}
+
 void vpu_v4l2_set_error(struct vpu_inst *inst)
 {
 	vpu_inst_lock(inst);
@@ -539,6 +546,15 @@ static void vpu_vb2_buf_finish(struct vb2_buffer *vb)
 	struct vpu_inst *inst = vb2_get_drv_priv(vb->vb2_queue);
 	struct vb2_queue *q = vb->vb2_queue;
 
+	if (V4L2_TYPE_IS_CAPTURE(vb->type)) {
+		struct vpu_vb2_buffer *vpu_buf = to_vpu_vb2_buffer(vbuf);
+		struct v4l2_ctrl *ctrl = v4l2_ctrl_find(&inst->ctrl_handler,
+							V4L2_CID_MPEG_VIDEO_AVERAGE_QP);
+
+		if (ctrl)
+			v4l2_ctrl_s_ctrl(ctrl, vpu_buf->average_qp);
+	}
+
 	if (vbuf->flags & V4L2_BUF_FLAG_LAST)
 		vpu_notify_eos(inst);
 
@@ -630,8 +646,6 @@ static const struct vb2_ops vpu_vb2_ops = {
 	.start_streaming    = vpu_vb2_start_streaming,
 	.stop_streaming     = vpu_vb2_stop_streaming,
 	.buf_queue          = vpu_vb2_buf_queue,
-	.wait_prepare       = vb2_ops_wait_prepare,
-	.wait_finish        = vb2_ops_wait_finish,
 };
 
 static int vpu_m2m_queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
@@ -825,6 +839,7 @@ int vpu_add_func(struct vpu_dev *vpu, struct vpu_func *func)
 		vfd->fops = vdec_get_fops();
 		vfd->ioctl_ops = vdec_get_ioctl_ops();
 	}
+	video_set_drvdata(vfd, vpu);
 
 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -1);
 	if (ret) {
@@ -832,7 +847,6 @@ int vpu_add_func(struct vpu_dev *vpu, struct vpu_func *func)
 		v4l2_m2m_release(func->m2m_dev);
 		return ret;
 	}
-	video_set_drvdata(vfd, vpu);
 	func->vfd = vfd;
 
 	ret = v4l2_m2m_register_media_controller(func->m2m_dev, func->vfd, func->function);
diff --git a/drivers/media/platform/amphion/vpu_v4l2.h b/drivers/media/platform/amphion/vpu_v4l2.h
index 60f43056a7a2..56f2939fa84d 100644
--- a/drivers/media/platform/amphion/vpu_v4l2.h
+++ b/drivers/media/platform/amphion/vpu_v4l2.h
@@ -12,6 +12,7 @@ void vpu_inst_lock(struct vpu_inst *inst);
 void vpu_inst_unlock(struct vpu_inst *inst);
 void vpu_set_buffer_state(struct vb2_v4l2_buffer *vbuf, unsigned int state);
 unsigned int vpu_get_buffer_state(struct vb2_v4l2_buffer *vbuf);
+void vpu_set_buffer_average_qp(struct vb2_v4l2_buffer *vbuf, u32 qp);
 
 int vpu_v4l2_open(struct file *file, struct vpu_inst *inst);
 int vpu_v4l2_close(struct file *file);
diff --git a/drivers/media/platform/amphion/vpu_windsor.c b/drivers/media/platform/amphion/vpu_windsor.c
index 5f1101d7cf9e..e7d37aa4b826 100644
--- a/drivers/media/platform/amphion/vpu_windsor.c
+++ b/drivers/media/platform/amphion/vpu_windsor.c
@@ -499,6 +499,7 @@ struct windsor_pic_info {
 	u32 proc_dacc_rng_wr_cnt;
 	s32 tv_s;
 	u32 tv_ns;
+	u32 average_qp;
 };
 
 u32 vpu_windsor_get_data_size(void)
@@ -734,6 +735,7 @@ static void vpu_windsor_unpack_pic_info(struct vpu_rpc_event *pkt, void *data)
 	info->wptr = get_ptr(windsor->str_buff_wptr);
 	info->crc = windsor->frame_crc;
 	info->timestamp = timespec64_to_ns(&ts);
+	info->average_qp = windsor->average_qp;
 }
 
 static void vpu_windsor_unpack_mem_req(struct vpu_rpc_event *pkt, void *data)
diff --git a/drivers/media/platform/aspeed/aspeed-video.c b/drivers/media/platform/aspeed/aspeed-video.c
index fc6050e3be0d..54cae0da9aca 100644
--- a/drivers/media/platform/aspeed/aspeed-video.c
+++ b/drivers/media/platform/aspeed/aspeed-video.c
@@ -1891,8 +1891,6 @@ static void aspeed_video_buf_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops aspeed_video_vb2_ops = {
 	.queue_setup = aspeed_video_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.buf_prepare = aspeed_video_buf_prepare,
 	.start_streaming = aspeed_video_start_streaming,
 	.stop_streaming = aspeed_video_stop_streaming,
@@ -2226,7 +2224,7 @@ static struct platform_driver aspeed_video_driver = {
 		.of_match_table = aspeed_video_of_match,
 	},
 	.probe = aspeed_video_probe,
-	.remove_new = aspeed_video_remove,
+	.remove = aspeed_video_remove,
 };
 
 module_platform_driver(aspeed_video_driver);
diff --git a/drivers/media/platform/atmel/atmel-isi.c b/drivers/media/platform/atmel/atmel-isi.c
index c1108df72dd5..a05a744cbb75 100644
--- a/drivers/media/platform/atmel/atmel-isi.c
+++ b/drivers/media/platform/atmel/atmel-isi.c
@@ -242,7 +242,7 @@ static irqreturn_t isi_interrupt(int irq, void *dev_id)
 #define	WAIT_ISI_DISABLE	0
 static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
 {
-	unsigned long timeout;
+	unsigned long time_left;
 	/*
 	 * The reset or disable will only succeed if we have a
 	 * pixel clock from the camera.
@@ -257,9 +257,9 @@ static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
 		isi_writel(isi, ISI_CTRL, ISI_CTRL_DIS);
 	}
 
-	timeout = wait_for_completion_timeout(&isi->complete,
-			msecs_to_jiffies(500));
-	if (timeout == 0)
+	time_left = wait_for_completion_timeout(&isi->complete,
+						msecs_to_jiffies(500));
+	if (time_left == 0)
 		return -ETIMEDOUT;
 
 	return 0;
@@ -526,8 +526,6 @@ static const struct vb2_ops isi_video_qops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int isi_g_fmt_vid_cap(struct file *file, void *priv,
@@ -1074,16 +1072,12 @@ static int isi_formats_init(struct atmel_isi *isi)
 		return -ENXIO;
 
 	isi->num_user_formats = num_fmts;
-	isi->user_formats = devm_kcalloc(isi->dev,
-					 num_fmts, sizeof(struct isi_format *),
-					 GFP_KERNEL);
+	isi->user_formats = devm_kmemdup_array(isi->dev, isi_fmts, num_fmts,
+					       sizeof(*isi_fmts), GFP_KERNEL);
 	if (!isi->user_formats)
 		return -ENOMEM;
 
-	memcpy(isi->user_formats, isi_fmts,
-	       num_fmts * sizeof(struct isi_format *));
 	isi->current_fmt = isi->user_formats[0];
-
 	return 0;
 }
 
@@ -1367,7 +1361,7 @@ static struct platform_driver atmel_isi_driver = {
 		.pm	= &atmel_isi_dev_pm_ops,
 	},
 	.probe		= atmel_isi_probe,
-	.remove_new	= atmel_isi_remove,
+	.remove		= atmel_isi_remove,
 };
 
 module_platform_driver(atmel_isi_driver);
diff --git a/drivers/media/platform/broadcom/Kconfig b/drivers/media/platform/broadcom/Kconfig
new file mode 100644
index 000000000000..32b76ebfcd9a
--- /dev/null
+++ b/drivers/media/platform/broadcom/Kconfig
@@ -0,0 +1,23 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config VIDEO_BCM2835_UNICAM
+	tristate "Broadcom BCM283x/BCM271x Unicam video capture driver"
+	depends on ARCH_BCM2835 || COMPILE_TEST
+	depends on COMMON_CLK && PM
+	depends on VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  Say Y here to enable support for the BCM283x/BCM271x CSI-2 receiver.
+	  This is a V4L2 driver that controls the CSI-2 receiver directly,
+	  independently from the VC4 firmware.
+
+	  This driver is mutually exclusive with the use of bcm2835-camera. The
+	  firmware will disable all access to the peripheral from within the
+	  firmware if it finds a DT node using it, and bcm2835-camera will
+	  therefore fail to probe.
+
+	  To compile this driver as a module, choose M here. The module will be
+	  called bcm2835-unicam.
diff --git a/drivers/media/platform/broadcom/Makefile b/drivers/media/platform/broadcom/Makefile
new file mode 100644
index 000000000000..03d2045aba2e
--- /dev/null
+++ b/drivers/media/platform/broadcom/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_VIDEO_BCM2835_UNICAM) += bcm2835-unicam.o
diff --git a/drivers/media/platform/broadcom/bcm2835-unicam-regs.h b/drivers/media/platform/broadcom/bcm2835-unicam-regs.h
new file mode 100644
index 000000000000..fce6fa92707c
--- /dev/null
+++ b/drivers/media/platform/broadcom/bcm2835-unicam-regs.h
@@ -0,0 +1,246 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/*
+ * Copyright (C) 2017-2020 Raspberry Pi Trading.
+ * Dave Stevenson <dave.stevenson@raspberrypi.com>
+ */
+
+#ifndef VC4_REGS_UNICAM_H
+#define VC4_REGS_UNICAM_H
+
+#include <linux/bits.h>
+
+/*
+ * The following values are taken from files found within the code drop
+ * made by Broadcom for the BCM21553 Graphics Driver, predominantly in
+ * brcm_usrlib/dag/vmcsx/vcinclude/hardware_vc4.h.
+ * They have been modified to be only the register offset.
+ */
+#define UNICAM_CTRL		0x000
+#define UNICAM_STA		0x004
+#define UNICAM_ANA		0x008
+#define UNICAM_PRI		0x00c
+#define UNICAM_CLK		0x010
+#define UNICAM_CLT		0x014
+#define UNICAM_DAT0		0x018
+#define UNICAM_DAT1		0x01c
+#define UNICAM_DAT2		0x020
+#define UNICAM_DAT3		0x024
+#define UNICAM_DLT		0x028
+#define UNICAM_CMP0		0x02c
+#define UNICAM_CMP1		0x030
+#define UNICAM_CAP0		0x034
+#define UNICAM_CAP1		0x038
+#define UNICAM_ICTL		0x100
+#define UNICAM_ISTA		0x104
+#define UNICAM_IDI0		0x108
+#define UNICAM_IPIPE		0x10c
+#define UNICAM_IBSA0		0x110
+#define UNICAM_IBEA0		0x114
+#define UNICAM_IBLS		0x118
+#define UNICAM_IBWP		0x11c
+#define UNICAM_IHWIN		0x120
+#define UNICAM_IHSTA		0x124
+#define UNICAM_IVWIN		0x128
+#define UNICAM_IVSTA		0x12c
+#define UNICAM_ICC		0x130
+#define UNICAM_ICS		0x134
+#define UNICAM_IDC		0x138
+#define UNICAM_IDPO		0x13c
+#define UNICAM_IDCA		0x140
+#define UNICAM_IDCD		0x144
+#define UNICAM_IDS		0x148
+#define UNICAM_DCS		0x200
+#define UNICAM_DBSA0		0x204
+#define UNICAM_DBEA0		0x208
+#define UNICAM_DBWP		0x20c
+#define UNICAM_DBCTL		0x300
+#define UNICAM_IBSA1		0x304
+#define UNICAM_IBEA1		0x308
+#define UNICAM_IDI1		0x30c
+#define UNICAM_DBSA1		0x310
+#define UNICAM_DBEA1		0x314
+#define UNICAM_MISC		0x400
+
+/*
+ * The following bitmasks are from the kernel released by Broadcom
+ * for Android - https://android.googlesource.com/kernel/bcm/
+ * The Rhea, Hawaii, and Java chips all contain the same VideoCore4
+ * Unicam block as BCM2835, as defined in eg
+ * arch/arm/mach-rhea/include/mach/rdb_A0/brcm_rdb_cam.h and similar.
+ * Values reworked to use the kernel BIT and GENMASK macros.
+ *
+ * Some of the bit mnenomics have been amended to match the datasheet.
+ */
+/* UNICAM_CTRL Register */
+#define UNICAM_CPE		BIT(0)
+#define UNICAM_MEM		BIT(1)
+#define UNICAM_CPR		BIT(2)
+#define UNICAM_CPM_MASK		GENMASK(3, 3)
+#define UNICAM_CPM_CSI2		0
+#define UNICAM_CPM_CCP2		1
+#define UNICAM_SOE		BIT(4)
+#define UNICAM_DCM_MASK		GENMASK(5, 5)
+#define UNICAM_DCM_STROBE	0
+#define UNICAM_DCM_DATA		1
+#define UNICAM_SLS		BIT(6)
+#define UNICAM_PFT_MASK		GENMASK(11, 8)
+#define UNICAM_OET_MASK		GENMASK(20, 12)
+
+/* UNICAM_STA Register */
+#define UNICAM_SYN		BIT(0)
+#define UNICAM_CS		BIT(1)
+#define UNICAM_SBE		BIT(2)
+#define UNICAM_PBE		BIT(3)
+#define UNICAM_HOE		BIT(4)
+#define UNICAM_PLE		BIT(5)
+#define UNICAM_SSC		BIT(6)
+#define UNICAM_CRCE		BIT(7)
+#define UNICAM_OES		BIT(8)
+#define UNICAM_IFO		BIT(9)
+#define UNICAM_OFO		BIT(10)
+#define UNICAM_BFO		BIT(11)
+#define UNICAM_DL		BIT(12)
+#define UNICAM_PS		BIT(13)
+#define UNICAM_IS		BIT(14)
+#define UNICAM_PI0		BIT(15)
+#define UNICAM_PI1		BIT(16)
+#define UNICAM_FSI_S		BIT(17)
+#define UNICAM_FEI_S		BIT(18)
+#define UNICAM_LCI_S		BIT(19)
+#define UNICAM_BUF0_RDY		BIT(20)
+#define UNICAM_BUF0_NO		BIT(21)
+#define UNICAM_BUF1_RDY		BIT(22)
+#define UNICAM_BUF1_NO		BIT(23)
+#define UNICAM_DI		BIT(24)
+
+#define UNICAM_STA_MASK_ALL \
+	(UNICAM_SBE  | UNICAM_PBE | UNICAM_HOE | UNICAM_PLE | UNICAM_SSC | \
+	 UNICAM_CRCE | UNICAM_IFO | UNICAM_OFO | UNICAM_DL  | UNICAM_PS  | \
+	 UNICAM_PI0  | UNICAM_PI1)
+
+/* UNICAM_ANA Register */
+#define UNICAM_APD		BIT(0)
+#define UNICAM_BPD		BIT(1)
+#define UNICAM_AR		BIT(2)
+#define UNICAM_DDL		BIT(3)
+#define UNICAM_CTATADJ_MASK	GENMASK(7, 4)
+#define UNICAM_PTATADJ_MASK	GENMASK(11, 8)
+
+/* UNICAM_PRI Register */
+#define UNICAM_PE		BIT(0)
+#define UNICAM_PT_MASK		GENMASK(2, 1)
+#define UNICAM_NP_MASK		GENMASK(7, 4)
+#define UNICAM_PP_MASK		GENMASK(11, 8)
+#define UNICAM_BS_MASK		GENMASK(15, 12)
+#define UNICAM_BL_MASK		GENMASK(17, 16)
+
+/* UNICAM_CLK Register */
+#define UNICAM_CLE		BIT(0)
+#define UNICAM_CLPD		BIT(1)
+#define UNICAM_CLLPE		BIT(2)
+#define UNICAM_CLHSE		BIT(3)
+#define UNICAM_CLTRE		BIT(4)
+#define UNICAM_CLAC_MASK	GENMASK(8, 5)
+#define UNICAM_CLSTE		BIT(29)
+
+/* UNICAM_CLT Register */
+#define UNICAM_CLT1_MASK	GENMASK(7, 0)
+#define UNICAM_CLT2_MASK	GENMASK(15, 8)
+
+/* UNICAM_DATn Registers */
+#define UNICAM_DLE		BIT(0)
+#define UNICAM_DLPD		BIT(1)
+#define UNICAM_DLLPE		BIT(2)
+#define UNICAM_DLHSE		BIT(3)
+#define UNICAM_DLTRE		BIT(4)
+#define UNICAM_DLSM		BIT(5)
+#define UNICAM_DLFO		BIT(28)
+#define UNICAM_DLSTE		BIT(29)
+
+#define UNICAM_DAT_MASK_ALL	(UNICAM_DLSTE | UNICAM_DLFO)
+
+/* UNICAM_DLT Register */
+#define UNICAM_DLT1_MASK	GENMASK(7, 0)
+#define UNICAM_DLT2_MASK	GENMASK(15, 8)
+#define UNICAM_DLT3_MASK	GENMASK(23, 16)
+
+/* UNICAM_ICTL Register */
+#define UNICAM_FSIE		BIT(0)
+#define UNICAM_FEIE		BIT(1)
+#define UNICAM_IBOB		BIT(2)
+#define UNICAM_FCM		BIT(3)
+#define UNICAM_TFC		BIT(4)
+#define UNICAM_LIP_MASK		GENMASK(6, 5)
+#define UNICAM_LCIE_MASK	GENMASK(28, 16)
+
+/* UNICAM_IDI0/1 Register */
+#define UNICAM_ID0_MASK		GENMASK(7, 0)
+#define UNICAM_ID1_MASK		GENMASK(15, 8)
+#define UNICAM_ID2_MASK		GENMASK(23, 16)
+#define UNICAM_ID3_MASK		GENMASK(31, 24)
+
+/* UNICAM_ISTA Register */
+#define UNICAM_FSI		BIT(0)
+#define UNICAM_FEI		BIT(1)
+#define UNICAM_LCI		BIT(2)
+
+#define UNICAM_ISTA_MASK_ALL	(UNICAM_FSI | UNICAM_FEI | UNICAM_LCI)
+
+/* UNICAM_IPIPE Register */
+#define UNICAM_PUM_MASK		GENMASK(2, 0)
+/* Unpacking modes */
+#define UNICAM_PUM_NONE		0
+#define UNICAM_PUM_UNPACK6	1
+#define UNICAM_PUM_UNPACK7	2
+#define UNICAM_PUM_UNPACK8	3
+#define UNICAM_PUM_UNPACK10	4
+#define UNICAM_PUM_UNPACK12	5
+#define UNICAM_PUM_UNPACK14	6
+#define UNICAM_PUM_UNPACK16	7
+#define UNICAM_DDM_MASK		GENMASK(6, 3)
+#define UNICAM_PPM_MASK		GENMASK(9, 7)
+/* Packing modes */
+#define UNICAM_PPM_NONE		0
+#define UNICAM_PPM_PACK8	1
+#define UNICAM_PPM_PACK10	2
+#define UNICAM_PPM_PACK12	3
+#define UNICAM_PPM_PACK14	4
+#define UNICAM_PPM_PACK16	5
+#define UNICAM_DEM_MASK		GENMASK(11, 10)
+#define UNICAM_DEBL_MASK	GENMASK(14, 12)
+#define UNICAM_ICM_MASK		GENMASK(16, 15)
+#define UNICAM_IDM_MASK		GENMASK(17, 17)
+
+/* UNICAM_ICC Register */
+#define UNICAM_ICFL_MASK	GENMASK(4, 0)
+#define UNICAM_ICFH_MASK	GENMASK(9, 5)
+#define UNICAM_ICST_MASK	GENMASK(12, 10)
+#define UNICAM_ICLT_MASK	GENMASK(15, 13)
+#define UNICAM_ICLL_MASK	GENMASK(31, 16)
+
+/* UNICAM_DCS Register */
+#define UNICAM_DIE		BIT(0)
+#define UNICAM_DIM		BIT(1)
+#define UNICAM_DBOB		BIT(3)
+#define UNICAM_FDE		BIT(4)
+#define UNICAM_LDP		BIT(5)
+#define UNICAM_EDL_MASK		GENMASK(15, 8)
+
+/* UNICAM_DBCTL Register */
+#define UNICAM_DBEN		BIT(0)
+#define UNICAM_BUF0_IE		BIT(1)
+#define UNICAM_BUF1_IE		BIT(2)
+
+/* UNICAM_CMP[0,1] register */
+#define UNICAM_PCE		BIT(31)
+#define UNICAM_GI		BIT(9)
+#define UNICAM_CPH		BIT(8)
+#define UNICAM_PCVC_MASK	GENMASK(7, 6)
+#define UNICAM_PCDT_MASK	GENMASK(5, 0)
+
+/* UNICAM_MISC register */
+#define UNICAM_FL0		BIT(6)
+#define UNICAM_FL1		BIT(9)
+
+#endif
diff --git a/drivers/media/platform/broadcom/bcm2835-unicam.c b/drivers/media/platform/broadcom/bcm2835-unicam.c
new file mode 100644
index 000000000000..f10064107d54
--- /dev/null
+++ b/drivers/media/platform/broadcom/bcm2835-unicam.c
@@ -0,0 +1,2757 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * BCM283x / BCM271x Unicam Capture Driver
+ *
+ * Copyright (C) 2017-2020 - Raspberry Pi (Trading) Ltd.
+ * Copyright (C) 2024 - Ideas on Board
+ *
+ * Dave Stevenson <dave.stevenson@raspberrypi.com>
+ *
+ * Based on TI am437x driver by
+ *   Benoit Parrot <bparrot@ti.com>
+ *   Lad, Prabhakar <prabhakar.csengg@gmail.com>
+ *
+ * and TI CAL camera interface driver by
+ *    Benoit Parrot <bparrot@ti.com>
+ *
+ *
+ * There are two camera drivers in the kernel for BCM283x - this one and
+ * bcm2835-camera (currently in staging).
+ *
+ * This driver directly controls the Unicam peripheral - there is no
+ * involvement with the VideoCore firmware. Unicam receives CSI-2 or CCP2 data
+ * and writes it into SDRAM. The only potential processing options are to
+ * repack Bayer data into an alternate format, and applying windowing. The
+ * repacking does not shift the data, so can repack V4L2_PIX_FMT_Sxxxx10P to
+ * V4L2_PIX_FMT_Sxxxx10, or V4L2_PIX_FMT_Sxxxx12P to V4L2_PIX_FMT_Sxxxx12, but
+ * not generically up to V4L2_PIX_FMT_Sxxxx16. Support for windowing may be
+ * added later.
+ *
+ * It should be possible to connect this driver to any sensor with a suitable
+ * output interface and V4L2 subdevice driver.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-mc.h>
+#include <media/v4l2-subdev.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "bcm2835-unicam-regs.h"
+
+#define UNICAM_MODULE_NAME		"unicam"
+
+/*
+ * Unicam must request a minimum of 250Mhz from the VPU clock.
+ * Otherwise the input FIFOs overrun and cause image corruption.
+ */
+#define UNICAM_MIN_VPU_CLOCK_RATE	(250 * 1000 * 1000)
+
+/* Unicam has an internal DMA alignment constraint of 16 bytes for each line. */
+#define UNICAM_DMA_BPL_ALIGNMENT	16
+
+/*
+ * The image stride is stored in a 16 bit register, and needs to be aligned to
+ * the DMA constraint. As the ISP in the same SoC has a 32 bytes alignment
+ * constraint on its input, set the image stride alignment to 32 bytes here as
+ * well to avoid incompatible configurations.
+ */
+#define UNICAM_IMAGE_BPL_ALIGNMENT	32
+#define UNICAM_IMAGE_MAX_BPL		((1U << 16) - UNICAM_IMAGE_BPL_ALIGNMENT)
+
+/*
+ * Max width is therefore determined by the max stride divided by the number of
+ * bits per pixel. Take 32bpp as a worst case. No imposed limit on the height,
+ * so adopt a square image for want of anything better.
+ */
+#define UNICAM_IMAGE_MIN_WIDTH		16
+#define UNICAM_IMAGE_MIN_HEIGHT		16
+#define UNICAM_IMAGE_MAX_WIDTH		(UNICAM_IMAGE_MAX_BPL / 4)
+#define UNICAM_IMAGE_MAX_HEIGHT		UNICAM_IMAGE_MAX_WIDTH
+
+/*
+ * There's no intrinsic limits on the width and height for embedded data. Use
+ * the same maximum values as for the image, to avoid overflows in the image
+ * size computation.
+ */
+#define UNICAM_META_MIN_WIDTH		1
+#define UNICAM_META_MIN_HEIGHT		1
+#define UNICAM_META_MAX_WIDTH		UNICAM_IMAGE_MAX_WIDTH
+#define UNICAM_META_MAX_HEIGHT		UNICAM_IMAGE_MAX_HEIGHT
+
+/*
+ * Size of the dummy buffer. Can be any size really, but the DMA
+ * allocation works in units of page sizes.
+ */
+#define UNICAM_DUMMY_BUF_SIZE		PAGE_SIZE
+
+enum unicam_pad {
+	UNICAM_SD_PAD_SINK,
+	UNICAM_SD_PAD_SOURCE_IMAGE,
+	UNICAM_SD_PAD_SOURCE_METADATA,
+	UNICAM_SD_NUM_PADS
+};
+
+enum unicam_node_type {
+	UNICAM_IMAGE_NODE,
+	UNICAM_METADATA_NODE,
+	UNICAM_MAX_NODES
+};
+
+/*
+ * struct unicam_format_info - Unicam media bus format information
+ * @fourcc: V4L2 pixel format FCC identifier. 0 if n/a.
+ * @unpacked_fourcc: V4L2 pixel format FCC identifier if the data is expanded
+ * out to 16bpp. 0 if n/a.
+ * @code: V4L2 media bus format code.
+ * @depth: Bits per pixel as delivered from the source.
+ * @csi_dt: CSI data type.
+ * @unpack: PUM value when unpacking to @unpacked_fourcc
+ */
+struct unicam_format_info {
+	u32	fourcc;
+	u32	unpacked_fourcc;
+	u32	code;
+	u8	depth;
+	u8	csi_dt;
+	u8	unpack;
+};
+
+struct unicam_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+	dma_addr_t dma_addr;
+	unsigned int size;
+};
+
+static inline struct unicam_buffer *to_unicam_buffer(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct unicam_buffer, vb.vb2_buf);
+}
+
+struct unicam_node {
+	bool registered;
+	unsigned int id;
+
+	/* Pointer to the current v4l2_buffer */
+	struct unicam_buffer *cur_frm;
+	/* Pointer to the next v4l2_buffer */
+	struct unicam_buffer *next_frm;
+	/* Used to store current pixel format */
+	struct v4l2_format fmt;
+	/* Buffer queue used in video-buf */
+	struct vb2_queue buffer_queue;
+	/* Queue of filled frames */
+	struct list_head dma_queue;
+	/* IRQ lock for DMA queue */
+	spinlock_t dma_queue_lock;
+	/* Identifies video device for this channel */
+	struct video_device video_dev;
+	/* Pointer to the parent handle */
+	struct unicam_device *dev;
+	struct media_pad pad;
+	/*
+	 * Dummy buffer intended to be used by unicam
+	 * if we have no other queued buffers to swap to.
+	 */
+	struct unicam_buffer dummy_buf;
+	void *dummy_buf_cpu_addr;
+};
+
+struct unicam_device {
+	struct kref kref;
+
+	/* peripheral base address */
+	void __iomem *base;
+	/* clock gating base address */
+	void __iomem *clk_gate_base;
+	/* lp clock handle */
+	struct clk *clock;
+	/* vpu clock handle */
+	struct clk *vpu_clock;
+	/* V4l2 device */
+	struct v4l2_device v4l2_dev;
+	struct media_device mdev;
+
+	/* parent device */
+	struct device *dev;
+	/* subdevice async notifier */
+	struct v4l2_async_notifier notifier;
+	unsigned int sequence;
+	bool frame_started;
+
+	/* Sensor node */
+	struct {
+		struct v4l2_subdev *subdev;
+		struct media_pad *pad;
+	} sensor;
+
+	/* Internal subdev */
+	struct {
+		struct v4l2_subdev sd;
+		struct media_pad pads[UNICAM_SD_NUM_PADS];
+		unsigned int enabled_streams;
+	} subdev;
+
+	enum v4l2_mbus_type bus_type;
+	/*
+	 * Stores bus.mipi_csi2.flags for CSI2 sensors, or
+	 * bus.mipi_csi1.strobe for CCP2.
+	 */
+	unsigned int bus_flags;
+	unsigned int max_data_lanes;
+
+	struct {
+		struct media_pipeline pipe;
+		unsigned int num_data_lanes;
+		unsigned int nodes;
+	} pipe;
+
+	/* Lock used for the video devices of both nodes */
+	struct mutex lock;
+	struct unicam_node node[UNICAM_MAX_NODES];
+};
+
+static inline struct unicam_device *
+notifier_to_unicam_device(struct v4l2_async_notifier *notifier)
+{
+	return container_of(notifier, struct unicam_device, notifier);
+}
+
+static inline struct unicam_device *
+sd_to_unicam_device(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct unicam_device, subdev.sd);
+}
+
+static void unicam_release(struct kref *kref)
+{
+	struct unicam_device *unicam =
+		container_of(kref, struct unicam_device, kref);
+
+	if (unicam->mdev.dev)
+		media_device_cleanup(&unicam->mdev);
+
+	mutex_destroy(&unicam->lock);
+	kfree(unicam);
+}
+
+static struct unicam_device *unicam_get(struct unicam_device *unicam)
+{
+	kref_get(&unicam->kref);
+
+	return unicam;
+}
+
+static void unicam_put(struct unicam_device *unicam)
+{
+	kref_put(&unicam->kref, unicam_release);
+}
+
+/* -----------------------------------------------------------------------------
+ * Misc helper functions
+ */
+
+static inline bool unicam_sd_pad_is_source(u32 pad)
+{
+	/* Camera RX has 1 sink pad, and N source pads */
+	return pad != UNICAM_SD_PAD_SINK;
+}
+
+static inline bool is_metadata_node(struct unicam_node *node)
+{
+	return node->video_dev.device_caps & V4L2_CAP_META_CAPTURE;
+}
+
+static inline bool is_image_node(struct unicam_node *node)
+{
+	return node->video_dev.device_caps & V4L2_CAP_VIDEO_CAPTURE;
+}
+
+/* -----------------------------------------------------------------------------
+ * Format data table and helper functions
+ */
+
+static const struct v4l2_mbus_framefmt unicam_default_image_format = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_UYVY8_1X16,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_SRGB,
+	.ycbcr_enc = V4L2_YCBCR_ENC_601,
+	.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+	.xfer_func = V4L2_XFER_FUNC_SRGB,
+	.flags = 0,
+};
+
+static const struct v4l2_mbus_framefmt unicam_default_meta_format = {
+	.width = 640,
+	.height = 2,
+	.code = MEDIA_BUS_FMT_META_8,
+	.field = V4L2_FIELD_NONE,
+};
+
+static const struct unicam_format_info unicam_image_formats[] = {
+	/* YUV Formats */
+	{
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.code		= MEDIA_BUS_FMT_YUYV8_1X16,
+		.depth		= 16,
+		.csi_dt		= MIPI_CSI2_DT_YUV422_8B,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.code		= MEDIA_BUS_FMT_UYVY8_1X16,
+		.depth		= 16,
+		.csi_dt		= MIPI_CSI2_DT_YUV422_8B,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_YVYU,
+		.code		= MEDIA_BUS_FMT_YVYU8_1X16,
+		.depth		= 16,
+		.csi_dt		= MIPI_CSI2_DT_YUV422_8B,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_VYUY,
+		.code		= MEDIA_BUS_FMT_VYUY8_1X16,
+		.depth		= 16,
+		.csi_dt		= MIPI_CSI2_DT_YUV422_8B,
+	}, {
+	/* RGB Formats */
+		.fourcc		= V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.code		= MEDIA_BUS_FMT_RGB565_1X16,
+		.depth		= 16,
+		.csi_dt		= MIPI_CSI2_DT_RGB565,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_RGB24, /* rgb */
+		.code		= MEDIA_BUS_FMT_RGB888_1X24,
+		.depth		= 24,
+		.csi_dt		= MIPI_CSI2_DT_RGB888,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_BGR24, /* bgr */
+		.code		= MEDIA_BUS_FMT_BGR888_1X24,
+		.depth		= 24,
+		.csi_dt		= MIPI_CSI2_DT_RGB888,
+	}, {
+	/* Bayer Formats */
+		.fourcc		= V4L2_PIX_FMT_SBGGR8,
+		.code		= MEDIA_BUS_FMT_SBGGR8_1X8,
+		.depth		= 8,
+		.csi_dt		= MIPI_CSI2_DT_RAW8,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGBRG8,
+		.code		= MEDIA_BUS_FMT_SGBRG8_1X8,
+		.depth		= 8,
+		.csi_dt		= MIPI_CSI2_DT_RAW8,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGRBG8,
+		.code		= MEDIA_BUS_FMT_SGRBG8_1X8,
+		.depth		= 8,
+		.csi_dt		= MIPI_CSI2_DT_RAW8,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SRGGB8,
+		.code		= MEDIA_BUS_FMT_SRGGB8_1X8,
+		.depth		= 8,
+		.csi_dt		= MIPI_CSI2_DT_RAW8,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SBGGR10P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SBGGR10,
+		.code		= MEDIA_BUS_FMT_SBGGR10_1X10,
+		.depth		= 10,
+		.csi_dt		= MIPI_CSI2_DT_RAW10,
+		.unpack		= UNICAM_PUM_UNPACK10,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGBRG10P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGBRG10,
+		.code		= MEDIA_BUS_FMT_SGBRG10_1X10,
+		.depth		= 10,
+		.csi_dt		= MIPI_CSI2_DT_RAW10,
+		.unpack		= UNICAM_PUM_UNPACK10,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGRBG10P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGRBG10,
+		.code		= MEDIA_BUS_FMT_SGRBG10_1X10,
+		.depth		= 10,
+		.csi_dt		= MIPI_CSI2_DT_RAW10,
+		.unpack		= UNICAM_PUM_UNPACK10,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SRGGB10P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SRGGB10,
+		.code		= MEDIA_BUS_FMT_SRGGB10_1X10,
+		.depth		= 10,
+		.csi_dt		= MIPI_CSI2_DT_RAW10,
+		.unpack		= UNICAM_PUM_UNPACK10,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SBGGR12P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SBGGR12,
+		.code		= MEDIA_BUS_FMT_SBGGR12_1X12,
+		.depth		= 12,
+		.csi_dt		= MIPI_CSI2_DT_RAW12,
+		.unpack		= UNICAM_PUM_UNPACK12,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGBRG12P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGBRG12,
+		.code		= MEDIA_BUS_FMT_SGBRG12_1X12,
+		.depth		= 12,
+		.csi_dt		= MIPI_CSI2_DT_RAW12,
+		.unpack		= UNICAM_PUM_UNPACK12,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGRBG12P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGRBG12,
+		.code		= MEDIA_BUS_FMT_SGRBG12_1X12,
+		.depth		= 12,
+		.csi_dt		= MIPI_CSI2_DT_RAW12,
+		.unpack		= UNICAM_PUM_UNPACK12,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SRGGB12P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SRGGB12,
+		.code		= MEDIA_BUS_FMT_SRGGB12_1X12,
+		.depth		= 12,
+		.csi_dt		= MIPI_CSI2_DT_RAW12,
+		.unpack		= UNICAM_PUM_UNPACK12,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SBGGR14P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SBGGR14,
+		.code		= MEDIA_BUS_FMT_SBGGR14_1X14,
+		.depth		= 14,
+		.csi_dt		= MIPI_CSI2_DT_RAW14,
+		.unpack		= UNICAM_PUM_UNPACK14,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGBRG14P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGBRG14,
+		.code		= MEDIA_BUS_FMT_SGBRG14_1X14,
+		.depth		= 14,
+		.csi_dt		= MIPI_CSI2_DT_RAW14,
+		.unpack		= UNICAM_PUM_UNPACK14,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SGRBG14P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SGRBG14,
+		.code		= MEDIA_BUS_FMT_SGRBG14_1X14,
+		.depth		= 14,
+		.csi_dt		= MIPI_CSI2_DT_RAW14,
+		.unpack		= UNICAM_PUM_UNPACK14,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_SRGGB14P,
+		.unpacked_fourcc = V4L2_PIX_FMT_SRGGB14,
+		.code		= MEDIA_BUS_FMT_SRGGB14_1X14,
+		.depth		= 14,
+		.csi_dt		= MIPI_CSI2_DT_RAW14,
+		.unpack		= UNICAM_PUM_UNPACK14,
+	}, {
+	/* 16 bit Bayer formats could be supported. */
+
+	/* Greyscale formats */
+		.fourcc		= V4L2_PIX_FMT_GREY,
+		.code		= MEDIA_BUS_FMT_Y8_1X8,
+		.depth		= 8,
+		.csi_dt		= MIPI_CSI2_DT_RAW8,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_Y10P,
+		.unpacked_fourcc = V4L2_PIX_FMT_Y10,
+		.code		= MEDIA_BUS_FMT_Y10_1X10,
+		.depth		= 10,
+		.csi_dt		= MIPI_CSI2_DT_RAW10,
+		.unpack		= UNICAM_PUM_UNPACK10,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_Y12P,
+		.unpacked_fourcc = V4L2_PIX_FMT_Y12,
+		.code		= MEDIA_BUS_FMT_Y12_1X12,
+		.depth		= 12,
+		.csi_dt		= MIPI_CSI2_DT_RAW12,
+		.unpack		= UNICAM_PUM_UNPACK12,
+	}, {
+		.fourcc		= V4L2_PIX_FMT_Y14P,
+		.unpacked_fourcc = V4L2_PIX_FMT_Y14,
+		.code		= MEDIA_BUS_FMT_Y14_1X14,
+		.depth		= 14,
+		.csi_dt		= MIPI_CSI2_DT_RAW14,
+		.unpack		= UNICAM_PUM_UNPACK14,
+	},
+};
+
+static const struct unicam_format_info unicam_meta_formats[] = {
+	{
+		.fourcc		= V4L2_META_FMT_GENERIC_8,
+		.code		= MEDIA_BUS_FMT_META_8,
+		.depth		= 8,
+	}, {
+		.fourcc		= V4L2_META_FMT_GENERIC_CSI2_10,
+		.code		= MEDIA_BUS_FMT_META_10,
+		.depth		= 10,
+	}, {
+		.fourcc		= V4L2_META_FMT_GENERIC_CSI2_12,
+		.code		= MEDIA_BUS_FMT_META_12,
+		.depth		= 12,
+	}, {
+		.fourcc		= V4L2_META_FMT_GENERIC_CSI2_14,
+		.code		= MEDIA_BUS_FMT_META_14,
+		.depth		= 14,
+	},
+};
+
+/* Format setup functions */
+static const struct unicam_format_info *
+unicam_find_format_by_code(u32 code, u32 pad)
+{
+	const struct unicam_format_info *formats;
+	unsigned int num_formats;
+	unsigned int i;
+
+	if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {
+		formats = unicam_image_formats;
+		num_formats = ARRAY_SIZE(unicam_image_formats);
+	} else {
+		formats = unicam_meta_formats;
+		num_formats = ARRAY_SIZE(unicam_meta_formats);
+	}
+
+	for (i = 0; i < num_formats; i++) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+static const struct unicam_format_info *
+unicam_find_format_by_fourcc(u32 fourcc, u32 pad)
+{
+	const struct unicam_format_info *formats;
+	unsigned int num_formats;
+	unsigned int i;
+
+	if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {
+		formats = unicam_image_formats;
+		num_formats = ARRAY_SIZE(unicam_image_formats);
+	} else {
+		formats = unicam_meta_formats;
+		num_formats = ARRAY_SIZE(unicam_meta_formats);
+	}
+
+	for (i = 0; i < num_formats; ++i) {
+		if (formats[i].fourcc == fourcc ||
+		    formats[i].unpacked_fourcc == fourcc)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+static void unicam_calc_image_size_bpl(struct unicam_device *unicam,
+				       const struct unicam_format_info *fmtinfo,
+				       struct v4l2_pix_format *pix)
+{
+	u32 min_bpl;
+
+	v4l_bound_align_image(&pix->width, UNICAM_IMAGE_MIN_WIDTH,
+			      UNICAM_IMAGE_MAX_WIDTH, 2,
+			      &pix->height, UNICAM_IMAGE_MIN_HEIGHT,
+			      UNICAM_IMAGE_MAX_HEIGHT, 0, 0);
+
+	/* Unpacking always goes to 16bpp */
+	if (pix->pixelformat == fmtinfo->unpacked_fourcc)
+		min_bpl = pix->width * 2;
+	else
+		min_bpl = pix->width * fmtinfo->depth / 8;
+	min_bpl = ALIGN(min_bpl, UNICAM_IMAGE_BPL_ALIGNMENT);
+
+	pix->bytesperline = ALIGN(pix->bytesperline, UNICAM_IMAGE_BPL_ALIGNMENT);
+	pix->bytesperline = clamp_t(unsigned int, pix->bytesperline, min_bpl,
+				    UNICAM_IMAGE_MAX_BPL);
+
+	pix->sizeimage = pix->height * pix->bytesperline;
+}
+
+static void unicam_calc_meta_size_bpl(struct unicam_device *unicam,
+				      const struct unicam_format_info *fmtinfo,
+				      struct v4l2_meta_format *meta)
+{
+	v4l_bound_align_image(&meta->width, UNICAM_META_MIN_WIDTH,
+			      UNICAM_META_MAX_WIDTH, 0,
+			      &meta->height, UNICAM_META_MIN_HEIGHT,
+			      UNICAM_META_MAX_HEIGHT, 0, 0);
+
+	meta->bytesperline = ALIGN(meta->width * fmtinfo->depth / 8,
+				   UNICAM_DMA_BPL_ALIGNMENT);
+	meta->buffersize = meta->height * meta->bytesperline;
+}
+
+/* -----------------------------------------------------------------------------
+ * Hardware handling
+ */
+
+static inline void unicam_clk_write(struct unicam_device *unicam, u32 val)
+{
+	/* Pass the CM_PASSWORD along with the value. */
+	writel(val | 0x5a000000, unicam->clk_gate_base);
+}
+
+static inline u32 unicam_reg_read(struct unicam_device *unicam, u32 offset)
+{
+	return readl(unicam->base + offset);
+}
+
+static inline void unicam_reg_write(struct unicam_device *unicam, u32 offset, u32 val)
+{
+	writel(val, unicam->base + offset);
+}
+
+static inline int unicam_get_field(u32 value, u32 mask)
+{
+	return (value & mask) >> __ffs(mask);
+}
+
+static inline void unicam_set_field(u32 *valp, u32 field, u32 mask)
+{
+	u32 val = *valp;
+
+	val &= ~mask;
+	val |= (field << __ffs(mask)) & mask;
+	*valp = val;
+}
+
+static inline void unicam_reg_write_field(struct unicam_device *unicam, u32 offset,
+					  u32 field, u32 mask)
+{
+	u32 val = unicam_reg_read(unicam, offset);
+
+	unicam_set_field(&val, field, mask);
+	unicam_reg_write(unicam, offset, val);
+}
+
+static void unicam_wr_dma_addr(struct unicam_node *node,
+			       struct unicam_buffer *buf)
+{
+	/*
+	 * Due to a HW bug causing buffer overruns in circular buffer mode under
+	 * certain (not yet fully known) conditions, the dummy buffer allocation
+	 * is set to a a single page size, but the hardware gets programmed with
+	 * a buffer size of 0.
+	 */
+	dma_addr_t endaddr = buf->dma_addr +
+			     (buf != &node->dummy_buf ? buf->size : 0);
+
+	if (node->id == UNICAM_IMAGE_NODE) {
+		unicam_reg_write(node->dev, UNICAM_IBSA0, buf->dma_addr);
+		unicam_reg_write(node->dev, UNICAM_IBEA0, endaddr);
+	} else {
+		unicam_reg_write(node->dev, UNICAM_DBSA0, buf->dma_addr);
+		unicam_reg_write(node->dev, UNICAM_DBEA0, endaddr);
+	}
+}
+
+static unsigned int unicam_get_lines_done(struct unicam_device *unicam)
+{
+	struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];
+	unsigned int stride = node->fmt.fmt.pix.bytesperline;
+	struct unicam_buffer *frm = node->cur_frm;
+	dma_addr_t cur_addr;
+
+	if (!frm)
+		return 0;
+
+	cur_addr = unicam_reg_read(unicam, UNICAM_IBWP);
+	return (unsigned int)(cur_addr - frm->dma_addr) / stride;
+}
+
+static void unicam_schedule_next_buffer(struct unicam_node *node)
+{
+	struct unicam_buffer *buf;
+
+	buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list);
+	node->next_frm = buf;
+	list_del(&buf->list);
+
+	unicam_wr_dma_addr(node, buf);
+}
+
+static void unicam_schedule_dummy_buffer(struct unicam_node *node)
+{
+	int node_id = is_image_node(node) ? UNICAM_IMAGE_NODE : UNICAM_METADATA_NODE;
+
+	dev_dbg(node->dev->dev, "Scheduling dummy buffer for node %d\n", node_id);
+
+	unicam_wr_dma_addr(node, &node->dummy_buf);
+
+	node->next_frm = NULL;
+}
+
+static void unicam_process_buffer_complete(struct unicam_node *node,
+					   unsigned int sequence)
+{
+	node->cur_frm->vb.field = node->fmt.fmt.pix.field;
+	node->cur_frm->vb.sequence = sequence;
+
+	vb2_buffer_done(&node->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+static void unicam_queue_event_sof(struct unicam_device *unicam)
+{
+	struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+		.u.frame_sync.frame_sequence = unicam->sequence,
+	};
+
+	v4l2_event_queue(&node->video_dev, &event);
+}
+
+static irqreturn_t unicam_isr(int irq, void *dev)
+{
+	struct unicam_device *unicam = dev;
+	unsigned int lines_done = unicam_get_lines_done(dev);
+	unsigned int sequence = unicam->sequence;
+	unsigned int i;
+	u32 ista, sta;
+	bool fe;
+	u64 ts;
+
+	sta = unicam_reg_read(unicam, UNICAM_STA);
+	/* Write value back to clear the interrupts */
+	unicam_reg_write(unicam, UNICAM_STA, sta);
+
+	ista = unicam_reg_read(unicam, UNICAM_ISTA);
+	/* Write value back to clear the interrupts */
+	unicam_reg_write(unicam, UNICAM_ISTA, ista);
+
+	dev_dbg(unicam->dev, "ISR: ISTA: 0x%X, STA: 0x%X, sequence %d, lines done %d\n",
+		ista, sta, sequence, lines_done);
+
+	if (!(sta & (UNICAM_IS | UNICAM_PI0)))
+		return IRQ_HANDLED;
+
+	/*
+	 * Look for either the Frame End interrupt or the Packet Capture status
+	 * to signal a frame end.
+	 */
+	fe = ista & UNICAM_FEI || sta & UNICAM_PI0;
+
+	/*
+	 * We must run the frame end handler first. If we have a valid next_frm
+	 * and we get a simultaneout FE + FS interrupt, running the FS handler
+	 * first would null out the next_frm ptr and we would have lost the
+	 * buffer forever.
+	 */
+	if (fe) {
+		bool inc_seq = unicam->frame_started;
+
+		/*
+		 * Ensure we have swapped buffers already as we can't
+		 * stop the peripheral. If no buffer is available, use a
+		 * dummy buffer to dump out frames until we get a new buffer
+		 * to use.
+		 */
+		for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
+			struct unicam_node *node = &unicam->node[i];
+
+			if (!vb2_start_streaming_called(&node->buffer_queue))
+				continue;
+
+			/*
+			 * If cur_frm == next_frm, it means we have not had
+			 * a chance to swap buffers, likely due to having
+			 * multiple interrupts occurring simultaneously (like FE
+			 * + FS + LS). In this case, we cannot signal the buffer
+			 * as complete, as the HW will reuse that buffer.
+			 */
+			if (node->cur_frm && node->cur_frm != node->next_frm) {
+				unicam_process_buffer_complete(node, sequence);
+				inc_seq = true;
+			}
+			node->cur_frm = node->next_frm;
+		}
+
+		/*
+		 * Increment the sequence number conditionally on either a FS
+		 * having already occurred, or in the FE + FS condition as
+		 * caught in the FE handler above. This ensures the sequence
+		 * number corresponds to the frames generated by the sensor, not
+		 * the frames dequeued to userland.
+		 */
+		if (inc_seq) {
+			unicam->sequence++;
+			unicam->frame_started = false;
+		}
+	}
+
+	if (ista & UNICAM_FSI) {
+		/*
+		 * Timestamp is to be when the first data byte was captured,
+		 * aka frame start.
+		 */
+		ts = ktime_get_ns();
+		for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
+			struct unicam_node *node = &unicam->node[i];
+
+			if (!vb2_start_streaming_called(&node->buffer_queue))
+				continue;
+
+			if (node->cur_frm)
+				node->cur_frm->vb.vb2_buf.timestamp = ts;
+			else
+				dev_dbg(unicam->v4l2_dev.dev,
+					"ISR: [%d] Dropping frame, buffer not available at FS\n",
+					i);
+			/*
+			 * Set the next frame output to go to a dummy frame
+			 * if we have not managed to obtain another frame
+			 * from the queue.
+			 */
+			unicam_schedule_dummy_buffer(node);
+		}
+
+		unicam_queue_event_sof(unicam);
+		unicam->frame_started = true;
+	}
+
+	/*
+	 * Cannot swap buffer at frame end, there may be a race condition
+	 * where the HW does not actually swap it if the new frame has
+	 * already started.
+	 */
+	if (ista & (UNICAM_FSI | UNICAM_LCI) && !fe) {
+		for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
+			struct unicam_node *node = &unicam->node[i];
+
+			if (!vb2_start_streaming_called(&node->buffer_queue))
+				continue;
+
+			spin_lock(&node->dma_queue_lock);
+			if (!list_empty(&node->dma_queue) && !node->next_frm)
+				unicam_schedule_next_buffer(node);
+			spin_unlock(&node->dma_queue_lock);
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void unicam_set_packing_config(struct unicam_device *unicam,
+				      const struct unicam_format_info *fmtinfo)
+{
+	struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];
+	u32 pack, unpack;
+	u32 val;
+
+	if (node->fmt.fmt.pix.pixelformat == fmtinfo->fourcc) {
+		unpack = UNICAM_PUM_NONE;
+		pack = UNICAM_PPM_NONE;
+	} else {
+		unpack = fmtinfo->unpack;
+		/* Repacking is always to 16bpp */
+		pack = UNICAM_PPM_PACK16;
+	}
+
+	val = 0;
+	unicam_set_field(&val, unpack, UNICAM_PUM_MASK);
+	unicam_set_field(&val, pack, UNICAM_PPM_MASK);
+	unicam_reg_write(unicam, UNICAM_IPIPE, val);
+}
+
+static void unicam_cfg_image_id(struct unicam_device *unicam, u8 vc, u8 dt)
+{
+	if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {
+		/* CSI2 mode  */
+		unicam_reg_write(unicam, UNICAM_IDI0, (vc << 6) | dt);
+	} else {
+		/* CCP2 mode */
+		unicam_reg_write(unicam, UNICAM_IDI0, 0x80 | dt);
+	}
+}
+
+static void unicam_enable_ed(struct unicam_device *unicam)
+{
+	u32 val = unicam_reg_read(unicam, UNICAM_DCS);
+
+	unicam_set_field(&val, 2, UNICAM_EDL_MASK);
+	/* Do not wrap at the end of the embedded data buffer */
+	unicam_set_field(&val, 0, UNICAM_DBOB);
+
+	unicam_reg_write(unicam, UNICAM_DCS, val);
+}
+
+static int unicam_get_image_vc_dt(struct unicam_device *unicam,
+				  struct v4l2_subdev_state *state,
+				  u8 *vc, u8 *dt)
+{
+	struct v4l2_mbus_frame_desc fd;
+	u32 stream;
+	int ret;
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+						    UNICAM_SD_PAD_SOURCE_IMAGE,
+						    0, NULL, &stream);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_frame_desc,
+			       unicam->sensor.pad->index, &fd);
+	if (ret)
+		return ret;
+
+	/* Only CSI-2 supports DTs. */
+	if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2)
+		return -EINVAL;
+
+	for (unsigned int i = 0; i < fd.num_entries; ++i) {
+		const struct v4l2_mbus_frame_desc_entry *fde = &fd.entry[i];
+
+		if (fde->stream == stream) {
+			*vc = fde->bus.csi2.vc;
+			*dt = fde->bus.csi2.dt;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void unicam_start_rx(struct unicam_device *unicam,
+			    struct v4l2_subdev_state *state)
+{
+	struct unicam_node *node = &unicam->node[UNICAM_IMAGE_NODE];
+	const struct unicam_format_info *fmtinfo;
+	const struct v4l2_mbus_framefmt *fmt;
+	unsigned int line_int_freq;
+	u8 vc, dt;
+	u32 val;
+	int ret;
+
+	fmt = v4l2_subdev_state_get_format(state, UNICAM_SD_PAD_SOURCE_IMAGE, 0);
+	fmtinfo = unicam_find_format_by_code(fmt->code,
+					     UNICAM_SD_PAD_SOURCE_IMAGE);
+	if (WARN_ON(!fmtinfo))
+		return;
+
+	/*
+	 * Enable lane clocks. The register is structured as follows:
+	 *
+	 * [9:8] - DAT3
+	 * [7:6] - DAT2
+	 * [5:4] - DAT1
+	 * [3:2] - DAT0
+	 * [1:0] - CLK
+	 *
+	 * Enabled lane must be set to b01, and disabled lanes to b00. The clock
+	 * lane is always enabled.
+	 */
+	val = 0x155 & GENMASK(unicam->pipe.num_data_lanes * 2 + 1, 0);
+	unicam_clk_write(unicam, val);
+
+	/* Basic init */
+	unicam_reg_write(unicam, UNICAM_CTRL, UNICAM_MEM);
+
+	/* Enable analogue control, and leave in reset. */
+	val = UNICAM_AR;
+	unicam_set_field(&val, 7, UNICAM_CTATADJ_MASK);
+	unicam_set_field(&val, 7, UNICAM_PTATADJ_MASK);
+	unicam_reg_write(unicam, UNICAM_ANA, val);
+	usleep_range(1000, 2000);
+
+	/* Come out of reset */
+	unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_AR);
+
+	/* Peripheral reset */
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR);
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR);
+
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE);
+
+	/* Enable Rx control. */
+	val = unicam_reg_read(unicam, UNICAM_CTRL);
+	if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {
+		unicam_set_field(&val, UNICAM_CPM_CSI2, UNICAM_CPM_MASK);
+		unicam_set_field(&val, UNICAM_DCM_STROBE, UNICAM_DCM_MASK);
+	} else {
+		unicam_set_field(&val, UNICAM_CPM_CCP2, UNICAM_CPM_MASK);
+		unicam_set_field(&val, unicam->bus_flags, UNICAM_DCM_MASK);
+	}
+	/* Packet framer timeout */
+	unicam_set_field(&val, 0xf, UNICAM_PFT_MASK);
+	unicam_set_field(&val, 128, UNICAM_OET_MASK);
+	unicam_reg_write(unicam, UNICAM_CTRL, val);
+
+	unicam_reg_write(unicam, UNICAM_IHWIN, 0);
+	unicam_reg_write(unicam, UNICAM_IVWIN, 0);
+
+	/* AXI bus access QoS setup */
+	val = unicam_reg_read(unicam, UNICAM_PRI);
+	unicam_set_field(&val, 0, UNICAM_BL_MASK);
+	unicam_set_field(&val, 0, UNICAM_BS_MASK);
+	unicam_set_field(&val, 0xe, UNICAM_PP_MASK);
+	unicam_set_field(&val, 8, UNICAM_NP_MASK);
+	unicam_set_field(&val, 2, UNICAM_PT_MASK);
+	unicam_set_field(&val, 1, UNICAM_PE);
+	unicam_reg_write(unicam, UNICAM_PRI, val);
+
+	unicam_reg_write_field(unicam, UNICAM_ANA, 0, UNICAM_DDL);
+
+	val = UNICAM_FSIE | UNICAM_FEIE | UNICAM_IBOB;
+	line_int_freq = max(fmt->height >> 2, 128);
+	unicam_set_field(&val, line_int_freq, UNICAM_LCIE_MASK);
+	unicam_reg_write(unicam, UNICAM_ICTL, val);
+	unicam_reg_write(unicam, UNICAM_STA, UNICAM_STA_MASK_ALL);
+	unicam_reg_write(unicam, UNICAM_ISTA, UNICAM_ISTA_MASK_ALL);
+
+	/* tclk_term_en */
+	unicam_reg_write_field(unicam, UNICAM_CLT, 2, UNICAM_CLT1_MASK);
+	/* tclk_settle */
+	unicam_reg_write_field(unicam, UNICAM_CLT, 6, UNICAM_CLT2_MASK);
+	/* td_term_en */
+	unicam_reg_write_field(unicam, UNICAM_DLT, 2, UNICAM_DLT1_MASK);
+	/* ths_settle */
+	unicam_reg_write_field(unicam, UNICAM_DLT, 6, UNICAM_DLT2_MASK);
+	/* trx_enable */
+	unicam_reg_write_field(unicam, UNICAM_DLT, 0, UNICAM_DLT3_MASK);
+
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_SOE);
+
+	/* Packet compare setup - required to avoid missing frame ends */
+	val = 0;
+	unicam_set_field(&val, 1, UNICAM_PCE);
+	unicam_set_field(&val, 1, UNICAM_GI);
+	unicam_set_field(&val, 1, UNICAM_CPH);
+	unicam_set_field(&val, 0, UNICAM_PCVC_MASK);
+	unicam_set_field(&val, 1, UNICAM_PCDT_MASK);
+	unicam_reg_write(unicam, UNICAM_CMP0, val);
+
+	/* Enable clock lane and set up terminations */
+	val = 0;
+	if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {
+		/* CSI2 */
+		unicam_set_field(&val, 1, UNICAM_CLE);
+		unicam_set_field(&val, 1, UNICAM_CLLPE);
+		if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) {
+			unicam_set_field(&val, 1, UNICAM_CLTRE);
+			unicam_set_field(&val, 1, UNICAM_CLHSE);
+		}
+	} else {
+		/* CCP2 */
+		unicam_set_field(&val, 1, UNICAM_CLE);
+		unicam_set_field(&val, 1, UNICAM_CLHSE);
+		unicam_set_field(&val, 1, UNICAM_CLTRE);
+	}
+	unicam_reg_write(unicam, UNICAM_CLK, val);
+
+	/*
+	 * Enable required data lanes with appropriate terminations.
+	 * The same value needs to be written to UNICAM_DATn registers for
+	 * the active lanes, and 0 for inactive ones.
+	 */
+	val = 0;
+	if (unicam->bus_type == V4L2_MBUS_CSI2_DPHY) {
+		/* CSI2 */
+		unicam_set_field(&val, 1, UNICAM_DLE);
+		unicam_set_field(&val, 1, UNICAM_DLLPE);
+		if (!(unicam->bus_flags & V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK)) {
+			unicam_set_field(&val, 1, UNICAM_DLTRE);
+			unicam_set_field(&val, 1, UNICAM_DLHSE);
+		}
+	} else {
+		/* CCP2 */
+		unicam_set_field(&val, 1, UNICAM_DLE);
+		unicam_set_field(&val, 1, UNICAM_DLHSE);
+		unicam_set_field(&val, 1, UNICAM_DLTRE);
+	}
+	unicam_reg_write(unicam, UNICAM_DAT0, val);
+
+	if (unicam->pipe.num_data_lanes == 1)
+		val = 0;
+	unicam_reg_write(unicam, UNICAM_DAT1, val);
+
+	if (unicam->max_data_lanes > 2) {
+		/*
+		 * Registers UNICAM_DAT2 and UNICAM_DAT3 only valid if the
+		 * instance supports more than 2 data lanes.
+		 */
+		if (unicam->pipe.num_data_lanes == 2)
+			val = 0;
+		unicam_reg_write(unicam, UNICAM_DAT2, val);
+
+		if (unicam->pipe.num_data_lanes == 3)
+			val = 0;
+		unicam_reg_write(unicam, UNICAM_DAT3, val);
+	}
+
+	unicam_reg_write(unicam, UNICAM_IBLS,
+			 node->fmt.fmt.pix.bytesperline);
+	unicam_wr_dma_addr(node, node->cur_frm);
+	unicam_set_packing_config(unicam, fmtinfo);
+
+	ret = unicam_get_image_vc_dt(unicam, state, &vc, &dt);
+	if (ret) {
+		/*
+		 * If the source doesn't support frame descriptors, default to
+		 * VC 0 and use the DT corresponding to the format.
+		 */
+		vc = 0;
+		dt = fmtinfo->csi_dt;
+	}
+
+	unicam_cfg_image_id(unicam, vc, dt);
+
+	val = unicam_reg_read(unicam, UNICAM_MISC);
+	unicam_set_field(&val, 1, UNICAM_FL0);
+	unicam_set_field(&val, 1, UNICAM_FL1);
+	unicam_reg_write(unicam, UNICAM_MISC, val);
+
+	/* Enable peripheral */
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPE);
+
+	/* Load image pointers */
+	unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_LIP_MASK);
+
+	/*
+	 * Enable trigger only for the first frame to
+	 * sync correctly to the FS from the source.
+	 */
+	unicam_reg_write_field(unicam, UNICAM_ICTL, 1, UNICAM_TFC);
+}
+
+static void unicam_start_metadata(struct unicam_device *unicam)
+{
+	struct unicam_node *node = &unicam->node[UNICAM_METADATA_NODE];
+
+	unicam_enable_ed(unicam);
+	unicam_wr_dma_addr(node, node->cur_frm);
+	unicam_reg_write_field(unicam, UNICAM_DCS, 1, UNICAM_LDP);
+}
+
+static void unicam_disable(struct unicam_device *unicam)
+{
+	/* Analogue lane control disable */
+	unicam_reg_write_field(unicam, UNICAM_ANA, 1, UNICAM_DDL);
+
+	/* Stop the output engine */
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_SOE);
+
+	/* Disable the data lanes. */
+	unicam_reg_write(unicam, UNICAM_DAT0, 0);
+	unicam_reg_write(unicam, UNICAM_DAT1, 0);
+
+	if (unicam->max_data_lanes > 2) {
+		unicam_reg_write(unicam, UNICAM_DAT2, 0);
+		unicam_reg_write(unicam, UNICAM_DAT3, 0);
+	}
+
+	/* Peripheral reset */
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 1, UNICAM_CPR);
+	usleep_range(50, 100);
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPR);
+
+	/* Disable peripheral */
+	unicam_reg_write_field(unicam, UNICAM_CTRL, 0, UNICAM_CPE);
+
+	/* Clear ED setup */
+	unicam_reg_write(unicam, UNICAM_DCS, 0);
+
+	/* Disable all lane clocks */
+	unicam_clk_write(unicam, 0);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 subdev operations
+ */
+
+static int __unicam_subdev_set_routing(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       struct v4l2_subdev_krouting *routing)
+{
+	struct v4l2_subdev_route *route;
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing(sd, state, routing);
+	if (ret)
+		return ret;
+
+	for_each_active_route(&state->routing, route) {
+		const struct v4l2_mbus_framefmt *def_fmt;
+		struct v4l2_mbus_framefmt *fmt;
+
+		if (route->source_pad == UNICAM_SD_PAD_SOURCE_IMAGE)
+			def_fmt = &unicam_default_image_format;
+		else
+			def_fmt = &unicam_default_meta_format;
+
+		fmt = v4l2_subdev_state_get_format(state, route->sink_pad,
+						   route->sink_stream);
+		*fmt = *def_fmt;
+		fmt = v4l2_subdev_state_get_format(state, route->source_pad,
+						   route->source_stream);
+		*fmt = *def_fmt;
+	}
+
+	return 0;
+}
+
+static int unicam_subdev_init_state(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = {
+		{
+			.sink_pad = UNICAM_SD_PAD_SINK,
+			.sink_stream = 0,
+			.source_pad = UNICAM_SD_PAD_SOURCE_IMAGE,
+			.source_stream = 0,
+			.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+		},
+	};
+
+	struct v4l2_subdev_krouting routing = {
+		.len_routes = ARRAY_SIZE(routes),
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	/* Initialize routing to single route to the fist source pad. */
+	return __unicam_subdev_set_routing(sd, state, &routing);
+}
+
+static int unicam_subdev_enum_mbus_code(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *state,
+					struct v4l2_subdev_mbus_code_enum *code)
+{
+	u32 pad, stream;
+	int ret;
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+						    code->pad, code->stream,
+						    &pad, &stream);
+	if (ret)
+		return ret;
+
+	if (unicam_sd_pad_is_source(code->pad)) {
+		/* No transcoding, source and sink codes must match. */
+		const struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_state_get_format(state, pad, stream);
+		if (!fmt)
+			return -EINVAL;
+
+		if (code->index > 0)
+			return -EINVAL;
+
+		code->code = fmt->code;
+	} else {
+		const struct unicam_format_info *formats;
+		unsigned int num_formats;
+
+		if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {
+			formats = unicam_image_formats;
+			num_formats = ARRAY_SIZE(unicam_image_formats);
+		} else {
+			formats = unicam_meta_formats;
+			num_formats = ARRAY_SIZE(unicam_meta_formats);
+		}
+
+		if (code->index >= num_formats)
+			return -EINVAL;
+
+		code->code = formats[code->index].code;
+	}
+
+	return 0;
+}
+
+static int unicam_subdev_enum_frame_size(struct v4l2_subdev *sd,
+					 struct v4l2_subdev_state *state,
+					 struct v4l2_subdev_frame_size_enum *fse)
+{
+	u32 pad, stream;
+	int ret;
+
+	if (fse->index > 0)
+		return -EINVAL;
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing, fse->pad,
+						    fse->stream, &pad,
+						    &stream);
+	if (ret)
+		return ret;
+
+	if (unicam_sd_pad_is_source(fse->pad)) {
+		/* No transcoding, source and sink formats must match. */
+		const struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_state_get_format(state, pad, stream);
+		if (!fmt)
+			return -EINVAL;
+
+		if (fse->code != fmt->code)
+			return -EINVAL;
+
+		fse->min_width = fmt->width;
+		fse->max_width = fmt->width;
+		fse->min_height = fmt->height;
+		fse->max_height = fmt->height;
+	} else {
+		const struct unicam_format_info *fmtinfo;
+
+		fmtinfo = unicam_find_format_by_code(fse->code, pad);
+		if (!fmtinfo)
+			return -EINVAL;
+
+		if (pad == UNICAM_SD_PAD_SOURCE_IMAGE) {
+			fse->min_width = UNICAM_IMAGE_MIN_WIDTH;
+			fse->max_width = UNICAM_IMAGE_MAX_WIDTH;
+			fse->min_height = UNICAM_IMAGE_MIN_HEIGHT;
+			fse->max_height = UNICAM_IMAGE_MAX_HEIGHT;
+		} else {
+			fse->min_width = UNICAM_META_MIN_WIDTH;
+			fse->max_width = UNICAM_META_MAX_WIDTH;
+			fse->min_height = UNICAM_META_MIN_HEIGHT;
+			fse->max_height = UNICAM_META_MAX_HEIGHT;
+		}
+	}
+
+	return 0;
+}
+
+static int unicam_subdev_set_format(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_format *format)
+{
+	struct unicam_device *unicam = sd_to_unicam_device(sd);
+	struct v4l2_mbus_framefmt *sink_format, *source_format;
+	const struct unicam_format_info *fmtinfo;
+	u32 source_pad, source_stream;
+	int ret;
+
+	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
+	    unicam->subdev.enabled_streams)
+		return -EBUSY;
+
+	/* No transcoding, source and sink formats must match. */
+	if (unicam_sd_pad_is_source(format->pad))
+		return v4l2_subdev_get_fmt(sd, state, format);
+
+	/*
+	 * Allowed formats for the stream on the sink pad depend on what source
+	 * pad the stream is routed to. Find the corresponding source pad and
+	 * use it to validate the media bus code.
+	 */
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+						    format->pad, format->stream,
+						    &source_pad, &source_stream);
+	if (ret)
+		return ret;
+
+	fmtinfo = unicam_find_format_by_code(format->format.code, source_pad);
+	if (!fmtinfo) {
+		fmtinfo = source_pad == UNICAM_SD_PAD_SOURCE_IMAGE
+			? &unicam_image_formats[0] : &unicam_meta_formats[0];
+		format->format.code = fmtinfo->code;
+	}
+
+	if (source_pad == UNICAM_SD_PAD_SOURCE_IMAGE) {
+		format->format.width = clamp_t(unsigned int,
+					       format->format.width,
+					       UNICAM_IMAGE_MIN_WIDTH,
+					       UNICAM_IMAGE_MAX_WIDTH);
+		format->format.height = clamp_t(unsigned int,
+						format->format.height,
+						UNICAM_IMAGE_MIN_HEIGHT,
+						UNICAM_IMAGE_MAX_HEIGHT);
+		format->format.field = V4L2_FIELD_NONE;
+	} else {
+		format->format.width = clamp_t(unsigned int,
+					       format->format.width,
+					       UNICAM_META_MIN_WIDTH,
+					       UNICAM_META_MAX_WIDTH);
+		format->format.height = clamp_t(unsigned int,
+						format->format.height,
+						UNICAM_META_MIN_HEIGHT,
+						UNICAM_META_MAX_HEIGHT);
+		format->format.field = V4L2_FIELD_NONE;
+
+		/* Colorspace don't apply to metadata. */
+		format->format.colorspace = 0;
+		format->format.ycbcr_enc = 0;
+		format->format.quantization = 0;
+		format->format.xfer_func = 0;
+	}
+
+	sink_format = v4l2_subdev_state_get_format(state, format->pad,
+						   format->stream);
+	source_format = v4l2_subdev_state_get_format(state, source_pad,
+						     source_stream);
+	*sink_format = format->format;
+	*source_format = format->format;
+
+	return 0;
+}
+
+static int unicam_subdev_set_routing(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     enum v4l2_subdev_format_whence which,
+				     struct v4l2_subdev_krouting *routing)
+{
+	struct unicam_device *unicam = sd_to_unicam_device(sd);
+
+	if (which == V4L2_SUBDEV_FORMAT_ACTIVE && unicam->subdev.enabled_streams)
+		return -EBUSY;
+
+	return __unicam_subdev_set_routing(sd, state, routing);
+}
+
+static int unicam_sd_enable_streams(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state, u32 pad,
+				    u64 streams_mask)
+{
+	struct unicam_device *unicam = sd_to_unicam_device(sd);
+	u32 other_pad, other_stream;
+	int ret;
+
+	if (!unicam->subdev.enabled_streams) {
+		/* Configure and start Unicam. */
+		unicam->sequence = 0;
+
+		if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE))
+			unicam_start_metadata(unicam);
+
+		unicam->frame_started = false;
+		unicam_start_rx(unicam, state);
+	}
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,
+						    &other_pad, &other_stream);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_enable_streams(unicam->sensor.subdev,
+					 unicam->sensor.pad->index,
+					 BIT(other_stream));
+	if (ret) {
+		dev_err(unicam->dev, "stream on failed in subdev\n");
+		return ret;
+	}
+
+	unicam->subdev.enabled_streams |= BIT(other_stream);
+
+	return 0;
+}
+
+static int unicam_sd_disable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state, u32 pad,
+				     u64 streams_mask)
+{
+	struct unicam_device *unicam = sd_to_unicam_device(sd);
+	u32 other_pad, other_stream;
+	int ret;
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,
+						    &other_pad, &other_stream);
+	if (ret)
+		return ret;
+
+	v4l2_subdev_disable_streams(unicam->sensor.subdev,
+				    unicam->sensor.pad->index,
+				    BIT(other_stream));
+
+	unicam->subdev.enabled_streams &= ~BIT(other_stream);
+
+	if (!unicam->subdev.enabled_streams)
+		unicam_disable(unicam);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops unicam_subdev_pad_ops = {
+	.enum_mbus_code		= unicam_subdev_enum_mbus_code,
+	.enum_frame_size	= unicam_subdev_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= unicam_subdev_set_format,
+	.set_routing		= unicam_subdev_set_routing,
+	.enable_streams		= unicam_sd_enable_streams,
+	.disable_streams	= unicam_sd_disable_streams,
+};
+
+static const struct v4l2_subdev_ops unicam_subdev_ops = {
+	.pad			= &unicam_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops unicam_subdev_internal_ops = {
+	.init_state		= unicam_subdev_init_state,
+};
+
+static const struct media_entity_operations unicam_subdev_media_ops = {
+	.link_validate		= v4l2_subdev_link_validate,
+	.has_pad_interdep	= v4l2_subdev_has_pad_interdep,
+};
+
+static int unicam_subdev_init(struct unicam_device *unicam)
+{
+	struct v4l2_subdev *sd = &unicam->subdev.sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &unicam_subdev_ops);
+	sd->internal_ops = &unicam_subdev_internal_ops;
+	v4l2_set_subdevdata(sd, unicam);
+
+	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	sd->entity.ops = &unicam_subdev_media_ops;
+	sd->dev = unicam->dev;
+	sd->owner = THIS_MODULE;
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
+
+	strscpy(sd->name, "unicam", sizeof(sd->name));
+
+	unicam->subdev.pads[UNICAM_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_IMAGE].flags = MEDIA_PAD_FL_SOURCE;
+	unicam->subdev.pads[UNICAM_SD_PAD_SOURCE_METADATA].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(unicam->subdev.pads),
+				     unicam->subdev.pads);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to initialize media entity: %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = v4l2_subdev_init_finalize(sd);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to initialize subdev: %d\n", ret);
+		goto err_entity;
+	}
+
+	ret = v4l2_device_register_subdev(&unicam->v4l2_dev, sd);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to register subdev: %d\n", ret);
+		goto err_subdev;
+	}
+
+	return 0;
+
+err_subdev:
+	v4l2_subdev_cleanup(sd);
+err_entity:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+static void unicam_subdev_cleanup(struct unicam_device *unicam)
+{
+	v4l2_subdev_cleanup(&unicam->subdev.sd);
+	media_entity_cleanup(&unicam->subdev.sd.entity);
+}
+
+/* -----------------------------------------------------------------------------
+ * Videobuf2 queue operations
+ */
+
+static int unicam_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+			      unsigned int *nplanes, unsigned int sizes[],
+			      struct device *alloc_devs[])
+{
+	struct unicam_node *node = vb2_get_drv_priv(vq);
+	u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage
+		 : node->fmt.fmt.meta.buffersize;
+
+	if (*nplanes) {
+		if (sizes[0] < size) {
+			dev_dbg(node->dev->dev, "sizes[0] %i < size %u\n",
+				sizes[0], size);
+			return -EINVAL;
+		}
+		size = sizes[0];
+	}
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int unicam_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct unicam_buffer *buf = to_unicam_buffer(vb);
+	u32 size = is_image_node(node) ? node->fmt.fmt.pix.sizeimage
+		 : node->fmt.fmt.meta.buffersize;
+
+	if (vb2_plane_size(vb, 0) < size) {
+		dev_dbg(node->dev->dev,
+			"data will not fit into plane (%lu < %u)\n",
+			vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	buf->dma_addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+	buf->size = size;
+
+	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
+
+	return 0;
+}
+
+static void unicam_return_buffers(struct unicam_node *node,
+				  enum vb2_buffer_state state)
+{
+	struct unicam_buffer *buf, *tmp;
+
+	list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) {
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+	}
+
+	if (node->cur_frm)
+		vb2_buffer_done(&node->cur_frm->vb.vb2_buf,
+				state);
+	if (node->next_frm && node->cur_frm != node->next_frm)
+		vb2_buffer_done(&node->next_frm->vb.vb2_buf,
+				state);
+
+	node->cur_frm = NULL;
+	node->next_frm = NULL;
+}
+
+static int unicam_num_data_lanes(struct unicam_device *unicam)
+{
+	struct v4l2_mbus_config mbus_config = { 0 };
+	unsigned int num_data_lanes;
+	int ret;
+
+	if (unicam->bus_type != V4L2_MBUS_CSI2_DPHY)
+		return unicam->max_data_lanes;
+
+	ret = v4l2_subdev_call(unicam->sensor.subdev, pad, get_mbus_config,
+			       unicam->sensor.pad->index, &mbus_config);
+	if (ret == -ENOIOCTLCMD)
+		return unicam->max_data_lanes;
+
+	if (ret < 0) {
+		dev_err(unicam->dev, "Failed to get mbus config: %d\n", ret);
+		return ret;
+	}
+
+	num_data_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+
+	if (num_data_lanes != 1 && num_data_lanes != 2 && num_data_lanes != 4) {
+		dev_err(unicam->dev,
+			"Device %s has requested %u data lanes, invalid\n",
+			unicam->sensor.subdev->name, num_data_lanes);
+		return -EINVAL;
+	}
+
+	if (num_data_lanes > unicam->max_data_lanes) {
+		dev_err(unicam->dev,
+			"Device %s has requested %u data lanes, >%u configured in DT\n",
+			unicam->sensor.subdev->name, num_data_lanes,
+			unicam->max_data_lanes);
+		return -EINVAL;
+	}
+
+	return num_data_lanes;
+}
+
+static int unicam_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct unicam_node *node = vb2_get_drv_priv(vq);
+	struct unicam_device *unicam = node->dev;
+	struct unicam_buffer *buf;
+	struct media_pipeline_pad_iter iter;
+	struct media_pad *pad;
+	unsigned long flags;
+	int ret;
+
+	dev_dbg(unicam->dev, "Starting stream on %s device\n",
+		is_metadata_node(node) ? "metadata" : "image");
+
+	/*
+	 * Start the pipeline. This validates all links, and populates the
+	 * pipeline structure.
+	 */
+	ret = video_device_pipeline_start(&node->video_dev, &unicam->pipe.pipe);
+	if (ret < 0) {
+		dev_dbg(unicam->dev, "Failed to start media pipeline: %d\n", ret);
+		goto err_buffers;
+	}
+
+	/*
+	 * Determine which video nodes are included in the pipeline, and get the
+	 * number of data lanes.
+	 */
+	if (unicam->pipe.pipe.start_count == 1) {
+		unicam->pipe.nodes = 0;
+
+		media_pipeline_for_each_pad(&unicam->pipe.pipe, &iter, pad) {
+			if (pad->entity != &unicam->subdev.sd.entity)
+				continue;
+
+			if (pad->index == UNICAM_SD_PAD_SOURCE_IMAGE)
+				unicam->pipe.nodes |= BIT(UNICAM_IMAGE_NODE);
+			else if (pad->index == UNICAM_SD_PAD_SOURCE_METADATA)
+				unicam->pipe.nodes |= BIT(UNICAM_METADATA_NODE);
+		}
+
+		if (!(unicam->pipe.nodes & BIT(UNICAM_IMAGE_NODE))) {
+			dev_dbg(unicam->dev,
+				"Pipeline does not include image node\n");
+			ret = -EPIPE;
+			goto err_pipeline;
+		}
+
+		ret = unicam_num_data_lanes(unicam);
+		if (ret < 0)
+			goto err_pipeline;
+
+		unicam->pipe.num_data_lanes = ret;
+
+		dev_dbg(unicam->dev, "Running with %u data lanes, nodes %u\n",
+			unicam->pipe.num_data_lanes, unicam->pipe.nodes);
+	}
+
+	/* Arm the node with the first buffer from the DMA queue. */
+	spin_lock_irqsave(&node->dma_queue_lock, flags);
+	buf = list_first_entry(&node->dma_queue, struct unicam_buffer, list);
+	node->cur_frm = buf;
+	node->next_frm = buf;
+	list_del(&buf->list);
+	spin_unlock_irqrestore(&node->dma_queue_lock, flags);
+
+	/*
+	 * Wait for all the video devices in the pipeline to have been started
+	 * before starting the hardware. In the general case, this would
+	 * prevent capturing multiple streams independently. However, the
+	 * Unicam DMA engines are not generic, they have been designed to
+	 * capture image data and embedded data from the same camera sensor.
+	 * Not only does the main use case not benefit from independent
+	 * capture, it requires proper synchronization of the streams at start
+	 * time.
+	 */
+	if (unicam->pipe.pipe.start_count < hweight32(unicam->pipe.nodes))
+		return 0;
+
+	ret = pm_runtime_resume_and_get(unicam->dev);
+	if (ret < 0) {
+		dev_err(unicam->dev, "PM runtime resume failed: %d\n", ret);
+		goto err_pipeline;
+	}
+
+	/* Enable the streams on the source. */
+	ret = v4l2_subdev_enable_streams(&unicam->subdev.sd,
+					 UNICAM_SD_PAD_SOURCE_IMAGE,
+					 BIT(0));
+	if (ret < 0) {
+		dev_err(unicam->dev, "stream on failed in subdev\n");
+		goto err_pm_put;
+	}
+
+	if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE)) {
+		ret = v4l2_subdev_enable_streams(&unicam->subdev.sd,
+						 UNICAM_SD_PAD_SOURCE_METADATA,
+						 BIT(0));
+		if (ret < 0) {
+			dev_err(unicam->dev, "stream on failed in subdev\n");
+			goto err_disable_streams;
+		}
+	}
+
+	return 0;
+
+err_disable_streams:
+	v4l2_subdev_disable_streams(&unicam->subdev.sd,
+				    UNICAM_SD_PAD_SOURCE_IMAGE, BIT(0));
+err_pm_put:
+	pm_runtime_put_sync(unicam->dev);
+err_pipeline:
+	video_device_pipeline_stop(&node->video_dev);
+err_buffers:
+	unicam_return_buffers(node, VB2_BUF_STATE_QUEUED);
+	return ret;
+}
+
+static void unicam_stop_streaming(struct vb2_queue *vq)
+{
+	struct unicam_node *node = vb2_get_drv_priv(vq);
+	struct unicam_device *unicam = node->dev;
+
+	/* Stop the hardware when the first video device gets stopped. */
+	if (unicam->pipe.pipe.start_count == hweight32(unicam->pipe.nodes)) {
+		if (unicam->pipe.nodes & BIT(UNICAM_METADATA_NODE))
+			v4l2_subdev_disable_streams(&unicam->subdev.sd,
+						    UNICAM_SD_PAD_SOURCE_METADATA,
+						    BIT(0));
+
+		v4l2_subdev_disable_streams(&unicam->subdev.sd,
+					    UNICAM_SD_PAD_SOURCE_IMAGE,
+					    BIT(0));
+
+		pm_runtime_put(unicam->dev);
+	}
+
+	video_device_pipeline_stop(&node->video_dev);
+
+	/* Clear all queued buffers for the node */
+	unicam_return_buffers(node, VB2_BUF_STATE_ERROR);
+}
+
+static void unicam_buffer_queue(struct vb2_buffer *vb)
+{
+	struct unicam_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct unicam_buffer *buf = to_unicam_buffer(vb);
+
+	spin_lock_irq(&node->dma_queue_lock);
+	list_add_tail(&buf->list, &node->dma_queue);
+	spin_unlock_irq(&node->dma_queue_lock);
+}
+
+static const struct vb2_ops unicam_video_qops = {
+	.queue_setup		= unicam_queue_setup,
+	.buf_prepare		= unicam_buffer_prepare,
+	.start_streaming	= unicam_start_streaming,
+	.stop_streaming		= unicam_stop_streaming,
+	.buf_queue		= unicam_buffer_queue,
+};
+
+/* -----------------------------------------------------------------------------
+ *  V4L2 video device operations
+ */
+
+static int unicam_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, UNICAM_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, UNICAM_MODULE_NAME, sizeof(cap->card));
+
+	cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE;
+
+	return 0;
+}
+
+static int unicam_enum_fmt_vid(struct file *file, void  *priv,
+			       struct v4l2_fmtdesc *f)
+{
+	unsigned int index;
+	unsigned int i;
+
+	for (i = 0, index = 0; i < ARRAY_SIZE(unicam_image_formats); i++) {
+		if (f->mbus_code && unicam_image_formats[i].code != f->mbus_code)
+			continue;
+
+		if (index == f->index) {
+			f->pixelformat = unicam_image_formats[i].fourcc;
+			return 0;
+		}
+
+		index++;
+
+		if (!unicam_image_formats[i].unpacked_fourcc)
+			continue;
+
+		if (index == f->index) {
+			f->pixelformat = unicam_image_formats[i].unpacked_fourcc;
+			return 0;
+		}
+
+		index++;
+	}
+
+	return -EINVAL;
+}
+
+static int unicam_g_fmt_vid(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	*f = node->fmt;
+
+	return 0;
+}
+
+static void __unicam_try_fmt_vid(struct unicam_node *node,
+				 struct v4l2_pix_format *pix)
+{
+	const struct unicam_format_info *fmtinfo;
+
+	/*
+	 * Default to the first format if the requested pixel format code isn't
+	 * supported.
+	 */
+	fmtinfo = unicam_find_format_by_fourcc(pix->pixelformat,
+					       UNICAM_SD_PAD_SOURCE_IMAGE);
+	if (!fmtinfo) {
+		fmtinfo = &unicam_image_formats[0];
+		pix->pixelformat = fmtinfo->fourcc;
+	}
+
+	unicam_calc_image_size_bpl(node->dev, fmtinfo, pix);
+
+	if (pix->field == V4L2_FIELD_ANY)
+		pix->field = V4L2_FIELD_NONE;
+}
+
+static int unicam_try_fmt_vid(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	__unicam_try_fmt_vid(node, &f->fmt.pix);
+	return 0;
+}
+
+static int unicam_s_fmt_vid(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	if (vb2_is_busy(&node->buffer_queue))
+		return -EBUSY;
+
+	__unicam_try_fmt_vid(node, &f->fmt.pix);
+	node->fmt = *f;
+
+	return 0;
+}
+
+static int unicam_enum_fmt_meta(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f)
+{
+	unsigned int i, index;
+
+	for (i = 0, index = 0; i < ARRAY_SIZE(unicam_meta_formats); i++) {
+		if (f->mbus_code && unicam_meta_formats[i].code != f->mbus_code)
+			continue;
+
+		if (index == f->index) {
+			f->pixelformat = unicam_meta_formats[i].fourcc;
+			f->type = V4L2_BUF_TYPE_META_CAPTURE;
+			f->flags = V4L2_FMT_FLAG_META_LINE_BASED;
+			return 0;
+		}
+
+		index++;
+	}
+
+	return -EINVAL;
+}
+
+static int unicam_g_fmt_meta(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	f->fmt.meta = node->fmt.fmt.meta;
+
+	return 0;
+}
+
+static const struct unicam_format_info *
+__unicam_try_fmt_meta(struct unicam_node *node, struct v4l2_meta_format *meta)
+{
+	const struct unicam_format_info *fmtinfo;
+
+	/*
+	 * Default to the first format if the requested pixel format code isn't
+	 * supported.
+	 */
+	fmtinfo = unicam_find_format_by_fourcc(meta->dataformat,
+					       UNICAM_SD_PAD_SOURCE_METADATA);
+	if (!fmtinfo) {
+		fmtinfo = &unicam_meta_formats[0];
+		meta->dataformat = fmtinfo->fourcc;
+	}
+
+	unicam_calc_meta_size_bpl(node->dev, fmtinfo, meta);
+
+	return fmtinfo;
+}
+
+static int unicam_try_fmt_meta(struct file *file, void *priv,
+			       struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	__unicam_try_fmt_meta(node, &f->fmt.meta);
+	return 0;
+}
+
+static int unicam_s_fmt_meta(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct unicam_node *node = video_drvdata(file);
+
+	if (vb2_is_busy(&node->buffer_queue))
+		return -EBUSY;
+
+	__unicam_try_fmt_meta(node, &f->fmt.meta);
+	node->fmt = *f;
+
+	return 0;
+}
+
+static int unicam_enum_framesizes(struct file *file, void *fh,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct unicam_node *node = video_drvdata(file);
+	int ret = -EINVAL;
+
+	if (fsize->index > 0)
+		return ret;
+
+	if (is_image_node(node)) {
+		if (!unicam_find_format_by_fourcc(fsize->pixel_format,
+						  UNICAM_SD_PAD_SOURCE_IMAGE))
+			return ret;
+
+		fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+		fsize->stepwise.min_width = UNICAM_IMAGE_MIN_WIDTH;
+		fsize->stepwise.max_width = UNICAM_IMAGE_MAX_WIDTH;
+		fsize->stepwise.step_width = 1;
+		fsize->stepwise.min_height = UNICAM_IMAGE_MIN_HEIGHT;
+		fsize->stepwise.max_height = UNICAM_IMAGE_MAX_HEIGHT;
+		fsize->stepwise.step_height = 1;
+	} else {
+		if (!unicam_find_format_by_fourcc(fsize->pixel_format,
+						  UNICAM_SD_PAD_SOURCE_METADATA))
+			return ret;
+
+		fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+		fsize->stepwise.min_width = UNICAM_META_MIN_WIDTH;
+		fsize->stepwise.max_width = UNICAM_META_MAX_WIDTH;
+		fsize->stepwise.step_width = 1;
+		fsize->stepwise.min_height = UNICAM_META_MIN_HEIGHT;
+		fsize->stepwise.max_height = UNICAM_META_MAX_HEIGHT;
+		fsize->stepwise.step_height = 1;
+	}
+
+	return 0;
+}
+
+static int unicam_log_status(struct file *file, void *fh)
+{
+	struct unicam_node *node = video_drvdata(file);
+	struct unicam_device *unicam = node->dev;
+	u32 reg;
+
+	/* status for sub devices */
+	v4l2_device_call_all(&unicam->v4l2_dev, 0, core, log_status);
+
+	dev_info(unicam->dev, "-----Receiver status-----\n");
+	dev_info(unicam->dev, "V4L2 width/height:   %ux%u\n",
+		 node->fmt.fmt.pix.width, node->fmt.fmt.pix.height);
+	dev_info(unicam->dev, "V4L2 format:         %08x\n",
+		 node->fmt.fmt.pix.pixelformat);
+	reg = unicam_reg_read(unicam, UNICAM_IPIPE);
+	dev_info(unicam->dev, "Unpacking/packing:   %u / %u\n",
+		 unicam_get_field(reg, UNICAM_PUM_MASK),
+		 unicam_get_field(reg, UNICAM_PPM_MASK));
+	dev_info(unicam->dev, "----Live data----\n");
+	dev_info(unicam->dev, "Programmed stride:   %4u\n",
+		 unicam_reg_read(unicam, UNICAM_IBLS));
+	dev_info(unicam->dev, "Detected resolution: %ux%u\n",
+		 unicam_reg_read(unicam, UNICAM_IHSTA),
+		 unicam_reg_read(unicam, UNICAM_IVSTA));
+	dev_info(unicam->dev, "Write pointer:       %08x\n",
+		 unicam_reg_read(unicam, UNICAM_IBWP));
+
+	return 0;
+}
+
+static int unicam_subscribe_event(struct v4l2_fh *fh,
+				  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_FRAME_SYNC:
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct v4l2_ioctl_ops unicam_ioctl_ops = {
+	.vidioc_querycap		= unicam_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= unicam_enum_fmt_vid,
+	.vidioc_g_fmt_vid_cap		= unicam_g_fmt_vid,
+	.vidioc_try_fmt_vid_cap		= unicam_try_fmt_vid,
+	.vidioc_s_fmt_vid_cap		= unicam_s_fmt_vid,
+
+	.vidioc_enum_fmt_meta_cap	= unicam_enum_fmt_meta,
+	.vidioc_g_fmt_meta_cap		= unicam_g_fmt_meta,
+	.vidioc_try_fmt_meta_cap	= unicam_try_fmt_meta,
+	.vidioc_s_fmt_meta_cap		= unicam_s_fmt_meta,
+
+	.vidioc_enum_framesizes		= unicam_enum_framesizes,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_expbuf			= vb2_ioctl_expbuf,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_log_status		= unicam_log_status,
+	.vidioc_subscribe_event		= unicam_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/* unicam capture driver file operations */
+static const struct v4l2_file_operations unicam_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vb2_fop_release,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
+};
+
+static int unicam_video_link_validate(struct media_link *link)
+{
+	struct video_device *vdev =
+		media_entity_to_video_device(link->sink->entity);
+	struct v4l2_subdev *sd =
+		media_entity_to_v4l2_subdev(link->source->entity);
+	struct unicam_node *node = video_get_drvdata(vdev);
+	const u32 pad = is_image_node(node) ? UNICAM_SD_PAD_SOURCE_IMAGE
+		      : UNICAM_SD_PAD_SOURCE_METADATA;
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_subdev_state *state;
+	int ret = 0;
+
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	format = v4l2_subdev_state_get_format(state, pad, 0);
+	if (!format) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (is_image_node(node)) {
+		const struct v4l2_pix_format *fmt = &node->fmt.fmt.pix;
+		const struct unicam_format_info *fmtinfo;
+
+		fmtinfo = unicam_find_format_by_fourcc(fmt->pixelformat,
+						       UNICAM_SD_PAD_SOURCE_IMAGE);
+		if (WARN_ON(!fmtinfo)) {
+			ret = -EPIPE;
+			goto out;
+		}
+
+		if (fmtinfo->code != format->code ||
+		    fmt->height != format->height ||
+		    fmt->width != format->width ||
+		    fmt->field != format->field) {
+			dev_dbg(node->dev->dev,
+				"image: (%u x %u) 0x%08x %s != (%u x %u) 0x%08x %s\n",
+				fmt->width, fmt->height, fmtinfo->code,
+				v4l2_field_names[fmt->field],
+				format->width, format->height, format->code,
+				v4l2_field_names[format->field]);
+			ret = -EPIPE;
+		}
+	} else {
+		const struct v4l2_meta_format *fmt = &node->fmt.fmt.meta;
+
+		const struct unicam_format_info *fmtinfo;
+
+		fmtinfo = unicam_find_format_by_fourcc(fmt->dataformat,
+						       UNICAM_SD_PAD_SOURCE_METADATA);
+		if (WARN_ON(!fmtinfo)) {
+			ret = -EPIPE;
+			goto out;
+		}
+
+		if (fmtinfo->code != format->code ||
+		    fmt->height != format->height ||
+		    fmt->width != format->width) {
+			dev_dbg(node->dev->dev,
+				"meta: (%u x %u) 0x%04x != (%u x %u) 0x%04x\n",
+				fmt->width, fmt->height, fmtinfo->code,
+				format->width, format->height, format->code);
+			ret = -EPIPE;
+		}
+	}
+
+out:
+	v4l2_subdev_unlock_state(state);
+	return ret;
+}
+
+static const struct media_entity_operations unicam_video_media_ops = {
+	.link_validate = unicam_video_link_validate,
+};
+
+static void unicam_node_release(struct video_device *vdev)
+{
+	struct unicam_node *node = video_get_drvdata(vdev);
+
+	unicam_put(node->dev);
+}
+
+static void unicam_set_default_format(struct unicam_node *node)
+{
+	if (is_image_node(node)) {
+		struct v4l2_pix_format *fmt = &node->fmt.fmt.pix;
+		const struct unicam_format_info *fmtinfo =
+			&unicam_image_formats[0];
+
+		node->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+		v4l2_fill_pix_format(fmt, &unicam_default_image_format);
+		fmt->pixelformat = fmtinfo->fourcc;
+		unicam_calc_image_size_bpl(node->dev, fmtinfo, fmt);
+	} else {
+		struct v4l2_meta_format *fmt = &node->fmt.fmt.meta;
+		const struct unicam_format_info *fmtinfo =
+			&unicam_meta_formats[0];
+
+		node->fmt.type = V4L2_BUF_TYPE_META_CAPTURE;
+
+		fmt->dataformat = fmtinfo->fourcc;
+		fmt->width = unicam_default_meta_format.width;
+		fmt->height = unicam_default_meta_format.height;
+		unicam_calc_meta_size_bpl(node->dev, fmtinfo, fmt);
+	}
+}
+
+static int unicam_register_node(struct unicam_device *unicam,
+				enum unicam_node_type type)
+{
+	const u32 pad_index = type == UNICAM_IMAGE_NODE
+			    ? UNICAM_SD_PAD_SOURCE_IMAGE
+			    : UNICAM_SD_PAD_SOURCE_METADATA;
+	struct unicam_node *node = &unicam->node[type];
+	struct video_device *vdev = &node->video_dev;
+	struct vb2_queue *q = &node->buffer_queue;
+	int ret;
+
+	node->dev = unicam_get(unicam);
+	node->id = type;
+
+	spin_lock_init(&node->dma_queue_lock);
+
+	INIT_LIST_HEAD(&node->dma_queue);
+
+	/* Initialize the videobuf2 queue. */
+	q->type = type == UNICAM_IMAGE_NODE ? V4L2_BUF_TYPE_VIDEO_CAPTURE
+					    : V4L2_BUF_TYPE_META_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = node;
+	q->ops = &unicam_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct unicam_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &unicam->lock;
+	q->min_queued_buffers = 1;
+	q->dev = unicam->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		dev_err(unicam->dev, "vb2_queue_init() failed\n");
+		goto err_unicam_put;
+	}
+
+	/* Initialize the video device. */
+	vdev->release = unicam_node_release;
+	vdev->fops = &unicam_fops;
+	vdev->ioctl_ops = &unicam_ioctl_ops;
+	vdev->v4l2_dev = &unicam->v4l2_dev;
+	vdev->vfl_dir = VFL_DIR_RX;
+	vdev->queue = q;
+	vdev->lock = &unicam->lock;
+	vdev->device_caps = type == UNICAM_IMAGE_NODE
+			  ? V4L2_CAP_VIDEO_CAPTURE : V4L2_CAP_META_CAPTURE;
+	vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
+	vdev->entity.ops = &unicam_video_media_ops;
+
+	snprintf(vdev->name, sizeof(vdev->name), "%s-%s", UNICAM_MODULE_NAME,
+		 type == UNICAM_IMAGE_NODE ? "image" : "embedded");
+
+	video_set_drvdata(vdev, node);
+
+	if (type == UNICAM_IMAGE_NODE)
+		vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
+
+	node->pad.flags = MEDIA_PAD_FL_SINK;
+
+	ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
+	if (ret)
+		goto err_unicam_put;
+
+	node->dummy_buf.size = UNICAM_DUMMY_BUF_SIZE;
+	node->dummy_buf_cpu_addr = dma_alloc_coherent(unicam->dev,
+						      node->dummy_buf.size,
+						      &node->dummy_buf.dma_addr,
+						      GFP_KERNEL);
+	if (!node->dummy_buf_cpu_addr) {
+		dev_err(unicam->dev, "Unable to allocate dummy buffer.\n");
+		ret = -ENOMEM;
+		goto err_entity_cleanup;
+	}
+
+	unicam_set_default_format(node);
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(unicam->dev, "Unable to register video device %s\n",
+			vdev->name);
+		goto err_dma_free;
+	}
+
+	node->registered = true;
+
+	ret = media_create_pad_link(&unicam->subdev.sd.entity,
+				    pad_index,
+				    &node->video_dev.entity,
+				    0,
+				    MEDIA_LNK_FL_ENABLED |
+				    MEDIA_LNK_FL_IMMUTABLE);
+	if (ret) {
+		/*
+		 * No need for cleanup, the caller will unregister the
+		 * video device, which will drop the reference on the
+		 * device and trigger the cleanup.
+		 */
+		dev_err(unicam->dev, "Unable to create pad link for %s\n",
+			unicam->sensor.subdev->name);
+		return ret;
+	}
+
+	return 0;
+
+err_dma_free:
+	dma_free_coherent(unicam->dev, node->dummy_buf.size,
+			  node->dummy_buf_cpu_addr,
+			  node->dummy_buf.dma_addr);
+err_entity_cleanup:
+	media_entity_cleanup(&vdev->entity);
+err_unicam_put:
+	unicam_put(unicam);
+	return ret;
+}
+
+static void unicam_unregister_nodes(struct unicam_device *unicam)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(unicam->node); i++) {
+		struct unicam_node *node = &unicam->node[i];
+
+		if (node->registered) {
+			vb2_video_unregister_device(&node->video_dev);
+			node->registered = false;
+		}
+
+		if (node->dummy_buf_cpu_addr)
+			dma_free_coherent(unicam->dev, node->dummy_buf.size,
+					  node->dummy_buf_cpu_addr,
+					  node->dummy_buf.dma_addr);
+	}
+}
+
+/* -----------------------------------------------------------------------------
+ * Power management
+ */
+
+static int unicam_runtime_resume(struct device *dev)
+{
+	struct unicam_device *unicam = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_set_min_rate(unicam->vpu_clock, UNICAM_MIN_VPU_CLOCK_RATE);
+	if (ret) {
+		dev_err(unicam->dev, "failed to set up VPU clock\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(unicam->vpu_clock);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to enable VPU clock: %d\n", ret);
+		goto err_vpu_clock;
+	}
+
+	ret = clk_set_rate(unicam->clock, 100 * 1000 * 1000);
+	if (ret) {
+		dev_err(unicam->dev, "failed to set up CSI clock\n");
+		goto err_vpu_prepare;
+	}
+
+	ret = clk_prepare_enable(unicam->clock);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to enable CSI clock: %d\n", ret);
+		goto err_vpu_prepare;
+	}
+
+	return 0;
+
+err_vpu_prepare:
+	clk_disable_unprepare(unicam->vpu_clock);
+err_vpu_clock:
+	if (clk_set_min_rate(unicam->vpu_clock, 0))
+		dev_err(unicam->dev, "failed to reset the VPU clock\n");
+
+	return ret;
+}
+
+static int unicam_runtime_suspend(struct device *dev)
+{
+	struct unicam_device *unicam = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(unicam->clock);
+
+	if (clk_set_min_rate(unicam->vpu_clock, 0))
+		dev_err(unicam->dev, "failed to reset the VPU clock\n");
+
+	clk_disable_unprepare(unicam->vpu_clock);
+
+	return 0;
+}
+
+static const struct dev_pm_ops unicam_pm_ops = {
+	RUNTIME_PM_OPS(unicam_runtime_suspend, unicam_runtime_resume, NULL)
+};
+
+/* -----------------------------------------------------------------------------
+ * V4L2 async notifier
+ */
+
+static int unicam_async_bound(struct v4l2_async_notifier *notifier,
+			      struct v4l2_subdev *subdev,
+			      struct v4l2_async_connection *asc)
+{
+	struct unicam_device *unicam = notifier_to_unicam_device(notifier);
+	struct media_pad *sink = &unicam->subdev.pads[UNICAM_SD_PAD_SINK];
+	struct media_pad *source;
+	int ret;
+
+	dev_dbg(unicam->dev, "Using sensor %s for capture\n",
+		subdev->name);
+
+	ret = v4l2_create_fwnode_links_to_pad(subdev, sink, MEDIA_LNK_FL_ENABLED |
+					      MEDIA_LNK_FL_IMMUTABLE);
+	if (ret)
+		return ret;
+
+	source = media_pad_remote_pad_unique(sink);
+	if (IS_ERR(source)) {
+		dev_err(unicam->dev, "No connected sensor pad\n");
+		return PTR_ERR(source);
+	}
+
+	unicam->sensor.subdev = subdev;
+	unicam->sensor.pad = source;
+
+	return 0;
+}
+
+static int unicam_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct unicam_device *unicam = notifier_to_unicam_device(notifier);
+	int ret;
+
+	ret = unicam_register_node(unicam, UNICAM_IMAGE_NODE);
+	if (ret) {
+		dev_err(unicam->dev, "Unable to register image video device.\n");
+		goto unregister;
+	}
+
+	ret = unicam_register_node(unicam, UNICAM_METADATA_NODE);
+	if (ret) {
+		dev_err(unicam->dev, "Unable to register metadata video device.\n");
+		goto unregister;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&unicam->v4l2_dev);
+	if (ret) {
+		dev_err(unicam->dev, "Unable to register subdev nodes.\n");
+		goto unregister;
+	}
+
+	return 0;
+
+unregister:
+	unicam_unregister_nodes(unicam);
+	unicam_put(unicam);
+
+	return ret;
+}
+
+static const struct v4l2_async_notifier_operations unicam_async_ops = {
+	.bound = unicam_async_bound,
+	.complete = unicam_async_complete,
+};
+
+static int unicam_async_nf_init(struct unicam_device *unicam)
+{
+	struct v4l2_fwnode_endpoint ep = { };
+	struct fwnode_handle *ep_handle;
+	struct v4l2_async_connection *asc;
+	int ret;
+
+	ret = of_property_read_u32(unicam->dev->of_node, "brcm,num-data-lanes",
+				   &unicam->max_data_lanes);
+	if (ret < 0) {
+		dev_err(unicam->dev, "Missing %s DT property\n",
+			"brcm,num-data-lanes");
+		return -EINVAL;
+	}
+
+	/* Get and parse the local endpoint. */
+	ep_handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(unicam->dev), 0, 0,
+						    FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep_handle) {
+		dev_err(unicam->dev, "No endpoint found\n");
+		return -ENODEV;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(ep_handle, &ep);
+	if (ret) {
+		dev_err(unicam->dev, "Failed to parse endpoint: %d\n", ret);
+		goto error;
+	}
+
+	unicam->bus_type = ep.bus_type;
+
+	switch (ep.bus_type) {
+	case V4L2_MBUS_CSI2_DPHY: {
+		unsigned int num_data_lanes = ep.bus.mipi_csi2.num_data_lanes;
+
+		if (num_data_lanes != 1 && num_data_lanes != 2 &&
+		    num_data_lanes != 4) {
+			dev_err(unicam->dev, "%u data lanes not supported\n",
+				num_data_lanes);
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (num_data_lanes > unicam->max_data_lanes) {
+			dev_err(unicam->dev,
+				"Endpoint uses %u data lanes when %u are supported\n",
+				num_data_lanes, unicam->max_data_lanes);
+			ret = -EINVAL;
+			goto error;
+		}
+
+		unicam->max_data_lanes = num_data_lanes;
+		unicam->bus_flags = ep.bus.mipi_csi2.flags;
+		break;
+	}
+
+	case V4L2_MBUS_CCP2:
+		unicam->max_data_lanes = 1;
+		unicam->bus_flags = ep.bus.mipi_csi1.strobe;
+		break;
+
+	default:
+		/* Unsupported bus type */
+		dev_err(unicam->dev, "Unsupported bus type %u\n", ep.bus_type);
+		ret = -EINVAL;
+		goto error;
+	}
+
+	/* Initialize and register the async notifier. */
+	v4l2_async_nf_init(&unicam->notifier, &unicam->v4l2_dev);
+
+	asc = v4l2_async_nf_add_fwnode_remote(&unicam->notifier, ep_handle,
+					      struct v4l2_async_connection);
+	fwnode_handle_put(ep_handle);
+	ep_handle = NULL;
+
+	if (IS_ERR(asc)) {
+		ret = PTR_ERR(asc);
+		dev_err(unicam->dev, "Failed to add entry to notifier: %d\n",
+			ret);
+		goto error;
+	}
+
+	unicam->notifier.ops = &unicam_async_ops;
+
+	ret = v4l2_async_nf_register(&unicam->notifier);
+	if (ret) {
+		dev_err(unicam->dev, "Error registering device notifier: %d\n",
+			ret);
+		goto error;
+	}
+
+	return 0;
+
+error:
+	fwnode_handle_put(ep_handle);
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Probe & remove
+ */
+
+static int unicam_media_init(struct unicam_device *unicam)
+{
+	int ret;
+
+	unicam->mdev.dev = unicam->dev;
+	strscpy(unicam->mdev.model, UNICAM_MODULE_NAME,
+		sizeof(unicam->mdev.model));
+	unicam->mdev.hw_revision = 0;
+
+	media_device_init(&unicam->mdev);
+
+	unicam->v4l2_dev.mdev = &unicam->mdev;
+
+	ret = v4l2_device_register(unicam->dev, &unicam->v4l2_dev);
+	if (ret < 0) {
+		dev_err(unicam->dev, "Unable to register v4l2 device\n");
+		goto err_media_cleanup;
+	}
+
+	ret = media_device_register(&unicam->mdev);
+	if (ret < 0) {
+		dev_err(unicam->dev,
+			"Unable to register media-controller device\n");
+		goto err_v4l2_unregister;
+	}
+
+	return 0;
+
+err_v4l2_unregister:
+	v4l2_device_unregister(&unicam->v4l2_dev);
+err_media_cleanup:
+	media_device_cleanup(&unicam->mdev);
+	return ret;
+}
+
+static int unicam_probe(struct platform_device *pdev)
+{
+	struct unicam_device *unicam;
+	int ret;
+
+	unicam = kzalloc(sizeof(*unicam), GFP_KERNEL);
+	if (!unicam)
+		return -ENOMEM;
+
+	kref_init(&unicam->kref);
+	mutex_init(&unicam->lock);
+
+	unicam->dev = &pdev->dev;
+	platform_set_drvdata(pdev, unicam);
+
+	unicam->base = devm_platform_ioremap_resource_byname(pdev, "unicam");
+	if (IS_ERR(unicam->base)) {
+		ret = PTR_ERR(unicam->base);
+		goto err_unicam_put;
+	}
+
+	unicam->clk_gate_base = devm_platform_ioremap_resource_byname(pdev, "cmi");
+	if (IS_ERR(unicam->clk_gate_base)) {
+		ret = PTR_ERR(unicam->clk_gate_base);
+		goto err_unicam_put;
+	}
+
+	unicam->clock = devm_clk_get(&pdev->dev, "lp");
+	if (IS_ERR(unicam->clock)) {
+		dev_err(unicam->dev, "Failed to get lp clock\n");
+		ret = PTR_ERR(unicam->clock);
+		goto err_unicam_put;
+	}
+
+	unicam->vpu_clock = devm_clk_get(&pdev->dev, "vpu");
+	if (IS_ERR(unicam->vpu_clock)) {
+		dev_err(unicam->dev, "Failed to get vpu clock\n");
+		ret = PTR_ERR(unicam->vpu_clock);
+		goto err_unicam_put;
+	}
+
+	ret = platform_get_irq(pdev, 0);
+	if (ret < 0)
+		goto err_unicam_put;
+
+	ret = devm_request_irq(&pdev->dev, ret, unicam_isr, 0,
+			       "unicam_capture0", unicam);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request interrupt\n");
+		goto err_unicam_put;
+	}
+
+	/* Enable the block power domain. */
+	pm_runtime_enable(&pdev->dev);
+
+	ret = unicam_media_init(unicam);
+	if (ret)
+		goto err_pm_runtime;
+
+	ret = unicam_subdev_init(unicam);
+	if (ret)
+		goto err_media_unregister;
+
+	ret = unicam_async_nf_init(unicam);
+	if (ret)
+		goto err_subdev_unregister;
+
+	return 0;
+
+err_subdev_unregister:
+	unicam_subdev_cleanup(unicam);
+err_media_unregister:
+	media_device_unregister(&unicam->mdev);
+err_pm_runtime:
+	pm_runtime_disable(&pdev->dev);
+err_unicam_put:
+	unicam_put(unicam);
+
+	return ret;
+}
+
+static void unicam_remove(struct platform_device *pdev)
+{
+	struct unicam_device *unicam = platform_get_drvdata(pdev);
+
+	unicam_unregister_nodes(unicam);
+	v4l2_device_unregister(&unicam->v4l2_dev);
+	media_device_unregister(&unicam->mdev);
+	v4l2_async_nf_unregister(&unicam->notifier);
+
+	unicam_subdev_cleanup(unicam);
+
+	unicam_put(unicam);
+
+	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct of_device_id unicam_of_match[] = {
+	{ .compatible = "brcm,bcm2835-unicam", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, unicam_of_match);
+
+static struct platform_driver unicam_driver = {
+	.probe		= unicam_probe,
+	.remove		= unicam_remove,
+	.driver = {
+		.name	= UNICAM_MODULE_NAME,
+		.pm	= pm_ptr(&unicam_pm_ops),
+		.of_match_table = unicam_of_match,
+	},
+};
+
+module_platform_driver(unicam_driver);
+
+MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com>");
+MODULE_DESCRIPTION("BCM2835 Unicam driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/cadence/cdns-csi2rx.c b/drivers/media/platform/cadence/cdns-csi2rx.c
index 2d7b0508cc9a..cebcae196eec 100644
--- a/drivers/media/platform/cadence/cdns-csi2rx.c
+++ b/drivers/media/platform/cadence/cdns-csi2rx.c
@@ -164,6 +164,8 @@ static void csi2rx_reset(struct csi2rx_priv *csi2rx)
 
 static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
 {
+	struct media_pad *src_pad =
+		&csi2rx->source_subdev->entity.pads[csi2rx->source_pad];
 	union phy_configure_opts opts = { };
 	struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy;
 	struct v4l2_subdev_format sd_fmt = {
@@ -181,7 +183,7 @@ static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx)
 
 	fmt = csi2rx_get_fmt_by_code(sd_fmt.format.code);
 
-	link_freq = v4l2_get_link_freq(csi2rx->source_subdev->ctrl_handler,
+	link_freq = v4l2_get_link_freq(src_pad,
 				       fmt->bpp, 2 * csi2rx->num_lanes);
 	if (link_freq < 0)
 		return link_freq;
@@ -239,10 +241,6 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
 
 	writel(reg, csi2rx->base + CSI2RX_STATIC_CFG_REG);
 
-	ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
-	if (ret)
-		goto err_disable_pclk;
-
 	/* Enable DPHY clk and data lanes. */
 	if (csi2rx->dphy) {
 		reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST;
@@ -252,6 +250,13 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
 		}
 
 		writel(reg, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
+
+		ret = csi2rx_configure_ext_dphy(csi2rx);
+		if (ret) {
+			dev_err(csi2rx->dev,
+				"Failed to configure external DPHY: %d\n", ret);
+			goto err_disable_pclk;
+		}
 	}
 
 	/*
@@ -291,14 +296,9 @@ static int csi2rx_start(struct csi2rx_priv *csi2rx)
 
 	reset_control_deassert(csi2rx->sys_rst);
 
-	if (csi2rx->dphy) {
-		ret = csi2rx_configure_ext_dphy(csi2rx);
-		if (ret) {
-			dev_err(csi2rx->dev,
-				"Failed to configure external DPHY: %d\n", ret);
-			goto err_disable_sysclk;
-		}
-	}
+	ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true);
+	if (ret)
+		goto err_disable_sysclk;
 
 	clk_disable_unprepare(csi2rx->p_clk);
 
@@ -312,6 +312,10 @@ err_disable_pixclk:
 		clk_disable_unprepare(csi2rx->pixel_clk[i - 1]);
 	}
 
+	if (csi2rx->dphy) {
+		writel(0, csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG);
+		phy_power_off(csi2rx->dphy);
+	}
 err_disable_pclk:
 	clk_disable_unprepare(csi2rx->p_clk);
 
@@ -749,7 +753,7 @@ MODULE_DEVICE_TABLE(of, csi2rx_of_table);
 
 static struct platform_driver csi2rx_driver = {
 	.probe	= csi2rx_probe,
-	.remove_new = csi2rx_remove,
+	.remove = csi2rx_remove,
 
 	.driver	= {
 		.name		= "cdns-csi2rx",
diff --git a/drivers/media/platform/cadence/cdns-csi2tx.c b/drivers/media/platform/cadence/cdns-csi2tx.c
index 3d98f91f1bee..e22b133f346d 100644
--- a/drivers/media/platform/cadence/cdns-csi2tx.c
+++ b/drivers/media/platform/cadence/cdns-csi2tx.c
@@ -644,7 +644,7 @@ static void csi2tx_remove(struct platform_device *pdev)
 
 static struct platform_driver csi2tx_driver = {
 	.probe	= csi2tx_probe,
-	.remove_new = csi2tx_remove,
+	.remove = csi2tx_remove,
 
 	.driver	= {
 		.name		= "cdns-csi2tx",
diff --git a/drivers/media/platform/chips-media/coda/coda-bit.c b/drivers/media/platform/chips-media/coda/coda-bit.c
index ed47d5bd8d61..84ded154adfe 100644
--- a/drivers/media/platform/chips-media/coda/coda-bit.c
+++ b/drivers/media/platform/chips-media/coda/coda-bit.c
@@ -585,7 +585,7 @@ static int coda_alloc_context_buffers(struct coda_ctx *ctx,
 
 	if (!ctx->slicebuf.vaddr && q_data->fourcc == V4L2_PIX_FMT_H264) {
 		/* worst case slice size */
-		size = (DIV_ROUND_UP(q_data->rect.width, 16) *
+		size = (unsigned long)(DIV_ROUND_UP(q_data->rect.width, 16) *
 			DIV_ROUND_UP(q_data->rect.height, 16)) * 3200 / 8 + 512;
 		ret = coda_alloc_context_buf(ctx, &ctx->slicebuf, size,
 					     "slicebuf");
diff --git a/drivers/media/platform/chips-media/coda/coda-common.c b/drivers/media/platform/chips-media/coda/coda-common.c
index 7da0194ec850..e6e3f5ec24f6 100644
--- a/drivers/media/platform/chips-media/coda/coda-common.c
+++ b/drivers/media/platform/chips-media/coda/coda-common.c
@@ -2171,8 +2171,6 @@ static const struct vb2_ops coda_qops = {
 	.buf_queue		= coda_buf_queue,
 	.start_streaming	= coda_start_streaming,
 	.stop_streaming		= coda_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int coda_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -3342,11 +3340,12 @@ static int coda_runtime_resume(struct device *dev)
 
 static const struct dev_pm_ops coda_pm_ops = {
 	SET_RUNTIME_PM_OPS(NULL, coda_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend, pm_runtime_force_resume)
 };
 
 static struct platform_driver coda_driver = {
 	.probe	= coda_probe,
-	.remove_new = coda_remove,
+	.remove = coda_remove,
 	.driver	= {
 		.name	= CODA_NAME,
 		.of_match_table = coda_dt_ids,
diff --git a/drivers/media/platform/chips-media/coda/coda-jpeg.c b/drivers/media/platform/chips-media/coda/coda-jpeg.c
index ba8f41002917..5746892658b1 100644
--- a/drivers/media/platform/chips-media/coda/coda-jpeg.c
+++ b/drivers/media/platform/chips-media/coda/coda-jpeg.c
@@ -5,7 +5,7 @@
  * Copyright (C) 2014 Philipp Zabel, Pengutronix
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/irqreturn.h>
 #include <linux/kernel.h>
 #include <linux/ktime.h>
diff --git a/drivers/media/platform/chips-media/wave5/wave5-helper.c b/drivers/media/platform/chips-media/wave5/wave5-helper.c
index 8433ecab230c..2c9d8cbca6e4 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-helper.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-helper.c
@@ -7,6 +7,8 @@
 
 #include "wave5-helper.h"
 
+#define DEFAULT_BS_SIZE(width, height) ((width) * (height) / 8 * 3)
+
 const char *state_to_str(enum vpu_instance_state state)
 {
 	switch (state) {
@@ -29,7 +31,13 @@ void wave5_cleanup_instance(struct vpu_instance *inst)
 {
 	int i;
 
-	if (list_is_singular(&inst->list))
+	/*
+	 * For Wave515 SRAM memory is allocated at
+	 * wave5_vpu_dec_register_device() and freed at
+	 * wave5_vpu_dec_unregister_device().
+	 */
+	if (list_is_singular(&inst->list) &&
+	    inst->dev->product_code != WAVE515_CODE)
 		wave5_vdi_free_sram(inst->dev);
 
 	for (i = 0; i < inst->fbc_buf_count; i++)
@@ -52,11 +60,11 @@ int wave5_vpu_release_device(struct file *filp,
 			     char *name)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(filp->private_data);
+	int ret = 0;
 
 	v4l2_m2m_ctx_release(inst->v4l2_fh.m2m_ctx);
 	if (inst->state != VPU_INST_STATE_NONE) {
 		u32 fail_res;
-		int ret;
 
 		ret = close_func(inst, &fail_res);
 		if (fail_res == WAVE5_SYSERR_VPU_STILL_RUNNING) {
@@ -72,7 +80,7 @@ int wave5_vpu_release_device(struct file *filp,
 
 	wave5_cleanup_instance(inst);
 
-	return 0;
+	return ret;
 }
 
 int wave5_vpu_queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq,
@@ -211,3 +219,25 @@ void wave5_return_bufs(struct vb2_queue *q, u32 state)
 		v4l2_m2m_buf_done(vbuf, state);
 	}
 }
+
+void wave5_update_pix_fmt(struct v4l2_pix_format_mplane *pix_mp,
+			  int pix_fmt_type,
+			  unsigned int width,
+			  unsigned int height,
+			  const struct v4l2_frmsize_stepwise *frmsize)
+{
+	v4l2_apply_frmsize_constraints(&width, &height, frmsize);
+
+	if (pix_fmt_type == VPU_FMT_TYPE_CODEC) {
+		pix_mp->width = width;
+		pix_mp->height = height;
+		pix_mp->num_planes = 1;
+		pix_mp->plane_fmt[0].bytesperline = 0;
+		pix_mp->plane_fmt[0].sizeimage = max(DEFAULT_BS_SIZE(width, height),
+						     pix_mp->plane_fmt[0].sizeimage);
+	} else {
+		v4l2_fill_pixfmt_mp(pix_mp, pix_mp->pixelformat, width, height);
+	}
+	pix_mp->flags = 0;
+	pix_mp->field = V4L2_FIELD_NONE;
+}
diff --git a/drivers/media/platform/chips-media/wave5/wave5-helper.h b/drivers/media/platform/chips-media/wave5/wave5-helper.h
index 6cee1c14d3ce..9937fce553fc 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-helper.h
+++ b/drivers/media/platform/chips-media/wave5/wave5-helper.h
@@ -28,4 +28,9 @@ const struct vpu_format *wave5_find_vpu_fmt_by_idx(unsigned int idx,
 						   const struct vpu_format fmt_list[MAX_FMTS]);
 enum wave_std wave5_to_vpu_std(unsigned int v4l2_pix_fmt, enum vpu_instance_type type);
 void wave5_return_bufs(struct vb2_queue *q, u32 state);
+void wave5_update_pix_fmt(struct v4l2_pix_format_mplane *pix_mp,
+			  int pix_fmt_type,
+			  unsigned int width,
+			  unsigned int height,
+			  const struct v4l2_frmsize_stepwise *frmsize);
 #endif
diff --git a/drivers/media/platform/chips-media/wave5/wave5-hw.c b/drivers/media/platform/chips-media/wave5/wave5-hw.c
index 2d82791f575e..d94cf84c3ee5 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-hw.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-hw.c
@@ -18,22 +18,33 @@
 #define QUEUE_REPORT_MASK		0xffff
 
 /* Encoder support fields */
-#define FEATURE_HEVC10BIT_ENC		BIT(3)
-#define FEATURE_AVC10BIT_ENC		BIT(11)
-#define FEATURE_AVC_ENCODER		BIT(1)
-#define FEATURE_HEVC_ENCODER		BIT(0)
+#define W521_FEATURE_HEVC10BIT_ENC	BIT(3)
+#define W521_FEATURE_AVC10BIT_ENC	BIT(11)
+#define W521_FEATURE_AVC_ENCODER	BIT(1)
+#define W521_FEATURE_HEVC_ENCODER	BIT(0)
+
+#define ENC_AVC_INTRA_IDR_PARAM_MASK	0x7ff
+#define ENC_AVC_INTRA_PERIOD_SHIFT		6
+#define ENC_AVC_IDR_PERIOD_SHIFT		17
+#define ENC_AVC_FORCED_IDR_HEADER_SHIFT		28
+
+#define ENC_HEVC_INTRA_QP_SHIFT			3
+#define ENC_HEVC_FORCED_IDR_HEADER_SHIFT	9
+#define ENC_HEVC_INTRA_PERIOD_SHIFT		16
 
 /* Decoder support fields */
-#define FEATURE_AVC_DECODER		BIT(3)
-#define FEATURE_HEVC_DECODER		BIT(2)
+#define W521_FEATURE_AVC_DECODER	BIT(3)
+#define W521_FEATURE_HEVC_DECODER	BIT(2)
+#define W515_FEATURE_HEVC10BIT_DEC	BIT(1)
+#define W515_FEATURE_HEVC_DECODER	BIT(0)
 
-#define FEATURE_BACKBONE		BIT(16)
-#define FEATURE_VCORE_BACKBONE		BIT(22)
-#define FEATURE_VCPU_BACKBONE		BIT(28)
+#define W521_FEATURE_BACKBONE		BIT(16)
+#define W521_FEATURE_VCORE_BACKBONE	BIT(22)
+#define W521_FEATURE_VCPU_BACKBONE	BIT(28)
 
 #define REMAP_CTRL_MAX_SIZE_BITS	((W5_REMAP_MAX_SIZE >> 12) & 0x1ff)
 #define REMAP_CTRL_REGISTER_VALUE(index)	(			\
-	(BIT(31) | (index << 12) | BIT(11) | REMAP_CTRL_MAX_SIZE_BITS)	\
+	(BIT(31) | ((index) << 12) | BIT(11) | REMAP_CTRL_MAX_SIZE_BITS)\
 )
 
 #define FASTIO_ADDRESS_MASK		GENMASK(15, 0)
@@ -155,6 +166,8 @@ static int wave5_wait_bus_busy(struct vpu_device *vpu_dev, unsigned int addr)
 {
 	u32 gdi_status_check_value = 0x3f;
 
+	if (vpu_dev->product_code == WAVE515_CODE)
+		gdi_status_check_value = 0x0738;
 	if (vpu_dev->product_code == WAVE521C_CODE ||
 	    vpu_dev->product_code == WAVE521_CODE ||
 	    vpu_dev->product_code == WAVE521E1_CODE)
@@ -186,6 +199,8 @@ unsigned int wave5_vpu_get_product_id(struct vpu_device *vpu_dev)
 	u32 val = vpu_read_reg(vpu_dev, W5_PRODUCT_NUMBER);
 
 	switch (val) {
+	case WAVE515_CODE:
+		return PRODUCT_ID_515;
 	case WAVE521C_CODE:
 		return PRODUCT_ID_521;
 	case WAVE521_CODE:
@@ -299,6 +314,27 @@ static int wave5_send_query(struct vpu_device *vpu_dev, struct vpu_instance *ins
 	return wave5_vpu_firmware_command_queue_error_check(vpu_dev, NULL);
 }
 
+static void setup_wave5_interrupts(struct vpu_device *vpu_dev)
+{
+	u32 reg_val = 0;
+
+	if (vpu_dev->attr.support_encoders) {
+		/* Encoder interrupt */
+		reg_val |= BIT(INT_WAVE5_ENC_SET_PARAM);
+		reg_val |= BIT(INT_WAVE5_ENC_PIC);
+		reg_val |= BIT(INT_WAVE5_BSBUF_FULL);
+	}
+
+	if (vpu_dev->attr.support_decoders) {
+		/* Decoder interrupt */
+		reg_val |= BIT(INT_WAVE5_INIT_SEQ);
+		reg_val |= BIT(INT_WAVE5_DEC_PIC);
+		reg_val |= BIT(INT_WAVE5_BSBUF_EMPTY);
+	}
+
+	return vpu_write_reg(vpu_dev, W5_VPU_VINT_ENABLE, reg_val);
+}
+
 static int setup_wave5_properties(struct device *dev)
 {
 	struct vpu_device *vpu_dev = dev_get_drvdata(dev);
@@ -328,17 +364,35 @@ static int setup_wave5_properties(struct device *dev)
 	hw_config_def1 = vpu_read_reg(vpu_dev, W5_RET_STD_DEF1);
 	hw_config_feature = vpu_read_reg(vpu_dev, W5_RET_CONF_FEATURE);
 
-	p_attr->support_hevc10bit_enc = FIELD_GET(FEATURE_HEVC10BIT_ENC, hw_config_feature);
-	p_attr->support_avc10bit_enc = FIELD_GET(FEATURE_AVC10BIT_ENC, hw_config_feature);
-
-	p_attr->support_decoders = FIELD_GET(FEATURE_AVC_DECODER, hw_config_def1) << STD_AVC;
-	p_attr->support_decoders |= FIELD_GET(FEATURE_HEVC_DECODER, hw_config_def1) << STD_HEVC;
-	p_attr->support_encoders = FIELD_GET(FEATURE_AVC_ENCODER, hw_config_def1) << STD_AVC;
-	p_attr->support_encoders |= FIELD_GET(FEATURE_HEVC_ENCODER, hw_config_def1) << STD_HEVC;
-
-	p_attr->support_backbone = FIELD_GET(FEATURE_BACKBONE, hw_config_def0);
-	p_attr->support_vcpu_backbone = FIELD_GET(FEATURE_VCPU_BACKBONE, hw_config_def0);
-	p_attr->support_vcore_backbone = FIELD_GET(FEATURE_VCORE_BACKBONE, hw_config_def0);
+	if (vpu_dev->product_code == WAVE515_CODE) {
+		p_attr->support_hevc10bit_dec = FIELD_GET(W515_FEATURE_HEVC10BIT_DEC,
+							  hw_config_feature);
+		p_attr->support_decoders = FIELD_GET(W515_FEATURE_HEVC_DECODER,
+						     hw_config_def1) << STD_HEVC;
+	} else {
+		p_attr->support_hevc10bit_enc = FIELD_GET(W521_FEATURE_HEVC10BIT_ENC,
+							  hw_config_feature);
+		p_attr->support_avc10bit_enc = FIELD_GET(W521_FEATURE_AVC10BIT_ENC,
+							 hw_config_feature);
+
+		p_attr->support_decoders = FIELD_GET(W521_FEATURE_AVC_DECODER,
+						     hw_config_def1) << STD_AVC;
+		p_attr->support_decoders |= FIELD_GET(W521_FEATURE_HEVC_DECODER,
+						      hw_config_def1) << STD_HEVC;
+		p_attr->support_encoders = FIELD_GET(W521_FEATURE_AVC_ENCODER,
+						     hw_config_def1) << STD_AVC;
+		p_attr->support_encoders |= FIELD_GET(W521_FEATURE_HEVC_ENCODER,
+						      hw_config_def1) << STD_HEVC;
+
+		p_attr->support_backbone = FIELD_GET(W521_FEATURE_BACKBONE,
+						     hw_config_def0);
+		p_attr->support_vcpu_backbone = FIELD_GET(W521_FEATURE_VCPU_BACKBONE,
+							  hw_config_def0);
+		p_attr->support_vcore_backbone = FIELD_GET(W521_FEATURE_VCORE_BACKBONE,
+							   hw_config_def0);
+	}
+
+	setup_wave5_interrupts(vpu_dev);
 
 	return 0;
 }
@@ -380,12 +434,18 @@ int wave5_vpu_init(struct device *dev, u8 *fw, size_t size)
 	common_vb = &vpu_dev->common_mem;
 
 	code_base = common_vb->daddr;
+
+	if (vpu_dev->product_code == WAVE515_CODE)
+		code_size = WAVE515_MAX_CODE_BUF_SIZE;
+	else
+		code_size = WAVE521_MAX_CODE_BUF_SIZE;
+
 	/* ALIGN TO 4KB */
-	code_size = (WAVE5_MAX_CODE_BUF_SIZE & ~0xfff);
+	code_size &= ~0xfff;
 	if (code_size < size * 2)
 		return -EINVAL;
 
-	temp_base = common_vb->daddr + WAVE5_TEMPBUF_OFFSET;
+	temp_base = code_base + code_size;
 	temp_size = WAVE5_TEMPBUF_SIZE;
 
 	ret = wave5_vdi_write_memory(vpu_dev, common_vb, 0, fw, size);
@@ -413,22 +473,15 @@ int wave5_vpu_init(struct device *dev, u8 *fw, size_t size)
 
 	/* These register must be reset explicitly */
 	vpu_write_reg(vpu_dev, W5_HW_OPTION, 0);
-	wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
-	wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
-	vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
-
-	/* Encoder interrupt */
-	reg_val = BIT(INT_WAVE5_ENC_SET_PARAM);
-	reg_val |= BIT(INT_WAVE5_ENC_PIC);
-	reg_val |= BIT(INT_WAVE5_BSBUF_FULL);
-	/* Decoder interrupt */
-	reg_val |= BIT(INT_WAVE5_INIT_SEQ);
-	reg_val |= BIT(INT_WAVE5_DEC_PIC);
-	reg_val |= BIT(INT_WAVE5_BSBUF_EMPTY);
-	vpu_write_reg(vpu_dev, W5_VPU_VINT_ENABLE, reg_val);
+
+	if (vpu_dev->product_code != WAVE515_CODE) {
+		wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
+		wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
+		vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
+	}
 
 	reg_val = vpu_read_reg(vpu_dev, W5_VPU_RET_VPU_CONFIG0);
-	if (FIELD_GET(FEATURE_BACKBONE, reg_val)) {
+	if (FIELD_GET(W521_FEATURE_BACKBONE, reg_val)) {
 		reg_val = ((WAVE5_PROC_AXI_ID << 28) |
 			   (WAVE5_PRP_AXI_ID << 24) |
 			   (WAVE5_FBD_Y_AXI_ID << 20) |
@@ -440,6 +493,24 @@ int wave5_vpu_init(struct device *dev, u8 *fw, size_t size)
 		wave5_fio_writel(vpu_dev, W5_BACKBONE_PROG_AXI_ID, reg_val);
 	}
 
+	if (vpu_dev->product_code == WAVE515_CODE) {
+		dma_addr_t task_buf_base;
+
+		vpu_write_reg(vpu_dev, W5_CMD_INIT_NUM_TASK_BUF, WAVE515_COMMAND_QUEUE_DEPTH);
+		vpu_write_reg(vpu_dev, W5_CMD_INIT_TASK_BUF_SIZE, WAVE515_ONE_TASKBUF_SIZE);
+
+		for (i = 0; i < WAVE515_COMMAND_QUEUE_DEPTH; i++) {
+			task_buf_base = temp_base + temp_size +
+					(i * WAVE515_ONE_TASKBUF_SIZE);
+			vpu_write_reg(vpu_dev,
+				      W5_CMD_INIT_ADDR_TASK_BUF0 + (i * 4),
+				      task_buf_base);
+		}
+
+		vpu_write_reg(vpu_dev, W515_CMD_ADDR_SEC_AXI, vpu_dev->sram_buf.daddr);
+		vpu_write_reg(vpu_dev, W515_CMD_SEC_AXI_SIZE, vpu_dev->sram_buf.size);
+	}
+
 	vpu_write_reg(vpu_dev, W5_VPU_BUSY_STATUS, 1);
 	vpu_write_reg(vpu_dev, W5_COMMAND, W5_INIT_VPU);
 	vpu_write_reg(vpu_dev, W5_VPU_REMAP_CORE_START, 1);
@@ -480,30 +551,41 @@ int wave5_vpu_build_up_dec_param(struct vpu_instance *inst,
 		return -EINVAL;
 	}
 
-	p_dec_info->vb_work.size = WAVE521DEC_WORKBUF_SIZE;
+	if (vpu_dev->product == PRODUCT_ID_515)
+		p_dec_info->vb_work.size = WAVE515DEC_WORKBUF_SIZE;
+	else
+		p_dec_info->vb_work.size = WAVE521DEC_WORKBUF_SIZE;
+
 	ret = wave5_vdi_allocate_dma_memory(inst->dev, &p_dec_info->vb_work);
 	if (ret)
 		return ret;
 
-	vpu_write_reg(inst->dev, W5_CMD_DEC_VCORE_INFO, 1);
+	if (inst->dev->product_code != WAVE515_CODE)
+		vpu_write_reg(inst->dev, W5_CMD_DEC_VCORE_INFO, 1);
 
 	wave5_vdi_clear_memory(inst->dev, &p_dec_info->vb_work);
 
 	vpu_write_reg(inst->dev, W5_ADDR_WORK_BASE, p_dec_info->vb_work.daddr);
 	vpu_write_reg(inst->dev, W5_WORK_SIZE, p_dec_info->vb_work.size);
 
-	vpu_write_reg(inst->dev, W5_CMD_ADDR_SEC_AXI, vpu_dev->sram_buf.daddr);
-	vpu_write_reg(inst->dev, W5_CMD_SEC_AXI_SIZE, vpu_dev->sram_buf.size);
+	if (inst->dev->product_code != WAVE515_CODE) {
+		vpu_write_reg(inst->dev, W5_CMD_ADDR_SEC_AXI, vpu_dev->sram_buf.daddr);
+		vpu_write_reg(inst->dev, W5_CMD_SEC_AXI_SIZE, vpu_dev->sram_buf.size);
+	}
 
 	vpu_write_reg(inst->dev, W5_CMD_DEC_BS_START_ADDR, p_dec_info->stream_buf_start_addr);
 	vpu_write_reg(inst->dev, W5_CMD_DEC_BS_SIZE, p_dec_info->stream_buf_size);
 
 	/* NOTE: SDMA reads MSB first */
 	vpu_write_reg(inst->dev, W5_CMD_BS_PARAM, BITSTREAM_ENDIANNESS_BIG_ENDIAN);
-	/* This register must be reset explicitly */
-	vpu_write_reg(inst->dev, W5_CMD_EXT_ADDR, 0);
-	vpu_write_reg(inst->dev, W5_CMD_NUM_CQ_DEPTH_M1, (COMMAND_QUEUE_DEPTH - 1));
 
+	if (inst->dev->product_code != WAVE515_CODE) {
+		/* This register must be reset explicitly */
+		vpu_write_reg(inst->dev, W5_CMD_EXT_ADDR, 0);
+		vpu_write_reg(inst->dev, W5_CMD_NUM_CQ_DEPTH_M1,
+			      WAVE521_COMMAND_QUEUE_DEPTH - 1);
+	}
+	vpu_write_reg(inst->dev, W5_CMD_ERR_CONCEAL, 0);
 	ret = send_firmware_command(inst, W5_CREATE_INSTANCE, true, NULL, NULL);
 	if (ret) {
 		wave5_vdi_free_dma_memory(vpu_dev, &p_dec_info->vb_work);
@@ -553,7 +635,7 @@ static u32 get_bitstream_options(struct dec_info *info)
 int wave5_vpu_dec_init_seq(struct vpu_instance *inst)
 {
 	struct dec_info *p_dec_info = &inst->codec_info->dec_info;
-	u32 cmd_option = INIT_SEQ_NORMAL;
+	u32 bs_option, cmd_option = INIT_SEQ_NORMAL;
 	u32 reg_val, fail_res;
 	int ret;
 
@@ -563,7 +645,13 @@ int wave5_vpu_dec_init_seq(struct vpu_instance *inst)
 	vpu_write_reg(inst->dev, W5_BS_RD_PTR, p_dec_info->stream_rd_ptr);
 	vpu_write_reg(inst->dev, W5_BS_WR_PTR, p_dec_info->stream_wr_ptr);
 
-	vpu_write_reg(inst->dev, W5_BS_OPTION, get_bitstream_options(p_dec_info));
+	bs_option = get_bitstream_options(p_dec_info);
+
+	/* Without RD_PTR_VALID_FLAG Wave515 ignores RD_PTR value */
+	if (inst->dev->product_code == WAVE515_CODE)
+		bs_option |= BSOPTION_RD_PTR_VALID_FLAG;
+
+	vpu_write_reg(inst->dev, W5_BS_OPTION, bs_option);
 
 	vpu_write_reg(inst->dev, W5_COMMAND_OPTION, cmd_option);
 	vpu_write_reg(inst->dev, W5_CMD_DEC_USER_MASK, p_dec_info->user_data_enable);
@@ -629,10 +717,12 @@ static void wave5_get_dec_seq_result(struct vpu_instance *inst, struct dec_initi
 		info->profile = FIELD_GET(SEQ_PARAM_PROFILE_MASK, reg_val);
 	}
 
-	info->vlc_buf_size = vpu_read_reg(inst->dev, W5_RET_VLC_BUF_SIZE);
-	info->param_buf_size = vpu_read_reg(inst->dev, W5_RET_PARAM_BUF_SIZE);
-	p_dec_info->vlc_buf_size = info->vlc_buf_size;
-	p_dec_info->param_buf_size = info->param_buf_size;
+	if (inst->dev->product_code != WAVE515_CODE) {
+		info->vlc_buf_size = vpu_read_reg(inst->dev, W5_RET_VLC_BUF_SIZE);
+		info->param_buf_size = vpu_read_reg(inst->dev, W5_RET_PARAM_BUF_SIZE);
+		p_dec_info->vlc_buf_size = info->vlc_buf_size;
+		p_dec_info->param_buf_size = info->param_buf_size;
+	}
 }
 
 int wave5_vpu_dec_get_seq_info(struct vpu_instance *inst, struct dec_initial_info *info)
@@ -734,22 +824,27 @@ int wave5_vpu_dec_register_framebuffer(struct vpu_instance *inst, struct frame_b
 
 		pic_size = (init_info->pic_width << 16) | (init_info->pic_height);
 
-		vb_buf.size = (p_dec_info->vlc_buf_size * VLC_BUF_NUM) +
-				(p_dec_info->param_buf_size * COMMAND_QUEUE_DEPTH);
-		vb_buf.daddr = 0;
+		if (inst->dev->product_code != WAVE515_CODE) {
+			vb_buf.size = (p_dec_info->vlc_buf_size * VLC_BUF_NUM) +
+				(p_dec_info->param_buf_size * WAVE521_COMMAND_QUEUE_DEPTH);
+			vb_buf.daddr = 0;
 
-		if (vb_buf.size != p_dec_info->vb_task.size) {
-			wave5_vdi_free_dma_memory(inst->dev, &p_dec_info->vb_task);
-			ret = wave5_vdi_allocate_dma_memory(inst->dev, &vb_buf);
-			if (ret)
-				goto free_fbc_c_tbl_buffers;
+			if (vb_buf.size != p_dec_info->vb_task.size) {
+				wave5_vdi_free_dma_memory(inst->dev,
+							  &p_dec_info->vb_task);
+				ret = wave5_vdi_allocate_dma_memory(inst->dev,
+								    &vb_buf);
+				if (ret)
+					goto free_fbc_c_tbl_buffers;
 
-			p_dec_info->vb_task = vb_buf;
-		}
+				p_dec_info->vb_task = vb_buf;
+			}
 
-		vpu_write_reg(inst->dev, W5_CMD_SET_FB_ADDR_TASK_BUF,
-			      p_dec_info->vb_task.daddr);
-		vpu_write_reg(inst->dev, W5_CMD_SET_FB_TASK_BUF_SIZE, vb_buf.size);
+			vpu_write_reg(inst->dev, W5_CMD_SET_FB_ADDR_TASK_BUF,
+				      p_dec_info->vb_task.daddr);
+			vpu_write_reg(inst->dev, W5_CMD_SET_FB_TASK_BUF_SIZE,
+				      vb_buf.size);
+		}
 	} else {
 		pic_size = (init_info->pic_width << 16) | (init_info->pic_height);
 
@@ -830,6 +925,43 @@ free_mv_buffers:
 	return ret;
 }
 
+static u32 wave5_vpu_dec_validate_sec_axi(struct vpu_instance *inst)
+{
+	u32 bitdepth = inst->codec_info->dec_info.initial_info.luma_bitdepth;
+	struct dec_info *p_dec_info = &inst->codec_info->dec_info;
+	u32 bit_size = 0, ip_size = 0, lf_size = 0, ret = 0;
+	u32 sram_size = inst->dev->sram_size;
+	u32 width = inst->src_fmt.width;
+
+	if (!sram_size)
+		return 0;
+
+	/*
+	 * TODO: calculate bit_size, ip_size, lf_size from width and bitdepth
+	 * for Wave521.
+	 */
+	if (inst->dev->product_code == WAVE515_CODE) {
+		bit_size = DIV_ROUND_UP(width, 16) * 5 * 8;
+		ip_size = ALIGN(width, 16) * 2 * bitdepth / 8;
+		lf_size = ALIGN(width, 16) * 10 * bitdepth / 8;
+	}
+
+	if (p_dec_info->sec_axi_info.use_bit_enable && sram_size >= bit_size) {
+		ret |= BIT(0);
+		sram_size -= bit_size;
+	}
+
+	if (p_dec_info->sec_axi_info.use_ip_enable && sram_size >= ip_size) {
+		ret |= BIT(9);
+		sram_size -= ip_size;
+	}
+
+	if (p_dec_info->sec_axi_info.use_lf_row_enable && sram_size >= lf_size)
+		ret |= BIT(15);
+
+	return ret;
+}
+
 int wave5_vpu_decode(struct vpu_instance *inst, u32 *fail_res)
 {
 	u32 reg_val;
@@ -842,9 +974,7 @@ int wave5_vpu_decode(struct vpu_instance *inst, u32 *fail_res)
 	vpu_write_reg(inst->dev, W5_BS_OPTION, get_bitstream_options(p_dec_info));
 
 	/* secondary AXI */
-	reg_val = p_dec_info->sec_axi_info.use_bit_enable |
-		(p_dec_info->sec_axi_info.use_ip_enable << 9) |
-		(p_dec_info->sec_axi_info.use_lf_row_enable << 15);
+	reg_val = wave5_vpu_dec_validate_sec_axi(inst);
 	vpu_write_reg(inst->dev, W5_USE_SEC_AXI, reg_val);
 
 	/* set attributes of user buffer */
@@ -992,11 +1122,18 @@ int wave5_vpu_re_init(struct device *dev, u8 *fw, size_t size)
 	common_vb = &vpu_dev->common_mem;
 
 	code_base = common_vb->daddr;
+
+	if (vpu_dev->product_code == WAVE515_CODE)
+		code_size = WAVE515_MAX_CODE_BUF_SIZE;
+	else
+		code_size = WAVE521_MAX_CODE_BUF_SIZE;
+
 	/* ALIGN TO 4KB */
-	code_size = (WAVE5_MAX_CODE_BUF_SIZE & ~0xfff);
+	code_size &= ~0xfff;
 	if (code_size < size * 2)
 		return -EINVAL;
-	temp_base = common_vb->daddr + WAVE5_TEMPBUF_OFFSET;
+
+	temp_base = code_base + code_size;
 	temp_size = WAVE5_TEMPBUF_SIZE;
 
 	old_code_base = vpu_read_reg(vpu_dev, W5_VPU_REMAP_PADDR);
@@ -1030,22 +1167,15 @@ int wave5_vpu_re_init(struct device *dev, u8 *fw, size_t size)
 
 		/* These register must be reset explicitly */
 		vpu_write_reg(vpu_dev, W5_HW_OPTION, 0);
-		wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
-		wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
-		vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
 
-		/* Encoder interrupt */
-		reg_val = BIT(INT_WAVE5_ENC_SET_PARAM);
-		reg_val |= BIT(INT_WAVE5_ENC_PIC);
-		reg_val |= BIT(INT_WAVE5_BSBUF_FULL);
-		/* Decoder interrupt */
-		reg_val |= BIT(INT_WAVE5_INIT_SEQ);
-		reg_val |= BIT(INT_WAVE5_DEC_PIC);
-		reg_val |= BIT(INT_WAVE5_BSBUF_EMPTY);
-		vpu_write_reg(vpu_dev, W5_VPU_VINT_ENABLE, reg_val);
+		if (vpu_dev->product_code != WAVE515_CODE) {
+			wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
+			wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
+			vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
+		}
 
 		reg_val = vpu_read_reg(vpu_dev, W5_VPU_RET_VPU_CONFIG0);
-		if (FIELD_GET(FEATURE_BACKBONE, reg_val)) {
+		if (FIELD_GET(W521_FEATURE_BACKBONE, reg_val)) {
 			reg_val = ((WAVE5_PROC_AXI_ID << 28) |
 					(WAVE5_PRP_AXI_ID << 24) |
 					(WAVE5_FBD_Y_AXI_ID << 20) |
@@ -1057,6 +1187,29 @@ int wave5_vpu_re_init(struct device *dev, u8 *fw, size_t size)
 			wave5_fio_writel(vpu_dev, W5_BACKBONE_PROG_AXI_ID, reg_val);
 		}
 
+		if (vpu_dev->product_code == WAVE515_CODE) {
+			dma_addr_t task_buf_base;
+			u32 i;
+
+			vpu_write_reg(vpu_dev, W5_CMD_INIT_NUM_TASK_BUF,
+				      WAVE515_COMMAND_QUEUE_DEPTH);
+			vpu_write_reg(vpu_dev, W5_CMD_INIT_TASK_BUF_SIZE,
+				      WAVE515_ONE_TASKBUF_SIZE);
+
+			for (i = 0; i < WAVE515_COMMAND_QUEUE_DEPTH; i++) {
+				task_buf_base = temp_base + temp_size +
+						(i * WAVE515_ONE_TASKBUF_SIZE);
+				vpu_write_reg(vpu_dev,
+					      W5_CMD_INIT_ADDR_TASK_BUF0 + (i * 4),
+					      task_buf_base);
+			}
+
+			vpu_write_reg(vpu_dev, W515_CMD_ADDR_SEC_AXI,
+				      vpu_dev->sram_buf.daddr);
+			vpu_write_reg(vpu_dev, W515_CMD_SEC_AXI_SIZE,
+				      vpu_dev->sram_buf.size);
+		}
+
 		vpu_write_reg(vpu_dev, W5_VPU_BUSY_STATUS, 1);
 		vpu_write_reg(vpu_dev, W5_COMMAND, W5_INIT_VPU);
 		vpu_write_reg(vpu_dev, W5_VPU_REMAP_CORE_START, 1);
@@ -1075,13 +1228,13 @@ int wave5_vpu_re_init(struct device *dev, u8 *fw, size_t size)
 	return setup_wave5_properties(dev);
 }
 
-static int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uint16_t *code,
-				size_t size)
+int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uint16_t *code,
+			 size_t size)
 {
 	u32 reg_val;
 	struct vpu_buf *common_vb;
-	dma_addr_t code_base;
-	u32 code_size, reason_code;
+	dma_addr_t code_base, temp_base;
+	u32 code_size, temp_size, reason_code;
 	struct vpu_device *vpu_dev = dev_get_drvdata(dev);
 	int ret;
 
@@ -1111,13 +1264,22 @@ static int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uin
 		common_vb = &vpu_dev->common_mem;
 
 		code_base = common_vb->daddr;
+
+		if (vpu_dev->product_code == WAVE515_CODE)
+			code_size = WAVE515_MAX_CODE_BUF_SIZE;
+		else
+			code_size = WAVE521_MAX_CODE_BUF_SIZE;
+
 		/* ALIGN TO 4KB */
-		code_size = (WAVE5_MAX_CODE_BUF_SIZE & ~0xfff);
+		code_size &= ~0xfff;
 		if (code_size < size * 2) {
 			dev_err(dev, "size too small\n");
 			return -EINVAL;
 		}
 
+		temp_base = code_base + code_size;
+		temp_size = WAVE5_TEMPBUF_SIZE;
+
 		/* Power on without DEBUG mode */
 		vpu_write_reg(vpu_dev, W5_PO_CONF, 0);
 
@@ -1130,22 +1292,17 @@ static int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uin
 
 		/* These register must be reset explicitly */
 		vpu_write_reg(vpu_dev, W5_HW_OPTION, 0);
-		wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
-		wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
-		vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
 
-		/* Encoder interrupt */
-		reg_val = BIT(INT_WAVE5_ENC_SET_PARAM);
-		reg_val |= BIT(INT_WAVE5_ENC_PIC);
-		reg_val |= BIT(INT_WAVE5_BSBUF_FULL);
-		/* Decoder interrupt */
-		reg_val |= BIT(INT_WAVE5_INIT_SEQ);
-		reg_val |= BIT(INT_WAVE5_DEC_PIC);
-		reg_val |= BIT(INT_WAVE5_BSBUF_EMPTY);
-		vpu_write_reg(vpu_dev, W5_VPU_VINT_ENABLE, reg_val);
+		if (vpu_dev->product_code != WAVE515_CODE) {
+			wave5_fio_writel(vpu_dev, W5_BACKBONE_PROC_EXT_ADDR, 0);
+			wave5_fio_writel(vpu_dev, W5_BACKBONE_AXI_PARAM, 0);
+			vpu_write_reg(vpu_dev, W5_SEC_AXI_PARAM, 0);
+		}
+
+		setup_wave5_interrupts(vpu_dev);
 
 		reg_val = vpu_read_reg(vpu_dev, W5_VPU_RET_VPU_CONFIG0);
-		if (FIELD_GET(FEATURE_BACKBONE, reg_val)) {
+		if (FIELD_GET(W521_FEATURE_BACKBONE, reg_val)) {
 			reg_val = ((WAVE5_PROC_AXI_ID << 28) |
 					(WAVE5_PRP_AXI_ID << 24) |
 					(WAVE5_FBD_Y_AXI_ID << 20) |
@@ -1157,6 +1314,29 @@ static int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uin
 			wave5_fio_writel(vpu_dev, W5_BACKBONE_PROG_AXI_ID, reg_val);
 		}
 
+		if (vpu_dev->product_code == WAVE515_CODE) {
+			dma_addr_t task_buf_base;
+			u32 i;
+
+			vpu_write_reg(vpu_dev, W5_CMD_INIT_NUM_TASK_BUF,
+				      WAVE515_COMMAND_QUEUE_DEPTH);
+			vpu_write_reg(vpu_dev, W5_CMD_INIT_TASK_BUF_SIZE,
+				      WAVE515_ONE_TASKBUF_SIZE);
+
+			for (i = 0; i < WAVE515_COMMAND_QUEUE_DEPTH; i++) {
+				task_buf_base = temp_base + temp_size +
+						(i * WAVE515_ONE_TASKBUF_SIZE);
+				vpu_write_reg(vpu_dev,
+					      W5_CMD_INIT_ADDR_TASK_BUF0 + (i * 4),
+					      task_buf_base);
+			}
+
+			vpu_write_reg(vpu_dev, W515_CMD_ADDR_SEC_AXI,
+				      vpu_dev->sram_buf.daddr);
+			vpu_write_reg(vpu_dev, W515_CMD_SEC_AXI_SIZE,
+				      vpu_dev->sram_buf.size);
+		}
+
 		vpu_write_reg(vpu_dev, W5_VPU_BUSY_STATUS, 1);
 		vpu_write_reg(vpu_dev, W5_COMMAND, W5_WAKEUP_VPU);
 		/* Start VPU after settings */
@@ -1401,7 +1581,7 @@ int wave5_vpu_build_up_enc_param(struct device *dev, struct vpu_instance *inst,
 	reg_val = (open_param->line_buf_int_en << 6) | BITSTREAM_ENDIANNESS_BIG_ENDIAN;
 	vpu_write_reg(inst->dev, W5_CMD_BS_PARAM, reg_val);
 	vpu_write_reg(inst->dev, W5_CMD_EXT_ADDR, 0);
-	vpu_write_reg(inst->dev, W5_CMD_NUM_CQ_DEPTH_M1, (COMMAND_QUEUE_DEPTH - 1));
+	vpu_write_reg(inst->dev, W5_CMD_NUM_CQ_DEPTH_M1, WAVE521_COMMAND_QUEUE_DEPTH - 1);
 
 	/* This register must be reset explicitly */
 	vpu_write_reg(inst->dev, W5_CMD_ENC_SRC_OPTIONS, 0);
@@ -1601,12 +1781,19 @@ int wave5_vpu_enc_init_seq(struct vpu_instance *inst)
 
 	if (inst->std == W_AVC_ENC)
 		vpu_write_reg(inst->dev, W5_CMD_ENC_SEQ_INTRA_PARAM, p_param->intra_qp |
-				((p_param->intra_period & 0x7ff) << 6) |
-				((p_param->avc_idr_period & 0x7ff) << 17));
+			      ((p_param->intra_period & ENC_AVC_INTRA_IDR_PARAM_MASK)
+				<< ENC_AVC_INTRA_PERIOD_SHIFT) |
+			      ((p_param->avc_idr_period & ENC_AVC_INTRA_IDR_PARAM_MASK)
+				<< ENC_AVC_IDR_PERIOD_SHIFT) |
+			      (p_param->forced_idr_header_enable
+			       << ENC_AVC_FORCED_IDR_HEADER_SHIFT));
 	else if (inst->std == W_HEVC_ENC)
 		vpu_write_reg(inst->dev, W5_CMD_ENC_SEQ_INTRA_PARAM,
-			      p_param->decoding_refresh_type | (p_param->intra_qp << 3) |
-				(p_param->intra_period << 16));
+			      p_param->decoding_refresh_type |
+			      (p_param->intra_qp << ENC_HEVC_INTRA_QP_SHIFT) |
+			      (p_param->forced_idr_header_enable
+			       << ENC_HEVC_FORCED_IDR_HEADER_SHIFT) |
+			      (p_param->intra_period << ENC_HEVC_INTRA_PERIOD_SHIFT));
 
 	reg_val = (p_param->rdo_skip << 2) |
 		(p_param->lambda_scaling_enable << 3) |
@@ -1855,7 +2042,7 @@ int wave5_vpu_enc_register_framebuffer(struct device *dev, struct vpu_instance *
 	p_enc_info->vb_sub_sam_buf = vb_sub_sam_buf;
 
 	vb_task.size = (p_enc_info->vlc_buf_size * VLC_BUF_NUM) +
-			(p_enc_info->param_buf_size * COMMAND_QUEUE_DEPTH);
+			(p_enc_info->param_buf_size * WAVE521_COMMAND_QUEUE_DEPTH);
 	vb_task.daddr = 0;
 	if (p_enc_info->vb_task.size == 0) {
 		ret = wave5_vdi_allocate_dma_memory(vpu_dev, &vb_task);
@@ -1943,6 +2130,31 @@ free_vb_fbc_y_tbl:
 	return ret;
 }
 
+static u32 wave5_vpu_enc_validate_sec_axi(struct vpu_instance *inst)
+{
+	struct enc_info *p_enc_info = &inst->codec_info->enc_info;
+	u32 rdo_size = 0, lf_size = 0, ret = 0;
+	u32 sram_size = inst->dev->sram_size;
+
+	if (!sram_size)
+		return 0;
+
+	/*
+	 * TODO: calculate rdo_size and lf_size from inst->src_fmt.width and
+	 * inst->codec_info->enc_info.open_param.wave_param.internal_bit_depth
+	 */
+
+	if (p_enc_info->sec_axi_info.use_enc_rdo_enable && sram_size >= rdo_size) {
+		ret |= BIT(11);
+		sram_size -= rdo_size;
+	}
+
+	if (p_enc_info->sec_axi_info.use_enc_lf_enable && sram_size >= lf_size)
+		ret |= BIT(15);
+
+	return ret;
+}
+
 int wave5_vpu_encode(struct vpu_instance *inst, struct enc_param *option, u32 *fail_res)
 {
 	u32 src_frame_format;
@@ -1964,8 +2176,7 @@ int wave5_vpu_encode(struct vpu_instance *inst, struct enc_param *option, u32 *f
 
 	vpu_write_reg(inst->dev, W5_CMD_ENC_PIC_SRC_AXI_SEL, DEFAULT_SRC_AXI);
 	/* secondary AXI */
-	reg_val = (p_enc_info->sec_axi_info.use_enc_rdo_enable << 11) |
-		(p_enc_info->sec_axi_info.use_enc_lf_enable << 15);
+	reg_val = wave5_vpu_enc_validate_sec_axi(inst);
 	vpu_write_reg(inst->dev, W5_CMD_ENC_PIC_USE_SEC_AXI, reg_val);
 
 	vpu_write_reg(inst->dev, W5_CMD_ENC_PIC_REPORT_PARAM, 0);
diff --git a/drivers/media/platform/chips-media/wave5/wave5-regdefine.h b/drivers/media/platform/chips-media/wave5/wave5-regdefine.h
index a15c6b2c3d8b..557344754c4c 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-regdefine.h
+++ b/drivers/media/platform/chips-media/wave5/wave5-regdefine.h
@@ -205,6 +205,9 @@ enum query_opt {
 #define W5_ADDR_TEMP_BASE                       (W5_REG_BASE + 0x011C)
 #define W5_TEMP_SIZE                            (W5_REG_BASE + 0x0120)
 #define W5_HW_OPTION                            (W5_REG_BASE + 0x012C)
+#define W5_CMD_INIT_NUM_TASK_BUF		(W5_REG_BASE + 0x0134)
+#define W5_CMD_INIT_ADDR_TASK_BUF0		(W5_REG_BASE + 0x0138)
+#define W5_CMD_INIT_TASK_BUF_SIZE		(W5_REG_BASE + 0x0178)
 #define W5_SEC_AXI_PARAM                        (W5_REG_BASE + 0x0180)
 
 /************************************************************************/
@@ -216,7 +219,9 @@ enum query_opt {
 #define W5_CMD_DEC_BS_SIZE                      (W5_REG_BASE + 0x0120)
 #define W5_CMD_BS_PARAM                         (W5_REG_BASE + 0x0124)
 #define W5_CMD_ADDR_SEC_AXI                     (W5_REG_BASE + 0x0130)
+#define W515_CMD_ADDR_SEC_AXI			(W5_REG_BASE + 0x0124)
 #define W5_CMD_SEC_AXI_SIZE                     (W5_REG_BASE + 0x0134)
+#define W515_CMD_SEC_AXI_SIZE			(W5_REG_BASE + 0x0128)
 #define W5_CMD_EXT_ADDR                         (W5_REG_BASE + 0x0138)
 #define W5_CMD_NUM_CQ_DEPTH_M1                  (W5_REG_BASE + 0x013C)
 #define W5_CMD_ERR_CONCEAL                      (W5_REG_BASE + 0x0140)
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vdi.c b/drivers/media/platform/chips-media/wave5/wave5-vdi.c
index 3809f70bc0b4..bb13267ced38 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vdi.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-vdi.c
@@ -18,7 +18,11 @@ static int wave5_vdi_allocate_common_memory(struct device *dev)
 	if (!vpu_dev->common_mem.vaddr) {
 		int ret;
 
-		vpu_dev->common_mem.size = SIZE_COMMON;
+		if (vpu_dev->product_code == WAVE515_CODE)
+			vpu_dev->common_mem.size = WAVE515_SIZE_COMMON;
+		else
+			vpu_dev->common_mem.size = WAVE521_SIZE_COMMON;
+
 		ret = wave5_vdi_allocate_dma_memory(vpu_dev, &vpu_dev->common_mem);
 		if (ret) {
 			dev_err(dev, "unable to allocate common buffer\n");
@@ -174,16 +178,19 @@ int wave5_vdi_allocate_array(struct vpu_device *vpu_dev, struct vpu_buf *array,
 void wave5_vdi_allocate_sram(struct vpu_device *vpu_dev)
 {
 	struct vpu_buf *vb = &vpu_dev->sram_buf;
+	dma_addr_t daddr;
+	void *vaddr;
+	size_t size;
 
-	if (!vpu_dev->sram_pool || !vpu_dev->sram_size)
+	if (!vpu_dev->sram_pool || vb->vaddr)
 		return;
 
-	if (!vb->vaddr) {
-		vb->size = vpu_dev->sram_size;
-		vb->vaddr = gen_pool_dma_alloc(vpu_dev->sram_pool, vb->size,
-					       &vb->daddr);
-		if (!vb->vaddr)
-			vb->size = 0;
+	size = min_t(size_t, vpu_dev->sram_size, gen_pool_avail(vpu_dev->sram_pool));
+	vaddr = gen_pool_dma_alloc(vpu_dev->sram_pool, size, &daddr);
+	if (vaddr) {
+		vb->vaddr = vaddr;
+		vb->daddr = daddr;
+		vb->size = size;
 	}
 
 	dev_dbg(vpu_dev->dev, "%s: sram daddr: %pad, size: %zu, vaddr: 0x%p\n",
@@ -197,9 +204,7 @@ void wave5_vdi_free_sram(struct vpu_device *vpu_dev)
 	if (!vb->size || !vb->vaddr)
 		return;
 
-	if (vb->vaddr)
-		gen_pool_free(vpu_dev->sram_pool, (unsigned long)vb->vaddr,
-			      vb->size);
+	gen_pool_free(vpu_dev->sram_pool, (unsigned long)vb->vaddr, vb->size);
 
 	memset(vb, 0, sizeof(*vb));
 }
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpu-dec.c b/drivers/media/platform/chips-media/wave5/wave5-vpu-dec.c
index ef227af72348..fd71f0c43ac3 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpu-dec.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpu-dec.c
@@ -5,116 +5,98 @@
  * Copyright (C) 2021-2023 CHIPS&MEDIA INC
  */
 
+#include <linux/pm_runtime.h>
 #include "wave5-helper.h"
 
 #define VPU_DEC_DEV_NAME "C&M Wave5 VPU decoder"
 #define VPU_DEC_DRV_NAME "wave5-dec"
 
-#define DEFAULT_SRC_SIZE(width, height) ({			\
-	(width) * (height) / 8 * 3;					\
-})
+static const struct v4l2_frmsize_stepwise dec_hevc_frmsize = {
+	.min_width = W5_MIN_DEC_PIC_8_WIDTH,
+	.max_width = W5_MAX_DEC_PIC_WIDTH,
+	.step_width = W5_DEC_CODEC_STEP_WIDTH,
+	.min_height = W5_MIN_DEC_PIC_8_HEIGHT,
+	.max_height = W5_MAX_DEC_PIC_HEIGHT,
+	.step_height = W5_DEC_CODEC_STEP_HEIGHT,
+};
+
+static const struct v4l2_frmsize_stepwise dec_h264_frmsize = {
+	.min_width = W5_MIN_DEC_PIC_32_WIDTH,
+	.max_width = W5_MAX_DEC_PIC_WIDTH,
+	.step_width = W5_DEC_CODEC_STEP_WIDTH,
+	.min_height = W5_MIN_DEC_PIC_32_HEIGHT,
+	.max_height = W5_MAX_DEC_PIC_HEIGHT,
+	.step_height = W5_DEC_CODEC_STEP_HEIGHT,
+};
+
+static const struct v4l2_frmsize_stepwise dec_raw_frmsize = {
+	.min_width = W5_MIN_DEC_PIC_8_WIDTH,
+	.max_width = W5_MAX_DEC_PIC_WIDTH,
+	.step_width = W5_DEC_RAW_STEP_WIDTH,
+	.min_height = W5_MIN_DEC_PIC_8_HEIGHT,
+	.max_height = W5_MAX_DEC_PIC_HEIGHT,
+	.step_height = W5_DEC_RAW_STEP_HEIGHT,
+};
 
 static const struct vpu_format dec_fmt_list[FMT_TYPES][MAX_FMTS] = {
 	[VPU_FMT_TYPE_CODEC] = {
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_HEVC,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_hevc_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_H264,
-			.max_width = 8192,
-			.min_width = 32,
-			.max_height = 4320,
-			.min_height = 32,
+			.v4l2_frmsize = &dec_h264_frmsize,
 		},
 	},
 	[VPU_FMT_TYPE_RAW] = {
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV420,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV12,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV21,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV422P,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV16,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV61,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV420M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV12M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV21M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV422M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV16M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV61M,
-			.max_width = 8192,
-			.min_width = 8,
-			.max_height = 4320,
-			.min_height = 8,
+			.v4l2_frmsize = &dec_raw_frmsize,
 		},
 	}
 };
@@ -233,74 +215,6 @@ static void wave5_handle_src_buffer(struct vpu_instance *inst, dma_addr_t rd_ptr
 	inst->remaining_consumed_bytes = consumed_bytes;
 }
 
-static void wave5_update_pix_fmt(struct v4l2_pix_format_mplane *pix_mp, unsigned int width,
-				 unsigned int height)
-{
-	switch (pix_mp->pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_NV12:
-	case V4L2_PIX_FMT_NV21:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height * 3 / 2;
-		break;
-	case V4L2_PIX_FMT_YUV422P:
-	case V4L2_PIX_FMT_NV16:
-	case V4L2_PIX_FMT_NV61:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height * 2;
-		break;
-	case V4L2_PIX_FMT_YUV420M:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[1].sizeimage = width * height / 4;
-		pix_mp->plane_fmt[2].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[2].sizeimage = width * height / 4;
-		break;
-	case V4L2_PIX_FMT_NV12M:
-	case V4L2_PIX_FMT_NV21M:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[1].sizeimage = width * height / 2;
-		break;
-	case V4L2_PIX_FMT_YUV422M:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[1].sizeimage = width * height / 2;
-		pix_mp->plane_fmt[2].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[2].sizeimage = width * height / 2;
-		break;
-	case V4L2_PIX_FMT_NV16M:
-	case V4L2_PIX_FMT_NV61M:
-		pix_mp->width = round_up(width, 32);
-		pix_mp->height = round_up(height, 16);
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = width * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[1].sizeimage = width * height;
-		break;
-	default:
-		pix_mp->width = width;
-		pix_mp->height = height;
-		pix_mp->plane_fmt[0].bytesperline = 0;
-		pix_mp->plane_fmt[0].sizeimage = max(DEFAULT_SRC_SIZE(width, height),
-						     pix_mp->plane_fmt[0].sizeimage);
-		break;
-	}
-}
-
 static int start_decode(struct vpu_instance *inst, u32 *fail_res)
 {
 	struct v4l2_m2m_ctx *m2m_ctx = inst->v4l2_fh.m2m_ctx;
@@ -388,6 +302,8 @@ static int handle_dynamic_resolution_change(struct vpu_instance *inst)
 	}
 
 	if (p_dec_info->initial_info_obtained) {
+		const struct vpu_format *vpu_fmt;
+
 		inst->conf_win.left = initial_info->pic_crop_rect.left;
 		inst->conf_win.top = initial_info->pic_crop_rect.top;
 		inst->conf_win.width = initial_info->pic_width -
@@ -395,10 +311,27 @@ static int handle_dynamic_resolution_change(struct vpu_instance *inst)
 		inst->conf_win.height = initial_info->pic_height -
 			initial_info->pic_crop_rect.top - initial_info->pic_crop_rect.bottom;
 
-		wave5_update_pix_fmt(&inst->src_fmt, initial_info->pic_width,
-				     initial_info->pic_height);
-		wave5_update_pix_fmt(&inst->dst_fmt, initial_info->pic_width,
-				     initial_info->pic_height);
+		vpu_fmt = wave5_find_vpu_fmt(inst->src_fmt.pixelformat,
+					     dec_fmt_list[VPU_FMT_TYPE_CODEC]);
+		if (!vpu_fmt)
+			return -EINVAL;
+
+		wave5_update_pix_fmt(&inst->src_fmt,
+				     VPU_FMT_TYPE_CODEC,
+				     initial_info->pic_width,
+				     initial_info->pic_height,
+				     vpu_fmt->v4l2_frmsize);
+
+		vpu_fmt = wave5_find_vpu_fmt(inst->dst_fmt.pixelformat,
+					     dec_fmt_list[VPU_FMT_TYPE_RAW]);
+		if (!vpu_fmt)
+			return -EINVAL;
+
+		wave5_update_pix_fmt(&inst->dst_fmt,
+				     VPU_FMT_TYPE_RAW,
+				     initial_info->pic_width,
+				     initial_info->pic_height,
+				     vpu_fmt->v4l2_frmsize);
 	}
 
 	v4l2_event_queue_fh(fh, &vpu_event_src_ch);
@@ -518,6 +451,8 @@ static void wave5_vpu_dec_finish_decode(struct vpu_instance *inst)
 	if (q_status.report_queue_count == 0 &&
 	    (q_status.instance_queue_count == 0 || dec_info.sequence_changed)) {
 		dev_dbg(inst->dev->dev, "%s: finishing job.\n", __func__);
+		pm_runtime_mark_last_busy(inst->dev->dev);
+		pm_runtime_put_autosuspend(inst->dev->dev);
 		v4l2_m2m_job_finish(inst->v4l2_m2m_dev, m2m_ctx);
 	}
 }
@@ -545,12 +480,12 @@ static int wave5_vpu_dec_enum_framesizes(struct file *f, void *fh, struct v4l2_f
 	}
 
 	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
-	fsize->stepwise.min_width = vpu_fmt->min_width;
-	fsize->stepwise.max_width = vpu_fmt->max_width;
-	fsize->stepwise.step_width = 1;
-	fsize->stepwise.min_height = vpu_fmt->min_height;
-	fsize->stepwise.max_height = vpu_fmt->max_height;
-	fsize->stepwise.step_height = 1;
+	fsize->stepwise.min_width = vpu_fmt->v4l2_frmsize->min_width;
+	fsize->stepwise.max_width = vpu_fmt->v4l2_frmsize->max_width;
+	fsize->stepwise.step_width = W5_DEC_CODEC_STEP_WIDTH;
+	fsize->stepwise.min_height = vpu_fmt->v4l2_frmsize->min_height;
+	fsize->stepwise.max_height = vpu_fmt->v4l2_frmsize->max_height;
+	fsize->stepwise.step_height = W5_DEC_CODEC_STEP_HEIGHT;
 
 	return 0;
 }
@@ -573,6 +508,7 @@ static int wave5_vpu_dec_try_fmt_cap(struct file *file, void *fh, struct v4l2_fo
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
 	struct dec_info *p_dec_info = &inst->codec_info->dec_info;
+	const struct v4l2_frmsize_stepwise *frmsize;
 	const struct vpu_format *vpu_fmt;
 	int width, height;
 
@@ -586,14 +522,12 @@ static int wave5_vpu_dec_try_fmt_cap(struct file *file, void *fh, struct v4l2_fo
 		width = inst->dst_fmt.width;
 		height = inst->dst_fmt.height;
 		f->fmt.pix_mp.pixelformat = inst->dst_fmt.pixelformat;
-		f->fmt.pix_mp.num_planes = inst->dst_fmt.num_planes;
+		frmsize = &dec_raw_frmsize;
 	} else {
-		const struct v4l2_format_info *info = v4l2_format_info(vpu_fmt->v4l2_pix_fmt);
-
-		width = clamp(f->fmt.pix_mp.width, vpu_fmt->min_width, vpu_fmt->max_width);
-		height = clamp(f->fmt.pix_mp.height, vpu_fmt->min_height, vpu_fmt->max_height);
+		width = f->fmt.pix_mp.width;
+		height = f->fmt.pix_mp.height;
 		f->fmt.pix_mp.pixelformat = vpu_fmt->v4l2_pix_fmt;
-		f->fmt.pix_mp.num_planes = info->mem_planes;
+		frmsize = vpu_fmt->v4l2_frmsize;
 	}
 
 	if (p_dec_info->initial_info_obtained) {
@@ -601,9 +535,8 @@ static int wave5_vpu_dec_try_fmt_cap(struct file *file, void *fh, struct v4l2_fo
 		height = inst->dst_fmt.height;
 	}
 
-	wave5_update_pix_fmt(&f->fmt.pix_mp, width, height);
-	f->fmt.pix_mp.flags = 0;
-	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	wave5_update_pix_fmt(&f->fmt.pix_mp, VPU_FMT_TYPE_RAW,
+			     width, height, frmsize);
 	f->fmt.pix_mp.colorspace = inst->colorspace;
 	f->fmt.pix_mp.ycbcr_enc = inst->ycbcr_enc;
 	f->fmt.pix_mp.quantization = inst->quantization;
@@ -715,7 +648,9 @@ static int wave5_vpu_dec_enum_fmt_out(struct file *file, void *fh, struct v4l2_f
 static int wave5_vpu_dec_try_fmt_out(struct file *file, void *fh, struct v4l2_format *f)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
+	const struct v4l2_frmsize_stepwise *frmsize;
 	const struct vpu_format *vpu_fmt;
+	int width, height;
 
 	dev_dbg(inst->dev->dev,
 		"%s: fourcc: %u width: %u height: %u num_planes: %u colorspace: %u field: %u\n",
@@ -724,20 +659,19 @@ static int wave5_vpu_dec_try_fmt_out(struct file *file, void *fh, struct v4l2_fo
 
 	vpu_fmt = wave5_find_vpu_fmt(f->fmt.pix_mp.pixelformat, dec_fmt_list[VPU_FMT_TYPE_CODEC]);
 	if (!vpu_fmt) {
+		width = inst->src_fmt.width;
+		height = inst->src_fmt.height;
 		f->fmt.pix_mp.pixelformat = inst->src_fmt.pixelformat;
-		f->fmt.pix_mp.num_planes = inst->src_fmt.num_planes;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, inst->src_fmt.width, inst->src_fmt.height);
+		frmsize = &dec_hevc_frmsize;
 	} else {
-		int width = clamp(f->fmt.pix_mp.width, vpu_fmt->min_width, vpu_fmt->max_width);
-		int height = clamp(f->fmt.pix_mp.height, vpu_fmt->min_height, vpu_fmt->max_height);
-
+		width = f->fmt.pix_mp.width;
+		height = f->fmt.pix_mp.height;
 		f->fmt.pix_mp.pixelformat = vpu_fmt->v4l2_pix_fmt;
-		f->fmt.pix_mp.num_planes = 1;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, width, height);
+		frmsize = vpu_fmt->v4l2_frmsize;
 	}
 
-	f->fmt.pix_mp.flags = 0;
-	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	wave5_update_pix_fmt(&f->fmt.pix_mp, VPU_FMT_TYPE_CODEC,
+			     width, height, frmsize);
 
 	return 0;
 }
@@ -745,6 +679,7 @@ static int wave5_vpu_dec_try_fmt_out(struct file *file, void *fh, struct v4l2_fo
 static int wave5_vpu_dec_s_fmt_out(struct file *file, void *fh, struct v4l2_format *f)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
+	const struct vpu_format *vpu_fmt;
 	int i, ret;
 
 	dev_dbg(inst->dev->dev,
@@ -779,7 +714,13 @@ static int wave5_vpu_dec_s_fmt_out(struct file *file, void *fh, struct v4l2_form
 	inst->quantization = f->fmt.pix_mp.quantization;
 	inst->xfer_func = f->fmt.pix_mp.xfer_func;
 
-	wave5_update_pix_fmt(&inst->dst_fmt, f->fmt.pix_mp.width, f->fmt.pix_mp.height);
+	vpu_fmt = wave5_find_vpu_fmt(inst->dst_fmt.pixelformat, dec_fmt_list[VPU_FMT_TYPE_RAW]);
+	if (!vpu_fmt)
+		return -EINVAL;
+
+	wave5_update_pix_fmt(&inst->dst_fmt, VPU_FMT_TYPE_RAW,
+			     f->fmt.pix_mp.width, f->fmt.pix_mp.height,
+			     vpu_fmt->v4l2_frmsize);
 
 	return 0;
 }
@@ -1002,6 +943,7 @@ static int wave5_vpu_dec_queue_setup(struct vb2_queue *q, unsigned int *num_buff
 	struct vpu_instance *inst = vb2_get_drv_priv(q);
 	struct v4l2_pix_format_mplane inst_format =
 		(q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ? inst->src_fmt : inst->dst_fmt;
+	unsigned int i;
 
 	dev_dbg(inst->dev->dev, "%s: num_buffers: %u | num_planes: %u | type: %u\n", __func__,
 		*num_buffers, *num_planes, q->type);
@@ -1015,31 +957,9 @@ static int wave5_vpu_dec_queue_setup(struct vb2_queue *q, unsigned int *num_buff
 		if (*num_buffers < inst->fbc_buf_count)
 			*num_buffers = inst->fbc_buf_count;
 
-		if (*num_planes == 1) {
-			if (inst->output_format == FORMAT_422)
-				sizes[0] = inst_format.width * inst_format.height * 2;
-			else
-				sizes[0] = inst_format.width * inst_format.height * 3 / 2;
-			dev_dbg(inst->dev->dev, "%s: size[0]: %u\n", __func__, sizes[0]);
-		} else if (*num_planes == 2) {
-			sizes[0] = inst_format.width * inst_format.height;
-			if (inst->output_format == FORMAT_422)
-				sizes[1] = inst_format.width * inst_format.height;
-			else
-				sizes[1] = inst_format.width * inst_format.height / 2;
-			dev_dbg(inst->dev->dev, "%s: size[0]: %u | size[1]: %u\n",
-				__func__, sizes[0], sizes[1]);
-		} else if (*num_planes == 3) {
-			sizes[0] = inst_format.width * inst_format.height;
-			if (inst->output_format == FORMAT_422) {
-				sizes[1] = inst_format.width * inst_format.height / 2;
-				sizes[2] = inst_format.width * inst_format.height / 2;
-			} else {
-				sizes[1] = inst_format.width * inst_format.height / 4;
-				sizes[2] = inst_format.width * inst_format.height / 4;
-			}
-			dev_dbg(inst->dev->dev, "%s: size[0]: %u | size[1]: %u | size[2]: %u\n",
-				__func__, sizes[0], sizes[1], sizes[2]);
+		for (i = 0; i < *num_planes; i++) {
+			sizes[i] = inst_format.plane_fmt[i].sizeimage;
+			dev_dbg(inst->dev->dev, "%s: size[%u]: %u\n", __func__, i, sizes[i]);
 		}
 	}
 
@@ -1055,6 +975,22 @@ static int wave5_prepare_fb(struct vpu_instance *inst)
 	int ret, i;
 	struct v4l2_m2m_buffer *buf, *n;
 	struct v4l2_m2m_ctx *m2m_ctx = inst->v4l2_fh.m2m_ctx;
+	u32 bitdepth = inst->codec_info->dec_info.initial_info.luma_bitdepth;
+
+	switch (bitdepth) {
+	case 8:
+		break;
+	case 10:
+		if (inst->std == W_HEVC_DEC &&
+		    inst->dev->attr.support_hevc10bit_dec)
+			break;
+
+		fallthrough;
+	default:
+		dev_err(inst->dev->dev, "no support for %d bit depth\n", bitdepth);
+
+		return -EINVAL;
+	}
 
 	linear_num = v4l2_m2m_num_dst_bufs_ready(m2m_ctx);
 	non_linear_num = inst->fbc_buf_count;
@@ -1063,7 +999,7 @@ static int wave5_prepare_fb(struct vpu_instance *inst)
 		struct frame_buffer *frame = &inst->frame_buf[i];
 		struct vpu_buf *vframe = &inst->frame_vbuf[i];
 
-		fb_stride = inst->dst_fmt.width;
+		fb_stride = ALIGN(inst->dst_fmt.width * bitdepth / 8, 32);
 		fb_height = ALIGN(inst->dst_fmt.height, 32);
 		luma_size = fb_stride * fb_height;
 
@@ -1382,6 +1318,7 @@ static int wave5_vpu_dec_start_streaming(struct vb2_queue *q, unsigned int count
 	int ret = 0;
 
 	dev_dbg(inst->dev->dev, "%s: type: %u\n", __func__, q->type);
+	pm_runtime_resume_and_get(inst->dev->dev);
 
 	v4l2_m2m_update_start_streaming_state(m2m_ctx, q);
 
@@ -1416,7 +1353,8 @@ static int wave5_vpu_dec_start_streaming(struct vb2_queue *q, unsigned int count
 		if (ret)
 			goto return_buffers;
 
-		if (inst->state == VPU_INST_STATE_INIT_SEQ) {
+		if (inst->state == VPU_INST_STATE_INIT_SEQ &&
+		    inst->dev->product_code == WAVE521C_CODE) {
 			if (initial_info->luma_bitdepth != 8) {
 				dev_info(inst->dev->dev, "%s: no support for %d bit depth",
 					 __func__, initial_info->luma_bitdepth);
@@ -1424,14 +1362,17 @@ static int wave5_vpu_dec_start_streaming(struct vb2_queue *q, unsigned int count
 				goto return_buffers;
 			}
 		}
-	}
 
+	}
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	return ret;
 
 free_bitstream_vbuf:
 	wave5_vdi_free_dma_memory(inst->dev, &inst->bitstream_vbuf);
 return_buffers:
 	wave5_return_bufs(q, VB2_BUF_STATE_QUEUED);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	return ret;
 }
 
@@ -1442,6 +1383,16 @@ static int streamoff_output(struct vb2_queue *q)
 	struct vb2_v4l2_buffer *buf;
 	int ret;
 	dma_addr_t new_rd_ptr;
+	struct dec_output_info dec_info;
+	unsigned int i;
+
+	for (i = 0; i < v4l2_m2m_num_dst_bufs_ready(m2m_ctx); i++) {
+		ret = wave5_vpu_dec_set_disp_flag(inst, i);
+		if (ret)
+			dev_dbg(inst->dev->dev,
+				"%s: Setting display flag of buf index: %u, fail: %d\n",
+				__func__, i, ret);
+	}
 
 	while ((buf = v4l2_m2m_src_buf_remove(m2m_ctx))) {
 		dev_dbg(inst->dev->dev, "%s: (Multiplanar) buf type %4u | index %4u\n",
@@ -1449,6 +1400,11 @@ static int streamoff_output(struct vb2_queue *q)
 		v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
 	}
 
+	while (wave5_vpu_dec_get_output_info(inst, &dec_info) == 0) {
+		if (dec_info.index_frame_display >= 0)
+			wave5_vpu_dec_set_disp_flag(inst, dec_info.index_frame_display);
+	}
+
 	ret = wave5_vpu_flush_instance(inst);
 	if (ret)
 		return ret;
@@ -1517,6 +1473,7 @@ static void wave5_vpu_dec_stop_streaming(struct vb2_queue *q)
 	bool check_cmd = TRUE;
 
 	dev_dbg(inst->dev->dev, "%s: type: %u\n", __func__, q->type);
+	pm_runtime_resume_and_get(inst->dev->dev);
 
 	while (check_cmd) {
 		struct queue_status_info q_status;
@@ -1531,7 +1488,7 @@ static void wave5_vpu_dec_stop_streaming(struct vb2_queue *q)
 			break;
 
 		if (wave5_vpu_dec_get_output_info(inst, &dec_output_info))
-			dev_dbg(inst->dev->dev, "Getting decoding results from fw, fail\n");
+			dev_dbg(inst->dev->dev, "there is no output info\n");
 	}
 
 	v4l2_m2m_update_stop_streaming_state(m2m_ctx, q);
@@ -1540,12 +1497,13 @@ static void wave5_vpu_dec_stop_streaming(struct vb2_queue *q)
 		streamoff_output(q);
 	else
 		streamoff_capture(q);
+
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 }
 
 static const struct vb2_ops wave5_vpu_dec_vb2_ops = {
 	.queue_setup = wave5_vpu_dec_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.buf_queue = wave5_vpu_dec_buf_queue,
 	.start_streaming = wave5_vpu_dec_start_streaming,
 	.stop_streaming = wave5_vpu_dec_stop_streaming,
@@ -1554,20 +1512,15 @@ static const struct vb2_ops wave5_vpu_dec_vb2_ops = {
 static void wave5_set_default_format(struct v4l2_pix_format_mplane *src_fmt,
 				     struct v4l2_pix_format_mplane *dst_fmt)
 {
-	unsigned int dst_pix_fmt = dec_fmt_list[VPU_FMT_TYPE_RAW][0].v4l2_pix_fmt;
-	const struct v4l2_format_info *dst_fmt_info = v4l2_format_info(dst_pix_fmt);
-
 	src_fmt->pixelformat = dec_fmt_list[VPU_FMT_TYPE_CODEC][0].v4l2_pix_fmt;
-	src_fmt->field = V4L2_FIELD_NONE;
-	src_fmt->flags = 0;
-	src_fmt->num_planes = 1;
-	wave5_update_pix_fmt(src_fmt, 720, 480);
-
-	dst_fmt->pixelformat = dst_pix_fmt;
-	dst_fmt->field = V4L2_FIELD_NONE;
-	dst_fmt->flags = 0;
-	dst_fmt->num_planes = dst_fmt_info->mem_planes;
-	wave5_update_pix_fmt(dst_fmt, 736, 480);
+	wave5_update_pix_fmt(src_fmt, VPU_FMT_TYPE_CODEC,
+			     W5_DEF_DEC_PIC_WIDTH, W5_DEF_DEC_PIC_HEIGHT,
+			     &dec_hevc_frmsize);
+
+	dst_fmt->pixelformat = dec_fmt_list[VPU_FMT_TYPE_RAW][0].v4l2_pix_fmt;
+	wave5_update_pix_fmt(dst_fmt, VPU_FMT_TYPE_RAW,
+			     W5_DEF_DEC_PIC_WIDTH, W5_DEF_DEC_PIC_HEIGHT,
+			     &dec_raw_frmsize);
 }
 
 static int wave5_vpu_dec_queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
@@ -1626,7 +1579,7 @@ static void wave5_vpu_dec_device_run(void *priv)
 	int ret = 0;
 
 	dev_dbg(inst->dev->dev, "%s: Fill the ring buffer with new bitstream data", __func__);
-
+	pm_runtime_resume_and_get(inst->dev->dev);
 	ret = fill_ringbuffer(inst);
 	if (ret) {
 		dev_warn(inst->dev->dev, "Filling ring buffer failed\n");
@@ -1709,6 +1662,8 @@ static void wave5_vpu_dec_device_run(void *priv)
 
 finish_job_and_return:
 	dev_dbg(inst->dev->dev, "%s: leave and finish job", __func__);
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	v4l2_m2m_job_finish(inst->v4l2_m2m_dev, m2m_ctx);
 }
 
@@ -1810,7 +1765,6 @@ static int wave5_vpu_open_dec(struct file *filp)
 	v4l2_fh_add(&inst->v4l2_fh);
 
 	INIT_LIST_HEAD(&inst->list);
-	list_add_tail(&inst->list, &dev->instances);
 
 	inst->v4l2_m2m_dev = inst->dev->v4l2_m2m_dec_dev;
 	inst->v4l2_fh.m2m_ctx =
@@ -1865,7 +1819,23 @@ static int wave5_vpu_open_dec(struct file *filp)
 		goto cleanup_inst;
 	}
 
-	wave5_vdi_allocate_sram(inst->dev);
+	/*
+	 * For Wave515 SRAM memory was already allocated
+	 * at wave5_vpu_dec_register_device()
+	 */
+	if (inst->dev->product_code != WAVE515_CODE)
+		wave5_vdi_allocate_sram(inst->dev);
+
+	ret = mutex_lock_interruptible(&dev->dev_lock);
+	if (ret)
+		goto cleanup_inst;
+
+	if (list_empty(&dev->instances))
+		pm_runtime_use_autosuspend(inst->dev->dev);
+
+	list_add_tail(&inst->list, &dev->instances);
+
+	mutex_unlock(&dev->dev_lock);
 
 	return 0;
 
@@ -1893,6 +1863,13 @@ int wave5_vpu_dec_register_device(struct vpu_device *dev)
 	struct video_device *vdev_dec;
 	int ret;
 
+	/*
+	 * Secondary AXI setup for Wave515 is done by INIT_VPU command,
+	 * i.e. wave5_vpu_init(), that's why we allocate SRAM memory early.
+	 */
+	if (dev->product_code == WAVE515_CODE)
+		wave5_vdi_allocate_sram(dev);
+
 	vdev_dec = devm_kzalloc(dev->v4l2_dev.dev, sizeof(*vdev_dec), GFP_KERNEL);
 	if (!vdev_dec)
 		return -ENOMEM;
@@ -1926,6 +1903,13 @@ int wave5_vpu_dec_register_device(struct vpu_device *dev)
 
 void wave5_vpu_dec_unregister_device(struct vpu_device *dev)
 {
+	/*
+	 * Here is a freeing pair for Wave515 SRAM memory allocation
+	 * happened at wave5_vpu_dec_register_device().
+	 */
+	if (dev->product_code == WAVE515_CODE)
+		wave5_vdi_free_sram(dev);
+
 	video_unregister_device(dev->video_dev_dec);
 	if (dev->v4l2_m2m_dec_dev)
 		v4l2_m2m_release(dev->v4l2_m2m_dec_dev);
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpu-enc.c b/drivers/media/platform/chips-media/wave5/wave5-vpu-enc.c
index 8bbf9d10b467..1e5fc5f8b856 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpu-enc.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpu-enc.c
@@ -5,70 +5,90 @@
  * Copyright (C) 2021-2023 CHIPS&MEDIA INC
  */
 
+#include <linux/pm_runtime.h>
 #include "wave5-helper.h"
 
 #define VPU_ENC_DEV_NAME "C&M Wave5 VPU encoder"
 #define VPU_ENC_DRV_NAME "wave5-enc"
 
+static const struct v4l2_frmsize_stepwise enc_frmsize[FMT_TYPES] = {
+	[VPU_FMT_TYPE_CODEC] = {
+		.min_width = W5_MIN_ENC_PIC_WIDTH,
+		.max_width = W5_MAX_ENC_PIC_WIDTH,
+		.step_width = W5_ENC_CODEC_STEP_WIDTH,
+		.min_height = W5_MIN_ENC_PIC_HEIGHT,
+		.max_height = W5_MAX_ENC_PIC_HEIGHT,
+		.step_height = W5_ENC_CODEC_STEP_HEIGHT,
+	},
+	[VPU_FMT_TYPE_RAW] = {
+		.min_width = W5_MIN_ENC_PIC_WIDTH,
+		.max_width = W5_MAX_ENC_PIC_WIDTH,
+		.step_width = W5_ENC_RAW_STEP_WIDTH,
+		.min_height = W5_MIN_ENC_PIC_HEIGHT,
+		.max_height = W5_MAX_ENC_PIC_HEIGHT,
+		.step_height = W5_ENC_RAW_STEP_HEIGHT,
+	},
+};
+
 static const struct vpu_format enc_fmt_list[FMT_TYPES][MAX_FMTS] = {
 	[VPU_FMT_TYPE_CODEC] = {
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_HEVC,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_CODEC],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_H264,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_CODEC],
 		},
 	},
 	[VPU_FMT_TYPE_RAW] = {
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV420,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV12,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV21,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV420M,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV12M,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 		{
 			.v4l2_pix_fmt = V4L2_PIX_FMT_NV21M,
-			.max_width = W5_MAX_ENC_PIC_WIDTH,
-			.min_width = W5_MIN_ENC_PIC_WIDTH,
-			.max_height = W5_MAX_ENC_PIC_HEIGHT,
-			.min_height = W5_MIN_ENC_PIC_HEIGHT,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV422P,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_NV16,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_NV61,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_YUV422M,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_NV16M,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
+		},
+		{
+			.v4l2_pix_fmt = V4L2_PIX_FMT_NV61M,
+			.v4l2_frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW],
 		},
 	}
 };
@@ -105,46 +125,6 @@ invalid_state_switch:
 	return -EINVAL;
 }
 
-static void wave5_update_pix_fmt(struct v4l2_pix_format_mplane *pix_mp, unsigned int width,
-				 unsigned int height)
-{
-	switch (pix_mp->pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_NV12:
-	case V4L2_PIX_FMT_NV21:
-		pix_mp->width = width;
-		pix_mp->height = height;
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = round_up(width, 32) * height * 3 / 2;
-		break;
-	case V4L2_PIX_FMT_YUV420M:
-		pix_mp->width = width;
-		pix_mp->height = height;
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = round_up(width, 32) * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[1].sizeimage = round_up(width, 32) * height / 4;
-		pix_mp->plane_fmt[2].bytesperline = round_up(width, 32) / 2;
-		pix_mp->plane_fmt[2].sizeimage = round_up(width, 32) * height / 4;
-		break;
-	case V4L2_PIX_FMT_NV12M:
-	case V4L2_PIX_FMT_NV21M:
-		pix_mp->width = width;
-		pix_mp->height = height;
-		pix_mp->plane_fmt[0].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[0].sizeimage = round_up(width, 32) * height;
-		pix_mp->plane_fmt[1].bytesperline = round_up(width, 32);
-		pix_mp->plane_fmt[1].sizeimage = round_up(width, 32) * height / 2;
-		break;
-	default:
-		pix_mp->width = width;
-		pix_mp->height = height;
-		pix_mp->plane_fmt[0].bytesperline = 0;
-		pix_mp->plane_fmt[0].sizeimage = width * height / 8 * 3;
-		break;
-	}
-}
-
 static int start_encode(struct vpu_instance *inst, u32 *fail_res)
 {
 	struct v4l2_m2m_ctx *m2m_ctx = inst->v4l2_fh.m2m_ctx;
@@ -153,13 +133,26 @@ static int start_encode(struct vpu_instance *inst, u32 *fail_res)
 	struct vb2_v4l2_buffer *dst_buf;
 	struct frame_buffer frame_buf;
 	struct enc_param pic_param;
-	u32 stride = ALIGN(inst->dst_fmt.width, 32);
-	u32 luma_size = (stride * inst->dst_fmt.height);
-	u32 chroma_size = ((stride / 2) * (inst->dst_fmt.height / 2));
+	const struct v4l2_format_info *info;
+	u32 stride = inst->src_fmt.plane_fmt[0].bytesperline;
+	u32 luma_size = 0;
+	u32 chroma_size = 0;
 
 	memset(&pic_param, 0, sizeof(struct enc_param));
 	memset(&frame_buf, 0, sizeof(struct frame_buffer));
 
+	info = v4l2_format_info(inst->src_fmt.pixelformat);
+	if (!info)
+		return -EINVAL;
+
+	if (info->mem_planes == 1) {
+		luma_size = stride * inst->dst_fmt.height;
+		chroma_size = luma_size / (info->hdiv * info->vdiv);
+	} else {
+		luma_size = inst->src_fmt.plane_fmt[0].sizeimage;
+		chroma_size = inst->src_fmt.plane_fmt[1].sizeimage;
+	}
+
 	dst_buf = v4l2_m2m_next_dst_buf(m2m_ctx);
 	if (!dst_buf) {
 		dev_dbg(inst->dev->dev, "%s: No destination buffer found\n", __func__);
@@ -359,13 +352,8 @@ static int wave5_vpu_enc_enum_framesizes(struct file *f, void *fh, struct v4l2_f
 			return -EINVAL;
 	}
 
-	fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
-	fsize->stepwise.min_width = vpu_fmt->min_width;
-	fsize->stepwise.max_width = vpu_fmt->max_width;
-	fsize->stepwise.step_width = 1;
-	fsize->stepwise.min_height = vpu_fmt->min_height;
-	fsize->stepwise.max_height = vpu_fmt->max_height;
-	fsize->stepwise.step_height = 1;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise = enc_frmsize[VPU_FMT_TYPE_CODEC];
 
 	return 0;
 }
@@ -390,7 +378,9 @@ static int wave5_vpu_enc_enum_fmt_cap(struct file *file, void *fh, struct v4l2_f
 static int wave5_vpu_enc_try_fmt_cap(struct file *file, void *fh, struct v4l2_format *f)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
+	const struct v4l2_frmsize_stepwise *frmsize;
 	const struct vpu_format *vpu_fmt;
+	int width, height;
 
 	dev_dbg(inst->dev->dev, "%s: fourcc: %u width: %u height: %u num_planes: %u field: %u\n",
 		__func__, f->fmt.pix_mp.pixelformat, f->fmt.pix_mp.width, f->fmt.pix_mp.height,
@@ -398,20 +388,19 @@ static int wave5_vpu_enc_try_fmt_cap(struct file *file, void *fh, struct v4l2_fo
 
 	vpu_fmt = wave5_find_vpu_fmt(f->fmt.pix_mp.pixelformat, enc_fmt_list[VPU_FMT_TYPE_CODEC]);
 	if (!vpu_fmt) {
+		width = inst->dst_fmt.width;
+		height = inst->dst_fmt.height;
 		f->fmt.pix_mp.pixelformat = inst->dst_fmt.pixelformat;
-		f->fmt.pix_mp.num_planes = inst->dst_fmt.num_planes;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, inst->dst_fmt.width, inst->dst_fmt.height);
+		frmsize = &enc_frmsize[VPU_FMT_TYPE_CODEC];
 	} else {
-		int width = clamp(f->fmt.pix_mp.width, vpu_fmt->min_width, vpu_fmt->max_width);
-		int height = clamp(f->fmt.pix_mp.height, vpu_fmt->min_height, vpu_fmt->max_height);
-
+		width = f->fmt.pix_mp.width;
+		height = f->fmt.pix_mp.height;
 		f->fmt.pix_mp.pixelformat = vpu_fmt->v4l2_pix_fmt;
-		f->fmt.pix_mp.num_planes = 1;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, width, height);
+		frmsize = vpu_fmt->v4l2_frmsize;
 	}
 
-	f->fmt.pix_mp.flags = 0;
-	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	wave5_update_pix_fmt(&f->fmt.pix_mp, VPU_FMT_TYPE_CODEC,
+			     width, height, frmsize);
 	f->fmt.pix_mp.colorspace = inst->colorspace;
 	f->fmt.pix_mp.ycbcr_enc = inst->ycbcr_enc;
 	f->fmt.pix_mp.quantization = inst->quantization;
@@ -498,7 +487,9 @@ static int wave5_vpu_enc_enum_fmt_out(struct file *file, void *fh, struct v4l2_f
 static int wave5_vpu_enc_try_fmt_out(struct file *file, void *fh, struct v4l2_format *f)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
+	const struct v4l2_frmsize_stepwise *frmsize;
 	const struct vpu_format *vpu_fmt;
+	int width, height;
 
 	dev_dbg(inst->dev->dev, "%s: fourcc: %u width: %u height: %u num_planes: %u field: %u\n",
 		__func__, f->fmt.pix_mp.pixelformat, f->fmt.pix_mp.width, f->fmt.pix_mp.height,
@@ -506,28 +497,27 @@ static int wave5_vpu_enc_try_fmt_out(struct file *file, void *fh, struct v4l2_fo
 
 	vpu_fmt = wave5_find_vpu_fmt(f->fmt.pix_mp.pixelformat, enc_fmt_list[VPU_FMT_TYPE_RAW]);
 	if (!vpu_fmt) {
+		width = inst->src_fmt.width;
+		height = inst->src_fmt.height;
 		f->fmt.pix_mp.pixelformat = inst->src_fmt.pixelformat;
-		f->fmt.pix_mp.num_planes = inst->src_fmt.num_planes;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, inst->src_fmt.width, inst->src_fmt.height);
+		frmsize = &enc_frmsize[VPU_FMT_TYPE_RAW];
 	} else {
-		int width = clamp(f->fmt.pix_mp.width, vpu_fmt->min_width, vpu_fmt->max_width);
-		int height = clamp(f->fmt.pix_mp.height, vpu_fmt->min_height, vpu_fmt->max_height);
-		const struct v4l2_format_info *info = v4l2_format_info(vpu_fmt->v4l2_pix_fmt);
-
+		width = f->fmt.pix_mp.width;
+		height = f->fmt.pix_mp.height;
 		f->fmt.pix_mp.pixelformat = vpu_fmt->v4l2_pix_fmt;
-		f->fmt.pix_mp.num_planes = info->mem_planes;
-		wave5_update_pix_fmt(&f->fmt.pix_mp, width, height);
+		frmsize = vpu_fmt->v4l2_frmsize;
 	}
 
-	f->fmt.pix_mp.flags = 0;
-	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
-
+	wave5_update_pix_fmt(&f->fmt.pix_mp, VPU_FMT_TYPE_RAW,
+			     width, height, frmsize);
 	return 0;
 }
 
 static int wave5_vpu_enc_s_fmt_out(struct file *file, void *fh, struct v4l2_format *f)
 {
 	struct vpu_instance *inst = wave5_to_vpu_inst(fh);
+	const struct vpu_format *vpu_fmt;
+	const struct v4l2_format_info *info;
 	int i, ret;
 
 	dev_dbg(inst->dev->dev, "%s: fourcc: %u width: %u height: %u num_planes: %u field: %u\n",
@@ -549,16 +539,20 @@ static int wave5_vpu_enc_s_fmt_out(struct file *file, void *fh, struct v4l2_form
 		inst->src_fmt.plane_fmt[i].sizeimage = f->fmt.pix_mp.plane_fmt[i].sizeimage;
 	}
 
-	if (inst->src_fmt.pixelformat == V4L2_PIX_FMT_NV12 ||
-	    inst->src_fmt.pixelformat == V4L2_PIX_FMT_NV12M) {
-		inst->cbcr_interleave = true;
-		inst->nv21 = false;
-	} else if (inst->src_fmt.pixelformat == V4L2_PIX_FMT_NV21 ||
-		   inst->src_fmt.pixelformat == V4L2_PIX_FMT_NV21M) {
-		inst->cbcr_interleave = true;
+	info = v4l2_format_info(inst->src_fmt.pixelformat);
+	if (!info)
+		return -EINVAL;
+
+	inst->cbcr_interleave = (info->comp_planes == 2) ? true : false;
+
+	switch (inst->src_fmt.pixelformat) {
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV21M:
+	case V4L2_PIX_FMT_NV61:
+	case V4L2_PIX_FMT_NV61M:
 		inst->nv21 = true;
-	} else {
-		inst->cbcr_interleave = false;
+		break;
+	default:
 		inst->nv21 = false;
 	}
 
@@ -567,7 +561,15 @@ static int wave5_vpu_enc_s_fmt_out(struct file *file, void *fh, struct v4l2_form
 	inst->quantization = f->fmt.pix_mp.quantization;
 	inst->xfer_func = f->fmt.pix_mp.xfer_func;
 
-	wave5_update_pix_fmt(&inst->dst_fmt, f->fmt.pix_mp.width, f->fmt.pix_mp.height);
+	vpu_fmt = wave5_find_vpu_fmt(inst->dst_fmt.pixelformat, enc_fmt_list[VPU_FMT_TYPE_CODEC]);
+	if (!vpu_fmt)
+		return -EINVAL;
+
+	wave5_update_pix_fmt(&inst->dst_fmt, VPU_FMT_TYPE_CODEC,
+			     f->fmt.pix_mp.width, f->fmt.pix_mp.height,
+			     vpu_fmt->v4l2_frmsize);
+	inst->conf_win.width = inst->dst_fmt.width;
+	inst->conf_win.height = inst->dst_fmt.height;
 
 	return 0;
 }
@@ -583,12 +585,17 @@ static int wave5_vpu_enc_g_selection(struct file *file, void *fh, struct v4l2_se
 	switch (s->target) {
 	case V4L2_SEL_TGT_CROP_DEFAULT:
 	case V4L2_SEL_TGT_CROP_BOUNDS:
-	case V4L2_SEL_TGT_CROP:
 		s->r.left = 0;
 		s->r.top = 0;
 		s->r.width = inst->dst_fmt.width;
 		s->r.height = inst->dst_fmt.height;
 		break;
+	case V4L2_SEL_TGT_CROP:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = inst->conf_win.width;
+		s->r.height = inst->conf_win.height;
+		break;
 	default:
 		return -EINVAL;
 	}
@@ -611,8 +618,10 @@ static int wave5_vpu_enc_s_selection(struct file *file, void *fh, struct v4l2_se
 
 	s->r.left = 0;
 	s->r.top = 0;
-	s->r.width = inst->src_fmt.width;
-	s->r.height = inst->src_fmt.height;
+	s->r.width = min(s->r.width, inst->dst_fmt.width);
+	s->r.height = min(s->r.height, inst->dst_fmt.height);
+
+	inst->conf_win = s->r;
 
 	return 0;
 }
@@ -1061,6 +1070,9 @@ static int wave5_vpu_enc_s_ctrl(struct v4l2_ctrl *ctrl)
 	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
 		inst->enc_param.entropy_coding_mode = ctrl->val;
 		break;
+	case V4L2_CID_MPEG_VIDEO_PREPEND_SPSPPS_TO_IDR:
+		inst->enc_param.forced_idr_header_enable = ctrl->val;
+		break;
 	case V4L2_CID_MIN_BUFFERS_FOR_OUTPUT:
 		break;
 	default:
@@ -1125,13 +1137,23 @@ static void wave5_vpu_enc_buf_queue(struct vb2_buffer *vb)
 	v4l2_m2m_buf_queue(m2m_ctx, vbuf);
 }
 
-static void wave5_set_enc_openparam(struct enc_open_param *open_param,
-				    struct vpu_instance *inst)
+static int wave5_set_enc_openparam(struct enc_open_param *open_param,
+				   struct vpu_instance *inst)
 {
 	struct enc_wave_param input = inst->enc_param;
+	const struct v4l2_format_info *info;
 	u32 num_ctu_row = ALIGN(inst->dst_fmt.height, 64) / 64;
 	u32 num_mb_row = ALIGN(inst->dst_fmt.height, 16) / 16;
 
+	info = v4l2_format_info(inst->src_fmt.pixelformat);
+	if (!info)
+		return -EINVAL;
+
+	if (info->hdiv == 2 && info->vdiv == 1)
+		open_param->src_format = FORMAT_422;
+	else
+		open_param->src_format = FORMAT_420;
+
 	open_param->wave_param.gop_preset_idx = PRESET_IDX_IPP_SINGLE;
 	open_param->wave_param.hvs_qp_scale = 2;
 	open_param->wave_param.hvs_max_delta_qp = 10;
@@ -1147,8 +1169,8 @@ static void wave5_set_enc_openparam(struct enc_open_param *open_param,
 	open_param->wave_param.lambda_scaling_enable = 1;
 
 	open_param->line_buf_int_en = true;
-	open_param->pic_width = inst->dst_fmt.width;
-	open_param->pic_height = inst->dst_fmt.height;
+	open_param->pic_width = inst->conf_win.width;
+	open_param->pic_height = inst->conf_win.height;
 	open_param->frame_rate_info = inst->frame_rate;
 	open_param->rc_enable = inst->rc_enable;
 	if (inst->rc_enable) {
@@ -1219,6 +1241,9 @@ static void wave5_set_enc_openparam(struct enc_open_param *open_param,
 		else
 			open_param->wave_param.intra_refresh_arg = num_ctu_row;
 	}
+	open_param->wave_param.forced_idr_header_enable = input.forced_idr_header_enable;
+
+	return 0;
 }
 
 static int initialize_sequence(struct vpu_instance *inst)
@@ -1247,7 +1272,7 @@ static int initialize_sequence(struct vpu_instance *inst)
 		__func__, initial_info.min_frame_buffer_count,
 		initial_info.min_src_frame_count);
 	inst->min_src_buf_count = initial_info.min_src_frame_count +
-				  COMMAND_QUEUE_DEPTH;
+				  WAVE521_COMMAND_QUEUE_DEPTH;
 
 	ctrl = v4l2_ctrl_find(&inst->v4l2_ctrl_hdl,
 			      V4L2_CID_MIN_BUFFERS_FOR_OUTPUT);
@@ -1306,6 +1331,7 @@ static int wave5_vpu_enc_start_streaming(struct vb2_queue *q, unsigned int count
 	struct v4l2_m2m_ctx *m2m_ctx = inst->v4l2_fh.m2m_ctx;
 	int ret = 0;
 
+	pm_runtime_resume_and_get(inst->dev->dev);
 	v4l2_m2m_update_start_streaming_state(m2m_ctx, q);
 
 	if (inst->state == VPU_INST_STATE_NONE && q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
@@ -1313,7 +1339,12 @@ static int wave5_vpu_enc_start_streaming(struct vb2_queue *q, unsigned int count
 
 		memset(&open_param, 0, sizeof(struct enc_open_param));
 
-		wave5_set_enc_openparam(&open_param, inst);
+		ret = wave5_set_enc_openparam(&open_param, inst);
+		if (ret) {
+			dev_dbg(inst->dev->dev, "%s: wave5_set_enc_openparam, fail: %d\n",
+				__func__, ret);
+			goto return_buffers;
+		}
 
 		ret = wave5_vpu_enc_open(inst, &open_param);
 		if (ret) {
@@ -1360,9 +1391,13 @@ static int wave5_vpu_enc_start_streaming(struct vb2_queue *q, unsigned int count
 	if (ret)
 		goto return_buffers;
 
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	return 0;
 return_buffers:
 	wave5_return_bufs(q, VB2_BUF_STATE_QUEUED);
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	return ret;
 }
 
@@ -1404,6 +1439,7 @@ static void wave5_vpu_enc_stop_streaming(struct vb2_queue *q)
 	 */
 
 	dev_dbg(inst->dev->dev, "%s: type: %u\n", __func__, q->type);
+	pm_runtime_resume_and_get(inst->dev->dev);
 
 	if (wave5_vpu_both_queues_are_streaming(inst))
 		switch_state(inst, VPU_INST_STATE_STOP);
@@ -1428,12 +1464,13 @@ static void wave5_vpu_enc_stop_streaming(struct vb2_queue *q)
 		streamoff_output(inst, q);
 	else
 		streamoff_capture(inst, q);
+
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 }
 
 static const struct vb2_ops wave5_vpu_enc_vb2_ops = {
 	.queue_setup = wave5_vpu_enc_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.buf_queue = wave5_vpu_enc_buf_queue,
 	.start_streaming = wave5_vpu_enc_start_streaming,
 	.stop_streaming = wave5_vpu_enc_stop_streaming,
@@ -1442,20 +1479,15 @@ static const struct vb2_ops wave5_vpu_enc_vb2_ops = {
 static void wave5_set_default_format(struct v4l2_pix_format_mplane *src_fmt,
 				     struct v4l2_pix_format_mplane *dst_fmt)
 {
-	unsigned int src_pix_fmt = enc_fmt_list[VPU_FMT_TYPE_RAW][0].v4l2_pix_fmt;
-	const struct v4l2_format_info *src_fmt_info = v4l2_format_info(src_pix_fmt);
-
-	src_fmt->pixelformat = src_pix_fmt;
-	src_fmt->field = V4L2_FIELD_NONE;
-	src_fmt->flags = 0;
-	src_fmt->num_planes = src_fmt_info->mem_planes;
-	wave5_update_pix_fmt(src_fmt, 416, 240);
+	src_fmt->pixelformat = enc_fmt_list[VPU_FMT_TYPE_RAW][0].v4l2_pix_fmt;
+	wave5_update_pix_fmt(src_fmt, VPU_FMT_TYPE_RAW,
+			     W5_DEF_ENC_PIC_WIDTH, W5_DEF_ENC_PIC_HEIGHT,
+			     &enc_frmsize[VPU_FMT_TYPE_RAW]);
 
 	dst_fmt->pixelformat = enc_fmt_list[VPU_FMT_TYPE_CODEC][0].v4l2_pix_fmt;
-	dst_fmt->field = V4L2_FIELD_NONE;
-	dst_fmt->flags = 0;
-	dst_fmt->num_planes = 1;
-	wave5_update_pix_fmt(dst_fmt, 416, 240);
+	wave5_update_pix_fmt(dst_fmt, VPU_FMT_TYPE_CODEC,
+			     W5_DEF_ENC_PIC_WIDTH, W5_DEF_ENC_PIC_HEIGHT,
+			     &enc_frmsize[VPU_FMT_TYPE_CODEC]);
 }
 
 static int wave5_vpu_enc_queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
@@ -1474,6 +1506,7 @@ static void wave5_vpu_enc_device_run(void *priv)
 	u32 fail_res = 0;
 	int ret = 0;
 
+	pm_runtime_resume_and_get(inst->dev->dev);
 	switch (inst->state) {
 	case VPU_INST_STATE_PIC_RUN:
 		ret = start_encode(inst, &fail_res);
@@ -1487,6 +1520,8 @@ static void wave5_vpu_enc_device_run(void *priv)
 			break;
 		}
 		dev_dbg(inst->dev->dev, "%s: leave with active job", __func__);
+		pm_runtime_mark_last_busy(inst->dev->dev);
+		pm_runtime_put_autosuspend(inst->dev->dev);
 		return;
 	default:
 		WARN(1, "Execution of a job in state %s is invalid.\n",
@@ -1494,6 +1529,8 @@ static void wave5_vpu_enc_device_run(void *priv)
 		break;
 	}
 	dev_dbg(inst->dev->dev, "%s: leave and finish job", __func__);
+	pm_runtime_mark_last_busy(inst->dev->dev);
+	pm_runtime_put_autosuspend(inst->dev->dev);
 	v4l2_m2m_job_finish(inst->v4l2_m2m_dev, m2m_ctx);
 }
 
@@ -1554,7 +1591,6 @@ static int wave5_vpu_open_enc(struct file *filp)
 	v4l2_fh_add(&inst->v4l2_fh);
 
 	INIT_LIST_HEAD(&inst->list);
-	list_add_tail(&inst->list, &dev->instances);
 
 	inst->v4l2_m2m_dev = inst->dev->v4l2_m2m_enc_dev;
 	inst->v4l2_fh.m2m_ctx =
@@ -1702,6 +1738,9 @@ static int wave5_vpu_open_enc(struct file *filp)
 			  0, 1, 1, 0);
 	v4l2_ctrl_new_std(v4l2_ctrl_hdl, &wave5_vpu_enc_ctrl_ops,
 			  V4L2_CID_MIN_BUFFERS_FOR_OUTPUT, 1, 32, 1, 1);
+	v4l2_ctrl_new_std(v4l2_ctrl_hdl, &wave5_vpu_enc_ctrl_ops,
+			  V4L2_CID_MPEG_VIDEO_PREPEND_SPSPPS_TO_IDR,
+			  0, 1, 1, 0);
 
 	if (v4l2_ctrl_hdl->error) {
 		ret = -ENODEV;
@@ -1712,6 +1751,8 @@ static int wave5_vpu_open_enc(struct file *filp)
 	v4l2_ctrl_handler_setup(v4l2_ctrl_hdl);
 
 	wave5_set_default_format(&inst->src_fmt, &inst->dst_fmt);
+	inst->conf_win.width = inst->dst_fmt.width;
+	inst->conf_win.height = inst->dst_fmt.height;
 	inst->colorspace = V4L2_COLORSPACE_REC709;
 	inst->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 	inst->quantization = V4L2_QUANTIZATION_DEFAULT;
@@ -1729,6 +1770,17 @@ static int wave5_vpu_open_enc(struct file *filp)
 
 	wave5_vdi_allocate_sram(inst->dev);
 
+	ret = mutex_lock_interruptible(&dev->dev_lock);
+	if (ret)
+		goto cleanup_inst;
+
+	if (list_empty(&dev->instances))
+		pm_runtime_use_autosuspend(inst->dev->dev);
+
+	list_add_tail(&inst->list, &dev->instances);
+
+	mutex_unlock(&dev->dev_lock);
+
 	return 0;
 
 cleanup_inst:
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpu.c b/drivers/media/platform/chips-media/wave5/wave5-vpu.c
index 1b3df5b04249..e1715d3f43b0 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpu.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpu.c
@@ -10,6 +10,8 @@
 #include <linux/clk.h>
 #include <linux/firmware.h>
 #include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
 #include "wave5-vpu.h"
 #include "wave5-regdefine.h"
 #include "wave5-vpuconfig.h"
@@ -24,8 +26,12 @@
 struct wave5_match_data {
 	int flags;
 	const char *fw_name;
+	u32 sram_size;
 };
 
+static int vpu_poll_interval = 5;
+module_param(vpu_poll_interval, int, 0644);
+
 int wave5_vpu_wait_interrupt(struct vpu_instance *inst, unsigned int timeout)
 {
 	int ret;
@@ -40,7 +46,7 @@ int wave5_vpu_wait_interrupt(struct vpu_instance *inst, unsigned int timeout)
 	return 0;
 }
 
-static irqreturn_t wave5_vpu_irq_thread(int irq, void *dev_id)
+static void wave5_vpu_handle_irq(void *dev_id)
 {
 	u32 seq_done;
 	u32 cmd_done;
@@ -48,42 +54,73 @@ static irqreturn_t wave5_vpu_irq_thread(int irq, void *dev_id)
 	struct vpu_instance *inst;
 	struct vpu_device *dev = dev_id;
 
-	if (wave5_vdi_read_register(dev, W5_VPU_VPU_INT_STS)) {
-		irq_reason = wave5_vdi_read_register(dev, W5_VPU_VINT_REASON);
-		wave5_vdi_write_register(dev, W5_VPU_VINT_REASON_CLR, irq_reason);
-		wave5_vdi_write_register(dev, W5_VPU_VINT_CLEAR, 0x1);
-
-		list_for_each_entry(inst, &dev->instances, list) {
-			seq_done = wave5_vdi_read_register(dev, W5_RET_SEQ_DONE_INSTANCE_INFO);
-			cmd_done = wave5_vdi_read_register(dev, W5_RET_QUEUE_CMD_DONE_INST);
-
-			if (irq_reason & BIT(INT_WAVE5_INIT_SEQ) ||
-			    irq_reason & BIT(INT_WAVE5_ENC_SET_PARAM)) {
-				if (seq_done & BIT(inst->id)) {
-					seq_done &= ~BIT(inst->id);
-					wave5_vdi_write_register(dev, W5_RET_SEQ_DONE_INSTANCE_INFO,
-								 seq_done);
-					complete(&inst->irq_done);
-				}
+	irq_reason = wave5_vdi_read_register(dev, W5_VPU_VINT_REASON);
+	seq_done = wave5_vdi_read_register(dev, W5_RET_SEQ_DONE_INSTANCE_INFO);
+	cmd_done = wave5_vdi_read_register(dev, W5_RET_QUEUE_CMD_DONE_INST);
+	wave5_vdi_write_register(dev, W5_VPU_VINT_REASON_CLR, irq_reason);
+	wave5_vdi_write_register(dev, W5_VPU_VINT_CLEAR, 0x1);
+
+	list_for_each_entry(inst, &dev->instances, list) {
+
+		if (irq_reason & BIT(INT_WAVE5_INIT_SEQ) ||
+		    irq_reason & BIT(INT_WAVE5_ENC_SET_PARAM)) {
+			if (dev->product_code == WAVE515_CODE &&
+			    (cmd_done & BIT(inst->id))) {
+				cmd_done &= ~BIT(inst->id);
+				wave5_vdi_write_register(dev, W5_RET_QUEUE_CMD_DONE_INST,
+							 cmd_done);
+				complete(&inst->irq_done);
+			} else if (seq_done & BIT(inst->id)) {
+				seq_done &= ~BIT(inst->id);
+				wave5_vdi_write_register(dev, W5_RET_SEQ_DONE_INSTANCE_INFO,
+							 seq_done);
+				complete(&inst->irq_done);
 			}
+		}
 
-			if (irq_reason & BIT(INT_WAVE5_DEC_PIC) ||
-			    irq_reason & BIT(INT_WAVE5_ENC_PIC)) {
-				if (cmd_done & BIT(inst->id)) {
-					cmd_done &= ~BIT(inst->id);
-					wave5_vdi_write_register(dev, W5_RET_QUEUE_CMD_DONE_INST,
-								 cmd_done);
-					inst->ops->finish_process(inst);
-				}
+		if (irq_reason & BIT(INT_WAVE5_DEC_PIC) ||
+		    irq_reason & BIT(INT_WAVE5_ENC_PIC)) {
+			if (cmd_done & BIT(inst->id)) {
+				cmd_done &= ~BIT(inst->id);
+				wave5_vdi_write_register(dev, W5_RET_QUEUE_CMD_DONE_INST,
+							 cmd_done);
+				inst->ops->finish_process(inst);
 			}
-
-			wave5_vpu_clear_interrupt(inst, irq_reason);
 		}
+
+		wave5_vpu_clear_interrupt(inst, irq_reason);
 	}
+}
+
+static irqreturn_t wave5_vpu_irq_thread(int irq, void *dev_id)
+{
+	struct vpu_device *dev = dev_id;
+
+	if (wave5_vdi_read_register(dev, W5_VPU_VPU_INT_STS))
+		wave5_vpu_handle_irq(dev);
 
 	return IRQ_HANDLED;
 }
 
+static void wave5_vpu_irq_work_fn(struct kthread_work *work)
+{
+	struct vpu_device *dev = container_of(work, struct vpu_device, work);
+
+	if (wave5_vdi_read_register(dev, W5_VPU_VPU_INT_STS))
+		wave5_vpu_handle_irq(dev);
+}
+
+static enum hrtimer_restart wave5_vpu_timer_callback(struct hrtimer *timer)
+{
+	struct vpu_device *dev =
+			container_of(timer, struct vpu_device, hrtimer);
+
+	kthread_queue_work(dev->worker, &dev->work);
+	hrtimer_forward_now(timer, ns_to_ktime(vpu_poll_interval * NSEC_PER_MSEC));
+
+	return HRTIMER_RESTART;
+}
+
 static int wave5_vpu_load_firmware(struct device *dev, const char *fw_name,
 				   u32 *revision)
 {
@@ -117,6 +154,45 @@ static int wave5_vpu_load_firmware(struct device *dev, const char *fw_name,
 	return 0;
 }
 
+static __maybe_unused int wave5_pm_suspend(struct device *dev)
+{
+	struct vpu_device *vpu = dev_get_drvdata(dev);
+
+	if (pm_runtime_suspended(dev))
+		return 0;
+
+	if (vpu->irq < 0)
+		hrtimer_cancel(&vpu->hrtimer);
+
+	wave5_vpu_sleep_wake(dev, true, NULL, 0);
+	clk_bulk_disable_unprepare(vpu->num_clks, vpu->clks);
+
+	return 0;
+}
+
+static __maybe_unused int wave5_pm_resume(struct device *dev)
+{
+	struct vpu_device *vpu = dev_get_drvdata(dev);
+	int ret = 0;
+
+	wave5_vpu_sleep_wake(dev, false, NULL, 0);
+	ret = clk_bulk_prepare_enable(vpu->num_clks, vpu->clks);
+	if (ret) {
+		dev_err(dev, "Enabling clocks, fail: %d\n", ret);
+		return ret;
+	}
+
+	if (vpu->irq < 0 && !hrtimer_active(&vpu->hrtimer))
+		hrtimer_start(&vpu->hrtimer, ns_to_ktime(vpu->vpu_poll_interval * NSEC_PER_MSEC),
+			      HRTIMER_MODE_REL_PINNED);
+
+	return ret;
+}
+
+static const struct dev_pm_ops wave5_pm_ops = {
+	SET_RUNTIME_PM_OPS(wave5_pm_suspend, wave5_pm_resume, NULL)
+};
+
 static int wave5_vpu_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -151,6 +227,16 @@ static int wave5_vpu_probe(struct platform_device *pdev)
 	dev_set_drvdata(&pdev->dev, dev);
 	dev->dev = &pdev->dev;
 
+	dev->resets = devm_reset_control_array_get_optional_exclusive(&pdev->dev);
+	if (IS_ERR(dev->resets)) {
+		return dev_err_probe(&pdev->dev, PTR_ERR(dev->resets),
+				     "Failed to get reset control\n");
+	}
+
+	ret = reset_control_deassert(dev->resets);
+	if (ret)
+		return dev_err_probe(&pdev->dev, ret, "Failed to deassert resets\n");
+
 	ret = devm_clk_bulk_get_all(&pdev->dev, &dev->clks);
 
 	/* continue without clock, assume externally managed */
@@ -163,20 +249,15 @@ static int wave5_vpu_probe(struct platform_device *pdev)
 	ret = clk_bulk_prepare_enable(dev->num_clks, dev->clks);
 	if (ret) {
 		dev_err(&pdev->dev, "Enabling clocks, fail: %d\n", ret);
-		return ret;
-	}
-
-	ret = of_property_read_u32(pdev->dev.of_node, "sram-size",
-				   &dev->sram_size);
-	if (ret) {
-		dev_warn(&pdev->dev, "sram-size not found\n");
-		dev->sram_size = 0;
+		goto err_reset_assert;
 	}
 
 	dev->sram_pool = of_gen_pool_get(pdev->dev.of_node, "sram", 0);
 	if (!dev->sram_pool)
 		dev_warn(&pdev->dev, "sram node not found\n");
 
+	dev->sram_size = match_data->sram_size;
+
 	dev->product_code = wave5_vdi_read_register(dev, VPU_PRODUCT_CODE_REGISTER);
 	ret = wave5_vdi_init(&pdev->dev);
 	if (ret < 0) {
@@ -185,6 +266,28 @@ static int wave5_vpu_probe(struct platform_device *pdev)
 	}
 	dev->product = wave5_vpu_get_product_id(dev);
 
+	dev->irq = platform_get_irq(pdev, 0);
+	if (dev->irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq resource, falling back to polling\n");
+		hrtimer_setup(&dev->hrtimer, &wave5_vpu_timer_callback, CLOCK_MONOTONIC,
+			      HRTIMER_MODE_REL_PINNED);
+		dev->worker = kthread_run_worker(0, "vpu_irq_thread");
+		if (IS_ERR(dev->worker)) {
+			dev_err(&pdev->dev, "failed to create vpu irq worker\n");
+			ret = PTR_ERR(dev->worker);
+			goto err_vdi_release;
+		}
+		dev->vpu_poll_interval = vpu_poll_interval;
+		kthread_init_work(&dev->work, wave5_vpu_irq_work_fn);
+	} else {
+		ret = devm_request_threaded_irq(&pdev->dev, dev->irq, NULL,
+						wave5_vpu_irq_thread, IRQF_ONESHOT, "vpu_irq", dev);
+		if (ret) {
+			dev_err(&pdev->dev, "Register interrupt handler, fail: %d\n", ret);
+			goto err_enc_unreg;
+		}
+	}
+
 	INIT_LIST_HEAD(&dev->instances);
 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
 	if (ret) {
@@ -207,20 +310,6 @@ static int wave5_vpu_probe(struct platform_device *pdev)
 		}
 	}
 
-	dev->irq = platform_get_irq(pdev, 0);
-	if (dev->irq < 0) {
-		dev_err(&pdev->dev, "failed to get irq resource\n");
-		ret = -ENXIO;
-		goto err_enc_unreg;
-	}
-
-	ret = devm_request_threaded_irq(&pdev->dev, dev->irq, NULL,
-					wave5_vpu_irq_thread, IRQF_ONESHOT, "vpu_irq", dev);
-	if (ret) {
-		dev_err(&pdev->dev, "Register interrupt handler, fail: %d\n", ret);
-		goto err_enc_unreg;
-	}
-
 	ret = wave5_vpu_load_firmware(&pdev->dev, match_data->fw_name, &fw_revision);
 	if (ret) {
 		dev_err(&pdev->dev, "wave5_vpu_load_firmware, fail: %d\n", ret);
@@ -232,6 +321,12 @@ static int wave5_vpu_probe(struct platform_device *pdev)
 		 (match_data->flags & WAVE5_IS_DEC) ? "'DECODE'" : "");
 	dev_info(&pdev->dev, "Product Code:      0x%x\n", dev->product_code);
 	dev_info(&pdev->dev, "Firmware Revision: %u\n", fw_revision);
+
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 100);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+	wave5_vpu_sleep_wake(&pdev->dev, true, NULL, 0);
+
 	return 0;
 
 err_enc_unreg:
@@ -246,6 +341,8 @@ err_vdi_release:
 	wave5_vdi_release(&pdev->dev);
 err_clk_dis:
 	clk_bulk_disable_unprepare(dev->num_clks, dev->clks);
+err_reset_assert:
+	reset_control_assert(dev->resets);
 
 	return ret;
 }
@@ -254,8 +351,17 @@ static void wave5_vpu_remove(struct platform_device *pdev)
 {
 	struct vpu_device *dev = dev_get_drvdata(&pdev->dev);
 
+	if (dev->irq < 0) {
+		kthread_destroy_worker(dev->worker);
+		hrtimer_cancel(&dev->hrtimer);
+	}
+
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
 	mutex_destroy(&dev->dev_lock);
 	mutex_destroy(&dev->hw_lock);
+	reset_control_assert(dev->resets);
 	clk_bulk_disable_unprepare(dev->num_clks, dev->clks);
 	wave5_vpu_enc_unregister_device(dev);
 	wave5_vpu_dec_unregister_device(dev);
@@ -267,6 +373,7 @@ static void wave5_vpu_remove(struct platform_device *pdev)
 static const struct wave5_match_data ti_wave521c_data = {
 	.flags = WAVE5_IS_ENC | WAVE5_IS_DEC,
 	.fw_name = "cnm/wave521c_k3_codec_fw.bin",
+	.sram_size = (64 * 1024),
 };
 
 static const struct of_device_id wave5_dt_ids[] = {
@@ -279,9 +386,10 @@ static struct platform_driver wave5_vpu_driver = {
 	.driver = {
 		.name = VPU_PLATFORM_DEVICE_NAME,
 		.of_match_table = of_match_ptr(wave5_dt_ids),
+		.pm = &wave5_pm_ops,
 		},
 	.probe = wave5_vpu_probe,
-	.remove_new = wave5_vpu_remove,
+	.remove = wave5_vpu_remove,
 };
 
 module_platform_driver(wave5_vpu_driver);
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpu.h b/drivers/media/platform/chips-media/wave5/wave5-vpu.h
index 32b7fd3730b5..3847332551fc 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpu.h
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpu.h
@@ -38,10 +38,7 @@ enum vpu_fmt_type {
 
 struct vpu_format {
 	unsigned int v4l2_pix_fmt;
-	unsigned int max_width;
-	unsigned int min_width;
-	unsigned int max_height;
-	unsigned int min_height;
+	const struct v4l2_frmsize_stepwise *v4l2_frmsize;
 };
 
 static inline struct vpu_instance *wave5_to_vpu_inst(struct v4l2_fh *vfh)
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpuapi.c b/drivers/media/platform/chips-media/wave5/wave5-vpuapi.c
index 1a3efb638dde..e5e879a13e8b 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpuapi.c
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpuapi.c
@@ -6,6 +6,8 @@
  */
 
 #include <linux/bug.h>
+#include <linux/pm_runtime.h>
+#include <linux/delay.h>
 #include "wave5-vpuapi.h"
 #include "wave5-regdefine.h"
 #include "wave5.h"
@@ -73,6 +75,16 @@ int wave5_vpu_flush_instance(struct vpu_instance *inst)
 				 inst->type == VPU_INST_TYPE_DEC ? "DECODER" : "ENCODER", inst->id);
 			mutex_unlock(&inst->dev->hw_lock);
 			return -ETIMEDOUT;
+		} else if (ret == -EBUSY) {
+			struct dec_output_info dec_info;
+
+			mutex_unlock(&inst->dev->hw_lock);
+			wave5_vpu_dec_get_output_info(inst, &dec_info);
+			ret = mutex_lock_interruptible(&inst->dev->hw_lock);
+			if (ret)
+				return ret;
+			if (dec_info.index_frame_display > 0)
+				wave5_vpu_dec_set_disp_flag(inst, dec_info.index_frame_display);
 		}
 	} while (ret != 0);
 	mutex_unlock(&inst->dev->hw_lock);
@@ -195,14 +207,20 @@ int wave5_vpu_dec_close(struct vpu_instance *inst, u32 *fail_res)
 	int retry = 0;
 	struct vpu_device *vpu_dev = inst->dev;
 	int i;
+	int inst_count = 0;
+	struct vpu_instance *inst_elm;
 
 	*fail_res = 0;
 	if (!inst->codec_info)
 		return -EINVAL;
 
+	pm_runtime_resume_and_get(inst->dev->dev);
+
 	ret = mutex_lock_interruptible(&vpu_dev->hw_lock);
-	if (ret)
+	if (ret) {
+		pm_runtime_put_sync(inst->dev->dev);
 		return ret;
+	}
 
 	do {
 		ret = wave5_vpu_dec_finish_seq(inst, fail_res);
@@ -232,9 +250,14 @@ int wave5_vpu_dec_close(struct vpu_instance *inst, u32 *fail_res)
 
 	wave5_vdi_free_dma_memory(vpu_dev, &p_dec_info->vb_task);
 
+	list_for_each_entry(inst_elm, &vpu_dev->instances, list)
+		inst_count++;
+	if (inst_count == 1)
+		pm_runtime_dont_use_autosuspend(vpu_dev->dev);
+
 unlock_and_return:
 	mutex_unlock(&vpu_dev->hw_lock);
-
+	pm_runtime_put_sync(inst->dev->dev);
 	return ret;
 }
 
@@ -697,25 +720,33 @@ int wave5_vpu_enc_close(struct vpu_instance *inst, u32 *fail_res)
 	int ret;
 	int retry = 0;
 	struct vpu_device *vpu_dev = inst->dev;
+	int inst_count = 0;
+	struct vpu_instance *inst_elm;
 
 	*fail_res = 0;
 	if (!inst->codec_info)
 		return -EINVAL;
 
+	pm_runtime_resume_and_get(inst->dev->dev);
+
 	ret = mutex_lock_interruptible(&vpu_dev->hw_lock);
-	if (ret)
+	if (ret) {
+		pm_runtime_resume_and_get(inst->dev->dev);
 		return ret;
+	}
 
 	do {
 		ret = wave5_vpu_enc_finish_seq(inst, fail_res);
 		if (ret < 0 && *fail_res != WAVE5_SYSERR_VPU_STILL_RUNNING) {
 			dev_warn(inst->dev->dev, "enc_finish_seq timed out\n");
+			pm_runtime_resume_and_get(inst->dev->dev);
 			mutex_unlock(&vpu_dev->hw_lock);
 			return ret;
 		}
 
 		if (*fail_res == WAVE5_SYSERR_VPU_STILL_RUNNING &&
 		    retry++ >= MAX_FIRMWARE_CALL_RETRY) {
+			pm_runtime_resume_and_get(inst->dev->dev);
 			mutex_unlock(&vpu_dev->hw_lock);
 			return -ETIMEDOUT;
 		}
@@ -734,7 +765,13 @@ int wave5_vpu_enc_close(struct vpu_instance *inst, u32 *fail_res)
 
 	wave5_vdi_free_dma_memory(vpu_dev, &p_enc_info->vb_task);
 
+	list_for_each_entry(inst_elm, &vpu_dev->instances, list)
+		inst_count++;
+	if (inst_count == 1)
+		pm_runtime_dont_use_autosuspend(vpu_dev->dev);
+
 	mutex_unlock(&vpu_dev->hw_lock);
+	pm_runtime_put_sync(inst->dev->dev);
 
 	return 0;
 }
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpuapi.h b/drivers/media/platform/chips-media/wave5/wave5-vpuapi.h
index 352f6e904e50..45615c15beca 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpuapi.h
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpuapi.h
@@ -18,6 +18,7 @@
 #include "wave5-vdi.h"
 
 enum product_id {
+	PRODUCT_ID_515,
 	PRODUCT_ID_521,
 	PRODUCT_ID_511,
 	PRODUCT_ID_517,
@@ -327,6 +328,7 @@ struct vpu_attr {
 	u32 support_backbone: 1;
 	u32 support_avc10bit_enc: 1;
 	u32 support_hevc10bit_enc: 1;
+	u32 support_hevc10bit_dec: 1;
 	u32 support_vcore_backbone: 1;
 	u32 support_vcpu_backbone: 1;
 };
@@ -566,6 +568,7 @@ struct enc_wave_param {
 	u32 lambda_scaling_enable: 1; /* enable lambda scaling using custom GOP */
 	u32 transform8x8_enable: 1; /* enable 8x8 intra prediction and 8x8 transform */
 	u32 mb_level_rc_enable: 1; /* enable MB-level rate control */
+	u32 forced_idr_header_enable: 1; /* enable header encoding before IDR frame */
 };
 
 struct enc_open_param {
@@ -756,7 +759,12 @@ struct vpu_device {
 	u32 product_code;
 	struct ida inst_ida;
 	struct clk_bulk_data *clks;
+	struct hrtimer hrtimer;
+	struct kthread_work work;
+	struct kthread_worker *worker;
+	int vpu_poll_interval;
 	int num_clks;
+	struct reset_control *resets;
 };
 
 struct vpu_instance;
diff --git a/drivers/media/platform/chips-media/wave5/wave5-vpuconfig.h b/drivers/media/platform/chips-media/wave5/wave5-vpuconfig.h
index d9751eedb0f9..1ea9f5f31499 100644
--- a/drivers/media/platform/chips-media/wave5/wave5-vpuconfig.h
+++ b/drivers/media/platform/chips-media/wave5/wave5-vpuconfig.h
@@ -8,6 +8,7 @@
 #ifndef _VPU_CONFIG_H_
 #define _VPU_CONFIG_H_
 
+#define WAVE515_CODE			0x5150
 #define WAVE517_CODE                    0x5170
 #define WAVE537_CODE                    0x5370
 #define WAVE511_CODE                    0x5110
@@ -21,19 +22,39 @@
 		((c) == WAVE517_CODE ||	(c) == WAVE537_CODE ||		\
 		 (c) == WAVE511_CODE || (c) == WAVE521_CODE ||		\
 		 (c) == WAVE521E1_CODE || (c) == WAVE521C_CODE ||	\
-		 (c) == WAVE521C_DUAL_CODE);				\
+		 (c) == WAVE521C_DUAL_CODE) || (c) == WAVE515_CODE;	\
 })
 
 #define WAVE517_WORKBUF_SIZE            (2 * 1024 * 1024)
 #define WAVE521ENC_WORKBUF_SIZE         (128 * 1024)      //HEVC 128K, AVC 40K
 #define WAVE521DEC_WORKBUF_SIZE         (1784 * 1024)
+#define WAVE515DEC_WORKBUF_SIZE		(2 * 1024 * 1024)
 
 #define MAX_NUM_INSTANCE                32
 
-#define W5_MIN_ENC_PIC_WIDTH            256
-#define W5_MIN_ENC_PIC_HEIGHT           128
-#define W5_MAX_ENC_PIC_WIDTH            8192
-#define W5_MAX_ENC_PIC_HEIGHT           8192
+#define W5_DEF_DEC_PIC_WIDTH            720U
+#define W5_DEF_DEC_PIC_HEIGHT           480U
+#define W5_MIN_DEC_PIC_8_WIDTH          8U
+#define W5_MIN_DEC_PIC_8_HEIGHT         8U
+#define W5_MIN_DEC_PIC_32_WIDTH         32U
+#define W5_MIN_DEC_PIC_32_HEIGHT        32U
+#define W5_MAX_DEC_PIC_WIDTH            8192U
+#define W5_MAX_DEC_PIC_HEIGHT           4320U
+#define W5_DEC_CODEC_STEP_WIDTH         1U
+#define W5_DEC_CODEC_STEP_HEIGHT        1U
+#define W5_DEC_RAW_STEP_WIDTH           32U
+#define W5_DEC_RAW_STEP_HEIGHT          16U
+
+#define W5_DEF_ENC_PIC_WIDTH            416U
+#define W5_DEF_ENC_PIC_HEIGHT           240U
+#define W5_MIN_ENC_PIC_WIDTH            256U
+#define W5_MIN_ENC_PIC_HEIGHT           128U
+#define W5_MAX_ENC_PIC_WIDTH            8192U
+#define W5_MAX_ENC_PIC_HEIGHT           8192U
+#define W5_ENC_CODEC_STEP_WIDTH         8U
+#define W5_ENC_CODEC_STEP_HEIGHT        8U
+#define W5_ENC_RAW_STEP_WIDTH           32U
+#define W5_ENC_RAW_STEP_HEIGHT          16U
 
 //  application specific configuration
 #define VPU_ENC_TIMEOUT                 60000
@@ -49,17 +70,21 @@
 /************************************************************************/
 #define VLC_BUF_NUM                     (2)
 
-#define COMMAND_QUEUE_DEPTH             (2)
+#define WAVE521_COMMAND_QUEUE_DEPTH	(2)
+#define WAVE515_COMMAND_QUEUE_DEPTH	(4)
 
 #define W5_REMAP_INDEX0                 0
 #define W5_REMAP_INDEX1                 1
 #define W5_REMAP_MAX_SIZE               (1024 * 1024)
 
-#define WAVE5_MAX_CODE_BUF_SIZE         (2 * 1024 * 1024)
-#define WAVE5_TEMPBUF_OFFSET            WAVE5_MAX_CODE_BUF_SIZE
+#define WAVE521_MAX_CODE_BUF_SIZE	(2 * 1024 * 1024)
+#define WAVE515_MAX_CODE_BUF_SIZE	(1024 * 1024)
 #define WAVE5_TEMPBUF_SIZE              (1024 * 1024)
 
-#define SIZE_COMMON                 (WAVE5_MAX_CODE_BUF_SIZE + WAVE5_TEMPBUF_SIZE)
+#define WAVE521_SIZE_COMMON		(WAVE521_MAX_CODE_BUF_SIZE + WAVE5_TEMPBUF_SIZE)
+#define WAVE515_ONE_TASKBUF_SIZE	(8 * 1024 * 1024)
+#define WAVE515_SIZE_COMMON		(WAVE515_MAX_CODE_BUF_SIZE + WAVE5_TEMPBUF_SIZE + \
+					 WAVE515_COMMAND_QUEUE_DEPTH * WAVE515_ONE_TASKBUF_SIZE)
 
 //=====4. VPU REPORT MEMORY  ======================//
 
diff --git a/drivers/media/platform/chips-media/wave5/wave5.h b/drivers/media/platform/chips-media/wave5/wave5.h
index 063028eccd3b..2caab356f3e1 100644
--- a/drivers/media/platform/chips-media/wave5/wave5.h
+++ b/drivers/media/platform/chips-media/wave5/wave5.h
@@ -22,6 +22,12 @@
  */
 #define BSOPTION_ENABLE_EXPLICIT_END		BIT(0)
 #define BSOPTION_HIGHLIGHT_STREAM_END		BIT(1)
+/*
+ * When RD_PTR_VALID_FLAG is 0 Wave515 ignores RD_PTR value and starts to
+ * decode from the access unit end position of the last decoded picture in
+ * bitstream buffer.
+ */
+#define BSOPTION_RD_PTR_VALID_FLAG		BIT(31)
 
 /*
  * Currently the driver only supports hardware with little endian but for source
@@ -56,6 +62,9 @@ int wave5_vpu_get_version(struct vpu_device *vpu_dev, u32 *revision);
 
 int wave5_vpu_init(struct device *dev, u8 *fw, size_t size);
 
+int wave5_vpu_sleep_wake(struct device *dev, bool i_sleep_wake, const uint16_t *code,
+			 size_t size);
+
 int wave5_vpu_reset(struct device *dev, enum sw_reset_mode reset_mode);
 
 int wave5_vpu_build_up_dec_param(struct vpu_instance *inst, struct dec_open_param *param);
diff --git a/drivers/media/platform/imagination/Kconfig b/drivers/media/platform/imagination/Kconfig
new file mode 100644
index 000000000000..a302c955483d
--- /dev/null
+++ b/drivers/media/platform/imagination/Kconfig
@@ -0,0 +1,14 @@
+# SPDX-License-Identifier: GPL-2.0
+config VIDEO_E5010_JPEG_ENC
+	tristate "Imagination E5010 JPEG Encoder Driver"
+	depends on VIDEO_DEV
+	depends on ARCH_K3 || COMPILE_TEST
+	select VIDEOBUF2_DMA_CONTIG
+	select VIDEOBUF2_VMALLOC
+	select V4L2_MEM2MEM_DEV
+	select V4L2_JPEG_HELPER
+	help
+	  This is a video4linux2 M2M driver for Imagination E5010 JPEG encoder,
+	  which supports JPEG and MJPEG baseline encoding of YUV422 and YUV420
+	  semiplanar video formats, with resolution ranging from 64x64 to 8K x 8K
+	  pixels. The module will be named as e5010_jpeg_enc.
diff --git a/drivers/media/platform/imagination/Makefile b/drivers/media/platform/imagination/Makefile
new file mode 100644
index 000000000000..d45b85b88575
--- /dev/null
+++ b/drivers/media/platform/imagination/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+e5010_jpeg_enc-objs := e5010-jpeg-enc-hw.o e5010-jpeg-enc.o
+obj-$(CONFIG_VIDEO_E5010_JPEG_ENC) += e5010_jpeg_enc.o
diff --git a/drivers/media/platform/imagination/e5010-core-regs.h b/drivers/media/platform/imagination/e5010-core-regs.h
new file mode 100644
index 000000000000..aaec498fe83f
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-core-regs.h
@@ -0,0 +1,585 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#ifndef _E5010_CORE_REGS_H
+#define _E5010_CORE_REGS_H
+
+#define JASPER_CORE_ID_OFFSET				(0x0000)
+#define JASPER_CORE_ID_CR_GROUP_ID_MASK			(0xFF000000)
+#define JASPER_CORE_ID_CR_GROUP_ID_SHIFT		(24)
+#define JASPER_CORE_ID_CR_CORE_ID_MASK			(0x00FF0000)
+#define JASPER_CORE_ID_CR_CORE_ID_SHIFT			(16)
+#define JASPER_CORE_ID_CR_UNIQUE_NUM_MASK		(0x0000FFF8)
+#define JASPER_CORE_ID_CR_UNIQUE_NUM_SHIFT		(3)
+#define JASPER_CORE_ID_CR_PELS_PER_CYCLE_MASK		(0x00000007)
+#define JASPER_CORE_ID_CR_PELS_PER_CYCLE_SHIFT		(0)
+
+#define JASPER_CORE_REV_OFFSET				(0x0004)
+#define JASPER_CORE_REV_CR_JASPER_DESIGNER_MASK		(0xFF000000)
+#define JASPER_CORE_REV_CR_JASPER_DESIGNER_SHIFT	(24)
+#define JASPER_CORE_REV_CR_JASPER_MAJOR_REV_MASK	(0x00FF0000)
+#define JASPER_CORE_REV_CR_JASPER_MAJOR_REV_SHIFT	(16)
+#define JASPER_CORE_REV_CR_JASPER_MINOR_REV_MASK	(0x0000FF00)
+#define JASPER_CORE_REV_CR_JASPER_MINOR_REV_SHIFT	(8)
+#define JASPER_CORE_REV_CR_JASPER_MAINT_REV_MASK	(0x000000FF)
+#define JASPER_CORE_REV_CR_JASPER_MAINT_REV_SHIFT	(0)
+
+#define JASPER_INTERRUPT_MASK_OFFSET					(0x0008)
+#define JASPER_INTERRUPT_MASK_CR_OUTPUT_ADDRESS_ERROR_ENABLE_MASK	(0x00000002)
+#define JASPER_INTERRUPT_MASK_CR_OUTPUT_ADDRESS_ERROR_ENABLE_SHIFT	(1)
+#define JASPER_INTERRUPT_MASK_CR_PICTURE_DONE_ENABLE_MASK		(0x00000001)
+#define JASPER_INTERRUPT_MASK_CR_PICTURE_DONE_ENABLE_SHIFT		(0)
+
+#define JASPER_INTERRUPT_STATUS_OFFSET					(0x000C)
+#define JASPER_INTERRUPT_STATUS_CR_OUTPUT_ADDRESS_ERROR_IRQ_MASK	(0x00000002)
+#define JASPER_INTERRUPT_STATUS_CR_OUTPUT_ADDRESS_ERROR_IRQ_SHIFT	(1)
+#define JASPER_INTERRUPT_STATUS_CR_PICTURE_DONE_IRQ_MASK		(0x00000001)
+#define JASPER_INTERRUPT_STATUS_CR_PICTURE_DONE_IRQ_SHIFT		(0)
+
+#define JASPER_INTERRUPT_CLEAR_OFFSET					(0x0010)
+#define JASPER_INTERRUPT_CLEAR_CR_OUTPUT_ERROR_CLEAR_MASK		(0x00000002)
+#define JASPER_INTERRUPT_CLEAR_CR_OUTPUT_ERROR_CLEAR_SHIFT		(1)
+#define JASPER_INTERRUPT_CLEAR_CR_PICTURE_DONE_CLEAR_MASK		(0x00000001)
+#define JASPER_INTERRUPT_CLEAR_CR_PICTURE_DONE_CLEAR_SHIFT		(0)
+
+#define JASPER_CLK_CONTROL_OFFSET				(0x0014)
+#define JASPER_CLK_CONTROL_CR_JASPER_AUTO_CLKG_ENABLE_MASK	(0x00000002)
+#define JASPER_CLK_CONTROL_CR_JASPER_AUTO_CLKG_ENABLE_SHIFT	(1)
+#define JASPER_CLK_CONTROL_CR_JASPER_MAN_CLKG_ENABLE_MASK	(0x00000001)
+#define JASPER_CLK_CONTROL_CR_JASPER_MAN_CLKG_ENABLE_SHIFT	(0)
+
+#define JASPER_CLK_STATUS_OFFSET			(0x0018)
+#define JASPER_CLK_STATUS_CR_JASPER_CLKG_STATUS_MASK	(0x00000001)
+#define JASPER_CLK_STATUS_CR_JASPER_CLKG_STATUS_SHIFT	(0)
+
+#define JASPER_RESET_OFFSET			(0x001C)
+#define JASPER_RESET_CR_SYS_RESET_MASK		(0x00000002)
+#define JASPER_RESET_CR_SYS_RESET_SHIFT		(1)
+#define JASPER_RESET_CR_CORE_RESET_MASK		(0x00000001)
+#define JASPER_RESET_CR_CORE_RESET_SHIFT	(0)
+
+#define JASPER_CORE_CTRL_OFFSET				(0x0020)
+#define JASPER_CORE_CTRL_CR_JASPER_ENCODE_START_MASK	(0x00000001)
+#define JASPER_CORE_CTRL_CR_JASPER_ENCODE_START_SHIFT	(0)
+
+#define JASPER_STATUS_OFFSET				(0x0024)
+#define JASPER_STATUS_CR_FLUSH_MODE_MASK		(0x00000002)
+#define JASPER_STATUS_CR_FLUSH_MODE_SHIFT		(1)
+#define JASPER_STATUS_CR_JASPER_BUSY_MASK		(0x00000001)
+#define JASPER_STATUS_CR_JASPER_BUSY_SHIFT		(0)
+
+#define JASPER_CRC_CLEAR_OFFSET					(0x0028)
+#define JASPER_CRC_CLEAR_CR_FRONT_END_CRC_CLEAR_MASK		(0x00000001)
+#define JASPER_CRC_CLEAR_CR_FRONT_END_CRC_CLEAR_SHIFT		(0)
+#define JASPER_CRC_CLEAR_CR_DCT_CRC_CLEAR_MASK			(0x00000002)
+#define JASPER_CRC_CLEAR_CR_DCT_CRC_CLEAR_SHIFT			(1)
+#define JASPER_CRC_CLEAR_CR_ZZ_CRC_CLEAR_MASK			(0x00000004)
+#define JASPER_CRC_CLEAR_CR_ZZ_CRC_CLEAR_SHIFT			(2)
+#define JASPER_CRC_CLEAR_CR_QUANT_CRC_CLEAR_MASK		(0x00000008)
+#define JASPER_CRC_CLEAR_CR_QUANT_CRC_CLEAR_SHIFT		(3)
+#define JASPER_CRC_CLEAR_CR_ENTROPY_ENCODER_CRC_CLEAR_MASK	(0x00000010)
+#define JASPER_CRC_CLEAR_CR_ENTROPY_ENCODER_CRC_CLEAR_SHIFT	(4)
+#define JASPER_CRC_CLEAR_CR_PACKING_BUFFER_CRC_CLEAR_MASK	(0x00000020)
+#define JASPER_CRC_CLEAR_CR_PACKING_BUFFER_CRC_CLEAR_SHIFT	(5)
+
+#define JASPER_INPUT_CTRL0_OFFSET			(0x002C)
+#define JASPER_INPUT_CTRL0_CR_INPUT_CHROMA_ORDER_MASK	(0x01000000)
+#define JASPER_INPUT_CTRL0_CR_INPUT_CHROMA_ORDER_SHIFT	(24)
+#define JASPER_INPUT_CTRL0_CR_INPUT_SUBSAMPLING_MASK	(0x00030000)
+#define JASPER_INPUT_CTRL0_CR_INPUT_SUBSAMPLING_SHIFT	(16)
+#define JASPER_INPUT_CTRL0_CR_INPUT_SOURCE_MASK		(0x00000004)
+#define JASPER_INPUT_CTRL0_CR_INPUT_SOURCE_SHIFT	(2)
+
+#define JASPER_INPUT_CTRL1_OFFSET			(0x0030)
+#define JASPER_INPUT_CTRL1_CR_INPUT_LUMA_STRIDE_MASK	(0x1FC00000)
+#define JASPER_INPUT_CTRL1_CR_INPUT_LUMA_STRIDE_SHIFT	(22)
+#define JASPER_INPUT_CTRL1_CR_INPUT_CHROMA_STRIDE_MASK	(0x00001FC0)
+#define JASPER_INPUT_CTRL1_CR_INPUT_CHROMA_STRIDE_SHIFT	(6)
+
+#define JASPER_MMU_CTRL_OFFSET				(0x0034)
+#define JASPER_MMU_CTRL_CR_JASPER_TILING_SCHEME_MASK	(0x00000002)
+#define JASPER_MMU_CTRL_CR_JASPER_TILING_SCHEME_SHIFT	(1)
+#define JASPER_MMU_CTRL_CR_JASPER_TILING_ENABLE_MASK	(0x00000001)
+#define JASPER_MMU_CTRL_CR_JASPER_TILING_ENABLE_SHIFT	(0)
+
+#define JASPER_IMAGE_SIZE_OFFSET				(0x0038)
+#define JASPER_IMAGE_SIZE_CR_IMAGE_VERTICAL_SIZE_MASK		(0x1FFF0000)
+#define JASPER_IMAGE_SIZE_CR_IMAGE_VERTICAL_SIZE_SHIFT		(16)
+#define JASPER_IMAGE_SIZE_CR_IMAGE_HORIZONTAL_SIZE_MASK		(0x00001FFF)
+#define JASPER_IMAGE_SIZE_CR_IMAGE_HORIZONTAL_SIZE_SHIFT	(0)
+
+#define INPUT_LUMA_BASE_OFFSET				(0x003C)
+#define INPUT_LUMA_BASE_CR_INPUT_LUMA_BASE_MASK		(0xFFFFFFC0)
+#define INPUT_LUMA_BASE_CR_INPUT_LUMA_BASE_SHIFT	(6)
+
+#define INPUT_CHROMA_BASE_OFFSET			(0x0040)
+#define INPUT_CHROMA_BASE_CR_INPUT_CHROMA_BASE_MASK	(0xFFFFFFC0)
+#define INPUT_CHROMA_BASE_CR_INPUT_CHROMA_BASE_SHIFT	(6)
+
+#define JASPER_OUTPUT_BASE_OFFSET		(0x0044)
+#define JASPER_OUTPUT_BASE_CR_OUTPUT_BASE_MASK	(0xFFFFFFFF)
+#define JASPER_OUTPUT_BASE_CR_OUTPUT_BASE_SHIFT	(0)
+
+#define JASPER_OUTPUT_SIZE_OFFSET			(0x0048)
+#define JASPER_OUTPUT_SIZE_CR_OUTPUT_SIZE_MASK		(0xFFFFFFFF)
+#define JASPER_OUTPUT_SIZE_CR_OUTPUT_SIZE_SHIFT		(0)
+#define JASPER_OUTPUT_MAX_SIZE_OFFSET			(0x004C)
+#define JASPER_OUTPUT_MAX_SIZE_CR_OUTPUT_MAX_SIZE_MASK	(0xFFFFFFFF)
+#define JASPER_OUTPUT_MAX_SIZE_CR_OUTPUT_MAX_SIZE_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE0_OFFSET					(0x0050)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_03_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_03_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_02_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_02_SHIFT	(16)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_01_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_01_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_00_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE0_CR_LUMA_QUANTIZATION_TABLE_00_SHIFT	(0)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_OFFSET					(0x0054)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_07_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_07_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_06_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_06_SHIFT	(16)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_05_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_05_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_04_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE1_CR_LUMA_QUANTIZATION_TABLE_04_SHIFT	(0)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_OFFSET					(0x0058)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_13_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_13_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_12_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_12_SHIFT	(16)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_11_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_11_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_10_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE2_CR_LUMA_QUANTIZATION_TABLE_10_SHIFT	(0)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_OFFSET					(0x005C)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_17_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_17_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_16_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_16_SHIFT	(16)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_15_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_15_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_14_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE3_CR_LUMA_QUANTIZATION_TABLE_14_SHIFT	(0)
+#define JASPER_LUMA_QUANTIZATION_TABLE4_OFFSET					(0x0060)
+#define JASPER_LUMA_QUANTIZATION_TABLE4_CR_LUMA_QUANTIZATION_TABLE_21_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE4_CR_LUMA_QUANTIZATION_TABLE_21_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE4_CR_LUMA_QUANTIZATION_TABLE_20_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE4_CR_LUMA_QUANTIZATION_TABLE_20_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE5_OFFSET					(0x0064)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_27_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_27_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_26_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_26_SHIFT	(16)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_25_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_25_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_24_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE5_CR_LUMA_QUANTIZATION_TABLE_24_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE6_OFFSET					(0x0068)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_33_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_33_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_32_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_32_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_31_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_31_SHIFT	(8)
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_30_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE6_CR_LUMA_QUANTIZATION_TABLE_30_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE7_OFFSET					(0x006C)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_37_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_37_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_36_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_36_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_35_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_35_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_34_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE7_CR_LUMA_QUANTIZATION_TABLE_34_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE8_OFFSET					(0x0070)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_43_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_43_SHIFT	(24)
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_42_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_42_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_41_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_41_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_40_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE8_CR_LUMA_QUANTIZATION_TABLE_40_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE9_OFFSET					(0x0074)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_47_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_47_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_46_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_46_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_45_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_45_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_44_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE9_CR_LUMA_QUANTIZATION_TABLE_44_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE10_OFFSET					(0x0078)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_53_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_53_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_52_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_52_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_51_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_51_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_50_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE10_CR_LUMA_QUANTIZATION_TABLE_50_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE11_OFFSET					(0x007C)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_57_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_57_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_56_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_56_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_55_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_55_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_54_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE11_CR_LUMA_QUANTIZATION_TABLE_54_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE12_OFFSET					(0x0080)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_63_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_63_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_62_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_62_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_61_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_61_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_60_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE12_CR_LUMA_QUANTIZATION_TABLE_60_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE13_OFFSET					(0x0084)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_67_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_67_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_66_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_66_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_65_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_65_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_64_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE13_CR_LUMA_QUANTIZATION_TABLE_64_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE14_OFFSET					(0x0088)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_73_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_73_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_72_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_72_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_71_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_71_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_70_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE14_CR_LUMA_QUANTIZATION_TABLE_70_SHIFT	(0)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE15_OFFSET					(0x008C)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_77_MASK	(0xFF000000)
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_77_SHIFT	(24)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_76_MASK	(0x00FF0000)
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_76_SHIFT	(16)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_75_MASK	(0x0000FF00)
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_75_SHIFT	(8)
+
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_74_MASK	(0x000000FF)
+#define JASPER_LUMA_QUANTIZATION_TABLE15_CR_LUMA_QUANTIZATION_TABLE_74_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_OFFSET				(0x0090)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_03_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_03_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_02_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_02_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_01_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_01_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_00_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE0_CR_CHROMA_QUANTIZATION_TABLE_00_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_OFFSET					(0x0094)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_07_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_07_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_06_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_06_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_05_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_05_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_04_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE1_CR_CHROMA_QUANTIZATION_TABLE_04_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_OFFSET					(0x0098)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_13_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_13_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_12_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_12_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_11_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_11_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_10_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE2_CR_CHROMA_QUANTIZATION_TABLE_10_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_OFFSET					(0x009C)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_17_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_17_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_16_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_16_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_15_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_15_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_14_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE3_CR_CHROMA_QUANTIZATION_TABLE_14_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_OFFSET					(0x00A0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_23_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_23_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_22_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_22_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_21_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_21_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_20_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE4_CR_CHROMA_QUANTIZATION_TABLE_20_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_OFFSET					(0x00A4)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_27_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_27_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_26_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_26_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_25_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_25_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_24_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE5_CR_CHROMA_QUANTIZATION_TABLE_24_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_OFFSET					(0x00A8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_33_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_33_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_32_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_32_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_31_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_31_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_30_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE6_CR_CHROMA_QUANTIZATION_TABLE_30_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_OFFSET					(0x00AC)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_37_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_37_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_36_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_36_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_35_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_35_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_34_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE7_CR_CHROMA_QUANTIZATION_TABLE_34_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_OFFSET					(0x00B0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_43_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_43_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_42_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_42_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_41_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_41_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_40_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE8_CR_CHROMA_QUANTIZATION_TABLE_40_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_OFFSET					(0x00B4)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_47_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_47_SHIFT		(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_46_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_46_SHIFT		(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_45_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_45_SHIFT		(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_44_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE9_CR_CHROMA_QUANTIZATION_TABLE_44_SHIFT		(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_OFFSET					(0x00B8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_53_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_53_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_52_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_52_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_51_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_51_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_50_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE10_CR_CHROMA_QUANTIZATION_TABLE_50_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_OFFSET					(0x00BC)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_57_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_57_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_56_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_56_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_55_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_55_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_54_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE11_CR_CHROMA_QUANTIZATION_TABLE_54_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_OFFSET					(0x00C0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_63_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_63_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_62_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_62_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_61_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_61_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_60_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE12_CR_CHROMA_QUANTIZATION_TABLE_60_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_OFFSET					(0x00C4)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_67_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_67_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_66_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_66_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_65_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_65_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_64_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE13_CR_CHROMA_QUANTIZATION_TABLE_64_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_OFFSET					(0x00C8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_73_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_73_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_72_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_72_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_71_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_71_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_70_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE14_CR_CHROMA_QUANTIZATION_TABLE_70_SHIFT	(0)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_OFFSET					(0x00CC)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_77_MASK		(0xFF000000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_77_SHIFT	(24)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_76_MASK		(0x00FF0000)
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_76_SHIFT	(16)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_75_MASK		(0x0000FF00)
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_75_SHIFT	(8)
+
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_74_MASK		(0x000000FF)
+#define JASPER_CHROMA_QUANTIZATION_TABLE15_CR_CHROMA_QUANTIZATION_TABLE_74_SHIFT	(0)
+
+#define JASPER_CRC_CTRL_OFFSET				(0x00D0)
+#define JASPER_CRC_CTRL_JASPER_CRC_ENABLE_MASK		(0x00000001)
+#define JASPER_CRC_CTRL_JASPER_CRC_ENABLE_SHIFT		(0)
+
+#define JASPER_FRONT_END_CRC_OFFSET				(0x00D4)
+#define JASPER_FRONT_END_CRC_CR_JASPER_FRONT_END_CRC_OUT_MASK	(0xFFFFFFFF)
+#define JASPER_FRONT_END_CRC_CR_JASPER_FRONT_END_CRC_OUT_SHIFT	(0)
+
+#define JASPER_DCT_CRC_OFFSET				(0x00D8)
+#define JASPER_DCT_CRC_CR_JASPER_DCT_CRC_OUT_MASK	(0xFFFFFFFF)
+#define JASPER_DCT_CRC_CR_JASPER_DCT_CRC_OUT_SHIFT	(0)
+
+#define JASPER_ZZ_CRC_OFFSET				(0x00DC)
+#define JASPER_ZZ_CRC_CR_JASPER_ZZ_CRC_OUT_MASK		(0xFFFFFFFF)
+#define JASPER_ZZ_CRC_CR_JASPER_ZZ_CRC_OUT_SHIFT	(0)
+
+#define JASPER_QUANT_CRC_OFFSET				(0x00E0)
+#define JASPER_QUANT_CRC_CR_JASPER_QUANT_CRC_OUT_MASK	(0xFFFFFFFF)
+#define JASPER_QUANT_CRC_CR_JASPER_QUANT_CRC_OUT_SHIFT	(0)
+
+#define JASPER_ENTROPY_ENCODER_CRC_OFFSET				(0x00E4)
+#define JASPER_ENTROPY_ENCODER_CRC_CR_JASPER_ENTROPY_CRC_OUT_MASK	(0xFFFFFFFF)
+#define JASPER_ENTROPY_ENCODER_CRC_CR_JASPER_ENTROPY_CRC_OUT_SHIFT	(0)
+
+#define JASPER_PACKING_BUFFER_DATA_CRC_OFFSET						(0x00E8)
+#define JASPER_PACKING_BUFFER_DATA_CRC_CR_JASPER_PACKING_DATA_CRC_OUT_MASK		(0xFFFFFFFF)
+#define JASPER_PACKING_BUFFER_DATA_CRC_CR_JASPER_PACKING_DATA_CRC_OUT_SHIFT		(0)
+
+#define JASPER_PACKING_BUFFER_ADDR_CRC_OFFSET						(0x00EC)
+#define JASPER_PACKING_BUFFER_ADDR_CRC_CR_JASPER_PACKING_ADDR_OUT_CRC_MASK		(0xFFFFFFFF)
+#define JASPER_PACKING_BUFFER_ADDR_CRC_CR_JASPER_PACKING_ADDR_OUT_CRC_SHIFT		(0)
+
+#define JASPER_CORE_BYTE_SIZE	(0x00F0)
+
+#endif
diff --git a/drivers/media/platform/imagination/e5010-jpeg-enc-hw.c b/drivers/media/platform/imagination/e5010-jpeg-enc-hw.c
new file mode 100644
index 000000000000..56d5941020fa
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-jpeg-enc-hw.c
@@ -0,0 +1,267 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/dev_printk.h>
+#include "e5010-jpeg-enc-hw.h"
+
+static void write_reg_field(void __iomem *base, unsigned int offset, u32 mask,
+			    unsigned int shift, u32 value)
+{
+	u32 reg;
+
+	value <<= shift;
+	if (mask != 0xffffffff) {
+		reg = readl(base + offset);
+		value = (value & mask) | (reg & ~mask);
+	}
+	writel(value, (base + offset));
+}
+
+static int write_reg_field_not_busy(void __iomem *jasper_base, void __iomem *wr_base,
+				    unsigned int offset, u32 mask, unsigned int shift,
+				    u32 value)
+{
+	int ret;
+	u32 val;
+
+	ret = readl_poll_timeout_atomic(jasper_base + JASPER_STATUS_OFFSET, val,
+					(val & JASPER_STATUS_CR_JASPER_BUSY_MASK) == 0,
+					2000, 50000);
+	if (ret)
+		return ret;
+
+	write_reg_field(wr_base, offset, mask, shift, value);
+
+	return 0;
+}
+
+void e5010_reset(struct device *dev, void __iomem *core_base, void __iomem *mmu_base)
+{
+	int ret = 0;
+	u32 val;
+
+	write_reg_field(core_base, JASPER_RESET_OFFSET,
+			JASPER_RESET_CR_CORE_RESET_MASK,
+			JASPER_RESET_CR_CORE_RESET_SHIFT, 1);
+
+	write_reg_field(mmu_base, MMU_MMU_CONTROL1_OFFSET,
+			MMU_MMU_CONTROL1_MMU_SOFT_RESET_MASK,
+			MMU_MMU_CONTROL1_MMU_SOFT_RESET_SHIFT, 1);
+
+	ret = readl_poll_timeout_atomic(mmu_base + MMU_MMU_CONTROL1_OFFSET, val,
+					(val & MMU_MMU_CONTROL1_MMU_SOFT_RESET_MASK) == 0,
+					2000, 50000);
+	if (ret)
+		dev_warn(dev, "MMU soft reset timed out, forcing system soft reset\n");
+
+	write_reg_field(core_base, JASPER_RESET_OFFSET,
+			JASPER_RESET_CR_SYS_RESET_MASK,
+			JASPER_RESET_CR_SYS_RESET_SHIFT, 1);
+}
+
+void e5010_hw_bypass_mmu(void __iomem *mmu_base, u32 enable)
+{
+	/* Bypass MMU */
+	write_reg_field(mmu_base,
+			MMU_MMU_ADDRESS_CONTROL_OFFSET,
+			MMU_MMU_ADDRESS_CONTROL_MMU_BYPASS_MASK,
+			MMU_MMU_ADDRESS_CONTROL_MMU_BYPASS_SHIFT,
+			enable);
+}
+
+int e5010_hw_enable_output_address_error_irq(void __iomem *core_base, u32 enable)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INTERRUPT_MASK_OFFSET,
+				       JASPER_INTERRUPT_MASK_CR_OUTPUT_ADDRESS_ERROR_ENABLE_MASK,
+				       JASPER_INTERRUPT_MASK_CR_OUTPUT_ADDRESS_ERROR_ENABLE_SHIFT,
+				       enable);
+}
+
+bool e5010_hw_pic_done_irq(void __iomem *core_base)
+{
+	u32 reg;
+
+	reg = readl(core_base + JASPER_INTERRUPT_STATUS_OFFSET);
+	return reg & JASPER_INTERRUPT_STATUS_CR_PICTURE_DONE_IRQ_MASK;
+}
+
+bool e5010_hw_output_address_irq(void __iomem *core_base)
+{
+	u32 reg;
+
+	reg = readl(core_base + JASPER_INTERRUPT_STATUS_OFFSET);
+	return reg & JASPER_INTERRUPT_STATUS_CR_OUTPUT_ADDRESS_ERROR_IRQ_MASK;
+}
+
+int e5010_hw_enable_picture_done_irq(void __iomem *core_base, u32 enable)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INTERRUPT_MASK_OFFSET,
+				       JASPER_INTERRUPT_MASK_CR_PICTURE_DONE_ENABLE_MASK,
+				       JASPER_INTERRUPT_MASK_CR_PICTURE_DONE_ENABLE_SHIFT,
+				       enable);
+}
+
+int e5010_hw_enable_auto_clock_gating(void __iomem *core_base, u32 enable)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_CLK_CONTROL_OFFSET,
+				       JASPER_CLK_CONTROL_CR_JASPER_AUTO_CLKG_ENABLE_MASK,
+				       JASPER_CLK_CONTROL_CR_JASPER_AUTO_CLKG_ENABLE_SHIFT,
+				       enable);
+}
+
+int e5010_hw_enable_manual_clock_gating(void __iomem *core_base, u32 enable)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_CLK_CONTROL_OFFSET,
+				       JASPER_CLK_CONTROL_CR_JASPER_MAN_CLKG_ENABLE_MASK,
+				       JASPER_CLK_CONTROL_CR_JASPER_MAN_CLKG_ENABLE_SHIFT, 0);
+}
+
+int e5010_hw_enable_crc_check(void __iomem *core_base, u32 enable)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_CRC_CTRL_OFFSET,
+				       JASPER_CRC_CTRL_JASPER_CRC_ENABLE_MASK,
+				       JASPER_CRC_CTRL_JASPER_CRC_ENABLE_SHIFT, enable);
+}
+
+int e5010_hw_set_input_source_to_memory(void __iomem *core_base, u32 set)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INPUT_CTRL0_OFFSET,
+				       JASPER_INPUT_CTRL0_CR_INPUT_SOURCE_MASK,
+				       JASPER_INPUT_CTRL0_CR_INPUT_SOURCE_SHIFT, set);
+}
+
+int e5010_hw_set_input_luma_addr(void __iomem *core_base, u32 val)
+{
+	return  write_reg_field_not_busy(core_base, core_base,
+				       INPUT_LUMA_BASE_OFFSET,
+				       INPUT_LUMA_BASE_CR_INPUT_LUMA_BASE_MASK, 0, val);
+}
+
+int e5010_hw_set_input_chroma_addr(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       INPUT_CHROMA_BASE_OFFSET,
+				       INPUT_CHROMA_BASE_CR_INPUT_CHROMA_BASE_MASK, 0, val);
+}
+
+int e5010_hw_set_output_base_addr(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_OUTPUT_BASE_OFFSET,
+				       JASPER_OUTPUT_BASE_CR_OUTPUT_BASE_MASK,
+				       JASPER_OUTPUT_BASE_CR_OUTPUT_BASE_SHIFT, val);
+}
+
+int e5010_hw_set_horizontal_size(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_IMAGE_SIZE_OFFSET,
+				       JASPER_IMAGE_SIZE_CR_IMAGE_HORIZONTAL_SIZE_MASK,
+				       JASPER_IMAGE_SIZE_CR_IMAGE_HORIZONTAL_SIZE_SHIFT,
+				       val);
+}
+
+int e5010_hw_set_vertical_size(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_IMAGE_SIZE_OFFSET,
+				       JASPER_IMAGE_SIZE_CR_IMAGE_VERTICAL_SIZE_MASK,
+				       JASPER_IMAGE_SIZE_CR_IMAGE_VERTICAL_SIZE_SHIFT,
+				       val);
+}
+
+int e5010_hw_set_luma_stride(void __iomem *core_base, u32 bytesperline)
+{
+	u32 val = bytesperline / 64;
+
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INPUT_CTRL1_OFFSET,
+				       JASPER_INPUT_CTRL1_CR_INPUT_LUMA_STRIDE_MASK,
+				       JASPER_INPUT_CTRL1_CR_INPUT_LUMA_STRIDE_SHIFT,
+				       val);
+}
+
+int e5010_hw_set_chroma_stride(void __iomem *core_base, u32 bytesperline)
+{
+	u32 val = bytesperline / 64;
+
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INPUT_CTRL1_OFFSET,
+				       JASPER_INPUT_CTRL1_CR_INPUT_CHROMA_STRIDE_MASK,
+				       JASPER_INPUT_CTRL1_CR_INPUT_CHROMA_STRIDE_SHIFT,
+				       val);
+}
+
+int e5010_hw_set_input_subsampling(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INPUT_CTRL0_OFFSET,
+				       JASPER_INPUT_CTRL0_CR_INPUT_SUBSAMPLING_MASK,
+				       JASPER_INPUT_CTRL0_CR_INPUT_SUBSAMPLING_SHIFT,
+				       val);
+}
+
+int e5010_hw_set_chroma_order(void __iomem *core_base, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base,
+				       JASPER_INPUT_CTRL0_OFFSET,
+				       JASPER_INPUT_CTRL0_CR_INPUT_CHROMA_ORDER_MASK,
+				       JASPER_INPUT_CTRL0_CR_INPUT_CHROMA_ORDER_SHIFT,
+				       val);
+}
+
+void e5010_hw_set_output_max_size(void __iomem *core_base, u32 val)
+{
+	write_reg_field(core_base, JASPER_OUTPUT_MAX_SIZE_OFFSET,
+			JASPER_OUTPUT_MAX_SIZE_CR_OUTPUT_MAX_SIZE_MASK,
+			JASPER_OUTPUT_MAX_SIZE_CR_OUTPUT_MAX_SIZE_SHIFT,
+			val);
+}
+
+int e5010_hw_set_qpvalue(void __iomem *core_base, u32 offset, u32 val)
+{
+	return write_reg_field_not_busy(core_base, core_base, offset, 0xffffffff, 0, val);
+}
+
+void e5010_hw_clear_output_error(void __iomem *core_base, u32 clear)
+{
+	/* Make sure interrupts are clear */
+	write_reg_field(core_base, JASPER_INTERRUPT_CLEAR_OFFSET,
+			JASPER_INTERRUPT_CLEAR_CR_OUTPUT_ERROR_CLEAR_MASK,
+			JASPER_INTERRUPT_CLEAR_CR_OUTPUT_ERROR_CLEAR_SHIFT, clear);
+}
+
+void e5010_hw_clear_picture_done(void __iomem *core_base, u32 clear)
+{
+	write_reg_field(core_base,
+			JASPER_INTERRUPT_CLEAR_OFFSET,
+			JASPER_INTERRUPT_CLEAR_CR_PICTURE_DONE_CLEAR_MASK,
+			JASPER_INTERRUPT_CLEAR_CR_PICTURE_DONE_CLEAR_SHIFT, clear);
+}
+
+int e5010_hw_get_output_size(void __iomem *core_base)
+{
+	return readl(core_base + JASPER_OUTPUT_SIZE_OFFSET);
+}
+
+void e5010_hw_encode_start(void __iomem *core_base, u32 start)
+{
+	write_reg_field(core_base, JASPER_CORE_CTRL_OFFSET,
+			JASPER_CORE_CTRL_CR_JASPER_ENCODE_START_MASK,
+			JASPER_CORE_CTRL_CR_JASPER_ENCODE_START_SHIFT, start);
+}
diff --git a/drivers/media/platform/imagination/e5010-jpeg-enc-hw.h b/drivers/media/platform/imagination/e5010-jpeg-enc-hw.h
new file mode 100644
index 000000000000..781d353c3226
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-jpeg-enc-hw.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#ifndef _E5010_JPEG_ENC_HW_H
+#define _E5010_JPEG_ENC_HW_H
+
+#include "e5010-core-regs.h"
+#include "e5010-mmu-regs.h"
+
+int e5010_hw_enable_output_address_error_irq(void __iomem *core_offset, u32 enable);
+int e5010_hw_enable_picture_done_irq(void __iomem *core_offset, u32 enable);
+int e5010_hw_enable_auto_clock_gating(void __iomem *core_offset, u32 enable);
+int e5010_hw_enable_manual_clock_gating(void __iomem *core_offset, u32 enable);
+int e5010_hw_enable_crc_check(void __iomem *core_offset, u32 enable);
+int e5010_hw_set_input_source_to_memory(void __iomem *core_offset, u32 set);
+int e5010_hw_set_input_luma_addr(void __iomem *core_offset, u32 val);
+int e5010_hw_set_input_chroma_addr(void __iomem *core_offset, u32 val);
+int e5010_hw_set_output_base_addr(void __iomem *core_offset, u32 val);
+int e5010_hw_get_output_size(void __iomem *core_offset);
+int e5010_hw_set_horizontal_size(void __iomem *core_offset, u32 val);
+int e5010_hw_set_vertical_size(void __iomem *core_offset, u32 val);
+int e5010_hw_set_luma_stride(void __iomem *core_offset, u32 bytesperline);
+int e5010_hw_set_chroma_stride(void __iomem *core_offset, u32 bytesperline);
+int e5010_hw_set_input_subsampling(void __iomem *core_offset, u32 val);
+int e5010_hw_set_chroma_order(void __iomem *core_offset, u32 val);
+int e5010_hw_set_qpvalue(void __iomem *core_offset, u32 offset, u32 value);
+void e5010_reset(struct device *dev, void __iomem *core_offset, void __iomem *mmu_offset);
+void e5010_hw_set_output_max_size(void __iomem *core_offset, u32 val);
+void e5010_hw_clear_picture_done(void __iomem *core_offset, u32 clear);
+void e5010_hw_encode_start(void __iomem *core_offset, u32 start);
+void e5010_hw_clear_output_error(void __iomem *core_offset, u32 clear);
+void e5010_hw_bypass_mmu(void __iomem *mmu_base, u32 enable);
+bool e5010_hw_pic_done_irq(void __iomem *core_base);
+bool e5010_hw_output_address_irq(void __iomem *core_base);
+#endif
diff --git a/drivers/media/platform/imagination/e5010-jpeg-enc.c b/drivers/media/platform/imagination/e5010-jpeg-enc.c
new file mode 100644
index 000000000000..ae868d9f73e1
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-jpeg-enc.c
@@ -0,0 +1,1635 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * TODO: Add MMU and memory tiling support
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/ioctl.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/pm_runtime.h>
+#include <media/jpeg.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-jpeg.h>
+#include <media/v4l2-rect.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-v4l2.h>
+#include "e5010-jpeg-enc.h"
+#include "e5010-jpeg-enc-hw.h"
+
+/* forward declarations */
+static const struct of_device_id e5010_of_match[];
+
+static const struct v4l2_file_operations e5010_fops;
+
+static const struct v4l2_ioctl_ops e5010_ioctl_ops;
+
+static const struct vb2_ops e5010_video_ops;
+
+static const struct v4l2_m2m_ops e5010_m2m_ops;
+
+static struct e5010_fmt e5010_formats[] = {
+	{
+		.fourcc = V4L2_PIX_FMT_NV12,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+		.chroma_order = CHROMA_ORDER_CB_CR,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV12M,
+		.num_planes = 2,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+		.chroma_order = CHROMA_ORDER_CB_CR,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV21,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+		.chroma_order = CHROMA_ORDER_CR_CB,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV21M,
+		.num_planes = 2,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_420,
+		.chroma_order = CHROMA_ORDER_CR_CB,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV16,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+		.chroma_order = CHROMA_ORDER_CB_CR,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV16M,
+		.num_planes = 2,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+		.chroma_order = CHROMA_ORDER_CB_CR,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV61,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+		.chroma_order = CHROMA_ORDER_CR_CB,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV61M,
+		.num_planes = 2,
+		.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.subsampling = V4L2_JPEG_CHROMA_SUBSAMPLING_422,
+		.chroma_order = CHROMA_ORDER_CR_CB,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 64,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_JPEG,
+		.num_planes = 1,
+		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.subsampling = 0,
+		.chroma_order = 0,
+		.frmsize = { MIN_DIMENSION, MAX_DIMENSION, 16,
+			     MIN_DIMENSION, MAX_DIMENSION, 8 },
+	},
+};
+
+static unsigned int debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "debug level");
+
+#define dprintk(dev, lvl, fmt, arg...) \
+	v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+static const struct v4l2_event e5010_eos_event = {
+	.type = V4L2_EVENT_EOS
+};
+
+static const char *type_name(enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		return "Output";
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		return "Capture";
+	default:
+		return "Invalid";
+	}
+}
+
+static struct e5010_q_data *get_queue(struct e5010_context *ctx, enum v4l2_buf_type type)
+{
+	return (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ? &ctx->out_queue : &ctx->cap_queue;
+}
+
+static void calculate_qp_tables(struct e5010_context *ctx)
+{
+	long long luminosity, contrast;
+	int quality, i;
+
+	quality = 50 - ctx->quality;
+
+	luminosity = LUMINOSITY * quality / 50;
+	contrast = CONTRAST * quality / 50;
+
+	if (quality > 0) {
+		luminosity *= INCREASE;
+		contrast *= INCREASE;
+	}
+
+	for (i = 0; i < V4L2_JPEG_PIXELS_IN_BLOCK; i++) {
+		long long delta = v4l2_jpeg_ref_table_chroma_qt[i] * contrast + luminosity;
+		int val = (int)(v4l2_jpeg_ref_table_chroma_qt[i] + delta);
+
+		clamp(val, 1, 255);
+		ctx->chroma_qp[i] = quality == -50 ? 1 : val;
+
+		delta = v4l2_jpeg_ref_table_luma_qt[i] * contrast + luminosity;
+		val = (int)(v4l2_jpeg_ref_table_luma_qt[i] + delta);
+		clamp(val, 1, 255);
+		ctx->luma_qp[i] = quality == -50 ? 1 : val;
+	}
+
+	ctx->update_qp = true;
+}
+
+static int update_qp_tables(struct e5010_context *ctx)
+{
+	struct e5010_dev *e5010 = ctx->e5010;
+	int i, ret = 0;
+	u32 lvalue, cvalue;
+
+	lvalue = 0;
+	cvalue = 0;
+
+	for (i = 0; i < QP_TABLE_SIZE; i++) {
+		lvalue |= ctx->luma_qp[i] << (8 * (i % 4));
+		cvalue |= ctx->chroma_qp[i] << (8 * (i % 4));
+		if (i % 4 == 3) {
+			ret |= e5010_hw_set_qpvalue(e5010->core_base,
+							JASPER_LUMA_QUANTIZATION_TABLE0_OFFSET
+							+ QP_TABLE_FIELD_OFFSET * ((i - 3) / 4),
+							lvalue);
+			ret |= e5010_hw_set_qpvalue(e5010->core_base,
+							JASPER_CHROMA_QUANTIZATION_TABLE0_OFFSET
+							+ QP_TABLE_FIELD_OFFSET * ((i - 3) / 4),
+							cvalue);
+			lvalue = 0;
+			cvalue = 0;
+		}
+	}
+
+	return ret;
+}
+
+static int e5010_set_input_subsampling(void __iomem *core_base, int subsampling)
+{
+	switch (subsampling) {
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_420:
+		return e5010_hw_set_input_subsampling(core_base, SUBSAMPLING_420);
+	case V4L2_JPEG_CHROMA_SUBSAMPLING_422:
+		return e5010_hw_set_input_subsampling(core_base, SUBSAMPLING_422);
+	default:
+		return -EINVAL;
+	};
+}
+
+static int e5010_querycap(struct file *file, void *priv, struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, E5010_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, E5010_MODULE_NAME, sizeof(cap->card));
+
+	return 0;
+}
+
+static struct e5010_fmt *find_format(struct v4l2_format *f)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(e5010_formats); ++i) {
+		if (e5010_formats[i].fourcc == f->fmt.pix_mp.pixelformat &&
+		    e5010_formats[i].type == f->type)
+			return &e5010_formats[i];
+	}
+
+	return NULL;
+}
+
+static int e5010_enum_fmt(struct file *file, void *priv, struct v4l2_fmtdesc *f)
+{
+	int i, index = 0;
+	struct e5010_fmt *fmt = NULL;
+	struct e5010_context *ctx = file->private_data;
+
+	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
+		v4l2_err(&ctx->e5010->v4l2_dev, "ENUMFMT with Invalid type: %d\n", f->type);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(e5010_formats); ++i) {
+		if (e5010_formats[i].type == f->type) {
+			if (index == f->index) {
+				fmt = &e5010_formats[i];
+				break;
+			}
+			index++;
+		}
+	}
+
+	if (!fmt)
+		return -EINVAL;
+
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+static int e5010_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct e5010_context *ctx = file->private_data;
+	struct e5010_q_data *queue;
+	int i;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = pix_mp->plane_fmt;
+
+	queue = get_queue(ctx, f->type);
+
+	pix_mp->flags = 0;
+	pix_mp->field = V4L2_FIELD_NONE;
+	pix_mp->pixelformat = queue->fmt->fourcc;
+	pix_mp->width = queue->width_adjusted;
+	pix_mp->height = queue->height_adjusted;
+	pix_mp->num_planes = queue->fmt->num_planes;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		if (!pix_mp->colorspace)
+			pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
+
+		for (i = 0; i < queue->fmt->num_planes; i++) {
+			plane_fmt[i].sizeimage = queue->sizeimage[i];
+			plane_fmt[i].bytesperline = queue->bytesperline[i];
+		}
+
+	} else {
+		pix_mp->colorspace = V4L2_COLORSPACE_JPEG;
+		plane_fmt[0].bytesperline = 0;
+		plane_fmt[0].sizeimage = queue->sizeimage[0];
+	}
+	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	pix_mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	pix_mp->quantization = V4L2_QUANTIZATION_DEFAULT;
+
+	return 0;
+}
+
+static int e5010_jpeg_try_fmt(struct v4l2_format *f, struct e5010_context *ctx)
+{
+	struct e5010_fmt *fmt;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = pix_mp->plane_fmt;
+
+	fmt = find_format(f);
+	if (!fmt) {
+		if (V4L2_TYPE_IS_OUTPUT(f->type))
+			pix_mp->pixelformat = V4L2_PIX_FMT_NV12;
+		else
+			pix_mp->pixelformat = V4L2_PIX_FMT_JPEG;
+		fmt = find_format(f);
+		if (!fmt)
+			return -EINVAL;
+	}
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		if (!pix_mp->colorspace)
+			pix_mp->colorspace = V4L2_COLORSPACE_JPEG;
+		if (!pix_mp->ycbcr_enc)
+			pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		if (!pix_mp->quantization)
+			pix_mp->quantization = V4L2_QUANTIZATION_DEFAULT;
+		if (!pix_mp->xfer_func)
+			pix_mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+
+		v4l2_apply_frmsize_constraints(&pix_mp->width,
+					       &pix_mp->height,
+					       &fmt->frmsize);
+
+		v4l2_fill_pixfmt_mp(pix_mp, pix_mp->pixelformat,
+				    pix_mp->width, pix_mp->height);
+
+	} else {
+		pix_mp->colorspace = V4L2_COLORSPACE_JPEG;
+		pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		pix_mp->quantization = V4L2_QUANTIZATION_DEFAULT;
+		pix_mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+		v4l2_apply_frmsize_constraints(&pix_mp->width,
+					       &pix_mp->height,
+					       &fmt->frmsize);
+		plane_fmt[0].sizeimage = pix_mp->width * pix_mp->height * JPEG_MAX_BYTES_PER_PIXEL;
+		plane_fmt[0].sizeimage += HEADER_SIZE;
+		plane_fmt[0].bytesperline = 0;
+		pix_mp->pixelformat = fmt->fourcc;
+		pix_mp->num_planes = fmt->num_planes;
+	}
+	pix_mp->flags = 0;
+	pix_mp->field = V4L2_FIELD_NONE;
+
+	dprintk(ctx->e5010, 2,
+		"ctx: 0x%p: format type %s:, wxh: %dx%d (plane0 : %d bytes, plane1 : %d bytes),fmt: %c%c%c%c\n",
+		ctx, type_name(f->type), pix_mp->width, pix_mp->height,
+		plane_fmt[0].sizeimage, plane_fmt[1].sizeimage,
+		(fmt->fourcc & 0xff),
+		(fmt->fourcc >>  8) & 0xff,
+		(fmt->fourcc >> 16) & 0xff,
+		(fmt->fourcc >> 24) & 0xff);
+
+	return 0;
+}
+
+static int e5010_try_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct e5010_context *ctx = file->private_data;
+
+	return e5010_jpeg_try_fmt(f, ctx);
+}
+
+static int e5010_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct e5010_context *ctx = file->private_data;
+	struct vb2_queue *vq;
+	int ret = 0, i = 0;
+	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = pix_mp->plane_fmt;
+	struct e5010_q_data *queue;
+	struct e5010_fmt *fmt;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->e5010->v4l2_dev, "queue busy\n");
+		return -EBUSY;
+	}
+
+	ret = e5010_jpeg_try_fmt(f, ctx);
+	if (ret)
+		return ret;
+
+	fmt = find_format(f);
+	queue = get_queue(ctx, f->type);
+
+	queue->fmt = fmt;
+	queue->width = pix_mp->width;
+	queue->height = pix_mp->height;
+
+	if (V4L2_TYPE_IS_OUTPUT(f->type)) {
+		for (i = 0; i < fmt->num_planes; i++) {
+			queue->bytesperline[i] = plane_fmt[i].bytesperline;
+			queue->sizeimage[i] = plane_fmt[i].sizeimage;
+		}
+		queue->crop.left = 0;
+		queue->crop.top = 0;
+		queue->crop.width = queue->width;
+		queue->crop.height = queue->height;
+	} else {
+		queue->sizeimage[0] = plane_fmt[0].sizeimage;
+		queue->sizeimage[1] = 0;
+		queue->bytesperline[0] = 0;
+		queue->bytesperline[1] = 0;
+	}
+
+	return 0;
+}
+
+static int e5010_enum_framesizes(struct file *file, void *priv, struct v4l2_frmsizeenum *fsize)
+{
+	struct v4l2_format f;
+	struct e5010_fmt *fmt;
+
+	if (fsize->index != 0)
+		return -EINVAL;
+
+	f.fmt.pix_mp.pixelformat = fsize->pixel_format;
+	if (f.fmt.pix_mp.pixelformat ==  V4L2_PIX_FMT_JPEG)
+		f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	else
+		f.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+
+	fmt = find_format(&f);
+	if (!fmt)
+		return -EINVAL;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise = fmt->frmsize;
+	fsize->reserved[0] = 0;
+	fsize->reserved[1] = 0;
+
+	return 0;
+}
+
+static int e5010_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct e5010_context *ctx = file->private_data;
+	struct e5010_q_data *queue;
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	queue = get_queue(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		s->r.left = 0;
+		s->r.top = 0;
+		s->r.width = queue->width;
+		s->r.height = queue->height;
+		break;
+	case V4L2_SEL_TGT_CROP:
+		memcpy(&s->r, &queue->crop, sizeof(s->r));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int e5010_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct e5010_context *ctx = file->private_data;
+	struct e5010_q_data *queue;
+	struct vb2_queue *vq;
+	struct v4l2_rect base_rect;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, s->type);
+	if (!vq)
+		return -EINVAL;
+
+	if (vb2_is_streaming(vq))
+		return -EBUSY;
+
+	if (s->target != V4L2_SEL_TGT_CROP ||
+	    s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	queue = get_queue(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	base_rect.top = 0;
+	base_rect.left = 0;
+	base_rect.width = queue->width;
+	base_rect.height = queue->height;
+
+	switch (s->flags) {
+	case 0:
+		s->r.width = round_down(s->r.width, queue->fmt->frmsize.step_width);
+		s->r.height = round_down(s->r.height, queue->fmt->frmsize.step_height);
+		s->r.left = round_down(s->r.left, queue->fmt->frmsize.step_width);
+		s->r.top = round_down(s->r.top, 2);
+
+		if (s->r.left + s->r.width > queue->width)
+			s->r.width = round_down(s->r.width + s->r.left - queue->width,
+						queue->fmt->frmsize.step_width);
+		if (s->r.top + s->r.height > queue->height)
+			s->r.top = round_down(s->r.top + s->r.height - queue->height, 2);
+		break;
+	case V4L2_SEL_FLAG_GE:
+		s->r.width = round_up(s->r.width, queue->fmt->frmsize.step_width);
+		s->r.height = round_up(s->r.height, queue->fmt->frmsize.step_height);
+		s->r.left = round_up(s->r.left, queue->fmt->frmsize.step_width);
+		s->r.top = round_up(s->r.top, 2);
+		break;
+	case V4L2_SEL_FLAG_LE:
+		s->r.width = round_down(s->r.width, queue->fmt->frmsize.step_width);
+		s->r.height = round_down(s->r.height, queue->fmt->frmsize.step_height);
+		s->r.left = round_down(s->r.left, queue->fmt->frmsize.step_width);
+		s->r.top = round_down(s->r.top, 2);
+		break;
+	case V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE:
+		if (!IS_ALIGNED(s->r.width, queue->fmt->frmsize.step_width) ||
+		    !IS_ALIGNED(s->r.height, queue->fmt->frmsize.step_height) ||
+		    !IS_ALIGNED(s->r.left, queue->fmt->frmsize.step_width) ||
+		    !IS_ALIGNED(s->r.top, 2))
+			return -ERANGE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!v4l2_rect_enclosed(&s->r, &base_rect))
+		return -ERANGE;
+
+	memcpy(&queue->crop, &s->r, sizeof(s->r));
+
+	if (!v4l2_rect_equal(&s->r, &base_rect))
+		queue->crop_set = true;
+
+	dprintk(ctx->e5010, 2, "ctx: 0x%p: crop rectangle: w: %d, h : %d, l : %d, t : %d\n",
+		ctx, queue->crop.width, queue->crop.height, queue->crop.left, queue->crop.top);
+
+	return 0;
+}
+
+static int e5010_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct e5010_context *ctx = priv;
+	struct e5010_dev *e5010 = ctx->e5010;
+	int ret = 0;
+
+	/* src_vq */
+	memset(src_vq, 0, sizeof(*src_vq));
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->drv_priv = ctx;
+	src_vq->buf_struct_size = sizeof(struct e5010_buffer);
+	src_vq->ops = &e5010_video_ops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->lock = &e5010->mutex;
+	src_vq->dev = e5010->v4l2_dev.dev;
+
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	/* dst_vq */
+	memset(dst_vq, 0, sizeof(*dst_vq));
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->drv_priv = ctx;
+	dst_vq->buf_struct_size = sizeof(struct e5010_buffer);
+	dst_vq->ops = &e5010_video_ops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->lock = &e5010->mutex;
+	dst_vq->dev = e5010->v4l2_dev.dev;
+
+	ret = vb2_queue_init(dst_vq);
+	if (ret) {
+		vb2_queue_release(src_vq);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int e5010_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct e5010_context *ctx =
+		container_of(ctrl->handler, struct e5010_context, ctrl_handler);
+
+	switch (ctrl->id) {
+	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
+		ctx->quality = ctrl->val;
+		calculate_qp_tables(ctx);
+		dprintk(ctx->e5010, 2, "ctx: 0x%p compression quality set to : %d\n", ctx,
+			ctx->quality);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops e5010_ctrl_ops = {
+	.s_ctrl = e5010_s_ctrl,
+};
+
+static void e5010_encode_ctrls(struct e5010_context *ctx)
+{
+	v4l2_ctrl_new_std(&ctx->ctrl_handler, &e5010_ctrl_ops,
+			  V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 75);
+}
+
+static int e5010_ctrls_setup(struct e5010_context *ctx)
+{
+	int err;
+
+	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 1);
+
+	e5010_encode_ctrls(ctx);
+
+	if (ctx->ctrl_handler.error) {
+		err = ctx->ctrl_handler.error;
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+
+		return err;
+	}
+
+	err = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
+	if (err)
+		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+
+	return err;
+}
+
+static void e5010_jpeg_set_default_params(struct e5010_context *ctx)
+{
+	struct e5010_q_data *queue;
+	struct v4l2_format f;
+	struct e5010_fmt *fmt;
+	struct v4l2_pix_format_mplane *pix_mp = &f.fmt.pix_mp;
+	struct v4l2_plane_pix_format *plane_fmt = pix_mp->plane_fmt;
+	int i = 0;
+
+	f.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12;
+	fmt = find_format(&f);
+	queue = &ctx->out_queue;
+	queue->fmt = fmt;
+	queue->width = DEFAULT_WIDTH;
+	queue->height = DEFAULT_HEIGHT;
+	pix_mp->width = queue->width;
+	pix_mp->height = queue->height;
+	queue->crop.left = 0;
+	queue->crop.top = 0;
+	queue->crop.width = queue->width;
+	queue->crop.height = queue->height;
+	v4l2_apply_frmsize_constraints(&pix_mp->width,
+				       &pix_mp->height,
+				       &fmt->frmsize);
+	v4l2_fill_pixfmt_mp(pix_mp, pix_mp->pixelformat,
+			    pix_mp->width, pix_mp->height);
+	for (i = 0; i < fmt->num_planes; i++) {
+		queue->bytesperline[i] = plane_fmt[i].bytesperline;
+		queue->sizeimage[i] = plane_fmt[i].sizeimage;
+	}
+	queue->width_adjusted = pix_mp->width;
+	queue->height_adjusted = pix_mp->height;
+
+	f.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_JPEG;
+	fmt = find_format(&f);
+	queue = &ctx->cap_queue;
+	queue->fmt = fmt;
+	queue->width = DEFAULT_WIDTH;
+	queue->height = DEFAULT_HEIGHT;
+	pix_mp->width = queue->width;
+	pix_mp->height = queue->height;
+	v4l2_apply_frmsize_constraints(&pix_mp->width,
+				       &pix_mp->height,
+				       &fmt->frmsize);
+	queue->sizeimage[0] = pix_mp->width * pix_mp->height * JPEG_MAX_BYTES_PER_PIXEL;
+	queue->sizeimage[0] += HEADER_SIZE;
+	queue->sizeimage[1] = 0;
+	queue->bytesperline[0] = 0;
+	queue->bytesperline[1] = 0;
+	queue->width_adjusted = pix_mp->width;
+	queue->height_adjusted = pix_mp->height;
+}
+
+static int e5010_open(struct file *file)
+{
+	struct e5010_dev *e5010 = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	struct e5010_context *ctx;
+	int ret = 0;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (mutex_lock_interruptible(&e5010->mutex)) {
+		ret = -ERESTARTSYS;
+		goto free;
+	}
+
+	v4l2_fh_init(&ctx->fh, vdev);
+	file->private_data = ctx;
+	v4l2_fh_add(&ctx->fh);
+
+	ctx->e5010 = e5010;
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(e5010->m2m_dev, ctx, queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		v4l2_err(&e5010->v4l2_dev, "failed to init m2m ctx\n");
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+		goto exit;
+	}
+
+	ret = e5010_ctrls_setup(ctx);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to setup e5010 jpeg controls\n");
+		goto err_ctrls_setup;
+	}
+	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
+
+	e5010_jpeg_set_default_params(ctx);
+
+	dprintk(e5010, 1, "Created instance: 0x%p, m2m_ctx: 0x%p\n", ctx, ctx->fh.m2m_ctx);
+
+	mutex_unlock(&e5010->mutex);
+	return 0;
+
+err_ctrls_setup:
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+exit:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	mutex_unlock(&e5010->mutex);
+free:
+	kfree(ctx);
+	return ret;
+}
+
+static int e5010_release(struct file *file)
+{
+	struct e5010_dev *e5010 = video_drvdata(file);
+	struct e5010_context *ctx = file->private_data;
+
+	dprintk(e5010, 1, "Releasing instance: 0x%p, m2m_ctx: 0x%p\n", ctx, ctx->fh.m2m_ctx);
+	mutex_lock(&e5010->mutex);
+	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	kfree(ctx);
+	mutex_unlock(&e5010->mutex);
+
+	return 0;
+}
+
+static void header_write(struct e5010_context *ctx, u8 *addr, unsigned int *offset,
+			 unsigned int no_bytes, unsigned long bits)
+{
+	u8 *w_addr = addr + *offset;
+	int i;
+
+	if ((*offset + no_bytes) > HEADER_SIZE) {
+		v4l2_warn(&ctx->e5010->v4l2_dev, "%s: %s: %d: Problem writing header. %d > HEADER_SIZE %d\n",
+			  __FILE__, __func__, __LINE__, *offset + no_bytes, HEADER_SIZE);
+		return;
+	}
+
+	for (i = no_bytes - 1; i >= 0; i--)
+		*(w_addr++) = ((u8 *)&bits)[i];
+
+	*offset += no_bytes;
+}
+
+static void encode_marker_segment(struct e5010_context *ctx, void *addr, unsigned int *offset)
+{
+	u8 *buffer = (u8 *)addr;
+	int i;
+
+	header_write(ctx, buffer, offset, 2, START_OF_IMAGE);
+	header_write(ctx, buffer, offset, 2, DQT_MARKER);
+	header_write(ctx, buffer, offset, 3, LQPQ << 4);
+	for (i = 0; i < V4L2_JPEG_PIXELS_IN_BLOCK; i++)
+		header_write(ctx, buffer, offset, 1, ctx->luma_qp[v4l2_jpeg_zigzag_scan_index[i]]);
+
+	header_write(ctx, buffer, offset, 2, DQT_MARKER);
+	header_write(ctx, buffer, offset, 3, (LQPQ << 4) | 1);
+	for (i = 0; i < V4L2_JPEG_PIXELS_IN_BLOCK; i++)
+		header_write(ctx, buffer, offset, 1,
+			     ctx->chroma_qp[v4l2_jpeg_zigzag_scan_index[i]]);
+
+	/* Huffman tables */
+	header_write(ctx, buffer, offset, 2, DHT_MARKER);
+	header_write(ctx, buffer, offset, 2, LH_DC);
+	header_write(ctx, buffer, offset, 1, V4L2_JPEG_LUM_HT | V4L2_JPEG_DC_HT);
+	for (i = 0 ; i < V4L2_JPEG_REF_HT_DC_LEN; i++)
+		header_write(ctx, buffer, offset, 1, v4l2_jpeg_ref_table_luma_dc_ht[i]);
+
+	header_write(ctx, buffer, offset, 2, DHT_MARKER);
+	header_write(ctx, buffer, offset, 2, LH_AC);
+	header_write(ctx, buffer, offset, 1, V4L2_JPEG_LUM_HT | V4L2_JPEG_AC_HT);
+	for (i = 0 ; i < V4L2_JPEG_REF_HT_AC_LEN; i++)
+		header_write(ctx, buffer, offset, 1, v4l2_jpeg_ref_table_luma_ac_ht[i]);
+
+	header_write(ctx, buffer, offset, 2, DHT_MARKER);
+	header_write(ctx, buffer, offset, 2, LH_DC);
+	header_write(ctx, buffer, offset, 1, V4L2_JPEG_CHR_HT | V4L2_JPEG_DC_HT);
+	for (i = 0 ; i < V4L2_JPEG_REF_HT_DC_LEN; i++)
+		header_write(ctx, buffer, offset, 1, v4l2_jpeg_ref_table_chroma_dc_ht[i]);
+
+	header_write(ctx, buffer, offset, 2, DHT_MARKER);
+	header_write(ctx, buffer, offset, 2, LH_AC);
+	header_write(ctx, buffer, offset, 1, V4L2_JPEG_CHR_HT | V4L2_JPEG_AC_HT);
+	for (i = 0 ; i < V4L2_JPEG_REF_HT_AC_LEN; i++)
+		header_write(ctx, buffer, offset, 1, v4l2_jpeg_ref_table_chroma_ac_ht[i]);
+}
+
+static void encode_frame_header(struct e5010_context *ctx, void *addr, unsigned int *offset)
+{
+	u8 *buffer = (u8 *)addr;
+
+	header_write(ctx, buffer, offset, 2, SOF_BASELINE_DCT);
+	header_write(ctx, buffer, offset, 2, 8 + (3 * UC_NUM_COMP));
+	header_write(ctx, buffer, offset, 1, PRECISION);
+	header_write(ctx, buffer, offset, 2, ctx->out_queue.crop.height);
+	header_write(ctx, buffer, offset, 2, ctx->out_queue.crop.width);
+	header_write(ctx, buffer, offset, 1, UC_NUM_COMP);
+
+	/* Luma details */
+	header_write(ctx, buffer, offset, 1, 1);
+	if (ctx->out_queue.fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_422)
+		header_write(ctx, buffer, offset, 1,
+			     HORZ_SAMPLING_FACTOR | (VERT_SAMPLING_FACTOR_422));
+	else
+		header_write(ctx, buffer, offset, 1,
+			     HORZ_SAMPLING_FACTOR | (VERT_SAMPLING_FACTOR_420));
+	header_write(ctx, buffer, offset, 1, 0);
+	/* Chroma details */
+	header_write(ctx, buffer, offset, 1, 2);
+	header_write(ctx, buffer, offset, 1, (HORZ_SAMPLING_FACTOR >> 1) | 1);
+	header_write(ctx, buffer, offset, 1, 1);
+	header_write(ctx, buffer, offset, 1, 3);
+	header_write(ctx, buffer, offset, 1, (HORZ_SAMPLING_FACTOR >> 1) | 1);
+	header_write(ctx, buffer, offset, 1, 1);
+}
+
+static void jpg_encode_sos_header(struct e5010_context *ctx, void *addr, unsigned int *offset)
+{
+	u8 *buffer = (u8 *)addr;
+	int i;
+
+	header_write(ctx, buffer, offset, 2, START_OF_SCAN);
+	header_write(ctx, buffer, offset, 2, 6 + (COMPONENTS_IN_SCAN << 1));
+	header_write(ctx, buffer, offset, 1, COMPONENTS_IN_SCAN);
+
+	for (i = 0; i < COMPONENTS_IN_SCAN; i++) {
+		header_write(ctx, buffer, offset, 1, i + 1);
+		if (i == 0)
+			header_write(ctx, buffer, offset, 1, 0);
+		else
+			header_write(ctx, buffer, offset, 1, 17);
+	}
+
+	header_write(ctx, buffer, offset, 1, 0);
+	header_write(ctx, buffer, offset, 1, 63);
+	header_write(ctx, buffer, offset, 1, 0);
+}
+
+static void write_header(struct e5010_context *ctx, void *addr)
+{
+	unsigned int offset = 0;
+
+	encode_marker_segment(ctx, addr, &offset);
+	encode_frame_header(ctx, addr, &offset);
+	jpg_encode_sos_header(ctx, addr, &offset);
+}
+
+static irqreturn_t e5010_irq(int irq, void *data)
+{
+	struct e5010_dev *e5010 = data;
+	struct e5010_context *ctx;
+	int output_size;
+	struct vb2_v4l2_buffer *src_buf, *dst_buf;
+	bool pic_done, out_addr_err;
+
+	spin_lock(&e5010->hw_lock);
+	pic_done = e5010_hw_pic_done_irq(e5010->core_base);
+	out_addr_err = e5010_hw_output_address_irq(e5010->core_base);
+
+	if (!pic_done && !out_addr_err) {
+		spin_unlock(&e5010->hw_lock);
+		return IRQ_NONE;
+	}
+
+	ctx = v4l2_m2m_get_curr_priv(e5010->m2m_dev);
+	if (WARN_ON(!ctx))
+		goto job_unlock;
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	if (!dst_buf || !src_buf) {
+		v4l2_err(&e5010->v4l2_dev, "ctx: 0x%p No source or destination buffer\n", ctx);
+		goto job_unlock;
+	}
+
+	if (out_addr_err) {
+		e5010_hw_clear_output_error(e5010->core_base, 1);
+		v4l2_warn(&e5010->v4l2_dev,
+			  "ctx: 0x%p Output bitstream size exceeded max size\n", ctx);
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, dst_buf->planes[0].length);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
+		if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx, src_buf)) {
+			dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+			v4l2_m2m_mark_stopped(ctx->fh.m2m_ctx);
+			v4l2_event_queue_fh(&ctx->fh, &e5010_eos_event);
+			dprintk(e5010, 2, "ctx: 0x%p Sending EOS\n", ctx);
+		}
+	}
+
+	if (pic_done) {
+		e5010_hw_clear_picture_done(e5010->core_base, 1);
+		dprintk(e5010, 3, "ctx: 0x%p Got output bitstream of size %d bytes\n",
+			ctx, readl(e5010->core_base + JASPER_OUTPUT_SIZE_OFFSET));
+
+		if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx, src_buf)) {
+			dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+			v4l2_m2m_mark_stopped(ctx->fh.m2m_ctx);
+			v4l2_event_queue_fh(&ctx->fh, &e5010_eos_event);
+			dprintk(e5010, 2, "ctx: 0x%p Sending EOS\n", ctx);
+		}
+		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		output_size = e5010_hw_get_output_size(e5010->core_base);
+		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, output_size + HEADER_SIZE);
+		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+		dprintk(e5010, 3,
+			"ctx: 0x%p frame done for dst_buf->sequence: %d src_buf->sequence: %d\n",
+			ctx, dst_buf->sequence, src_buf->sequence);
+	}
+
+	v4l2_m2m_job_finish(e5010->m2m_dev, ctx->fh.m2m_ctx);
+	dprintk(e5010, 3, "ctx: 0x%p Finish job\n", ctx);
+
+job_unlock:
+	spin_unlock(&e5010->hw_lock);
+	return IRQ_HANDLED;
+}
+
+static int e5010_init_device(struct e5010_dev *e5010)
+{
+	int ret = 0;
+
+	/*TODO: Set MMU in bypass mode until support for the same is added in driver*/
+	e5010_hw_bypass_mmu(e5010->mmu_base, 1);
+
+	if (e5010_hw_enable_auto_clock_gating(e5010->core_base, 1))
+		v4l2_warn(&e5010->v4l2_dev, "failed to enable auto clock gating\n");
+
+	if (e5010_hw_enable_manual_clock_gating(e5010->core_base, 0))
+		v4l2_warn(&e5010->v4l2_dev, "failed to disable manual clock gating\n");
+
+	if (e5010_hw_enable_crc_check(e5010->core_base, 0))
+		v4l2_warn(&e5010->v4l2_dev, "failed to disable CRC check\n");
+
+	if (e5010_hw_enable_output_address_error_irq(e5010->core_base, 1))
+		v4l2_err(&e5010->v4l2_dev, "failed to enable Output Address Error interrupts\n");
+
+	ret = e5010_hw_set_input_source_to_memory(e5010->core_base, 1);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input source to memory\n");
+		return ret;
+	}
+
+	ret = e5010_hw_enable_picture_done_irq(e5010->core_base, 1);
+	if (ret)
+		v4l2_err(&e5010->v4l2_dev, "failed to enable Picture Done interrupts\n");
+
+	return ret;
+}
+
+static int e5010_probe(struct platform_device *pdev)
+{
+	struct e5010_dev *e5010;
+	int irq, ret = 0;
+	struct device *dev = &pdev->dev;
+
+	ret = dma_set_mask(dev, DMA_BIT_MASK(32));
+	if (ret)
+		return dev_err_probe(dev, ret, "32-bit consistent DMA enable failed\n");
+
+	e5010 = devm_kzalloc(dev, sizeof(*e5010), GFP_KERNEL);
+	if (!e5010)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, e5010);
+
+	e5010->dev = dev;
+
+	mutex_init(&e5010->mutex);
+	spin_lock_init(&e5010->hw_lock);
+
+	e5010->vdev = video_device_alloc();
+	if (!e5010->vdev) {
+		dev_err(dev, "failed to allocate video device\n");
+		return -ENOMEM;
+	}
+
+	snprintf(e5010->vdev->name, sizeof(e5010->vdev->name), "%s", E5010_MODULE_NAME);
+	e5010->vdev->fops = &e5010_fops;
+	e5010->vdev->ioctl_ops = &e5010_ioctl_ops;
+	e5010->vdev->minor = -1;
+	e5010->vdev->release = video_device_release;
+	e5010->vdev->vfl_dir = VFL_DIR_M2M;
+	e5010->vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+	e5010->vdev->v4l2_dev = &e5010->v4l2_dev;
+	e5010->vdev->lock = &e5010->mutex;
+
+	ret = v4l2_device_register(dev, &e5010->v4l2_dev);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to register v4l2 device\n");
+		goto fail_after_video_device_alloc;
+	}
+
+
+	e5010->m2m_dev = v4l2_m2m_init(&e5010_m2m_ops);
+	if (IS_ERR(e5010->m2m_dev)) {
+		ret = PTR_ERR(e5010->m2m_dev);
+		e5010->m2m_dev = NULL;
+		dev_err_probe(dev, ret, "failed to init mem2mem device\n");
+		goto fail_after_v4l2_register;
+	}
+
+	video_set_drvdata(e5010->vdev, e5010);
+
+	e5010->core_base = devm_platform_ioremap_resource_byname(pdev, "core");
+	if (IS_ERR(e5010->core_base)) {
+		ret = PTR_ERR(e5010->core_base);
+		dev_err_probe(dev, ret, "Missing 'core' resources area\n");
+		goto fail_after_v4l2_register;
+	}
+
+	e5010->mmu_base = devm_platform_ioremap_resource_byname(pdev, "mmu");
+	if (IS_ERR(e5010->mmu_base)) {
+		ret = PTR_ERR(e5010->mmu_base);
+		dev_err_probe(dev, ret, "Missing 'mmu' resources area\n");
+		goto fail_after_v4l2_register;
+	}
+
+	e5010->last_context_run = NULL;
+
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(dev, irq, e5010_irq, 0,
+			       E5010_MODULE_NAME, e5010);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to register IRQ %d\n", irq);
+		goto fail_after_v4l2_register;
+	}
+
+	e5010->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(e5010->clk)) {
+		ret = PTR_ERR(e5010->clk);
+		dev_err_probe(dev, ret, "failed to get clock\n");
+		goto fail_after_v4l2_register;
+	}
+
+	pm_runtime_enable(dev);
+
+	ret = video_register_device(e5010->vdev, VFL_TYPE_VIDEO, 0);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to register video device\n");
+		goto fail_after_video_register_device;
+	}
+
+	v4l2_info(&e5010->v4l2_dev, "Device registered as /dev/video%d\n",
+		  e5010->vdev->num);
+
+	return 0;
+
+fail_after_video_register_device:
+	v4l2_m2m_release(e5010->m2m_dev);
+fail_after_v4l2_register:
+	v4l2_device_unregister(&e5010->v4l2_dev);
+fail_after_video_device_alloc:
+	video_device_release(e5010->vdev);
+	return ret;
+}
+
+static void e5010_remove(struct platform_device *pdev)
+{
+	struct e5010_dev *e5010 = platform_get_drvdata(pdev);
+
+	pm_runtime_disable(e5010->dev);
+	video_unregister_device(e5010->vdev);
+	v4l2_m2m_release(e5010->m2m_dev);
+	v4l2_device_unregister(&e5010->v4l2_dev);
+}
+
+static void e5010_vb2_buffers_return(struct vb2_queue *q, enum vb2_buffer_state state)
+{
+	struct vb2_v4l2_buffer *vbuf;
+	struct e5010_context *ctx = vb2_get_drv_priv(q);
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
+		while ((vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx))) {
+			dprintk(ctx->e5010, 2, "ctx: 0x%p, buf type %s | index %d\n",
+				ctx, type_name(vbuf->vb2_buf.type), vbuf->vb2_buf.index);
+			v4l2_m2m_buf_done(vbuf, state);
+		}
+	} else {
+		while ((vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx))) {
+			dprintk(ctx->e5010, 2, "ctx: 0x%p, buf type %s | index %d\n",
+				ctx, type_name(vbuf->vb2_buf.type), vbuf->vb2_buf.index);
+			vb2_set_plane_payload(&vbuf->vb2_buf, 0, 0);
+			v4l2_m2m_buf_done(vbuf, state);
+		}
+	}
+}
+
+static int e5010_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers, unsigned int *nplanes,
+			     unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(vq);
+	struct e5010_q_data *queue;
+	int i;
+
+	queue = get_queue(ctx, vq->type);
+
+	if (*nplanes) {
+		if (*nplanes != queue->fmt->num_planes)
+			return -EINVAL;
+		for (i = 0; i < *nplanes; i++) {
+			if (sizes[i] < queue->sizeimage[i])
+				return -EINVAL;
+		}
+		return 0;
+	}
+
+	*nplanes = queue->fmt->num_planes;
+	for (i = 0; i < *nplanes; i++)
+		sizes[i] = queue->sizeimage[i];
+
+	dprintk(ctx->e5010, 2,
+		"ctx: 0x%p, type %s, buffer(s): %d, planes %d, plane1: bytes %d plane2: %d bytes\n",
+		ctx, type_name(vq->type), *nbuffers, *nplanes, sizes[0], sizes[1]);
+
+	return 0;
+}
+
+static void e5010_buf_finish(struct vb2_buffer *vb)
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	void *d_addr;
+
+	if (vb->state != VB2_BUF_STATE_DONE || V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type))
+		return;
+
+	d_addr = vb2_plane_vaddr(vb, 0);
+	write_header(ctx, d_addr);
+}
+
+static int e5010_buf_out_validate(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct e5010_context *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+	if (vbuf->field != V4L2_FIELD_NONE)
+		dprintk(ctx->e5010, 1, "ctx: 0x%p, field isn't supported\n", ctx);
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+static int e5010_buf_prepare(struct vb2_buffer *vb)
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct e5010_q_data *queue;
+	int i;
+
+	vbuf->field = V4L2_FIELD_NONE;
+
+	queue = get_queue(ctx, vb->vb2_queue->type);
+
+	for (i = 0; i < queue->fmt->num_planes; i++) {
+		if (vb2_plane_size(vb, i) < (unsigned long)queue->sizeimage[i]) {
+			v4l2_err(&ctx->e5010->v4l2_dev, "plane %d too small (%lu < %lu)", i,
+				 vb2_plane_size(vb, i), (unsigned long)queue->sizeimage[i]);
+
+			return -EINVAL;
+		}
+	}
+
+	if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type)) {
+		vb2_set_plane_payload(vb, 0, 0);
+		vb2_set_plane_payload(vb, 1, 0);
+	}
+
+	return 0;
+}
+
+static void e5010_buf_queue(struct vb2_buffer *vb)
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type) &&
+	    vb2_is_streaming(vb->vb2_queue) &&
+	    v4l2_m2m_dst_buf_is_last(ctx->fh.m2m_ctx)) {
+		struct e5010_q_data *queue = get_queue(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+		vbuf->sequence = queue->sequence++;
+		v4l2_m2m_last_buffer_done(ctx->fh.m2m_ctx, vbuf);
+		v4l2_event_queue_fh(&ctx->fh, &e5010_eos_event);
+		return;
+	}
+
+	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int e5010_encoder_cmd(struct file *file, void *priv,
+			     struct v4l2_encoder_cmd *cmd)
+{
+	struct e5010_context *ctx = file->private_data;
+	int ret;
+	struct vb2_queue *cap_vq;
+
+	cap_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	ret = v4l2_m2m_ioctl_try_encoder_cmd(file, &ctx->fh, cmd);
+	if (ret < 0)
+		return ret;
+
+	if (!vb2_is_streaming(v4l2_m2m_get_src_vq(ctx->fh.m2m_ctx)) ||
+	    !vb2_is_streaming(v4l2_m2m_get_dst_vq(ctx->fh.m2m_ctx)))
+		return 0;
+
+	ret = v4l2_m2m_ioctl_encoder_cmd(file, &ctx->fh, cmd);
+	if (ret < 0)
+		return ret;
+
+	if (cmd->cmd == V4L2_ENC_CMD_STOP &&
+	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+		v4l2_event_queue_fh(&ctx->fh, &e5010_eos_event);
+
+	if (cmd->cmd == V4L2_ENC_CMD_START &&
+	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+		vb2_clear_last_buffer_dequeued(cap_vq);
+
+	return 0;
+}
+
+static int e5010_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(q);
+	int ret;
+
+	struct e5010_q_data *queue = get_queue(ctx, q->type);
+
+	v4l2_m2m_update_start_streaming_state(ctx->fh.m2m_ctx, q);
+	queue->sequence = 0;
+
+	ret = pm_runtime_resume_and_get(ctx->e5010->dev);
+	if (ret < 0) {
+		v4l2_err(&ctx->e5010->v4l2_dev, "failed to power up jpeg\n");
+		goto fail;
+	}
+
+	ret = e5010_init_device(ctx->e5010);
+	if (ret) {
+		v4l2_err(&ctx->e5010->v4l2_dev, "failed to Enable e5010 device\n");
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	e5010_vb2_buffers_return(q, VB2_BUF_STATE_QUEUED);
+
+	return ret;
+}
+
+static void e5010_stop_streaming(struct vb2_queue *q)
+{
+	struct e5010_context *ctx = vb2_get_drv_priv(q);
+
+	e5010_vb2_buffers_return(q, VB2_BUF_STATE_ERROR);
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type))
+		v4l2_m2m_update_stop_streaming_state(ctx->fh.m2m_ctx, q);
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type) &&
+	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx)) {
+		v4l2_event_queue_fh(&ctx->fh, &e5010_eos_event);
+	}
+
+	pm_runtime_put_sync(ctx->e5010->dev);
+}
+
+static void e5010_device_run(void *priv)
+{
+	struct e5010_context *ctx = priv;
+	struct e5010_dev *e5010 = ctx->e5010;
+	struct vb2_v4l2_buffer *s_vb, *d_vb;
+	u32 reg = 0;
+	int ret = 0, luma_crop_offset = 0, chroma_crop_offset = 0;
+	unsigned long flags;
+	int num_planes = ctx->out_queue.fmt->num_planes;
+
+	spin_lock_irqsave(&e5010->hw_lock, flags);
+	s_vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	WARN_ON(!s_vb);
+	d_vb = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	WARN_ON(!d_vb);
+	if (!s_vb || !d_vb)
+		goto no_ready_buf_err;
+
+	s_vb->sequence = ctx->out_queue.sequence++;
+	d_vb->sequence = ctx->cap_queue.sequence++;
+
+	v4l2_m2m_buf_copy_metadata(s_vb, d_vb, false);
+
+	if (ctx != e5010->last_context_run || ctx->update_qp) {
+		dprintk(e5010, 1, "ctx updated: 0x%p -> 0x%p, updating qp tables\n",
+			e5010->last_context_run, ctx);
+		ret = update_qp_tables(ctx);
+	}
+
+	if (ret) {
+		ctx->update_qp = true;
+		v4l2_err(&e5010->v4l2_dev, "failed to update QP tables\n");
+		goto device_busy_err;
+	} else {
+		e5010->last_context_run = ctx;
+		ctx->update_qp = false;
+	}
+
+	/* Set I/O Buffer addresses */
+	reg = (u32)vb2_dma_contig_plane_dma_addr(&s_vb->vb2_buf, 0);
+
+	if (ctx->out_queue.crop_set) {
+		luma_crop_offset = ctx->out_queue.bytesperline[0] * ctx->out_queue.crop.top +
+				   ctx->out_queue.crop.left;
+
+		if (ctx->out_queue.fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_422) {
+			chroma_crop_offset =
+				ctx->out_queue.bytesperline[0] * ctx->out_queue.crop.top
+				+ ctx->out_queue.crop.left;
+		} else {
+			chroma_crop_offset =
+				ctx->out_queue.bytesperline[0] * ctx->out_queue.crop.top / 2
+				+ ctx->out_queue.crop.left;
+		}
+
+		dprintk(e5010, 1, "Luma crop offset : %x, chroma crop offset : %x\n",
+			luma_crop_offset, chroma_crop_offset);
+	}
+
+	ret = e5010_hw_set_input_luma_addr(e5010->core_base, reg + luma_crop_offset);
+	if (ret || !reg) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input luma address\n");
+		goto device_busy_err;
+	}
+
+	if (num_planes == 1)
+		reg += (ctx->out_queue.bytesperline[0]) * (ctx->out_queue.height);
+	else
+		reg = (u32)vb2_dma_contig_plane_dma_addr(&s_vb->vb2_buf, 1);
+
+	dprintk(e5010, 3,
+		"ctx: 0x%p, luma_addr: 0x%x, chroma_addr: 0x%x, out_addr: 0x%x\n",
+		ctx, (u32)vb2_dma_contig_plane_dma_addr(&s_vb->vb2_buf, 0) + luma_crop_offset,
+		reg + chroma_crop_offset, (u32)vb2_dma_contig_plane_dma_addr(&d_vb->vb2_buf, 0));
+
+	dprintk(e5010, 3,
+		"ctx: 0x%p, buf indices: src_index: %d, dst_index: %d\n",
+		ctx, s_vb->vb2_buf.index, d_vb->vb2_buf.index);
+
+	ret = e5010_hw_set_input_chroma_addr(e5010->core_base, reg + chroma_crop_offset);
+	if (ret || !reg) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input chroma address\n");
+		goto device_busy_err;
+	}
+
+	reg = (u32)vb2_dma_contig_plane_dma_addr(&d_vb->vb2_buf, 0);
+	reg += HEADER_SIZE;
+	ret = e5010_hw_set_output_base_addr(e5010->core_base, reg);
+	if (ret || !reg) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set output base address\n");
+		goto device_busy_err;
+	}
+
+	/* Set input settings */
+	ret = e5010_hw_set_horizontal_size(e5010->core_base, ctx->out_queue.crop.width - 1);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input width\n");
+		goto device_busy_err;
+	}
+
+	ret = e5010_hw_set_vertical_size(e5010->core_base, ctx->out_queue.crop.height - 1);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input width\n");
+		goto device_busy_err;
+	}
+
+	ret = e5010_hw_set_luma_stride(e5010->core_base, ctx->out_queue.bytesperline[0]);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set luma stride\n");
+		goto device_busy_err;
+	}
+
+	ret = e5010_hw_set_chroma_stride(e5010->core_base, ctx->out_queue.bytesperline[0]);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set chroma stride\n");
+		goto device_busy_err;
+	}
+
+	ret = e5010_set_input_subsampling(e5010->core_base, ctx->out_queue.fmt->subsampling);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set input subsampling\n");
+		goto device_busy_err;
+	}
+
+	ret = e5010_hw_set_chroma_order(e5010->core_base, ctx->out_queue.fmt->chroma_order);
+	if (ret) {
+		v4l2_err(&e5010->v4l2_dev, "failed to set chroma order\n");
+		goto device_busy_err;
+	}
+
+	e5010_hw_set_output_max_size(e5010->core_base, d_vb->planes[0].length);
+	e5010_hw_encode_start(e5010->core_base, 1);
+
+	spin_unlock_irqrestore(&e5010->hw_lock, flags);
+
+	return;
+
+device_busy_err:
+	e5010_reset(e5010->dev, e5010->core_base, e5010->mmu_base);
+
+no_ready_buf_err:
+	if (s_vb) {
+		v4l2_m2m_src_buf_remove_by_buf(ctx->fh.m2m_ctx, s_vb);
+		v4l2_m2m_buf_done(s_vb, VB2_BUF_STATE_ERROR);
+	}
+
+	if (d_vb) {
+		v4l2_m2m_dst_buf_remove_by_buf(ctx->fh.m2m_ctx, d_vb);
+		/* Payload set to 1 since 0 payload can trigger EOS */
+		vb2_set_plane_payload(&d_vb->vb2_buf, 0, 1);
+		v4l2_m2m_buf_done(d_vb, VB2_BUF_STATE_ERROR);
+	}
+	v4l2_m2m_job_finish(e5010->m2m_dev, ctx->fh.m2m_ctx);
+	spin_unlock_irqrestore(&e5010->hw_lock, flags);
+}
+
+#ifdef CONFIG_PM
+static int e5010_runtime_resume(struct device *dev)
+{
+	struct e5010_dev *e5010 = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(e5010->clk);
+	if (ret < 0) {
+		v4l2_err(&e5010->v4l2_dev, "failed to enable clock\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int e5010_runtime_suspend(struct device *dev)
+{
+	struct e5010_dev *e5010 = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(e5010->clk);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+static int e5010_suspend(struct device *dev)
+{
+	struct e5010_dev *e5010 = dev_get_drvdata(dev);
+
+	v4l2_m2m_suspend(e5010->m2m_dev);
+
+	return pm_runtime_force_suspend(dev);
+}
+
+static int e5010_resume(struct device *dev)
+{
+	struct e5010_dev *e5010 = dev_get_drvdata(dev);
+	int ret;
+
+	ret = pm_runtime_force_resume(dev);
+	if (ret < 0)
+		return ret;
+
+	ret = e5010_init_device(e5010);
+	if (ret) {
+		dev_err(dev, "Failed to re-enable e5010 device\n");
+		return ret;
+	}
+
+	v4l2_m2m_resume(e5010->m2m_dev);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops	e5010_pm_ops = {
+	SET_RUNTIME_PM_OPS(e5010_runtime_suspend,
+			   e5010_runtime_resume, NULL)
+	SET_SYSTEM_SLEEP_PM_OPS(e5010_suspend, e5010_resume)
+};
+
+static const struct v4l2_ioctl_ops e5010_ioctl_ops = {
+	.vidioc_querycap = e5010_querycap,
+
+	.vidioc_enum_fmt_vid_cap = e5010_enum_fmt,
+	.vidioc_g_fmt_vid_cap_mplane = e5010_g_fmt,
+	.vidioc_try_fmt_vid_cap_mplane = e5010_try_fmt,
+	.vidioc_s_fmt_vid_cap_mplane = e5010_s_fmt,
+
+	.vidioc_enum_fmt_vid_out = e5010_enum_fmt,
+	.vidioc_g_fmt_vid_out_mplane = e5010_g_fmt,
+	.vidioc_try_fmt_vid_out_mplane = e5010_try_fmt,
+	.vidioc_s_fmt_vid_out_mplane = e5010_s_fmt,
+
+	.vidioc_g_selection = e5010_g_selection,
+	.vidioc_s_selection = e5010_s_selection,
+
+	.vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+	.vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+	.vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+
+	.vidioc_streamon = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+	.vidioc_log_status = v4l2_ctrl_log_status,
+
+	.vidioc_subscribe_event = e5010_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_try_encoder_cmd = v4l2_m2m_ioctl_try_encoder_cmd,
+	.vidioc_encoder_cmd = e5010_encoder_cmd,
+
+	.vidioc_enum_framesizes = e5010_enum_framesizes,
+};
+
+static const struct vb2_ops e5010_video_ops = {
+	.queue_setup = e5010_queue_setup,
+	.buf_queue = e5010_buf_queue,
+	.buf_finish = e5010_buf_finish,
+	.buf_prepare = e5010_buf_prepare,
+	.buf_out_validate = e5010_buf_out_validate,
+	.start_streaming = e5010_start_streaming,
+	.stop_streaming = e5010_stop_streaming,
+};
+
+static const struct v4l2_file_operations e5010_fops = {
+	.owner = THIS_MODULE,
+	.open = e5010_open,
+	.release = e5010_release,
+	.poll = v4l2_m2m_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_m2m_ops e5010_m2m_ops = {
+	.device_run = e5010_device_run,
+};
+
+static const struct of_device_id e5010_of_match[] = {
+	{.compatible = "img,e5010-jpeg-enc"},   { /* end */},
+};
+MODULE_DEVICE_TABLE(of, e5010_of_match);
+
+static struct platform_driver e5010_driver = {
+	.probe = e5010_probe,
+	.remove = e5010_remove,
+	.driver = {
+		.name = E5010_MODULE_NAME,
+		.of_match_table = e5010_of_match,
+		.pm = &e5010_pm_ops,
+	},
+};
+module_platform_driver(e5010_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Imagination E5010 JPEG encoder driver");
diff --git a/drivers/media/platform/imagination/e5010-jpeg-enc.h b/drivers/media/platform/imagination/e5010-jpeg-enc.h
new file mode 100644
index 000000000000..71f49ead6898
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-jpeg-enc.h
@@ -0,0 +1,168 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+
+#ifndef _E5010_JPEG_ENC_H
+#define _E5010_JPEG_ENC_H
+
+#define MAX_PLANES			2
+#define HEADER_SIZE			0x025D
+#define MIN_DIMENSION			64
+#define MAX_DIMENSION			8192
+#define DEFAULT_WIDTH			640
+#define DEFAULT_HEIGHT			480
+#define E5010_MODULE_NAME		"e5010"
+#define JPEG_MAX_BYTES_PER_PIXEL	2
+
+/* JPEG marker definitions */
+#define START_OF_IMAGE			0xFFD8
+#define SOF_BASELINE_DCT		0xFFC0
+#define END_OF_IMAGE			0xFFD9
+#define START_OF_SCAN			0xFFDA
+
+/* Definitions for the huffman table specification in the Marker segment */
+#define DHT_MARKER			0xFFC4
+#define LH_DC				0x001F
+#define LH_AC				0x00B5
+
+/* Definitions for the quantization table specification in the Marker segment */
+#define DQT_MARKER			0xFFDB
+#define ACMAX				0x03FF
+#define DCMAX				0x07FF
+
+/* Length and precision of the quantization table parameters */
+#define LQPQ				0x00430
+#define QMAX				255
+
+/* Misc JPEG header definitions */
+#define UC_NUM_COMP			3
+#define PRECISION			8
+#define HORZ_SAMPLING_FACTOR		(2 << 4)
+#define VERT_SAMPLING_FACTOR_422	1
+#define VERT_SAMPLING_FACTOR_420	2
+#define COMPONENTS_IN_SCAN		3
+#define PELS_IN_BLOCK			64
+
+/* Used for Qp table generation */
+#define LUMINOSITY			10
+#define CONTRAST			1
+#define INCREASE			2
+#define QP_TABLE_SIZE			(8 * 8)
+#define QP_TABLE_FIELD_OFFSET		0x04
+
+/*
+ * vb2 queue structure
+ * contains queue data information
+ *
+ * @fmt: format info
+ * @width: frame width
+ * @height: frame height
+ * @bytesperline: bytes per line in memory
+ * @size_image: image size in memory
+ */
+struct e5010_q_data {
+	struct e5010_fmt	*fmt;
+	u32			width;
+	u32			height;
+	u32			width_adjusted;
+	u32			height_adjusted;
+	u32			sizeimage[MAX_PLANES];
+	u32			bytesperline[MAX_PLANES];
+	u32			sequence;
+	struct v4l2_rect	crop;
+	bool			crop_set;
+};
+
+/*
+ * Driver device structure
+ * Holds all memory handles and global parameters
+ * Shared by all instances
+ */
+struct e5010_dev {
+	struct device *dev;
+	struct v4l2_device	v4l2_dev;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct video_device	*vdev;
+	void __iomem		*core_base;
+	void __iomem		*mmu_base;
+	struct clk		*clk;
+	struct e5010_context	*last_context_run;
+	/* Protect access to device data */
+	struct mutex		mutex;
+	/* Protect access to hardware*/
+	spinlock_t		hw_lock;
+};
+
+/*
+ * Driver context structure
+ * One of these exists for every m2m context
+ * Holds context specific data
+ */
+struct e5010_context {
+	struct v4l2_fh			fh;
+	struct e5010_dev		*e5010;
+	struct e5010_q_data		out_queue;
+	struct e5010_q_data		cap_queue;
+	int				quality;
+	bool				update_qp;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	u8				luma_qp[QP_TABLE_SIZE];
+	u8				chroma_qp[QP_TABLE_SIZE];
+};
+
+/*
+ * Buffer structure
+ * Contains info for all buffers
+ */
+struct e5010_buffer {
+	struct v4l2_m2m_buffer buffer;
+};
+
+enum {
+	CHROMA_ORDER_CB_CR = 0, //UV ordering
+	CHROMA_ORDER_CR_CB = 1, //VU ordering
+};
+
+enum {
+	SUBSAMPLING_420 = 1,
+	SUBSAMPLING_422 = 2,
+};
+
+/*
+ * e5010 format structure
+ * contains format information
+ */
+struct e5010_fmt {
+	u32					fourcc;
+	unsigned int				num_planes;
+	unsigned int				type;
+	u32					subsampling;
+	u32					chroma_order;
+	const struct v4l2_frmsize_stepwise	frmsize;
+};
+
+/*
+ * struct e5010_ctrl - contains info for each supported v4l2 control
+ */
+struct e5010_ctrl {
+	unsigned int		cid;
+	enum v4l2_ctrl_type	type;
+	unsigned char		name[32];
+	int			minimum;
+	int			maximum;
+	int			step;
+	int			default_value;
+	unsigned char		compound;
+};
+
+#endif
diff --git a/drivers/media/platform/imagination/e5010-mmu-regs.h b/drivers/media/platform/imagination/e5010-mmu-regs.h
new file mode 100644
index 000000000000..bfba06956cf2
--- /dev/null
+++ b/drivers/media/platform/imagination/e5010-mmu-regs.h
@@ -0,0 +1,311 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Imagination E5010 JPEG Encoder driver.
+ *
+ * Copyright (C) 2023 Texas Instruments Incorporated - https://www.ti.com/
+ *
+ * Author: David Huang <d-huang@ti.com>
+ * Author: Devarsh Thakkar <devarsht@ti.com>
+ */
+
+#ifndef _E5010_MMU_REGS_H
+#define _E5010_MMU_REGS_H
+
+#define MMU_MMU_DIR_BASE_ADDR_OFFSET					(0x0020)
+#define MMU_MMU_DIR_BASE_ADDR_STRIDE					(4)
+#define MMU_MMU_DIR_BASE_ADDR_NO_ENTRIES				(4)
+
+#define MMU_MMU_DIR_BASE_ADDR_MMU_DIR_BASE_ADDR_MASK			(0xFFFFFFFF)
+#define MMU_MMU_DIR_BASE_ADDR_MMU_DIR_BASE_ADDR_SHIFT			(0)
+
+#define MMU_MMU_TILE_CFG_OFFSET						(0x0040)
+#define MMU_MMU_TILE_CFG_STRIDE						(4)
+#define MMU_MMU_TILE_CFG_NO_ENTRIES					(4)
+
+#define MMU_MMU_TILE_CFG_TILE_128INTERLEAVE_MASK			(0x00000010)
+#define MMU_MMU_TILE_CFG_TILE_128INTERLEAVE_SHIFT			(4)
+
+#define MMU_MMU_TILE_CFG_TILE_ENABLE_MASK				(0x00000008)
+#define MMU_MMU_TILE_CFG_TILE_ENABLE_SHIFT				(3)
+
+#define MMU_MMU_TILE_CFG_TILE_STRIDE_MASK				(0x00000007)
+#define MMU_MMU_TILE_CFG_TILE_STRIDE_SHIFT				(0)
+
+#define MMU_MMU_TILE_MIN_ADDR_OFFSET					(0x0050)
+#define MMU_MMU_TILE_MIN_ADDR_STRIDE					(4)
+#define MMU_MMU_TILE_MIN_ADDR_NO_ENTRIES				(4)
+
+#define MMU_MMU_TILE_MIN_ADDR_TILE_MIN_ADDR_MASK			(0xFFFFFFFF)
+#define MMU_MMU_TILE_MIN_ADDR_TILE_MIN_ADDR_SHIFT			(0)
+
+#define MMU_MMU_TILE_MAX_ADDR_OFFSET					(0x0060)
+#define MMU_MMU_TILE_MAX_ADDR_STRIDE					(4)
+#define MMU_MMU_TILE_MAX_ADDR_NO_ENTRIES				(4)
+
+#define MMU_MMU_TILE_MAX_ADDR_TILE_MAX_ADDR_MASK			(0xFFFFFFFF)
+#define MMU_MMU_TILE_MAX_ADDR_TILE_MAX_ADDR_SHIFT			(0)
+
+#define MMU_MMU_CONTROL0_OFFSET						(0x0000)
+
+#define MMU_MMU_CONTROL0_MMU_TILING_SCHEME_MASK				(0x00000001)
+#define MMU_MMU_CONTROL0_MMU_TILING_SCHEME_SHIFT			(0)
+
+#define MMU_MMU_CONTROL0_MMU_CACHE_POLICY_MASK				(0x00000100)
+#define MMU_MMU_CONTROL0_MMU_CACHE_POLICY_SHIFT				(8)
+
+#define MMU_MMU_CONTROL0_FORCE_CACHE_POLICY_BYPASS_MASK			(0x00000200)
+#define MMU_MMU_CONTROL0_FORCE_CACHE_POLICY_BYPASS_SHIFT		(9)
+
+#define MMU_MMU_CONTROL0_STALL_ON_PROTOCOL_FAULT_MASK			(0x00001000)
+#define MMU_MMU_CONTROL0_STALL_ON_PROTOCOL_FAULT_SHIFT			(12)
+
+#define MMU_MMU_CONTROL1_OFFSET						(0x0008)
+
+#define MMU_MMU_CONTROL1_MMU_FLUSH_MASK					(0x00000008)
+#define MMU_MMU_CONTROL1_MMU_FLUSH_SHIFT				(3)
+#define MMU_MMU_CONTROL1_MMU_FLUSH_NO_REPS				(4)
+#define MMU_MMU_CONTROL1_MMU_FLUSH_SIZE					(1)
+
+#define MMU_MMU_CONTROL1_MMU_INVALDC_MASK				(0x00000800)
+#define MMU_MMU_CONTROL1_MMU_INVALDC_SHIFT				(11)
+#define MMU_MMU_CONTROL1_MMU_INVALDC_NO_REPS				(4)
+#define MMU_MMU_CONTROL1_MMU_INVALDC_SIZE				(1)
+
+#define MMU_MMU_CONTROL1_MMU_FAULT_CLEAR_MASK				(0x00010000)
+#define MMU_MMU_CONTROL1_MMU_FAULT_CLEAR_SHIFT				(16)
+
+#define MMU_MMU_CONTROL1_PROTOCOL_FAULT_CLEAR_MASK			(0x00100000)
+#define MMU_MMU_CONTROL1_PROTOCOL_FAULT_CLEAR_SHIFT			(20)
+
+#define MMU_MMU_CONTROL1_MMU_PAUSE_SET_MASK				(0x01000000)
+#define MMU_MMU_CONTROL1_MMU_PAUSE_SET_SHIFT				(24)
+
+#define MMU_MMU_CONTROL1_MMU_PAUSE_CLEAR_MASK				(0x02000000)
+#define MMU_MMU_CONTROL1_MMU_PAUSE_CLEAR_SHIFT				(25)
+
+#define MMU_MMU_CONTROL1_MMU_SOFT_RESET_MASK				(0x10000000)
+#define MMU_MMU_CONTROL1_MMU_SOFT_RESET_SHIFT				(28)
+
+#define MMU_MMU_BANK_INDEX_OFFSET					(0x0010)
+
+#define MMU_MMU_BANK_INDEX_MMU_BANK_INDEX_MASK				(0xC0000000)
+#define MMU_MMU_BANK_INDEX_MMU_BANK_INDEX_SHIFT				(30)
+#define MMU_MMU_BANK_INDEX_MMU_BANK_INDEX_NO_REPS			(16)
+#define MMU_MMU_BANK_INDEX_MMU_BANK_INDEX_SIZE				(2)
+
+#define MMU_REQUEST_PRIORITY_ENABLE_OFFSET				(0x0018)
+
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_PRIORITY_ENABLE_MASK		(0x00008000)
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_PRIORITY_ENABLE_SHIFT		(15)
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_PRIORITY_ENABLE_NO_REPS		(16)
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_PRIORITY_ENABLE_SIZE		(1)
+
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_MMU_PRIORITY_ENABLE_MASK	(0x00010000)
+#define MMU_REQUEST_PRIORITY_ENABLE_CMD_MMU_PRIORITY_ENABLE_SHIFT	(16)
+
+#define MMU_REQUEST_LIMITED_THROUGHPUT_OFFSET				(0x001C)
+
+#define MMU_REQUEST_LIMITED_THROUGHPUT_LIMITED_WORDS_MASK		(0x000003FF)
+#define MMU_REQUEST_LIMITED_THROUGHPUT_LIMITED_WORDS_SHIFT		(0)
+
+#define MMU_REQUEST_LIMITED_THROUGHPUT_REQUEST_GAP_MASK			(0x0FFF0000)
+#define MMU_REQUEST_LIMITED_THROUGHPUT_REQUEST_GAP_SHIFT		(16)
+
+#define MMU_MMU_ADDRESS_CONTROL_OFFSET					(0x0070)
+#define MMU_MMU_ADDRESS_CONTROL_TRUSTED					(IMG_TRUE)
+
+#define MMU_MMU_ADDRESS_CONTROL_MMU_BYPASS_MASK				(0x00000001)
+#define MMU_MMU_ADDRESS_CONTROL_MMU_BYPASS_SHIFT			(0)
+
+#define MMU_MMU_ADDRESS_CONTROL_MMU_ENABLE_EXT_ADDRESSING_MASK		(0x00000010)
+#define MMU_MMU_ADDRESS_CONTROL_MMU_ENABLE_EXT_ADDRESSING_SHIFT		(4)
+
+#define MMU_MMU_ADDRESS_CONTROL_UPPER_ADDRESS_FIXED_MASK		(0x00FF0000)
+#define MMU_MMU_ADDRESS_CONTROL_UPPER_ADDRESS_FIXED_SHIFT		(16)
+
+#define MMU_MMU_CONFIG0_OFFSET						(0x0080)
+
+#define MMU_MMU_CONFIG0_NUM_REQUESTORS_MASK				(0x0000000F)
+#define MMU_MMU_CONFIG0_NUM_REQUESTORS_SHIFT				(0)
+
+#define MMU_MMU_CONFIG0_EXTENDED_ADDR_RANGE_MASK			(0x000000F0)
+#define MMU_MMU_CONFIG0_EXTENDED_ADDR_RANGE_SHIFT			(4)
+
+#define MMU_MMU_CONFIG0_GROUP_OVERRIDE_SIZE_MASK			(0x00000700)
+#define MMU_MMU_CONFIG0_GROUP_OVERRIDE_SIZE_SHIFT			(8)
+
+#define MMU_MMU_CONFIG0_ADDR_COHERENCY_SUPPORTED_MASK			(0x00001000)
+#define MMU_MMU_CONFIG0_ADDR_COHERENCY_SUPPORTED_SHIFT			(12)
+
+#define MMU_MMU_CONFIG0_MMU_SUPPORTED_MASK				(0x00002000)
+#define MMU_MMU_CONFIG0_MMU_SUPPORTED_SHIFT				(13)
+
+#define MMU_MMU_CONFIG0_TILE_ADDR_GRANULARITY_MASK			(0x001F0000)
+#define MMU_MMU_CONFIG0_TILE_ADDR_GRANULARITY_SHIFT			(16)
+
+#define MMU_MMU_CONFIG0_NO_READ_REORDER_MASK				(0x00200000)
+#define MMU_MMU_CONFIG0_NO_READ_REORDER_SHIFT				(21)
+
+#define MMU_MMU_CONFIG0_TAGS_SUPPORTED_MASK				(0xFFC00000)
+#define MMU_MMU_CONFIG0_TAGS_SUPPORTED_SHIFT				(22)
+
+#define MMU_MMU_CONFIG1_OFFSET						(0x0084)
+
+#define MMU_MMU_CONFIG1_PAGE_SIZE_MASK					(0x0000000F)
+#define MMU_MMU_CONFIG1_PAGE_SIZE_SHIFT					(0)
+
+#define MMU_MMU_CONFIG1_PAGE_CACHE_ENTRIES_MASK				(0x0000FF00)
+#define MMU_MMU_CONFIG1_PAGE_CACHE_ENTRIES_SHIFT			(8)
+
+#define MMU_MMU_CONFIG1_DIR_CACHE_ENTRIES_MASK				(0x001F0000)
+#define MMU_MMU_CONFIG1_DIR_CACHE_ENTRIES_SHIFT				(16)
+
+#define MMU_MMU_CONFIG1_BANDWIDTH_COUNT_SUPPORTED_MASK			(0x01000000)
+#define MMU_MMU_CONFIG1_BANDWIDTH_COUNT_SUPPORTED_SHIFT			(24)
+
+#define MMU_MMU_CONFIG1_STALL_COUNT_SUPPORTED_MASK			(0x02000000)
+#define MMU_MMU_CONFIG1_STALL_COUNT_SUPPORTED_SHIFT			(25)
+
+#define MMU_MMU_CONFIG1_LATENCY_COUNT_SUPPORTED_MASK			(0x04000000)
+#define MMU_MMU_CONFIG1_LATENCY_COUNT_SUPPORTED_SHIFT			(26)
+
+#define MMU_MMU_STATUS0_OFFSET						(0x0088)
+
+#define MMU_MMU_STATUS0_MMU_PF_N_RW_MASK				(0x00000001)
+#define MMU_MMU_STATUS0_MMU_PF_N_RW_SHIFT				(0)
+
+#define MMU_MMU_STATUS0_MMU_FAULT_ADDR_MASK				(0xFFFFF000)
+#define MMU_MMU_STATUS0_MMU_FAULT_ADDR_SHIFT				(12)
+
+#define MMU_MMU_STATUS1_OFFSET						(0x008C)
+
+#define MMU_MMU_STATUS1_MMU_FAULT_REQ_STAT_MASK				(0x0000FFFF)
+#define MMU_MMU_STATUS1_MMU_FAULT_REQ_STAT_SHIFT			(0)
+
+#define MMU_MMU_STATUS1_MMU_FAULT_REQ_ID_MASK				(0x000F0000)
+#define MMU_MMU_STATUS1_MMU_FAULT_REQ_ID_SHIFT				(16)
+
+#define MMU_MMU_STATUS1_MMU_FAULT_INDEX_MASK				(0x03000000)
+#define MMU_MMU_STATUS1_MMU_FAULT_INDEX_SHIFT				(24)
+
+#define MMU_MMU_STATUS1_MMU_FAULT_RNW_MASK				(0x10000000)
+#define MMU_MMU_STATUS1_MMU_FAULT_RNW_SHIFT				(28)
+
+#define MMU_MMU_MEM_REQ_OFFSET						(0x0090)
+
+#define MMU_MMU_MEM_REQ_TAG_OUTSTANDING_MASK				(0x000003FF)
+#define MMU_MMU_MEM_REQ_TAG_OUTSTANDING_SHIFT				(0)
+
+#define MMU_MMU_MEM_REQ_EXT_WRRESP_FAULT_MASK				(0x00001000)
+#define MMU_MMU_MEM_REQ_EXT_WRRESP_FAULT_SHIFT				(12)
+
+#define MMU_MMU_MEM_REQ_EXT_RDRESP_FAULT_MASK				(0x00002000)
+#define MMU_MMU_MEM_REQ_EXT_RDRESP_FAULT_SHIFT				(13)
+
+#define MMU_MMU_MEM_REQ_EXT_READ_BURST_FAULT_MASK			(0x00004000)
+#define MMU_MMU_MEM_REQ_EXT_READ_BURST_FAULT_SHIFT			(14)
+
+#define MMU_MMU_MEM_REQ_INT_PROTOCOL_FAULT_MASK				(0x80000000)
+#define MMU_MMU_MEM_REQ_INT_PROTOCOL_FAULT_SHIFT			(31)
+#define MMU_MMU_MEM_REQ_INT_PROTOCOL_FAULT_NO_REPS			(16)
+#define MMU_MMU_MEM_REQ_INT_PROTOCOL_FAULT_SIZE				(1)
+
+#define MMU_MMU_FAULT_SELECT_OFFSET					(0x00A0)
+
+#define MMU_MMU_FAULT_SELECT_MMU_FAULT_SELECT_MASK			(0x0000000F)
+#define MMU_MMU_FAULT_SELECT_MMU_FAULT_SELECT_SHIFT			(0)
+
+#define MMU_PROTOCOL_FAULT_OFFSET					(0x00A8)
+
+#define MMU_PROTOCOL_FAULT_FAULT_PAGE_BREAK_MASK			(0x00000001)
+#define MMU_PROTOCOL_FAULT_FAULT_PAGE_BREAK_SHIFT			(0)
+
+#define MMU_PROTOCOL_FAULT_FAULT_WRITE_MASK				(0x00000010)
+#define MMU_PROTOCOL_FAULT_FAULT_WRITE_SHIFT				(4)
+
+#define MMU_PROTOCOL_FAULT_FAULT_READ_MASK				(0x00000020)
+#define MMU_PROTOCOL_FAULT_FAULT_READ_SHIFT				(5)
+
+#define MMU_TOTAL_READ_REQ_OFFSET					(0x0100)
+
+#define MMU_TOTAL_READ_REQ_TOTAL_READ_REQ_MASK				(0xFFFFFFFF)
+#define MMU_TOTAL_READ_REQ_TOTAL_READ_REQ_SHIFT				(0)
+
+#define MMU_TOTAL_WRITE_REQ_OFFSET					(0x0104)
+
+#define MMU_TOTAL_WRITE_REQ_TOTAL_WRITE_REQ_MASK			(0xFFFFFFFF)
+#define MMU_TOTAL_WRITE_REQ_TOTAL_WRITE_REQ_SHIFT			(0)
+
+#define MMU_READS_LESS_64_REQ_OFFSET					(0x0108)
+
+#define MMU_READS_LESS_64_REQ_READS_LESS_64_REQ_MASK			(0xFFFFFFFF)
+#define MMU_READS_LESS_64_REQ_READS_LESS_64_REQ_SHIFT			(0)
+
+#define MMU_WRITES_LESS_64_REQ_OFFSET					(0x010C)
+
+#define MMU_WRITES_LESS_64_REQ_WRITES_LESS_64_REQ_MASK			(0xFFFFFFFF)
+#define MMU_WRITES_LESS_64_REQ_WRITES_LESS_64_REQ_SHIFT			(0)
+
+#define MMU_EXT_CMD_STALL_OFFSET					(0x0120)
+
+#define MMU_EXT_CMD_STALL_EXT_CMD_STALL_MASK				(0xFFFFFFFF)
+#define MMU_EXT_CMD_STALL_EXT_CMD_STALL_SHIFT				(0)
+
+#define MMU_WRITE_REQ_STALL_OFFSET					(0x0124)
+
+#define MMU_WRITE_REQ_STALL_WRITE_REQ_STALL_MASK			(0xFFFFFFFF)
+#define MMU_WRITE_REQ_STALL_WRITE_REQ_STALL_SHIFT			(0)
+
+#define MMU_MMU_MISS_STALL_OFFSET					(0x0128)
+
+#define MMU_MMU_MISS_STALL_MMU_MISS_STALL_MASK				(0xFFFFFFFF)
+#define MMU_MMU_MISS_STALL_MMU_MISS_STALL_SHIFT				(0)
+
+#define MMU_ADDRESS_STALL_OFFSET					(0x012C)
+
+#define MMU_ADDRESS_STALL_ADDRESS_STALL_MASK				(0xFFFFFFFF)
+#define MMU_ADDRESS_STALL_ADDRESS_STALL_SHIFT				(0)
+
+#define MMU_TAG_STALL_OFFSET						(0x0130)
+
+#define MMU_TAG_STALL_TAG_STALL_MASK					(0xFFFFFFFF)
+#define MMU_TAG_STALL_TAG_STALL_SHIFT					(0)
+
+#define MMU_PEAK_READ_OUTSTANDING_OFFSET				(0x0140)
+
+#define MMU_PEAK_READ_OUTSTANDING_PEAK_TAG_OUTSTANDING_MASK		(0x000003FF)
+#define MMU_PEAK_READ_OUTSTANDING_PEAK_TAG_OUTSTANDING_SHIFT		(0)
+
+#define MMU_PEAK_READ_OUTSTANDING_PEAK_READ_LATENCY_MASK		(0xFFFF0000)
+#define MMU_PEAK_READ_OUTSTANDING_PEAK_READ_LATENCY_SHIFT		(16)
+
+#define MMU_AVERAGE_READ_LATENCY_OFFSET					(0x0144)
+
+#define MMU_AVERAGE_READ_LATENCY_AVERAGE_READ_LATENCY_MASK		(0xFFFFFFFF)
+#define MMU_AVERAGE_READ_LATENCY_AVERAGE_READ_LATENCY_SHIFT		(0)
+
+#define MMU_STATISTICS_CONTROL_OFFSET					(0x0160)
+
+#define MMU_STATISTICS_CONTROL_BANDWIDTH_STATS_INIT_MASK		(0x00000001)
+#define MMU_STATISTICS_CONTROL_BANDWIDTH_STATS_INIT_SHIFT		(0)
+
+#define MMU_STATISTICS_CONTROL_STALL_STATS_INIT_MASK			(0x00000002)
+#define MMU_STATISTICS_CONTROL_STALL_STATS_INIT_SHIFT			(1)
+
+#define MMU_STATISTICS_CONTROL_LATENCY_STATS_INIT_MASK			(0x00000004)
+#define MMU_STATISTICS_CONTROL_LATENCY_STATS_INIT_SHIFT			(2)
+
+#define MMU_MMU_VERSION_OFFSET						(0x01D0)
+
+#define MMU_MMU_VERSION_MMU_MAJOR_REV_MASK				(0x00FF0000)
+#define MMU_MMU_VERSION_MMU_MAJOR_REV_SHIFT				(16)
+
+#define MMU_MMU_VERSION_MMU_MINOR_REV_MASK				(0x0000FF00)
+#define MMU_MMU_VERSION_MMU_MINOR_REV_SHIFT				(8)
+
+#define MMU_MMU_VERSION_MMU_MAINT_REV_MASK				(0x000000FF)
+#define MMU_MMU_VERSION_MMU_MAINT_REV_SHIFT				(0)
+
+#define MMU_BYTE_SIZE							(0x01D4)
+
+#endif
diff --git a/drivers/media/platform/intel/pxa_camera.c b/drivers/media/platform/intel/pxa_camera.c
index d904952bf00e..bef1e7137f23 100644
--- a/drivers/media/platform/intel/pxa_camera.c
+++ b/drivers/media/platform/intel/pxa_camera.c
@@ -43,6 +43,7 @@
 #include <linux/videodev2.h>
 
 #include <linux/platform_data/media/camera-pxa.h>
+#include <linux/workqueue.h>
 
 #define PXA_CAM_VERSION "0.0.6"
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
@@ -683,7 +684,7 @@ struct pxa_camera_dev {
 	unsigned int		buf_sequence;
 
 	struct pxa_buffer	*active;
-	struct tasklet_struct	task_eof;
+	struct work_struct	eof_bh_work;
 
 	u32			save_cicr[5];
 };
@@ -1146,9 +1147,9 @@ static void pxa_camera_deactivate(struct pxa_camera_dev *pcdev)
 	clk_disable_unprepare(pcdev->clk);
 }
 
-static void pxa_camera_eof(struct tasklet_struct *t)
+static void pxa_camera_eof_bh_work(struct work_struct *t)
 {
-	struct pxa_camera_dev *pcdev = from_tasklet(pcdev, t, task_eof);
+	struct pxa_camera_dev *pcdev = from_work(pcdev, t, eof_bh_work);
 	unsigned long cifr;
 	struct pxa_buffer *buf;
 
@@ -1185,7 +1186,7 @@ static irqreturn_t pxa_camera_irq(int irq, void *data)
 	if (status & CISR_EOF) {
 		cicr0 = __raw_readl(pcdev->base + CICR0) | CICR0_EOFM;
 		__raw_writel(cicr0, pcdev->base + CICR0);
-		tasklet_schedule(&pcdev->task_eof);
+		queue_work(system_bh_wq, &pcdev->eof_bh_work);
 	}
 
 	return IRQ_HANDLED;
@@ -1503,8 +1504,6 @@ static const struct vb2_ops pxac_vb2_ops = {
 	.buf_cleanup		= pxac_vb2_cleanup,
 	.start_streaming	= pxac_vb2_start_streaming,
 	.stop_streaming		= pxac_vb2_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int pxa_camera_init_videobuf2(struct pxa_camera_dev *pcdev)
@@ -2383,7 +2382,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 		}
 	}
 
-	tasklet_setup(&pcdev->task_eof, pxa_camera_eof);
+	INIT_WORK(&pcdev->eof_bh_work, pxa_camera_eof_bh_work);
 
 	pxa_camera_activate(pcdev);
 
@@ -2409,7 +2408,7 @@ static int pxa_camera_probe(struct platform_device *pdev)
 	return 0;
 exit_deactivate:
 	pxa_camera_deactivate(pcdev);
-	tasklet_kill(&pcdev->task_eof);
+	cancel_work_sync(&pcdev->eof_bh_work);
 exit_free_dma:
 	dma_release_channel(pcdev->dma_chans[2]);
 exit_free_dma_u:
@@ -2428,7 +2427,7 @@ static void pxa_camera_remove(struct platform_device *pdev)
 	struct pxa_camera_dev *pcdev = platform_get_drvdata(pdev);
 
 	pxa_camera_deactivate(pcdev);
-	tasklet_kill(&pcdev->task_eof);
+	cancel_work_sync(&pcdev->eof_bh_work);
 	dma_release_channel(pcdev->dma_chans[0]);
 	dma_release_channel(pcdev->dma_chans[1]);
 	dma_release_channel(pcdev->dma_chans[2]);
@@ -2459,7 +2458,7 @@ static struct platform_driver pxa_camera_driver = {
 		.of_match_table = pxa_camera_of_match,
 	},
 	.probe		= pxa_camera_probe,
-	.remove_new	= pxa_camera_remove,
+	.remove		= pxa_camera_remove,
 };
 
 module_platform_driver(pxa_camera_driver);
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 96b35a5d6174..5188f3189096 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -724,10 +724,6 @@ static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = {
 /*
  * Queue operations
  */
-struct vb2_dc_conf {
-	struct device           *dev;
-};
-
 static int deinterlace_queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], struct device *alloc_devs[])
@@ -788,8 +784,6 @@ static const struct vb2_ops deinterlace_qops = {
 	.queue_setup	 = deinterlace_queue_setup,
 	.buf_prepare	 = deinterlace_buf_prepare,
 	.buf_queue	 = deinterlace_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -997,7 +991,7 @@ static void deinterlace_remove(struct platform_device *pdev)
 
 static struct platform_driver deinterlace_pdrv = {
 	.probe		= deinterlace_probe,
-	.remove_new	= deinterlace_remove,
+	.remove		= deinterlace_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
 	},
diff --git a/drivers/media/platform/marvell/mcam-core.c b/drivers/media/platform/marvell/mcam-core.c
index 66688b4aece5..b8360d37000a 100644
--- a/drivers/media/platform/marvell/mcam-core.c
+++ b/drivers/media/platform/marvell/mcam-core.c
@@ -439,9 +439,9 @@ static void mcam_ctlr_dma_vmalloc(struct mcam_camera *cam)
 /*
  * Copy data out to user space in the vmalloc case
  */
-static void mcam_frame_tasklet(struct tasklet_struct *t)
+static void mcam_frame_work(struct work_struct *t)
 {
-	struct mcam_camera *cam = from_tasklet(cam, t, s_tasklet);
+	struct mcam_camera *cam = from_work(cam, t, s_bh_work);
 	int i;
 	unsigned long flags;
 	struct mcam_vb_buffer *buf;
@@ -493,7 +493,7 @@ static int mcam_check_dma_buffers(struct mcam_camera *cam)
 
 static void mcam_vmalloc_done(struct mcam_camera *cam, int frame)
 {
-	tasklet_schedule(&cam->s_tasklet);
+	queue_work(system_bh_wq, &cam->s_bh_work);
 }
 
 #else /* MCAM_MODE_VMALLOC */
@@ -935,7 +935,12 @@ static int mclk_enable(struct clk_hw *hw)
 	ret = pm_runtime_resume_and_get(cam->dev);
 	if (ret < 0)
 		return ret;
-	clk_enable(cam->clk[0]);
+	ret = clk_enable(cam->clk[0]);
+	if (ret) {
+		pm_runtime_put(cam->dev);
+		return ret;
+	}
+
 	mcam_reg_write(cam, REG_CLKCTRL, (mclk_src << 29) | mclk_div);
 	mcam_ctlr_power_up(cam);
 
@@ -1203,8 +1208,6 @@ static const struct vb2_ops mcam_vb2_ops = {
 	.buf_queue		= mcam_vb_buf_queue,
 	.start_streaming	= mcam_vb_start_streaming,
 	.stop_streaming		= mcam_vb_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 
@@ -1267,8 +1270,6 @@ static const struct vb2_ops mcam_vb2_sg_ops = {
 	.buf_cleanup		= mcam_vb_sg_buf_cleanup,
 	.start_streaming	= mcam_vb_start_streaming,
 	.stop_streaming		= mcam_vb_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 #endif /* MCAM_MODE_DMA_SG */
@@ -1305,7 +1306,7 @@ static int mcam_setup_vb2(struct mcam_camera *cam)
 		break;
 	case B_vmalloc:
 #ifdef MCAM_MODE_VMALLOC
-		tasklet_setup(&cam->s_tasklet, mcam_frame_tasklet);
+		INIT_WORK(&cam->s_bh_work, mcam_frame_work);
 		vq->ops = &mcam_vb2_ops;
 		vq->mem_ops = &vb2_vmalloc_memops;
 		cam->dma_setup = mcam_ctlr_dma_vmalloc;
@@ -1981,5 +1982,6 @@ int mccic_resume(struct mcam_camera *cam)
 }
 EXPORT_SYMBOL_GPL(mccic_resume);
 
+MODULE_DESCRIPTION("Marvell camera core driver");
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
diff --git a/drivers/media/platform/marvell/mcam-core.h b/drivers/media/platform/marvell/mcam-core.h
index 51e66db45af6..989dc6859a53 100644
--- a/drivers/media/platform/marvell/mcam-core.h
+++ b/drivers/media/platform/marvell/mcam-core.h
@@ -9,6 +9,7 @@
 
 #include <linux/list.h>
 #include <linux/clk-provider.h>
+#include <linux/workqueue.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-dev.h>
@@ -167,7 +168,7 @@ struct mcam_camera {
 	unsigned int dma_buf_size;	/* allocated size */
 	void *dma_bufs[MAX_DMA_BUFS];	/* Internal buffer addresses */
 	dma_addr_t dma_handles[MAX_DMA_BUFS]; /* Buffer bus addresses */
-	struct tasklet_struct s_tasklet;
+	struct work_struct s_bh_work;
 #endif
 	unsigned int sequence;		/* Frame sequence number */
 	unsigned int buf_seq[MAX_DMA_BUFS]; /* Sequence for individual bufs */
diff --git a/drivers/media/platform/marvell/mmp-driver.c b/drivers/media/platform/marvell/mmp-driver.c
index 170907cc1885..d3da7ebb4a2b 100644
--- a/drivers/media/platform/marvell/mmp-driver.c
+++ b/drivers/media/platform/marvell/mmp-driver.c
@@ -30,6 +30,7 @@
 
 MODULE_ALIAS("platform:mmp-camera");
 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
+MODULE_DESCRIPTION("Support for the camera device found on Marvell MMP processors");
 MODULE_LICENSE("GPL");
 
 static char *mcam_clks[] = {"axi", "func", "phy"};
@@ -231,12 +232,22 @@ static int mmpcam_probe(struct platform_device *pdev)
 	mcam_init_clk(mcam);
 
 	/*
+	 * Register with V4L.
+	 */
+
+	ret = v4l2_device_register(mcam->dev, &mcam->v4l2_dev);
+	if (ret)
+		return ret;
+
+	/*
 	 * Create a match of the sensor against its OF node.
 	 */
 	ep = fwnode_graph_get_next_endpoint(of_fwnode_handle(pdev->dev.of_node),
 					    NULL);
-	if (!ep)
-		return -ENODEV;
+	if (!ep) {
+		ret = -ENODEV;
+		goto out_v4l2_device_unregister;
+	}
 
 	v4l2_async_nf_init(&mcam->notifier, &mcam->v4l2_dev);
 
@@ -245,7 +256,7 @@ static int mmpcam_probe(struct platform_device *pdev)
 	fwnode_handle_put(ep);
 	if (IS_ERR(asd)) {
 		ret = PTR_ERR(asd);
-		goto out;
+		goto out_v4l2_device_unregister;
 	}
 
 	/*
@@ -253,7 +264,7 @@ static int mmpcam_probe(struct platform_device *pdev)
 	 */
 	ret = mccic_register(mcam);
 	if (ret)
-		goto out;
+		goto out_v4l2_device_unregister;
 
 	/*
 	 * Add OF clock provider.
@@ -282,6 +293,8 @@ static int mmpcam_probe(struct platform_device *pdev)
 	return 0;
 out:
 	mccic_shutdown(mcam);
+out_v4l2_device_unregister:
+	v4l2_device_unregister(&mcam->v4l2_dev);
 
 	return ret;
 }
@@ -292,6 +305,7 @@ static void mmpcam_remove(struct platform_device *pdev)
 	struct mcam_camera *mcam = &cam->mcam;
 
 	mccic_shutdown(mcam);
+	v4l2_device_unregister(&mcam->v4l2_dev);
 	pm_runtime_force_suspend(mcam->dev);
 }
 
@@ -358,7 +372,7 @@ MODULE_DEVICE_TABLE(of, mmpcam_of_match);
 
 static struct platform_driver mmpcam_driver = {
 	.probe		= mmpcam_probe,
-	.remove_new	= mmpcam_remove,
+	.remove		= mmpcam_remove,
 	.driver = {
 		.name	= "mmp-camera",
 		.of_match_table = mmpcam_of_match,
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c
index ac48658e2de4..7eb12449b63a 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c
@@ -884,8 +884,6 @@ static const struct vb2_ops mtk_jpeg_dec_qops = {
 	.queue_setup        = mtk_jpeg_queue_setup,
 	.buf_prepare        = mtk_jpeg_buf_prepare,
 	.buf_queue          = mtk_jpeg_dec_buf_queue,
-	.wait_prepare       = vb2_ops_wait_prepare,
-	.wait_finish        = vb2_ops_wait_finish,
 	.stop_streaming     = mtk_jpeg_dec_stop_streaming,
 };
 
@@ -893,8 +891,6 @@ static const struct vb2_ops mtk_jpeg_enc_qops = {
 	.queue_setup        = mtk_jpeg_queue_setup,
 	.buf_prepare        = mtk_jpeg_buf_prepare,
 	.buf_queue          = mtk_jpeg_enc_buf_queue,
-	.wait_prepare       = vb2_ops_wait_prepare,
-	.wait_finish        = vb2_ops_wait_finish,
 	.stop_streaming     = mtk_jpeg_enc_stop_streaming,
 };
 
@@ -1030,6 +1026,7 @@ static void mtk_jpeg_dec_device_run(void *priv)
 	spin_lock_irqsave(&jpeg->hw_lock, flags);
 	mtk_jpeg_dec_reset(jpeg->reg_base);
 	mtk_jpeg_dec_set_config(jpeg->reg_base,
+				jpeg->variant->support_34bit,
 				&jpeg_src_buf->dec_param,
 				jpeg_src_buf->bs_size,
 				&bs,
@@ -1293,6 +1290,11 @@ static int mtk_jpeg_single_core_init(struct platform_device *pdev,
 	return 0;
 }
 
+static void mtk_jpeg_destroy_workqueue(void *data)
+{
+	destroy_workqueue(data);
+}
+
 static int mtk_jpeg_probe(struct platform_device *pdev)
 {
 	struct mtk_jpeg_dev *jpeg;
@@ -1337,6 +1339,11 @@ static int mtk_jpeg_probe(struct platform_device *pdev)
 							  | WQ_FREEZABLE);
 		if (!jpeg->workqueue)
 			return -EINVAL;
+		ret = devm_add_action_or_reset(&pdev->dev,
+					       mtk_jpeg_destroy_workqueue,
+					       jpeg->workqueue);
+		if (ret)
+			return ret;
 	}
 
 	ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
@@ -1564,7 +1571,8 @@ static irqreturn_t mtk_jpeg_enc_done(struct mtk_jpeg_dev *jpeg)
 	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 
-	result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base);
+	result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base,
+						 jpeg->variant->support_34bit);
 	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, result_size);
 
 	buf_state = VB2_BUF_STATE_DONE;
@@ -1764,6 +1772,7 @@ retry_select:
 	ctx->total_frame_num++;
 	mtk_jpeg_dec_reset(comp_jpeg[hw_id]->reg_base);
 	mtk_jpeg_dec_set_config(comp_jpeg[hw_id]->reg_base,
+				jpeg->variant->support_34bit,
 				&jpeg_src_buf->dec_param,
 				jpeg_src_buf->bs_size,
 				&bs,
@@ -1950,7 +1959,7 @@ MODULE_DEVICE_TABLE(of, mtk_jpeg_match);
 
 static struct platform_driver mtk_jpeg_driver = {
 	.probe = mtk_jpeg_probe,
-	.remove_new = mtk_jpeg_remove,
+	.remove = mtk_jpeg_remove,
 	.driver = {
 		.name           = MTK_JPEG_NAME,
 		.of_match_table = mtk_jpeg_match,
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h
index 8877eb39e807..02ed0ed5b736 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.h
@@ -34,6 +34,8 @@
 
 #define MTK_JPEG_MAX_EXIF_SIZE	(64 * 1024)
 
+#define MTK_JPEG_ADDR_MASK GENMASK(1, 0)
+
 /**
  * enum mtk_jpeg_ctx_state - states of the context state machine
  * @MTK_JPEG_INIT:		current state is initialized
@@ -62,6 +64,7 @@ enum mtk_jpeg_ctx_state {
  * @cap_q_default_fourcc:	capture queue default fourcc
  * @multi_core:		mark jpeg hw is multi_core or not
  * @jpeg_worker:		jpeg dec or enc worker
+ * @support_34bit:	flag to check support for 34-bit DMA address
  */
 struct mtk_jpeg_variant {
 	struct clk_bulk_data *clks;
@@ -78,6 +81,7 @@ struct mtk_jpeg_variant {
 	u32 cap_q_default_fourcc;
 	bool multi_core;
 	void (*jpeg_worker)(struct work_struct *work);
+	bool support_34bit;
 };
 
 struct mtk_jpeg_src_buf {
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c
index 4a6ee211e18f..e78e1d11093c 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.c
@@ -5,6 +5,8 @@
  *         Rick Chang <rick.chang@mediatek.com>
  */
 
+#include <linux/bitfield.h>
+#include <linux/bits.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
@@ -279,23 +281,43 @@ static void mtk_jpeg_dec_set_brz_factor(void __iomem *base, u8 yscale_w,
 	writel(val, base + JPGDEC_REG_BRZ_FACTOR);
 }
 
-static void mtk_jpeg_dec_set_dst_bank0(void __iomem *base, u32 addr_y,
-				       u32 addr_u, u32 addr_v)
+static void mtk_jpeg_dec_set_dst_bank0(void __iomem *base, bool support_34bit,
+				       dma_addr_t addr_y, dma_addr_t addr_u, dma_addr_t addr_v)
 {
+	u32 val;
+
 	mtk_jpeg_verify_align(addr_y, 16, JPGDEC_REG_DEST_ADDR0_Y);
-	writel(addr_y, base + JPGDEC_REG_DEST_ADDR0_Y);
+	writel(lower_32_bits(addr_y), base + JPGDEC_REG_DEST_ADDR0_Y);
 	mtk_jpeg_verify_align(addr_u, 16, JPGDEC_REG_DEST_ADDR0_U);
-	writel(addr_u, base + JPGDEC_REG_DEST_ADDR0_U);
+	writel(lower_32_bits(addr_u), base + JPGDEC_REG_DEST_ADDR0_U);
 	mtk_jpeg_verify_align(addr_v, 16, JPGDEC_REG_DEST_ADDR0_V);
-	writel(addr_v, base + JPGDEC_REG_DEST_ADDR0_V);
+	writel(lower_32_bits(addr_v), base + JPGDEC_REG_DEST_ADDR0_V);
+	if (support_34bit) {
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_y));
+		writel(val, base + JPGDEC_REG_DEST_ADDR0_Y_EXT);
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_u));
+		writel(val, base + JPGDEC_REG_DEST_ADDR0_U_EXT);
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_v));
+		writel(val, base + JPGDEC_REG_DEST_ADDR0_V_EXT);
+	}
 }
 
-static void mtk_jpeg_dec_set_dst_bank1(void __iomem *base, u32 addr_y,
-				       u32 addr_u, u32 addr_v)
+static void mtk_jpeg_dec_set_dst_bank1(void __iomem *base, bool support_34bit,
+				       dma_addr_t addr_y, dma_addr_t addr_u, dma_addr_t addr_v)
 {
-	writel(addr_y, base + JPGDEC_REG_DEST_ADDR1_Y);
-	writel(addr_u, base + JPGDEC_REG_DEST_ADDR1_U);
-	writel(addr_v, base + JPGDEC_REG_DEST_ADDR1_V);
+	u32 val;
+
+	writel(lower_32_bits(addr_y), base + JPGDEC_REG_DEST_ADDR1_Y);
+	writel(lower_32_bits(addr_u), base + JPGDEC_REG_DEST_ADDR1_U);
+	writel(lower_32_bits(addr_v), base + JPGDEC_REG_DEST_ADDR1_V);
+	if (support_34bit) {
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_y));
+		writel(val, base + JPGDEC_REG_DEST_ADDR1_Y_EXT);
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_u));
+		writel(val, base + JPGDEC_REG_DEST_ADDR1_U_EXT);
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr_v));
+		writel(val, base + JPGDEC_REG_DEST_ADDR1_V_EXT);
+	}
 }
 
 static void mtk_jpeg_dec_set_mem_stride(void __iomem *base, u32 stride_y,
@@ -322,18 +344,30 @@ static void mtk_jpeg_dec_set_dec_mode(void __iomem *base, u32 mode)
 	writel(mode & 0x03, base + JPGDEC_REG_OPERATION_MODE);
 }
 
-static void mtk_jpeg_dec_set_bs_write_ptr(void __iomem *base, u32 ptr)
+static void mtk_jpeg_dec_set_bs_write_ptr(void __iomem *base, bool support_34bit, dma_addr_t ptr)
 {
+	u32 val;
+
 	mtk_jpeg_verify_align(ptr, 16, JPGDEC_REG_FILE_BRP);
-	writel(ptr, base + JPGDEC_REG_FILE_BRP);
+	writel(lower_32_bits(ptr), base + JPGDEC_REG_FILE_BRP);
+	if (support_34bit) {
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(ptr));
+		writel(val, base + JPGDEC_REG_FILE_BRP_EXT);
+	}
 }
 
-static void mtk_jpeg_dec_set_bs_info(void __iomem *base, u32 addr, u32 size,
-				     u32 bitstream_size)
+static void mtk_jpeg_dec_set_bs_info(void __iomem *base, bool support_34bit,
+				     dma_addr_t addr, u32 size, u32 bitstream_size)
 {
+	u32 val;
+
 	mtk_jpeg_verify_align(addr, 16, JPGDEC_REG_FILE_ADDR);
 	mtk_jpeg_verify_align(size, 128, JPGDEC_REG_FILE_TOTAL_SIZE);
-	writel(addr, base + JPGDEC_REG_FILE_ADDR);
+	writel(lower_32_bits(addr), base + JPGDEC_REG_FILE_ADDR);
+	if (support_34bit) {
+		val = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(addr));
+		writel(val, base + JPGDEC_REG_FILE_ADDR_EXT);
+	}
 	writel(size, base + JPGDEC_REG_FILE_TOTAL_SIZE);
 	writel(bitstream_size, base + JPGDEC_REG_BIT_STREAM_SIZE);
 }
@@ -404,6 +438,7 @@ static void mtk_jpeg_dec_set_sampling_factor(void __iomem *base, u32 comp_num,
 }
 
 void mtk_jpeg_dec_set_config(void __iomem *base,
+			     bool support_34bits,
 			     struct mtk_jpeg_dec_param *cfg,
 			     u32 bitstream_size,
 			     struct mtk_jpeg_bs *bs,
@@ -413,8 +448,8 @@ void mtk_jpeg_dec_set_config(void __iomem *base,
 	mtk_jpeg_dec_set_dec_mode(base, 0);
 	mtk_jpeg_dec_set_comp0_du(base, cfg->unit_num);
 	mtk_jpeg_dec_set_total_mcu(base, cfg->total_mcu);
-	mtk_jpeg_dec_set_bs_info(base, bs->str_addr, bs->size, bitstream_size);
-	mtk_jpeg_dec_set_bs_write_ptr(base, bs->end_addr);
+	mtk_jpeg_dec_set_bs_info(base, support_34bits, bs->str_addr, bs->size, bitstream_size);
+	mtk_jpeg_dec_set_bs_write_ptr(base, support_34bits, bs->end_addr);
 	mtk_jpeg_dec_set_du_membership(base, cfg->membership, 1,
 				       (cfg->comp_num == 1) ? 1 : 0);
 	mtk_jpeg_dec_set_comp_id(base, cfg->comp_id[0], cfg->comp_id[1],
@@ -432,9 +467,9 @@ void mtk_jpeg_dec_set_config(void __iomem *base,
 				    cfg->mem_stride[1]);
 	mtk_jpeg_dec_set_img_stride(base, cfg->img_stride[0],
 				    cfg->img_stride[1]);
-	mtk_jpeg_dec_set_dst_bank0(base, fb->plane_addr[0],
+	mtk_jpeg_dec_set_dst_bank0(base, support_34bits, fb->plane_addr[0],
 				   fb->plane_addr[1], fb->plane_addr[2]);
-	mtk_jpeg_dec_set_dst_bank1(base, 0, 0, 0);
+	mtk_jpeg_dec_set_dst_bank1(base, support_34bits, 0, 0, 0);
 	mtk_jpeg_dec_set_dma_group(base, cfg->dma_mcu, cfg->dma_group,
 				   cfg->dma_last_mcu);
 	mtk_jpeg_dec_set_pause_mcu_idx(base, cfg->total_mcu);
@@ -578,11 +613,6 @@ static int mtk_jpegdec_hw_init_irq(struct mtk_jpegdec_comp_dev *dev)
 	return 0;
 }
 
-static void mtk_jpegdec_destroy_workqueue(void *data)
-{
-	destroy_workqueue(data);
-}
-
 static int mtk_jpegdec_hw_probe(struct platform_device *pdev)
 {
 	struct mtk_jpegdec_clk *jpegdec_clk;
@@ -606,12 +636,6 @@ static int mtk_jpegdec_hw_probe(struct platform_device *pdev)
 	dev->plat_dev = pdev;
 	dev->dev = &pdev->dev;
 
-	ret = devm_add_action_or_reset(&pdev->dev,
-				       mtk_jpegdec_destroy_workqueue,
-				       master_dev->workqueue);
-	if (ret)
-		return ret;
-
 	spin_lock_init(&dev->hw_lock);
 	dev->hw_state = MTK_JPEG_HW_IDLE;
 
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h
index 8c31c6b12417..2948c9c300a4 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_hw.h
@@ -71,6 +71,7 @@ int mtk_jpeg_dec_fill_param(struct mtk_jpeg_dec_param *param);
 u32 mtk_jpeg_dec_get_int_status(void __iomem *dec_reg_base);
 u32 mtk_jpeg_dec_enum_result(u32 irq_result);
 void mtk_jpeg_dec_set_config(void __iomem *base,
+			     bool support_34bits,
 			     struct mtk_jpeg_dec_param *cfg,
 			     u32 bitstream_size,
 			     struct mtk_jpeg_bs *bs,
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h
index 27b7711ca341..e94f52de7c69 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_dec_reg.h
@@ -46,5 +46,13 @@
 #define JPGDEC_REG_INTERRUPT_STATUS	0x0274
 #define JPGDEC_REG_STATUS		0x0278
 #define JPGDEC_REG_BIT_STREAM_SIZE	0x0344
+#define JPGDEC_REG_DEST_ADDR0_Y_EXT	0x0360
+#define JPGDEC_REG_DEST_ADDR0_U_EXT	0x0364
+#define JPGDEC_REG_DEST_ADDR0_V_EXT	0x0368
+#define JPGDEC_REG_DEST_ADDR1_Y_EXT	0x036c
+#define JPGDEC_REG_DEST_ADDR1_U_EXT	0x0370
+#define JPGDEC_REG_DEST_ADDR1_V_EXT	0x0374
+#define JPGDEC_REG_FILE_ADDR_EXT	0x0378
+#define JPGDEC_REG_FILE_BRP_EXT		0x037c
 
 #endif /* _MTK_JPEG_REG_H */
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c
index f8fa3b841ccf..9ab27aee302a 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.c
@@ -5,6 +5,8 @@
  *
  */
 
+#include <linux/bitfield.h>
+#include <linux/bits.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
@@ -62,9 +64,9 @@ void mtk_jpeg_enc_reset(void __iomem *base)
 }
 EXPORT_SYMBOL_GPL(mtk_jpeg_enc_reset);
 
-u32 mtk_jpeg_enc_get_file_size(void __iomem *base)
+u32 mtk_jpeg_enc_get_file_size(void __iomem *base, bool support_34bit)
 {
-	return readl(base + JPEG_ENC_DMA_ADDR0) -
+	return (readl(base + JPEG_ENC_DMA_ADDR0) << ((support_34bit) ? 2 : 0)) -
 	       readl(base + JPEG_ENC_DST_ADDR0);
 }
 EXPORT_SYMBOL_GPL(mtk_jpeg_enc_get_file_size);
@@ -84,14 +86,24 @@ void mtk_jpeg_set_enc_src(struct mtk_jpeg_ctx *ctx,  void __iomem *base,
 {
 	int i;
 	dma_addr_t dma_addr;
+	u32 addr_ext;
+	bool support_34bit = ctx->jpeg->variant->support_34bit;
 
 	for (i = 0; i < src_buf->num_planes; i++) {
 		dma_addr = vb2_dma_contig_plane_dma_addr(src_buf, i) +
 			   src_buf->planes[i].data_offset;
-		if (!i)
-			writel(dma_addr, base + JPEG_ENC_SRC_LUMA_ADDR);
+		if (i == 0)
+			writel(lower_32_bits(dma_addr), base + JPEG_ENC_SRC_LUMA_ADDR);
 		else
-			writel(dma_addr, base + JPEG_ENC_SRC_CHROMA_ADDR);
+			writel(lower_32_bits(dma_addr), base + JPEG_ENC_SRC_CHROMA_ADDR);
+
+		if (support_34bit) {
+			addr_ext = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(dma_addr));
+			if (i == 0)
+				writel(addr_ext, base + JPEG_ENC_SRC_LUMA_ADDR_EXT);
+			else
+				writel(addr_ext, base + JPEG_ENC_SRC_CHRO_ADDR_EXT);
+		}
 	}
 }
 EXPORT_SYMBOL_GPL(mtk_jpeg_set_enc_src);
@@ -103,6 +115,8 @@ void mtk_jpeg_set_enc_dst(struct mtk_jpeg_ctx *ctx, void __iomem *base,
 	size_t size;
 	u32 dma_addr_offset;
 	u32 dma_addr_offsetmask;
+	u32 addr_ext;
+	bool support_34bit = ctx->jpeg->variant->support_34bit;
 
 	dma_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
 	dma_addr_offset = ctx->enable_exif ? MTK_JPEG_MAX_EXIF_SIZE : 0;
@@ -113,6 +127,12 @@ void mtk_jpeg_set_enc_dst(struct mtk_jpeg_ctx *ctx, void __iomem *base,
 	writel(dma_addr_offsetmask & 0xf, base + JPEG_ENC_BYTE_OFFSET_MASK);
 	writel(dma_addr & ~0xf, base + JPEG_ENC_DST_ADDR0);
 	writel((dma_addr + size) & ~0xf, base + JPEG_ENC_STALL_ADDR0);
+
+	if (support_34bit) {
+		addr_ext = FIELD_PREP(MTK_JPEG_ADDR_MASK, upper_32_bits(dma_addr));
+		writel(addr_ext, base + JPEG_ENC_DEST_ADDR0_EXT);
+		writel(addr_ext + size, base + JPEG_ENC_STALL_ADDR0_EXT);
+	}
 }
 EXPORT_SYMBOL_GPL(mtk_jpeg_set_enc_dst);
 
@@ -278,7 +298,8 @@ static irqreturn_t mtk_jpegenc_hw_irq_handler(int irq, void *priv)
 	if (!(irq_status & JPEG_ENC_INT_STATUS_DONE))
 		dev_warn(jpeg->dev, "Jpg Enc occurs unknown Err.");
 
-	result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base);
+	result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base,
+						 ctx->jpeg->variant->support_34bit);
 	vb2_set_plane_payload(&dst_buf->vb2_buf, 0, result_size);
 	buf_state = VB2_BUF_STATE_DONE;
 	v4l2_m2m_buf_done(src_buf, buf_state);
diff --git a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h
index 61c60e4e58ea..31ec9030ae88 100644
--- a/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h
+++ b/drivers/media/platform/mediatek/jpeg/mtk_jpeg_enc_hw.h
@@ -68,6 +68,11 @@
 #define JPEG_ENC_DCM_CTRL		0x300
 #define JPEG_ENC_CODEC_SEL		0x314
 #define JPEG_ENC_ULTRA_THRES		0x318
+#define JPEG_ENC_SRC_LUMA_ADDR_EXT      0x584
+#define JPEG_ENC_SRC_CHRO_ADDR_EXT      0x588
+#define JPEG_ENC_Q_TBL_ADDR_EXT         0x58C
+#define JPEG_ENC_DEST_ADDR0_EXT         0x590
+#define JPEG_ENC_STALL_ADDR0_EXT        0x594
 
 /**
  * struct mtk_jpeg_enc_qlt - JPEG encoder quality data
@@ -80,7 +85,7 @@ struct mtk_jpeg_enc_qlt {
 };
 
 void mtk_jpeg_enc_reset(void __iomem *base);
-u32 mtk_jpeg_enc_get_file_size(void __iomem *base);
+u32 mtk_jpeg_enc_get_file_size(void __iomem *base, bool support_34bit);
 void mtk_jpeg_enc_start(void __iomem *enc_reg_base);
 void mtk_jpeg_set_enc_src(struct mtk_jpeg_ctx *ctx,  void __iomem *base,
 			  struct vb2_buffer *src_buf);
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c
index 917cdf38f230..80fdc6ff57e0 100644
--- a/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_core.c
@@ -298,7 +298,7 @@ static const struct dev_pm_ops mtk_mdp_pm_ops = {
 
 static struct platform_driver mtk_mdp_driver = {
 	.probe		= mtk_mdp_probe,
-	.remove_new	= mtk_mdp_remove,
+	.remove		= mtk_mdp_remove,
 	.driver = {
 		.name	= MTK_MDP_MODULE_NAME,
 		.pm	= &mtk_mdp_pm_ops,
diff --git a/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c
index f14779e7596e..d0fd77dcf8e2 100644
--- a/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c
+++ b/drivers/media/platform/mediatek/mdp/mtk_mdp_m2m.c
@@ -342,7 +342,7 @@ static int mtk_mdp_try_crop(struct mtk_mdp_ctx *ctx, u32 type,
 	if (r->left & 1)
 		r->left -= 1;
 
-	mtk_mdp_dbg(2, "[%d] crop l,t,w,h:%d,%d,%d,%d, max:%dx%d", ctx->id,
+	mtk_mdp_dbg(2, "[%d] crop (%d,%d)/%ux%u, max:%dx%d", ctx->id,
 		    r->left, r->top, r->width,
 		    r->height, max_w, max_h);
 	return 0;
@@ -584,8 +584,6 @@ static const struct vb2_ops mtk_mdp_m2m_qops = {
 	.buf_queue	 = mtk_mdp_m2m_buf_queue,
 	.stop_streaming	 = mtk_mdp_m2m_stop_streaming,
 	.start_streaming = mtk_mdp_m2m_start_streaming,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int mtk_mdp_m2m_querycap(struct file *file, void *fh,
diff --git a/drivers/media/platform/mediatek/mdp3/mdp_cfg_data.c b/drivers/media/platform/mediatek/mdp3/mdp_cfg_data.c
index ecca52b45307..0b4c50bc1776 100644
--- a/drivers/media/platform/mediatek/mdp3/mdp_cfg_data.c
+++ b/drivers/media/platform/mediatek/mdp3/mdp_cfg_data.c
@@ -46,6 +46,53 @@ enum mt8183_mdp_comp_id {
 	MT8183_MDP_COMP_WROT1,          /* 25 */
 };
 
+enum mt8188_mdp_comp_id {
+	/* MT8188 Comp id */
+	/* ISP */
+	MT8188_MDP_COMP_WPEI = 0,
+	MT8188_MDP_COMP_WPEO,           /* 1 */
+
+	/* MDP */
+	MT8188_MDP_COMP_CAMIN,          /* 2 */
+	MT8188_MDP_COMP_RDMA0,          /* 3 */
+	MT8188_MDP_COMP_RDMA2,          /* 4 */
+	MT8188_MDP_COMP_RDMA3,          /* 5 */
+	MT8188_MDP_COMP_FG0,            /* 6 */
+	MT8188_MDP_COMP_FG2,            /* 7 */
+	MT8188_MDP_COMP_FG3,            /* 8 */
+	MT8188_MDP_COMP_TO_SVPP2MOUT,   /* 9 */
+	MT8188_MDP_COMP_TO_SVPP3MOUT,   /* 10 */
+	MT8188_MDP_COMP_TO_WARP0MOUT,   /* 11 */
+	MT8188_MDP_COMP_VPP0_SOUT,      /* 12 */
+	MT8188_MDP_COMP_VPP1_SOUT,      /* 13 */
+	MT8188_MDP_COMP_PQ0_SOUT,       /* 14 */
+	MT8188_MDP_COMP_HDR0,           /* 15 */
+	MT8188_MDP_COMP_HDR2,           /* 16 */
+	MT8188_MDP_COMP_HDR3,           /* 17 */
+	MT8188_MDP_COMP_AAL0,           /* 18 */
+	MT8188_MDP_COMP_AAL2,           /* 19 */
+	MT8188_MDP_COMP_AAL3,           /* 20 */
+	MT8188_MDP_COMP_RSZ0,           /* 21 */
+	MT8188_MDP_COMP_RSZ2,           /* 22 */
+	MT8188_MDP_COMP_RSZ3,           /* 23 */
+	MT8188_MDP_COMP_TDSHP0,         /* 24 */
+	MT8188_MDP_COMP_TDSHP2,         /* 25 */
+	MT8188_MDP_COMP_TDSHP3,         /* 26 */
+	MT8188_MDP_COMP_COLOR0,         /* 27 */
+	MT8188_MDP_COMP_COLOR2,         /* 28 */
+	MT8188_MDP_COMP_COLOR3,         /* 29 */
+	MT8188_MDP_COMP_OVL0,           /* 30 */
+	MT8188_MDP_COMP_PAD0,           /* 31 */
+	MT8188_MDP_COMP_PAD2,           /* 32 */
+	MT8188_MDP_COMP_PAD3,           /* 33 */
+	MT8188_MDP_COMP_TCC0,           /* 34 */
+	MT8188_MDP_COMP_WROT0,          /* 35 */
+	MT8188_MDP_COMP_WROT2,          /* 36 */
+	MT8188_MDP_COMP_WROT3,          /* 37 */
+	MT8188_MDP_COMP_MERGE2,         /* 38 */
+	MT8188_MDP_COMP_MERGE3,         /* 39 */
+};
+
 enum mt8195_mdp_comp_id {
 	/* MT8195 Comp id */
 	/* ISP */
@@ -123,6 +170,13 @@ static const struct of_device_id mt8183_mdp_probe_infra[MDP_INFRA_MAX] = {
 	[MDP_INFRA_SCP] = { .compatible = "mediatek,mt8183-scp" }
 };
 
+static const struct of_device_id mt8188_mdp_probe_infra[MDP_INFRA_MAX] = {
+	[MDP_INFRA_MMSYS] = { .compatible = "mediatek,mt8188-vppsys0" },
+	[MDP_INFRA_MMSYS2] = { .compatible = "mediatek,mt8188-vppsys1" },
+	[MDP_INFRA_MUTEX] = { .compatible = "mediatek,mt8188-vpp-mutex" },
+	[MDP_INFRA_MUTEX2] = { .compatible = "mediatek,mt8188-vpp-mutex" },
+};
+
 static const struct of_device_id mt8195_mdp_probe_infra[MDP_INFRA_MAX] = {
 	[MDP_INFRA_MMSYS] = { .compatible = "mediatek,mt8195-vppsys0" },
 	[MDP_INFRA_MMSYS2] = { .compatible = "mediatek,mt8195-vppsys1" },
@@ -167,6 +221,40 @@ static const u32 mt8183_mutex_idx[MDP_MAX_COMP_COUNT] = {
 	[MDP_COMP_CCORR0] = MUTEX_MOD_IDX_MDP_CCORR0,
 };
 
+static const u32 mt8188_mutex_idx[MDP_MAX_COMP_COUNT] = {
+	[MDP_COMP_RDMA0] = MUTEX_MOD_IDX_MDP_RDMA0,
+	[MDP_COMP_RDMA2] = MUTEX_MOD_IDX_MDP_RDMA2,
+	[MDP_COMP_RDMA3] = MUTEX_MOD_IDX_MDP_RDMA3,
+	[MDP_COMP_FG0] = MUTEX_MOD_IDX_MDP_FG0,
+	[MDP_COMP_FG2] = MUTEX_MOD_IDX_MDP_FG2,
+	[MDP_COMP_FG3] = MUTEX_MOD_IDX_MDP_FG3,
+	[MDP_COMP_HDR0] = MUTEX_MOD_IDX_MDP_HDR0,
+	[MDP_COMP_HDR2] = MUTEX_MOD_IDX_MDP_HDR2,
+	[MDP_COMP_HDR3] = MUTEX_MOD_IDX_MDP_HDR3,
+	[MDP_COMP_AAL0] = MUTEX_MOD_IDX_MDP_AAL0,
+	[MDP_COMP_AAL2] = MUTEX_MOD_IDX_MDP_AAL2,
+	[MDP_COMP_AAL3] = MUTEX_MOD_IDX_MDP_AAL3,
+	[MDP_COMP_RSZ0] = MUTEX_MOD_IDX_MDP_RSZ0,
+	[MDP_COMP_RSZ2] = MUTEX_MOD_IDX_MDP_RSZ2,
+	[MDP_COMP_RSZ3] = MUTEX_MOD_IDX_MDP_RSZ3,
+	[MDP_COMP_MERGE2] = MUTEX_MOD_IDX_MDP_MERGE2,
+	[MDP_COMP_MERGE3] = MUTEX_MOD_IDX_MDP_MERGE3,
+	[MDP_COMP_TDSHP0] = MUTEX_MOD_IDX_MDP_TDSHP0,
+	[MDP_COMP_TDSHP2] = MUTEX_MOD_IDX_MDP_TDSHP2,
+	[MDP_COMP_TDSHP3] = MUTEX_MOD_IDX_MDP_TDSHP3,
+	[MDP_COMP_COLOR0] = MUTEX_MOD_IDX_MDP_COLOR0,
+	[MDP_COMP_COLOR2] = MUTEX_MOD_IDX_MDP_COLOR2,
+	[MDP_COMP_COLOR3] = MUTEX_MOD_IDX_MDP_COLOR3,
+	[MDP_COMP_OVL0] = MUTEX_MOD_IDX_MDP_OVL0,
+	[MDP_COMP_PAD0] = MUTEX_MOD_IDX_MDP_PAD0,
+	[MDP_COMP_PAD2] = MUTEX_MOD_IDX_MDP_PAD2,
+	[MDP_COMP_PAD3] = MUTEX_MOD_IDX_MDP_PAD3,
+	[MDP_COMP_TCC0] = MUTEX_MOD_IDX_MDP_TCC0,
+	[MDP_COMP_WROT0] = MUTEX_MOD_IDX_MDP_WROT0,
+	[MDP_COMP_WROT2] = MUTEX_MOD_IDX_MDP_WROT2,
+	[MDP_COMP_WROT3] = MUTEX_MOD_IDX_MDP_WROT3,
+};
+
 static const u32 mt8195_mutex_idx[MDP_MAX_COMP_COUNT] = {
 	[MDP_COMP_RDMA0] = MUTEX_MOD_IDX_MDP_RDMA0,
 	[MDP_COMP_RDMA1] = MUTEX_MOD_IDX_MDP_RDMA1,
@@ -288,6 +376,171 @@ static const struct mdp_comp_data mt8183_mdp_comp_data[MDP_MAX_COMP_COUNT] = {
 	},
 };
 
+static const struct mdp_comp_data mt8188_mdp_comp_data[MDP_MAX_COMP_COUNT] = {
+	[MDP_COMP_WPEI] = {
+		{MDP_COMP_TYPE_WPEI, 0, MT8188_MDP_COMP_WPEI, MDP_MM_SUBSYS_0},
+		{0, 0, 0}
+	},
+	[MDP_COMP_WPEO] = {
+		{MDP_COMP_TYPE_EXTO, 0, MT8188_MDP_COMP_WPEO, MDP_MM_SUBSYS_0},
+		{0, 0, 0}
+	},
+	[MDP_COMP_CAMIN] = {
+		{MDP_COMP_TYPE_DL_PATH, 0, MT8188_MDP_COMP_CAMIN, MDP_MM_SUBSYS_0},
+		{3, 3, 0}
+	},
+	[MDP_COMP_RDMA0] = {
+		{MDP_COMP_TYPE_RDMA, 0, MT8188_MDP_COMP_RDMA0, MDP_MM_SUBSYS_0},
+		{3, 0, 0}
+	},
+	[MDP_COMP_RDMA2] = {
+		{MDP_COMP_TYPE_RDMA, 1, MT8188_MDP_COMP_RDMA2, MDP_MM_SUBSYS_1},
+		{3, 0, 0}
+	},
+	[MDP_COMP_RDMA3] = {
+		{MDP_COMP_TYPE_RDMA, 2, MT8188_MDP_COMP_RDMA3, MDP_MM_SUBSYS_1},
+		{3, 0, 0}
+	},
+	[MDP_COMP_FG0] = {
+		{MDP_COMP_TYPE_FG, 0, MT8188_MDP_COMP_FG0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_FG2] = {
+		{MDP_COMP_TYPE_FG, 1, MT8188_MDP_COMP_FG2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_FG3] = {
+		{MDP_COMP_TYPE_FG, 2, MT8188_MDP_COMP_FG3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_HDR0] = {
+		{MDP_COMP_TYPE_HDR, 0, MT8188_MDP_COMP_HDR0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_HDR2] = {
+		{MDP_COMP_TYPE_HDR, 1, MT8188_MDP_COMP_HDR2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_HDR3] = {
+		{MDP_COMP_TYPE_HDR, 2, MT8188_MDP_COMP_HDR3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_AAL0] = {
+		{MDP_COMP_TYPE_AAL, 0, MT8188_MDP_COMP_AAL0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_AAL2] = {
+		{MDP_COMP_TYPE_AAL, 1, MT8188_MDP_COMP_AAL2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_AAL3] = {
+		{MDP_COMP_TYPE_AAL, 2, MT8188_MDP_COMP_AAL3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_RSZ0] = {
+		{MDP_COMP_TYPE_RSZ, 0, MT8188_MDP_COMP_RSZ0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_RSZ2] = {
+		{MDP_COMP_TYPE_RSZ, 1, MT8188_MDP_COMP_RSZ2, MDP_MM_SUBSYS_1},
+		{2, 0, 0},
+		{MDP_COMP_MERGE2, true, true}
+	},
+	[MDP_COMP_RSZ3] = {
+		{MDP_COMP_TYPE_RSZ, 2, MT8188_MDP_COMP_RSZ3, MDP_MM_SUBSYS_1},
+		{2, 0, 0},
+		{MDP_COMP_MERGE3, true, true}
+	},
+	[MDP_COMP_TDSHP0] = {
+		{MDP_COMP_TYPE_TDSHP, 0, MT8188_MDP_COMP_TDSHP0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_TDSHP2] = {
+		{MDP_COMP_TYPE_TDSHP, 1, MT8188_MDP_COMP_TDSHP2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_TDSHP3] = {
+		{MDP_COMP_TYPE_TDSHP, 2, MT8188_MDP_COMP_TDSHP3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_COLOR0] = {
+		{MDP_COMP_TYPE_COLOR, 0, MT8188_MDP_COMP_COLOR0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_COLOR2] = {
+		{MDP_COMP_TYPE_COLOR, 1, MT8188_MDP_COMP_COLOR2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_COLOR3] = {
+		{MDP_COMP_TYPE_COLOR, 2, MT8188_MDP_COMP_COLOR3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_OVL0] = {
+		{MDP_COMP_TYPE_OVL, 0, MT8188_MDP_COMP_OVL0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_PAD0] = {
+		{MDP_COMP_TYPE_PAD, 0, MT8188_MDP_COMP_PAD0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_PAD2] = {
+		{MDP_COMP_TYPE_PAD, 1, MT8188_MDP_COMP_PAD2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_PAD3] = {
+		{MDP_COMP_TYPE_PAD, 2, MT8188_MDP_COMP_PAD3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_TCC0] = {
+		{MDP_COMP_TYPE_TCC, 0, MT8188_MDP_COMP_TCC0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_WROT0] = {
+		{MDP_COMP_TYPE_WROT, 0, MT8188_MDP_COMP_WROT0, MDP_MM_SUBSYS_0},
+		{1, 0, 0}
+	},
+	[MDP_COMP_WROT2] = {
+		{MDP_COMP_TYPE_WROT, 1, MT8188_MDP_COMP_WROT2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_WROT3] = {
+		{MDP_COMP_TYPE_WROT, 2, MT8188_MDP_COMP_WROT3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_MERGE2] = {
+		{MDP_COMP_TYPE_MERGE, 0, MT8188_MDP_COMP_MERGE2, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_MERGE3] = {
+		{MDP_COMP_TYPE_MERGE, 1, MT8188_MDP_COMP_MERGE3, MDP_MM_SUBSYS_1},
+		{1, 0, 0}
+	},
+	[MDP_COMP_PQ0_SOUT] = {
+		{MDP_COMP_TYPE_DUMMY, 0, MT8188_MDP_COMP_PQ0_SOUT, MDP_MM_SUBSYS_0},
+		{0, 0, 0}
+	},
+	[MDP_COMP_TO_WARP0MOUT] = {
+		{MDP_COMP_TYPE_DUMMY, 1, MT8188_MDP_COMP_TO_WARP0MOUT, MDP_MM_SUBSYS_0},
+		{0, 0, 0}
+	},
+	[MDP_COMP_TO_SVPP2MOUT] = {
+		{MDP_COMP_TYPE_DUMMY, 2, MT8188_MDP_COMP_TO_SVPP2MOUT, MDP_MM_SUBSYS_1},
+		{0, 0, 0}
+	},
+	[MDP_COMP_TO_SVPP3MOUT] = {
+		{MDP_COMP_TYPE_DUMMY, 3, MT8188_MDP_COMP_TO_SVPP3MOUT, MDP_MM_SUBSYS_1},
+		{0, 0, 0}
+	},
+	[MDP_COMP_VPP0_SOUT] = {
+		{MDP_COMP_TYPE_PATH, 0, MT8188_MDP_COMP_VPP0_SOUT, MDP_MM_SUBSYS_1},
+		{2, 6, 0}
+	},
+	[MDP_COMP_VPP1_SOUT] = {
+		{MDP_COMP_TYPE_PATH, 1, MT8188_MDP_COMP_VPP1_SOUT, MDP_MM_SUBSYS_0},
+		{2, 8, 0}
+	},
+};
+
 static const struct mdp_comp_data mt8195_mdp_comp_data[MDP_MAX_COMP_COUNT] = {
 	[MDP_COMP_WPEI] = {
 		{MDP_COMP_TYPE_WPEI, 0, MT8195_MDP_COMP_WPEI, MDP_MM_SUBSYS_0},
@@ -1046,6 +1299,15 @@ static const struct mdp_pipe_info mt8183_pipe_info[] = {
 	[MDP_PIPE_RDMA0] = {MDP_PIPE_RDMA0, MDP_MM_SUBSYS_0, 3}
 };
 
+static const struct mdp_pipe_info mt8188_pipe_info[] = {
+	[MDP_PIPE_WPEI] = {MDP_PIPE_WPEI, MDP_MM_SUBSYS_0, 0},
+	[MDP_PIPE_RDMA0] = {MDP_PIPE_RDMA0, MDP_MM_SUBSYS_0, 1},
+	[MDP_PIPE_RDMA2] = {MDP_PIPE_RDMA2, MDP_MM_SUBSYS_1, 0},
+	[MDP_PIPE_RDMA3] = {MDP_PIPE_RDMA3, MDP_MM_SUBSYS_1, 1},
+	[MDP_PIPE_VPP1_SOUT] = {MDP_PIPE_VPP1_SOUT, MDP_MM_SUBSYS_0, 2},
+	[MDP_PIPE_VPP0_SOUT] = {MDP_PIPE_VPP0_SOUT, MDP_MM_SUBSYS_1, 2},
+};
+
 static const struct mdp_pipe_info mt8195_pipe_info[] = {
 	[MDP_PIPE_WPEI] = {MDP_PIPE_WPEI, MDP_MM_SUBSYS_0, 0},
 	[MDP_PIPE_WPEI2] = {MDP_PIPE_WPEI2, MDP_MM_SUBSYS_0, 1},
@@ -1082,6 +1344,24 @@ const struct mtk_mdp_driver_data mt8183_mdp_driver_data = {
 	.pp_used = MDP_PP_USED_1,
 };
 
+const struct mtk_mdp_driver_data mt8188_mdp_driver_data = {
+	.mdp_plat_id = MT8188,
+	.mdp_con_res = 0x14001000,
+	.mdp_probe_infra = mt8188_mdp_probe_infra,
+	.mdp_sub_comp_dt_ids = mt8195_sub_comp_dt_ids,
+	.mdp_cfg = &mt8195_plat_cfg,
+	.mdp_mutex_table_idx = mt8188_mutex_idx,
+	.comp_data = mt8188_mdp_comp_data,
+	.comp_data_len = ARRAY_SIZE(mt8188_mdp_comp_data),
+	.format = mt8195_formats,
+	.format_len = ARRAY_SIZE(mt8195_formats),
+	.def_limit = &mt8195_mdp_def_limit,
+	.pipe_info = mt8188_pipe_info,
+	.pipe_info_len = ARRAY_SIZE(mt8188_pipe_info),
+	.pp_criteria = &mt8195_mdp_pp_criteria,
+	.pp_used = MDP_PP_USED_2,
+};
+
 const struct mtk_mdp_driver_data mt8195_mdp_driver_data = {
 	.mdp_plat_id = MT8195,
 	.mdp_con_res = 0x14001000,
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h b/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h
index f83ac408306e..4764c5b5107b 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-img-ipi.h
@@ -116,6 +116,7 @@ struct img_frameparam {
 
 /* Platform config indicator */
 #define MT8183 8183
+#define MT8188 8195
 #define MT8195 8195
 
 #define CFG_CHECK(plat, p_id) ((plat) == (p_id))
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cfg.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cfg.h
index 49cdf45f6e59..7f7625299ce7 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cfg.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cfg.h
@@ -10,6 +10,7 @@
 #include <linux/types.h>
 
 extern const struct mtk_mdp_driver_data mt8183_mdp_driver_data;
+extern const struct mtk_mdp_driver_data mt8188_mdp_driver_data;
 extern const struct mtk_mdp_driver_data mt8195_mdp_driver_data;
 
 struct mdp_dev;
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
index 1d64bac34b90..e5ccf673e152 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.c
@@ -114,19 +114,15 @@ static struct img_config *__get_config_offset(struct mdp_dev *mdp,
 	if (pp_idx >= mdp->mdp_data->pp_used)
 		goto err_param;
 
-	if (CFG_CHECK(MT8183, p_id))
+	if (CFG_CHECK(MT8183, p_id)) {
 		cfg_c = CFG_OFST(MT8183, param->config, pp_idx);
-	else if (CFG_CHECK(MT8195, p_id))
-		cfg_c = CFG_OFST(MT8195, param->config, pp_idx);
-	else
-		goto err_param;
-
-	if (CFG_CHECK(MT8183, p_id))
 		cfg_n = CFG_OFST(MT8183, param->config, pp_idx + 1);
-	else if (CFG_CHECK(MT8195, p_id))
+	} else if (CFG_CHECK(MT8195, p_id)) {
+		cfg_c = CFG_OFST(MT8195, param->config, pp_idx);
 		cfg_n = CFG_OFST(MT8195, param->config, pp_idx + 1);
-	else
+	} else {
 		goto err_param;
+	}
 
 	if ((long)cfg_n - (long)mdp->vpu.config > bound) {
 		dev_err(dev, "config offset %ld OOB %ld\n", (long)cfg_n, bound);
@@ -325,8 +321,7 @@ static int mdp_path_config_subfrm(struct mdp_cmdq_cmd *cmd,
 	/* Enable mux settings */
 	for (index = 0; index < ctrl->num_sets; index++) {
 		set = &ctrl->sets[index];
-		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
-				    set->value, 0xFFFFFFFF);
+		cmdq_pkt_write(&cmd->pkt, set->subsys_id, set->reg, set->value);
 	}
 	/* Config sub-frame information */
 	for (index = (num_comp - 1); index >= 0; index--) {
@@ -381,8 +376,7 @@ static int mdp_path_config_subfrm(struct mdp_cmdq_cmd *cmd,
 	/* Disable mux settings */
 	for (index = 0; index < ctrl->num_sets; index++) {
 		set = &ctrl->sets[index];
-		cmdq_pkt_write_mask(&cmd->pkt, set->subsys_id, set->reg,
-				    0, 0xFFFFFFFF);
+		cmdq_pkt_write(&cmd->pkt, set->subsys_id, set->reg, 0);
 	}
 
 	return 0;
@@ -471,43 +465,6 @@ static int mdp_path_config(struct mdp_dev *mdp, struct mdp_cmdq_cmd *cmd,
 	return 0;
 }
 
-static int mdp_cmdq_pkt_create(struct cmdq_client *client, struct cmdq_pkt *pkt,
-			       size_t size)
-{
-	struct device *dev;
-	dma_addr_t dma_addr;
-
-	pkt->va_base = kzalloc(size, GFP_KERNEL);
-	if (!pkt->va_base)
-		return -ENOMEM;
-
-	pkt->buf_size = size;
-	pkt->cl = (void *)client;
-
-	dev = client->chan->mbox->dev;
-	dma_addr = dma_map_single(dev, pkt->va_base, pkt->buf_size,
-				  DMA_TO_DEVICE);
-	if (dma_mapping_error(dev, dma_addr)) {
-		dev_err(dev, "dma map failed, size=%u\n", (u32)(u64)size);
-		kfree(pkt->va_base);
-		return -ENOMEM;
-	}
-
-	pkt->pa_base = dma_addr;
-
-	return 0;
-}
-
-static void mdp_cmdq_pkt_destroy(struct cmdq_pkt *pkt)
-{
-	struct cmdq_client *client = (struct cmdq_client *)pkt->cl;
-
-	dma_unmap_single(client->chan->mbox->dev, pkt->pa_base, pkt->buf_size,
-			 DMA_TO_DEVICE);
-	kfree(pkt->va_base);
-	pkt->va_base = NULL;
-}
-
 static void mdp_auto_release_work(struct work_struct *work)
 {
 	struct mdp_cmdq_cmd *cmd;
@@ -524,7 +481,7 @@ static void mdp_auto_release_work(struct work_struct *work)
 	mdp_comp_clocks_off(&mdp->pdev->dev, cmd->comps,
 			    cmd->num_comps);
 
-	if (atomic_dec_and_test(&mdp->job_count)) {
+	if (refcount_dec_and_test(&mdp->job_count)) {
 		if (cmd->mdp_ctx)
 			mdp_m2m_job_finish(cmd->mdp_ctx);
 
@@ -538,7 +495,7 @@ static void mdp_auto_release_work(struct work_struct *work)
 		wake_up(&mdp->callback_wq);
 	}
 
-	mdp_cmdq_pkt_destroy(&cmd->pkt);
+	cmdq_pkt_destroy(mdp->cmdq_clt[cmd->pp_idx], &cmd->pkt);
 	kfree(cmd->comps);
 	cmd->comps = NULL;
 	kfree(cmd);
@@ -575,10 +532,10 @@ static void mdp_handle_cmdq_callback(struct mbox_client *cl, void *mssg)
 		mdp_comp_clocks_off(&mdp->pdev->dev, cmd->comps,
 				    cmd->num_comps);
 
-		if (atomic_dec_and_test(&mdp->job_count))
+		if (refcount_dec_and_test(&mdp->job_count))
 			wake_up(&mdp->callback_wq);
 
-		mdp_cmdq_pkt_destroy(&cmd->pkt);
+		cmdq_pkt_destroy(mdp->cmdq_clt[cmd->pp_idx], &cmd->pkt);
 		kfree(cmd->comps);
 		cmd->comps = NULL;
 		kfree(cmd);
@@ -607,20 +564,13 @@ static struct mdp_cmdq_cmd *mdp_cmdq_prepare(struct mdp_dev *mdp,
 		goto err_uninit;
 	}
 
-	if (CFG_CHECK(MT8183, p_id))
-		num_comp = CFG_GET(MT8183, config, num_components);
-	else if (CFG_CHECK(MT8195, p_id))
-		num_comp = CFG_GET(MT8195, config, num_components);
-	else
-		goto err_uninit;
-
 	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
 	if (!cmd) {
 		ret = -ENOMEM;
 		goto err_uninit;
 	}
 
-	ret = mdp_cmdq_pkt_create(mdp->cmdq_clt[pp_idx], &cmd->pkt, SZ_16K);
+	ret = cmdq_pkt_create(mdp->cmdq_clt[pp_idx], &cmd->pkt, SZ_16K);
 	if (ret)
 		goto err_free_cmd;
 
@@ -632,6 +582,7 @@ static struct mdp_cmdq_cmd *mdp_cmdq_prepare(struct mdp_dev *mdp,
 		ret = -EINVAL;
 		goto err_destroy_pkt;
 	}
+
 	comps = kcalloc(num_comp, sizeof(*comps), GFP_KERNEL);
 	if (!comps) {
 		ret = -ENOMEM;
@@ -676,7 +627,8 @@ static struct mdp_cmdq_cmd *mdp_cmdq_prepare(struct mdp_dev *mdp,
 		dev_err(dev, "mdp_path_config error %d\n", pp_idx);
 		goto err_free_path;
 	}
-	cmdq_pkt_finalize(&cmd->pkt);
+	cmdq_pkt_eoc(&cmd->pkt);
+	cmdq_pkt_jump_rel(&cmd->pkt, CMDQ_INST_SIZE, mdp->cmdq_shift_pa[pp_idx]);
 
 	for (i = 0; i < num_comp; i++) {
 		s32 inner_id = MDP_COMP_NONE;
@@ -699,6 +651,7 @@ static struct mdp_cmdq_cmd *mdp_cmdq_prepare(struct mdp_dev *mdp,
 	cmd->comps = comps;
 	cmd->num_comps = num_comp;
 	cmd->mdp_ctx = param->mdp_ctx;
+	cmd->pp_idx = pp_idx;
 
 	kfree(path);
 	return cmd;
@@ -710,7 +663,7 @@ err_free_path:
 err_free_comps:
 	kfree(comps);
 err_destroy_pkt:
-	mdp_cmdq_pkt_destroy(&cmd->pkt);
+	cmdq_pkt_destroy(mdp->cmdq_clt[pp_idx], &cmd->pkt);
 err_free_cmd:
 	kfree(cmd);
 err_uninit:
@@ -724,9 +677,9 @@ int mdp_cmdq_send(struct mdp_dev *mdp, struct mdp_cmdq_param *param)
 	int i, ret;
 	u8 pp_used = __get_pp_num(param->param->type);
 
-	atomic_set(&mdp->job_count, pp_used);
+	refcount_set(&mdp->job_count, pp_used);
 	if (atomic_read(&mdp->suspended)) {
-		atomic_set(&mdp->job_count, 0);
+		refcount_set(&mdp->job_count, 0);
 		return -ECANCELED;
 	}
 
@@ -764,7 +717,7 @@ err_clock_off:
 		mdp_comp_clocks_off(&mdp->pdev->dev, cmd[i]->comps,
 				    cmd[i]->num_comps);
 err_cancel_job:
-	atomic_set(&mdp->job_count, 0);
+	refcount_set(&mdp->job_count, 0);
 
 	return ret;
 }
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h
index 53a30ad7e0b0..222611e03a06 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-cmdq.h
@@ -12,8 +12,6 @@
 #include <linux/soc/mediatek/mtk-cmdq.h>
 #include "mtk-img-ipi.h"
 
-struct platform_device *mdp_get_plat_device(struct platform_device *pdev);
-
 struct mdp_cmdq_param {
 	struct img_config *config;
 	struct img_ipi_frameparam *param;
@@ -35,6 +33,7 @@ struct mdp_cmdq_cmd {
 	struct mdp_comp *comps;
 	void *mdp_ctx;
 	u8 num_comps;
+	u8 pp_idx;
 };
 
 struct mdp_dev;
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c
index 8f62fb167156..683c066ed975 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.c
@@ -72,14 +72,14 @@ static int init_rdma(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 
 		/* Disable RSZ1 */
 		if (ctx->comp->inner_id == rdma0 && prz1)
-			MM_REG_WRITE(cmd, subsys_id, prz1->reg_base, PRZ_ENABLE,
-				     0x0, BIT(0));
+			MM_REG_WRITE_MASK(cmd, subsys_id, prz1->reg_base,
+					  PRZ_ENABLE, 0x0, BIT(0));
 	}
 
 	/* Reset RDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_RESET, BIT(0), BIT(0));
-	MM_REG_POLL(cmd, subsys_id, base, MDP_RDMA_MON_STA_1, BIT(8), BIT(8));
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_RESET, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_RESET, BIT(0), BIT(0));
+	MM_REG_POLL_MASK(cmd, subsys_id, base, MDP_RDMA_MON_STA_1, BIT(8), BIT(8));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_RESET, 0x0, BIT(0));
 	return 0;
 }
 
@@ -98,26 +98,25 @@ static int config_rdma_frame(struct mdp_comp_ctx *ctx,
 
 	if (mdp_cfg && mdp_cfg->rdma_support_10bit) {
 		if (block10bit)
-			MM_REG_WRITE(cmd, subsys_id, base,
-				     MDP_RDMA_RESV_DUMMY_0, 0x7, 0x7);
+			MM_REG_WRITE_MASK(cmd, subsys_id, base,
+					  MDP_RDMA_RESV_DUMMY_0, 0x7, 0x7);
 		else
-			MM_REG_WRITE(cmd, subsys_id, base,
-				     MDP_RDMA_RESV_DUMMY_0, 0x0, 0x7);
+			MM_REG_WRITE_MASK(cmd, subsys_id, base,
+					  MDP_RDMA_RESV_DUMMY_0, 0x0, 0x7);
 	}
 
 	/* Setup smi control */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_GMCIF_CON,
-		     (7 <<  4) + //burst type to 8
-		     (1 << 16),  //enable pre-ultra
-		     0x00030071);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_GMCIF_CON,
+			  (7 <<  4) + //burst type to 8
+			  (1 << 16),  //enable pre-ultra
+			  0x00030071);
 
 	/* Setup source frame info */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.src_ctrl);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.src_ctrl);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_CON, reg,
-		     0x03C8FE0F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_SRC_CON, reg, 0x03C8FE0F);
 
 	if (mdp_cfg)
 		if (mdp_cfg->rdma_support_10bit && en_ufo) {
@@ -126,17 +125,15 @@ static int config_rdma_frame(struct mdp_comp_ctx *ctx,
 				reg = CFG_COMP(MT8183, ctx->param, rdma.ufo_dec_y);
 			else if (CFG_CHECK(MT8195, p_id))
 				reg = CFG_COMP(MT8195, ctx->param, rdma.ufo_dec_y);
-			MM_REG_WRITE(cmd, subsys_id,
-				     base, MDP_RDMA_UFO_DEC_LENGTH_BASE_Y,
-				     reg, 0xFFFFFFFF);
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_UFO_DEC_LENGTH_BASE_Y, reg);
 
 			if (CFG_CHECK(MT8183, p_id))
 				reg = CFG_COMP(MT8183, ctx->param, rdma.ufo_dec_c);
 			else if (CFG_CHECK(MT8195, p_id))
 				reg = CFG_COMP(MT8195, ctx->param, rdma.ufo_dec_c);
-			MM_REG_WRITE(cmd, subsys_id,
-				     base, MDP_RDMA_UFO_DEC_LENGTH_BASE_C,
-				     reg, 0xFFFFFFFF);
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     MDP_RDMA_UFO_DEC_LENGTH_BASE_C, reg);
 
 			/* Set 10bit source frame pitch */
 			if (block10bit) {
@@ -144,9 +141,9 @@ static int config_rdma_frame(struct mdp_comp_ctx *ctx,
 					reg = CFG_COMP(MT8183, ctx->param, rdma.mf_bkgd_in_pxl);
 				else if (CFG_CHECK(MT8195, p_id))
 					reg = CFG_COMP(MT8195, ctx->param, rdma.mf_bkgd_in_pxl);
-				MM_REG_WRITE(cmd, subsys_id,
-					     base, MDP_RDMA_MF_BKGD_SIZE_IN_PXL,
-					     reg, 0x001FFFFF);
+				MM_REG_WRITE_MASK(cmd, subsys_id, base,
+						  MDP_RDMA_MF_BKGD_SIZE_IN_PXL,
+						  reg, 0x001FFFFF);
 			}
 		}
 
@@ -157,128 +154,121 @@ static int config_rdma_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8195, ctx->param, rdma.control);
 		rdma_con_mask = 0x1130;
 	}
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_CON, reg,
-		     rdma_con_mask);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_CON, reg, rdma_con_mask);
 
 	/* Setup source buffer base */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_0, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_0, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_1, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_1, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_2, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_BASE_2, reg);
 
 	/* Setup source buffer end */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova_end[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova_end[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_0,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_0, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova_end[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova_end[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_1,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_1, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.iova_end[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.iova_end[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_2,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_END_2, reg);
 
 	/* Setup source frame pitch */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.mf_bkgd);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.mf_bkgd);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_BKGD_SIZE_IN_BYTE,
-		     reg, 0x001FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_MF_BKGD_SIZE_IN_BYTE,
+			  reg, 0x001FFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.sf_bkgd);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.sf_bkgd);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SF_BKGD_SIZE_IN_BYTE,
-		     reg, 0x001FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_SF_BKGD_SIZE_IN_BYTE,
+			  reg, 0x001FFFFF);
 
 	/* Setup color transform */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.transform);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.transform);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_TRANSFORM_0,
-		     reg, 0x0F110000);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_TRANSFORM_0,
+			  reg, 0x0F110000);
 
 	if (!mdp_cfg || !mdp_cfg->rdma_esl_setting)
 		goto rdma_config_done;
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.dmabuf_con0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_0,
-		     reg, 0x0FFF00FF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_0,
+			  reg, 0x0FFF00FF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_high_con0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_0,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_0,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_low_con0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_0,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_0,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.dmabuf_con1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_1,
-		     reg, 0x0F7F007F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_1,
+			  reg, 0x0F7F007F);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_high_con1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_1,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_1,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_low_con1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_1,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_1,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.dmabuf_con2);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_2,
-		     reg, 0x0F3F003F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_2,
+			  reg, 0x0F3F003F);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_high_con2);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_2,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_HIGH_CON_2,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.ultra_th_low_con2);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_2,
-		     reg, 0x3FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_ULTRA_TH_LOW_CON_2,
+			  reg, 0x3FFFFFFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.dmabuf_con3);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_3,
-		     reg, 0x0F3F003F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_DMABUF_CON_3,
+			  reg, 0x0F3F003F);
 
 rdma_config_done:
 	return 0;
@@ -297,15 +287,14 @@ static int config_rdma_subfrm(struct mdp_comp_ctx *ctx,
 	u32 reg = 0;
 
 	/* Enable RDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_EN, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_EN, BIT(0), BIT(0));
 
 	/* Set Y pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].offset[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].offset[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_0,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_0, reg);
 
 	/* Set 10bit UFO mode */
 	if (mdp_cfg) {
@@ -315,8 +304,7 @@ static int config_rdma_subfrm(struct mdp_comp_ctx *ctx,
 			else if (CFG_CHECK(MT8195, p_id))
 				reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].offset_0_p);
 			MM_REG_WRITE(cmd, subsys_id, base,
-				     MDP_RDMA_SRC_OFFSET_0_P,
-				     reg, 0xFFFFFFFF);
+				     MDP_RDMA_SRC_OFFSET_0_P, reg);
 		}
 	}
 
@@ -325,40 +313,38 @@ static int config_rdma_subfrm(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].offset[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].offset[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_1,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_1, reg);
 
 	/* Set V pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].offset[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].offset[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_2,
-		     reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_SRC_OFFSET_2, reg);
 
 	/* Set source size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].src);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_SRC_SIZE, reg,
-		     0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_MF_SRC_SIZE, reg,
+			  0x1FFF1FFF);
 
 	/* Set target size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].clip);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_CLIP_SIZE,
-		     reg, 0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_MF_CLIP_SIZE,
+			  reg, 0x1FFF1FFF);
 
 	/* Set crop offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rdma.subfrms[index].clip_ofst);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rdma.subfrms[index].clip_ofst);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_MF_OFFSET_1,
-		     reg, 0x003F001F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_MF_OFFSET_1,
+			  reg, 0x003F001F);
 
 	if (CFG_CHECK(MT8183, p_id)) {
 		csf_l = CFG_COMP(MT8183, ctx->param, subfrms[index].in.left);
@@ -369,8 +355,8 @@ static int config_rdma_subfrm(struct mdp_comp_ctx *ctx,
 	}
 	if (mdp_cfg && mdp_cfg->rdma_upsample_repeat_only)
 		if ((csf_r - csf_l + 1) > 320)
-			MM_REG_WRITE(cmd, subsys_id, base,
-				     MDP_RDMA_RESV_DUMMY_0, BIT(2), BIT(2));
+			MM_REG_WRITE_MASK(cmd, subsys_id, base,
+					  MDP_RDMA_RESV_DUMMY_0, BIT(2), BIT(2));
 
 	return 0;
 }
@@ -393,7 +379,7 @@ static int wait_rdma_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
 
 	/* Disable RDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_RDMA_EN, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_RDMA_EN, 0x0, BIT(0));
 	return 0;
 }
 
@@ -411,10 +397,10 @@ static int init_rsz(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u8 subsys_id = ctx->comp->subsys_id;
 
 	/* Reset RSZ */
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x10000, BIT(16));
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(16));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_ENABLE, 0x10000, BIT(16));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(16));
 	/* Enable RSZ */
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_ENABLE, BIT(0), BIT(0));
 
 	if (CFG_CHECK(MT8195, p_id)) {
 		struct device *dev;
@@ -437,7 +423,7 @@ static int config_rsz_frame(struct mdp_comp_ctx *ctx,
 	u32 reg = 0;
 
 	if (mdp_cfg && mdp_cfg->rsz_etc_control)
-		MM_REG_WRITE(cmd, subsys_id, base, RSZ_ETC_CONTROL, 0x0, 0xFFFFFFFF);
+		MM_REG_WRITE(cmd, subsys_id, base, RSZ_ETC_CONTROL, 0x0);
 
 	if (CFG_CHECK(MT8183, p_id))
 		bypass = CFG_COMP(MT8183, ctx->param, frame.bypass);
@@ -446,7 +432,7 @@ static int config_rsz_frame(struct mdp_comp_ctx *ctx,
 
 	if (bypass) {
 		/* Disable RSZ */
-		MM_REG_WRITE(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(0));
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_ENABLE, 0x0, BIT(0));
 		return 0;
 	}
 
@@ -454,29 +440,27 @@ static int config_rsz_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, rsz.control1);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.control1);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1, reg,
-		     0x03FFFDF3);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CONTROL_1, reg, 0x03FFFDF3);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rsz.control2);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.control2);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_2, reg,
-		     0x0FFFC290);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CONTROL_2, reg, 0x0FFFC290);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rsz.coeff_step_x);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.coeff_step_x);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_HORIZONTAL_COEFF_STEP,
-		     reg, 0x007FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_HORIZONTAL_COEFF_STEP, reg,
+			  0x007FFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rsz.coeff_step_y);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.coeff_step_y);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_VERTICAL_COEFF_STEP,
-		     reg, 0x007FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_VERTICAL_COEFF_STEP, reg,
+			  0x007FFFFF);
 
 	return 0;
 }
@@ -495,15 +479,13 @@ static int config_rsz_subfrm(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, rsz.subfrms[index].control2);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.subfrms[index].control2);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_2, reg,
-		     0x00003800);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CONTROL_2, reg, 0x00003800);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rsz.subfrms[index].src);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_INPUT_IMAGE, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_INPUT_IMAGE, reg);
 
 	if (CFG_CHECK(MT8183, p_id)) {
 		csf_l = CFG_COMP(MT8183, ctx->param, subfrms[index].in.left);
@@ -514,60 +496,56 @@ static int config_rsz_subfrm(struct mdp_comp_ctx *ctx,
 	}
 	if (mdp_cfg && mdp_cfg->rsz_disable_dcm_small_sample)
 		if ((csf_r - csf_l + 1) <= 16)
-			MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1,
-				     BIT(27), BIT(27));
+			MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CONTROL_1,
+					  BIT(27), BIT(27));
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].luma.left);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].luma.left);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_HORIZONTAL_INTEGER_OFFSET,
-		     reg, 0xFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_LUMA_HORIZONTAL_INTEGER_OFFSET,
+			  reg, 0xFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].luma.left_subpix);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].luma.left_subpix);
-	MM_REG_WRITE(cmd, subsys_id,
-		     base, PRZ_LUMA_HORIZONTAL_SUBPIXEL_OFFSET,
-		     reg, 0x1FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_LUMA_HORIZONTAL_SUBPIXEL_OFFSET,
+			  reg, 0x1FFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].luma.top);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].luma.top);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_INTEGER_OFFSET,
-		     reg, 0xFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_INTEGER_OFFSET,
+			  reg, 0xFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].luma.top_subpix);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].luma.top_subpix);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_SUBPIXEL_OFFSET,
-		     reg, 0x1FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_LUMA_VERTICAL_SUBPIXEL_OFFSET,
+			  reg, 0x1FFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].chroma.left);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].chroma.left);
-	MM_REG_WRITE(cmd, subsys_id,
-		     base, PRZ_CHROMA_HORIZONTAL_INTEGER_OFFSET,
-		     reg, 0xFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CHROMA_HORIZONTAL_INTEGER_OFFSET,
+			  reg, 0xFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, subfrms[index].chroma.left_subpix);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, subfrms[index].chroma.left_subpix);
-	MM_REG_WRITE(cmd, subsys_id,
-		     base, PRZ_CHROMA_HORIZONTAL_SUBPIXEL_OFFSET,
-		     reg, 0x1FFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CHROMA_HORIZONTAL_SUBPIXEL_OFFSET,
+			  reg, 0x1FFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, rsz.subfrms[index].clip);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, rsz.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, PRZ_OUTPUT_IMAGE, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, PRZ_OUTPUT_IMAGE, reg);
 
 	if (CFG_CHECK(MT8195, p_id)) {
 		struct device *dev;
@@ -596,19 +574,19 @@ static int config_rsz_subfrm(struct mdp_comp_ctx *ctx,
 		if (CFG_CHECK(MT8195, p_id))
 			reg = CFG_COMP(MT8195, ctx->param, rsz.subfrms[index].merge_cfg);
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_CFG_0, reg, 0xFFFFFFFF);
+			     MDP_MERGE_CFG_0, reg);
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_CFG_4, reg, 0xFFFFFFFF);
+			     MDP_MERGE_CFG_4, reg);
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_CFG_24, reg, 0xFFFFFFFF);
+			     MDP_MERGE_CFG_24, reg);
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_CFG_25, reg, 0xFFFFFFFF);
+			     MDP_MERGE_CFG_25, reg);
 
 		/* Bypass mode */
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_CFG_12, BIT(0), 0xFFFFFFFF);
+			     MDP_MERGE_CFG_12, BIT(0));
 		MM_REG_WRITE(cmd, merge->subsys_id, merge->reg_base,
-			     MDP_MERGE_ENABLE, BIT(0), 0xFFFFFFFF);
+			     MDP_MERGE_ENABLE, BIT(0));
 	}
 
 rsz_subfrm_done:
@@ -634,8 +612,8 @@ static int advance_rsz_subfrm(struct mdp_comp_ctx *ctx,
 		}
 
 		if ((csf_r - csf_l + 1) <= 16)
-			MM_REG_WRITE(cmd, subsys_id, base, PRZ_CONTROL_1, 0x0,
-				     BIT(27));
+			MM_REG_WRITE_MASK(cmd, subsys_id, base, PRZ_CONTROL_1, 0x0,
+					  BIT(27));
 	}
 
 	return 0;
@@ -655,15 +633,15 @@ static int init_wrot(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u8 subsys_id = ctx->comp->subsys_id;
 
 	/* Reset WROT */
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_SOFT_RST, BIT(0), BIT(0));
-	MM_REG_POLL(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_SOFT_RST, BIT(0), BIT(0));
+	MM_REG_POLL_MASK(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, BIT(0), BIT(0));
 
 	/* Reset setting */
 	if (CFG_CHECK(MT8195, p_id))
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_CTRL, 0x0, 0xFFFFFFFF);
+		MM_REG_WRITE(cmd, subsys_id, base, VIDO_CTRL, 0x0);
 
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_SOFT_RST, 0x0, BIT(0));
-	MM_REG_POLL(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_SOFT_RST, 0x0, BIT(0));
+	MM_REG_POLL_MASK(cmd, subsys_id, base, VIDO_SOFT_RST_STAT, 0x0, BIT(0));
 	return 0;
 }
 
@@ -681,39 +659,36 @@ static int config_wrot_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, wrot.iova[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.iova[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.iova[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.iova[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_C, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_C, reg);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.iova[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.iova[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_V, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, VIDO_BASE_ADDR_V, reg);
 
 	if (mdp_cfg && mdp_cfg->wrot_support_10bit) {
 		if (CFG_CHECK(MT8195, p_id))
 			reg = CFG_COMP(MT8195, ctx->param, wrot.scan_10bit);
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_SCAN_10BIT,
-			     reg, 0x0000000F);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_SCAN_10BIT,
+				  reg, 0x0000000F);
 
 		if (CFG_CHECK(MT8195, p_id))
 			reg = CFG_COMP(MT8195, ctx->param, wrot.pending_zero);
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_PENDING_ZERO,
-			     reg, 0x04000000);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_PENDING_ZERO,
+				  reg, 0x04000000);
 	}
 
 	if (CFG_CHECK(MT8195, p_id)) {
 		reg = CFG_COMP(MT8195, ctx->param, wrot.bit_number);
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_CTRL_2,
-			     reg, 0x00000007);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_CTRL_2,
+				  reg, 0x00000007);
 	}
 
 	/* Write frame related registers */
@@ -721,14 +696,13 @@ static int config_wrot_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, wrot.control);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.control);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_CTRL, reg,
-		     0xF131510F);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_CTRL, reg, 0xF131510F);
 
 	/* Write pre-ultra threshold */
 	if (CFG_CHECK(MT8195, p_id)) {
 		reg = CFG_COMP(MT8195, ctx->param, wrot.pre_ultra);
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_DMA_PREULTRA, reg,
-			     0x00FFFFFF);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_DMA_PREULTRA, reg,
+				  0x00FFFFFF);
 	}
 
 	/* Write frame Y pitch */
@@ -736,37 +710,34 @@ static int config_wrot_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, wrot.stride[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.stride[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE, reg,
-		     0x0000FFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_STRIDE, reg, 0x0000FFFF);
 
 	/* Write frame UV pitch */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.stride[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.stride[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE_C, reg,
-		     0xFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_STRIDE_C, reg, 0xFFFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.stride[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.stride[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_STRIDE_V, reg,
-		     0xFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_STRIDE_V, reg, 0xFFFF);
 
 	/* Write matrix control */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.mat_ctrl);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.mat_ctrl);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAT_CTRL, reg, 0xF3);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_MAT_CTRL, reg, 0xF3);
 
 	/* Set the fixed ALPHA as 0xFF */
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_DITHER, 0xFF000000,
-		     0xFF000000);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_DITHER, 0xFF000000,
+			  0xFF000000);
 
 	/* Set VIDO_EOL_SEL */
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_RSV_1, BIT(31), BIT(31));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_RSV_1, BIT(31), BIT(31));
 
 	/* Set VIDO_FIFO_TEST */
 	if (CFG_CHECK(MT8183, p_id))
@@ -775,8 +746,8 @@ static int config_wrot_frame(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8195, ctx->param, wrot.fifo_test);
 
 	if (reg != 0)
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_FIFO_TEST,
-			     reg, 0xFFF);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_FIFO_TEST, reg,
+				  0xFFF);
 
 	/* Filter enable */
 	if (mdp_cfg && mdp_cfg->wrot_filter_constraint) {
@@ -784,13 +755,13 @@ static int config_wrot_frame(struct mdp_comp_ctx *ctx,
 			reg = CFG_COMP(MT8183, ctx->param, wrot.filter);
 		else if (CFG_CHECK(MT8195, p_id))
 			reg = CFG_COMP(MT8195, ctx->param, wrot.filter);
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE,
-			     reg, 0x77);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE, reg,
+				  0x77);
 
 		/* Turn off WROT DMA DCM */
 		if (CFG_CHECK(MT8195, p_id))
-			MM_REG_WRITE(cmd, subsys_id, base, VIDO_ROT_EN,
-				     (0x1 << 23) + (0x1 << 20), 0x900000);
+			MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_ROT_EN,
+					  (0x1 << 23) + (0x1 << 20), 0x900000);
 	}
 
 	return 0;
@@ -808,57 +779,52 @@ static int config_wrot_subfrm(struct mdp_comp_ctx *ctx,
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].offset[0]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].offset[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_OFST_ADDR, reg, 0x0FFFFFFF);
 
 	/* Write U pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].offset[1]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].offset[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR_C,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_OFST_ADDR_C, reg, 0x0FFFFFFF);
 
 	/* Write V pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].offset[2]);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].offset[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_OFST_ADDR_V,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_OFST_ADDR_V, reg,
+			  0x0FFFFFFF);
 
 	/* Write source size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].src);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_IN_SIZE, reg,
-		     0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_IN_SIZE, reg, 0x1FFF1FFF);
 
 	/* Write target size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].clip);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_TAR_SIZE, reg,
-		     0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_TAR_SIZE, reg, 0x1FFF1FFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].clip_ofst);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].clip_ofst);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_CROP_OFST, reg,
-		     0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_CROP_OFST, reg, 0x1FFF1FFF);
 
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wrot.subfrms[index].main_buf);
 	else if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, wrot.subfrms[index].main_buf);
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE,
-		     reg, 0x1FFF7F00);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE, reg,
+			  0x1FFF7F00);
 
 	/* Enable WROT */
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_ROT_EN, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_ROT_EN, BIT(0), BIT(0));
 
 	return 0;
 }
@@ -881,11 +847,11 @@ static int wait_wrot_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
 
 	if (mdp_cfg && mdp_cfg->wrot_filter_constraint)
-		MM_REG_WRITE(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE, 0x0,
-			     0x77);
+		MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_MAIN_BUF_SIZE, 0x0,
+				  0x77);
 
 	/* Disable WROT */
-	MM_REG_WRITE(cmd, subsys_id, base, VIDO_ROT_EN, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, VIDO_ROT_EN, 0x0, BIT(0));
 
 	return 0;
 }
@@ -904,9 +870,9 @@ static int init_wdma(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u8 subsys_id = ctx->comp->subsys_id;
 
 	/* Reset WDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_RST, BIT(0), BIT(0));
-	MM_REG_POLL(cmd, subsys_id, base, WDMA_FLOW_CTRL_DBG, BIT(0), BIT(0));
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_RST, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_RST, BIT(0), BIT(0));
+	MM_REG_POLL_MASK(cmd, subsys_id, base, WDMA_FLOW_CTRL_DBG, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_RST, 0x0, BIT(0));
 	return 0;
 }
 
@@ -918,40 +884,35 @@ static int config_wdma_frame(struct mdp_comp_ctx *ctx,
 	u8 subsys_id = ctx->comp->subsys_id;
 	u32 reg = 0;
 
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_BUF_CON2, 0x10101050,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_BUF_CON2, 0x10101050);
 
 	/* Setup frame information */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.wdma_cfg);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CFG, reg,
-		     0x0F01B8F0);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_CFG, reg, 0x0F01B8F0);
 	/* Setup frame base address */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.iova[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_ADDR, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_ADDR, reg);
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.iova[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_U_ADDR, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_U_ADDR, reg);
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.iova[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_V_ADDR, reg,
-		     0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_V_ADDR, reg);
 	/* Setup Y pitch */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.w_in_byte);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_W_IN_BYTE,
-		     reg, 0x0000FFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_DST_W_IN_BYTE, reg,
+			  0x0000FFFF);
 	/* Setup UV pitch */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.uv_stride);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_UV_PITCH,
-		     reg, 0x0000FFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_DST_UV_PITCH, reg,
+			  0x0000FFFF);
 	/* Set the fixed ALPHA as 0xFF */
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_ALPHA, 0x800000FF,
-		     0x800000FF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_ALPHA, 0x800000FF,
+			  0x800000FF);
 
 	return 0;
 }
@@ -966,36 +927,33 @@ static int config_wdma_subfrm(struct mdp_comp_ctx *ctx,
 	/* Write Y pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].offset[0]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_ADDR_OFFSET,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_DST_ADDR_OFFSET, reg,
+			  0x0FFFFFFF);
 	/* Write U pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].offset[1]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_U_ADDR_OFFSET,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_DST_U_ADDR_OFFSET, reg,
+			  0x0FFFFFFF);
 	/* Write V pixel offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].offset[2]);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_DST_V_ADDR_OFFSET,
-		     reg, 0x0FFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_DST_V_ADDR_OFFSET, reg,
+			  0x0FFFFFFF);
 	/* Write source size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_SRC_SIZE, reg,
-		     0x3FFF3FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_SRC_SIZE, reg, 0x3FFF3FFF);
 	/* Write target size */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CLIP_SIZE, reg,
-		     0x3FFF3FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_CLIP_SIZE, reg, 0x3FFF3FFF);
 	/* Write clip offset */
 	if (CFG_CHECK(MT8183, p_id))
 		reg = CFG_COMP(MT8183, ctx->param, wdma.subfrms[index].clip_ofst);
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_CLIP_COORD, reg,
-		     0x3FFF3FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_CLIP_COORD, reg, 0x3FFF3FFF);
 
 	/* Enable WDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_EN, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_EN, BIT(0), BIT(0));
 
 	return 0;
 }
@@ -1007,7 +965,7 @@ static int wait_wdma_event(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 
 	MM_REG_WAIT(cmd, ctx->comp->gce_event[MDP_GCE_EVENT_EOF]);
 	/* Disable WDMA */
-	MM_REG_WRITE(cmd, subsys_id, base, WDMA_EN, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, WDMA_EN, 0x0, BIT(0));
 	return 0;
 }
 
@@ -1033,19 +991,17 @@ static int reset_luma_hist(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 
 	/* Reset histogram */
 	for (i = 0; i <= hist_num; i++)
-		MM_REG_WRITE_MASK(cmd, subsys_id, base,
-				  (MDP_LUMA_HIST_INIT + (i << 2)),
-				  0, 0xFFFFFFFF);
+		MM_REG_WRITE(cmd, subsys_id, base,
+			     (MDP_LUMA_HIST_INIT + (i << 2)), 0);
 
 	if (mdp_cfg->tdshp_constrain)
 		MM_REG_WRITE(cmd, subsys_id, base,
-			     MDP_DC_TWO_D_W1_RESULT_INIT, 0, 0xFFFFFFFF);
+			     MDP_DC_TWO_D_W1_RESULT_INIT, 0);
 
 	if (mdp_cfg->tdshp_contour)
 		for (i = 0; i < hist_num; i++)
-			MM_REG_WRITE_MASK(cmd, subsys_id, base,
-					  (MDP_CONTOUR_HIST_INIT + (i << 2)),
-					  0, 0xFFFFFFFF);
+			MM_REG_WRITE(cmd, subsys_id, base,
+				     (MDP_CONTOUR_HIST_INIT + (i << 2)), 0);
 
 	return 0;
 }
@@ -1055,9 +1011,9 @@ static int init_tdshp(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	phys_addr_t base = ctx->comp->reg_base;
 	u16 subsys_id = ctx->comp->subsys_id;
 
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_CTRL, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_TDSHP_CTRL, BIT(0), BIT(0));
 	/* Enable FIFO */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_CFG, BIT(1), BIT(1));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_TDSHP_CFG, BIT(1), BIT(1));
 
 	return reset_luma_hist(ctx, cmd);
 }
@@ -1072,7 +1028,7 @@ static int config_tdshp_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.cfg);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_CFG, reg, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_TDSHP_CFG, reg, BIT(0));
 
 	return 0;
 }
@@ -1086,26 +1042,24 @@ static int config_tdshp_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_INPUT_SIZE,
-		     reg, MDP_TDSHP_INPUT_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_INPUT_SIZE, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.subfrms[index].clip_ofst);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_OUTPUT_OFFSET,
-		     reg, 0x00FF00FF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_TDSHP_OUTPUT_OFFSET, reg,
+			  0x00FF00FF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_OUTPUT_SIZE,
-		     reg, MDP_TDSHP_OUTPUT_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_TDSHP_OUTPUT_SIZE, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.subfrms[index].hist_cfg_0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HIST_CFG_00, reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_HIST_CFG_00, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, tdshp.subfrms[index].hist_cfg_1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HIST_CFG_01, reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_HIST_CFG_01, reg);
 
 	return 0;
 }
@@ -1122,21 +1076,19 @@ static int init_color(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	phys_addr_t base = ctx->comp->reg_base;
 	u16 subsys_id = ctx->comp->subsys_id;
 
-	MM_REG_WRITE(cmd, subsys_id, base,
-		     MDP_COLOR_START, 0x1, BIT(1) | BIT(0));
-	MM_REG_WRITE(cmd, subsys_id, base,
-		     MDP_COLOR_WIN_X_MAIN, 0xFFFF0000, 0xFFFFFFFF);
-	MM_REG_WRITE(cmd, subsys_id, base,
-		     MDP_COLOR_WIN_Y_MAIN, 0xFFFF0000, 0xFFFFFFFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_START, 0x1,
+			  BIT(1) | BIT(0));
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_WIN_X_MAIN, 0xFFFF0000);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_WIN_Y_MAIN, 0xFFFF0000);
 
 	/* Reset color matrix */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_CM1_EN, 0x0, BIT(0));
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_CM2_EN, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_CM1_EN, 0x0, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_CM2_EN, 0x0, BIT(0));
 
 	/* Enable interrupt */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_INTEN, 0x7, 0x7);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_INTEN, 0x7, 0x7);
 
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_OUT_SEL, 0x333, 0x333);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_OUT_SEL, 0x333, 0x333);
 
 	return 0;
 }
@@ -1151,8 +1103,7 @@ static int config_color_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, color.start);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_START,
-		     reg, MDP_COLOR_START_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_START, reg);
 
 	return 0;
 }
@@ -1166,13 +1117,13 @@ static int config_color_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, color.subfrms[index].in_hsize);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_INTERNAL_IP_WIDTH,
-		     reg, 0x00003FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_INTERNAL_IP_WIDTH,
+			  reg, 0x00003FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, color.subfrms[index].in_vsize);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_COLOR_INTERNAL_IP_HEIGHT,
-		     reg, 0x00003FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_COLOR_INTERNAL_IP_HEIGHT,
+			  reg, 0x00003FFF);
 
 	return 0;
 }
@@ -1190,9 +1141,9 @@ static int init_ccorr(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u8 subsys_id = ctx->comp->subsys_id;
 
 	/* CCORR enable */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_EN, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_CCORR_EN, BIT(0), BIT(0));
 	/* Relay mode */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_CFG, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_CCORR_CFG, BIT(0), BIT(0));
 	return 0;
 }
 
@@ -1214,8 +1165,8 @@ static int config_ccorr_subfrm(struct mdp_comp_ctx *ctx,
 
 	hsize = csf_r - csf_l + 1;
 	vsize = csf_b - csf_t + 1;
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_CCORR_SIZE,
-		     (hsize << 16) + (vsize <<  0), 0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_CCORR_SIZE,
+			  (hsize << 16) + (vsize <<  0), 0x1FFF1FFF);
 	return 0;
 }
 
@@ -1231,7 +1182,7 @@ static int init_aal(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u16 subsys_id = ctx->comp->subsys_id;
 
 	/* Always set MDP_AAL enable to 1 */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_EN, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_AAL_EN, BIT(0), BIT(0));
 
 	return 0;
 }
@@ -1246,11 +1197,11 @@ static int config_aal_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, aal.cfg_main);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_CFG_MAIN, reg, BIT(7));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_AAL_CFG_MAIN, reg, BIT(7));
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, aal.cfg);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_CFG, reg, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_AAL_CFG, reg, BIT(0));
 
 	return 0;
 }
@@ -1264,18 +1215,16 @@ static int config_aal_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, aal.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_SIZE,
-		     reg, MDP_AAL_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_SIZE, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, aal.subfrms[index].clip_ofst);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_OUTPUT_OFFSET,
-		     reg, 0x00FF00FF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_AAL_OUTPUT_OFFSET, reg,
+			  0x00FF00FF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, aal.subfrms[index].clip);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_OUTPUT_SIZE,
-		     reg, MDP_AAL_OUTPUT_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_AAL_OUTPUT_SIZE, reg);
 
 	return 0;
 }
@@ -1293,7 +1242,7 @@ static int init_hdr(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	u16 subsys_id = ctx->comp->subsys_id;
 
 	/* Always set MDP_HDR enable to 1 */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_TOP, BIT(0), BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_TOP, BIT(0), BIT(0));
 
 	return 0;
 }
@@ -1308,11 +1257,11 @@ static int config_hdr_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.top);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_TOP, reg, BIT(29) | BIT(28));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_TOP, reg, BIT(29) | BIT(28));
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.relay);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_RELAY, reg, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_RELAY, reg, BIT(0));
 
 	return 0;
 }
@@ -1326,37 +1275,36 @@ static int config_hdr_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].win_size);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_TILE_POS,
-		     reg, MDP_HDR_TILE_POS_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_TILE_POS, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].src);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_SIZE_0, reg, 0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_SIZE_0, reg, 0x1FFF1FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].clip_ofst0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_SIZE_1, reg, 0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_SIZE_1, reg, 0x1FFF1FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].clip_ofst1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_SIZE_2, reg, 0x1FFF1FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_SIZE_2, reg, 0x1FFF1FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].hist_ctrl_0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_HIST_CTRL_0, reg, 0x00003FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_HIST_CTRL_0, reg, 0x00003FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].hist_ctrl_1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_HIST_CTRL_1, reg, 0x00003FFF);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_HIST_CTRL_1, reg, 0x00003FFF);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].hdr_top);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_TOP, reg, BIT(6) | BIT(5));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_TOP, reg, BIT(6) | BIT(5));
 
 	/* Enable histogram */
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, hdr.subfrms[index].hist_addr);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_HDR_HIST_ADDR, reg, BIT(9));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_HDR_HIST_ADDR, reg, BIT(9));
 
 	return 0;
 }
@@ -1373,8 +1321,8 @@ static int init_fg(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	phys_addr_t base = ctx->comp->reg_base;
 	u16 subsys_id = ctx->comp->subsys_id;
 
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TRIGGER, BIT(2), BIT(2));
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TRIGGER, 0x0, BIT(2));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_FG_TRIGGER, BIT(2), BIT(2));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_FG_TRIGGER, 0x0, BIT(2));
 
 	return 0;
 }
@@ -1389,11 +1337,11 @@ static int config_fg_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, fg.ctrl_0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_FG_CTRL_0, reg, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_FG_FG_CTRL_0, reg, BIT(0));
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, fg.ck_en);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_FG_CK_EN, reg, 0x7);
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_FG_FG_CK_EN, reg, 0x7);
 
 	return 0;
 }
@@ -1407,11 +1355,11 @@ static int config_fg_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, fg.subfrms[index].info_0);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TILE_INFO_0, reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TILE_INFO_0, reg);
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, fg.subfrms[index].info_1);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TILE_INFO_1, reg, 0xFFFFFFFF);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_FG_TILE_INFO_1, reg);
 
 	return 0;
 }
@@ -1428,14 +1376,11 @@ static int init_ovl(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	phys_addr_t base = ctx->comp->reg_base;
 	u16 subsys_id = ctx->comp->subsys_id;
 
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_EN,
-		     BIT(0), MDP_OVL_EN_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_EN, BIT(0));
 
 	/* Set to relay mode */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_SRC_CON,
-		     BIT(9), MDP_OVL_SRC_CON_MASK);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_DP_CON,
-		     BIT(0), MDP_OVL_DP_CON_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_SRC_CON, BIT(9));
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_DP_CON, BIT(0));
 
 	return 0;
 }
@@ -1450,11 +1395,11 @@ static int config_ovl_frame(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, ovl.L0_con);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_L0_CON, reg, BIT(29) | BIT(28));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_OVL_L0_CON, reg, BIT(29) | BIT(28));
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, ovl.src_con);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_SRC_CON, reg, BIT(0));
+	MM_REG_WRITE_MASK(cmd, subsys_id, base, MDP_OVL_SRC_CON, reg, BIT(0));
 
 	return 0;
 }
@@ -1468,14 +1413,12 @@ static int config_ovl_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, ovl.subfrms[index].L0_src_size);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_L0_SRC_SIZE,
-		     reg, MDP_OVL_L0_SRC_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_L0_SRC_SIZE, reg);
 
 	/* Setup output size */
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, ovl.subfrms[index].roi_size);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_ROI_SIZE,
-		     reg, MDP_OVL_ROI_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_OVL_ROI_SIZE, reg);
 
 	return 0;
 }
@@ -1492,13 +1435,10 @@ static int init_pad(struct mdp_comp_ctx *ctx, struct mdp_cmdq_cmd *cmd)
 	phys_addr_t base = ctx->comp->reg_base;
 	u16 subsys_id = ctx->comp->subsys_id;
 
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_CON,
-		     BIT(1), MDP_PAD_CON_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_CON, BIT(1));
 	/* Reset */
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_W_SIZE,
-		     0, MDP_PAD_W_SIZE_MASK);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_H_SIZE,
-		     0, MDP_PAD_H_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_W_SIZE, 0);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_H_SIZE, 0);
 
 	return 0;
 }
@@ -1512,8 +1452,7 @@ static int config_pad_subfrm(struct mdp_comp_ctx *ctx,
 
 	if (CFG_CHECK(MT8195, p_id))
 		reg = CFG_COMP(MT8195, ctx->param, pad.subfrms[index].pic_size);
-	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_PIC_SIZE,
-		     reg, MDP_PAD_PIC_SIZE_MASK);
+	MM_REG_WRITE(cmd, subsys_id, base, MDP_PAD_PIC_SIZE, reg);
 
 	return 0;
 }
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
index 3e5d2da1c807..681906c16419 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-comp.h
@@ -9,18 +9,18 @@
 
 #include "mtk-mdp3-cmdq.h"
 
-#define MM_REG_WRITE_MASK(cmd, id, base, ofst, val, mask, ...)	\
-	cmdq_pkt_write_mask(&((cmd)->pkt), id,			\
-		(base) + (ofst), (val), (mask), ##__VA_ARGS__)
-
-#define MM_REG_WRITE(cmd, id, base, ofst, val, mask, ...)	\
+#define MM_REG_WRITE_MASK(cmd, id, base, ofst, val, mask)	\
 do {								\
 	typeof(mask) (m) = (mask);				\
-	MM_REG_WRITE_MASK(cmd, id, base, ofst, val,		\
+	cmdq_pkt_write_mask(&((cmd)->pkt), id, (base) + (ofst),	\
+			    (val),				\
 		(((m) & (ofst##_MASK)) == (ofst##_MASK)) ?	\
-			(0xffffffff) : (m), ##__VA_ARGS__);	\
+			(0xffffffff) : (m));			\
 } while (0)
 
+#define MM_REG_WRITE(cmd, id, base, ofst, val)			\
+	cmdq_pkt_write(&((cmd)->pkt), id, (base) + (ofst), (val))
+
 #define MM_REG_WAIT(cmd, evt)					\
 do {								\
 	typeof(cmd) (c) = (cmd);				\
@@ -49,20 +49,17 @@ do {								\
 	cmdq_pkt_set_event(&((c)->pkt), (e));			\
 } while (0)
 
-#define MM_REG_POLL_MASK(cmd, id, base, ofst, val, _mask, ...)	\
+#define MM_REG_POLL_MASK(cmd, id, base, ofst, val, _mask)	\
 do {								\
 	typeof(_mask) (_m) = (_mask);				\
 	cmdq_pkt_poll_mask(&((cmd)->pkt), id,			\
-		(base) + (ofst), (val), (_m), ##__VA_ARGS__);	\
+		(base) + (ofst), (val),				\
+		(((_m) & (ofst##_MASK)) == (ofst##_MASK)) ?	\
+			(0xffffffff) : (_m));			\
 } while (0)
 
-#define MM_REG_POLL(cmd, id, base, ofst, val, mask, ...)	\
-do {								\
-	typeof(mask) (m) = (mask);				\
-	MM_REG_POLL_MASK((cmd), id, base, ofst, val,		\
-		(((m) & (ofst##_MASK)) == (ofst##_MASK)) ?	\
-			(0xffffffff) : (m), ##__VA_ARGS__);	\
-} while (0)
+#define MM_REG_POLL(cmd, id, base, ofst, val)			\
+	cmdq_pkt_poll(&((cmd)->pkt), id, (base) + (ofst), (val))
 
 enum mtk_mdp_comp_id {
 	MDP_COMP_NONE = -1,	/* Invalid engine */
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c
index 5209f531ef8d..8de2c8e4d333 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.c
@@ -21,6 +21,9 @@ static const struct of_device_id mdp_of_ids[] = {
 	{ .compatible = "mediatek,mt8183-mdp3-rdma",
 	  .data = &mt8183_mdp_driver_data,
 	},
+	{ .compatible = "mediatek,mt8188-mdp3-rdma",
+	  .data = &mt8188_mdp_driver_data,
+	},
 	{ .compatible = "mediatek,mt8195-mdp3-rdma",
 	  .data = &mt8195_mdp_driver_data,
 	},
@@ -76,25 +79,6 @@ static struct platform_device *__get_pdev_by_id(struct platform_device *pdev,
 	return mdp_pdev;
 }
 
-struct platform_device *mdp_get_plat_device(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-	struct device_node *mdp_node;
-	struct platform_device *mdp_pdev;
-
-	mdp_node = of_parse_phandle(dev->of_node, MDP_PHANDLE_NAME, 0);
-	if (!mdp_node) {
-		dev_err(dev, "can't get node %s\n", MDP_PHANDLE_NAME);
-		return NULL;
-	}
-
-	mdp_pdev = of_find_device_by_node(mdp_node);
-	of_node_put(mdp_node);
-
-	return mdp_pdev;
-}
-EXPORT_SYMBOL_GPL(mdp_get_plat_device);
-
 int mdp_vpu_get_locked(struct mdp_dev *mdp)
 {
 	int ret = 0;
@@ -309,6 +293,8 @@ static int mdp_probe(struct platform_device *pdev)
 			ret = PTR_ERR(mdp->cmdq_clt[i]);
 			goto err_mbox_destroy;
 		}
+
+		mdp->cmdq_shift_pa[i] = cmdq_get_shift_pa(mdp->cmdq_clt[i]->chan);
 	}
 
 	init_waitqueue_head(&mdp->callback_wq);
@@ -380,14 +366,14 @@ static int __maybe_unused mdp_suspend(struct device *dev)
 
 	atomic_set(&mdp->suspended, 1);
 
-	if (atomic_read(&mdp->job_count)) {
+	if (refcount_read(&mdp->job_count)) {
 		ret = wait_event_timeout(mdp->callback_wq,
-					 !atomic_read(&mdp->job_count),
+					 !refcount_read(&mdp->job_count),
 					 2 * HZ);
 		if (ret == 0) {
 			dev_err(dev,
 				"%s:flushed cmdq task incomplete, count=%d\n",
-				__func__, atomic_read(&mdp->job_count));
+				__func__, refcount_read(&mdp->job_count));
 			return -EBUSY;
 		}
 	}
@@ -410,7 +396,7 @@ static const struct dev_pm_ops mdp_pm_ops = {
 
 static struct platform_driver mdp_driver = {
 	.probe		= mdp_probe,
-	.remove_new	= mdp_remove,
+	.remove		= mdp_remove,
 	.driver = {
 		.name	= MDP_MODULE_NAME,
 		.pm	= &mdp_pm_ops,
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h
index 8c09e984fd01..05cade1d098e 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-core.h
@@ -126,6 +126,7 @@ struct mdp_dev {
 	u32					id_count;
 	struct ida				mdp_ida;
 	struct cmdq_client			*cmdq_clt[MDP_PP_MAX];
+	u8					cmdq_shift_pa[MDP_PP_MAX];
 	wait_queue_head_t			callback_wq;
 
 	struct v4l2_device			v4l2_dev;
@@ -134,7 +135,7 @@ struct mdp_dev {
 	/* synchronization protect for m2m device operation */
 	struct mutex				m2m_lock;
 	atomic_t				suspended;
-	atomic_t				job_count;
+	refcount_t				job_count;
 };
 
 struct mdp_pipe_info {
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c
index 35a8b059bde5..59ce5cce0698 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-m2m.c
@@ -104,14 +104,14 @@ static void mdp_m2m_device_run(void *priv)
 	task.cb_data = NULL;
 	task.mdp_ctx = ctx;
 
-	if (atomic_read(&ctx->mdp_dev->job_count)) {
+	if (refcount_read(&ctx->mdp_dev->job_count)) {
 		ret = wait_event_timeout(ctx->mdp_dev->callback_wq,
-					 !atomic_read(&ctx->mdp_dev->job_count),
+					 !refcount_read(&ctx->mdp_dev->job_count),
 					 2 * HZ);
 		if (ret == 0) {
 			dev_err(&ctx->mdp_dev->pdev->dev,
 				"%d jobs not yet done\n",
-				atomic_read(&ctx->mdp_dev->job_count));
+				refcount_read(&ctx->mdp_dev->job_count));
 			goto worker_end;
 		}
 	}
@@ -266,8 +266,6 @@ static void mdp_m2m_buf_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops mdp_m2m_qops = {
 	.queue_setup	= mdp_m2m_queue_setup,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.buf_prepare	= mdp_m2m_buf_prepare,
 	.start_streaming = mdp_m2m_start_streaming,
 	.stop_streaming	= mdp_m2m_stop_streaming,
diff --git a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c
index 657356f87743..644b223b2877 100644
--- a/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c
+++ b/drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c
@@ -236,7 +236,7 @@ int mdp_try_crop(struct mdp_m2m_ctx *ctx, struct v4l2_rect *r,
 	u32 framew, frameh, walign, halign;
 	int ret;
 
-	dev_dbg(dev, "%d target:%d, set:(%d,%d) %ux%u", ctx->id,
+	dev_dbg(dev, "%d target:%d, set:(%d,%d)/%ux%u", ctx->id,
 		s->target, s->r.left, s->r.top, s->r.width, s->r.height);
 
 	left = s->r.left;
@@ -275,7 +275,7 @@ int mdp_try_crop(struct mdp_m2m_ctx *ctx, struct v4l2_rect *r,
 	r->width = right - left;
 	r->height = bottom - top;
 
-	dev_dbg(dev, "%d crop:(%d,%d) %ux%u", ctx->id,
+	dev_dbg(dev, "%d crop:(%d,%d)/%ux%u", ctx->id,
 		r->left, r->top, r->width, r->height);
 	return 0;
 }
diff --git a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_scp.c b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_scp.c
index 6bbe55de6ce9..1b0bc47355c0 100644
--- a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_scp.c
+++ b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_scp.c
@@ -79,6 +79,11 @@ struct mtk_vcodec_fw *mtk_vcodec_fw_scp_init(void *priv, enum mtk_vcodec_fw_use
 	}
 
 	fw = devm_kzalloc(&plat_dev->dev, sizeof(*fw), GFP_KERNEL);
+	if (!fw) {
+		scp_put(scp);
+		return ERR_PTR(-ENOMEM);
+	}
+
 	fw->type = SCP;
 	fw->ops = &mtk_vcodec_rproc_msg;
 	fw->scp = scp;
diff --git a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_vpu.c b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_vpu.c
index 4c34344dc7dc..d7027d600208 100644
--- a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_vpu.c
+++ b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_fw_vpu.c
@@ -50,12 +50,12 @@ static void mtk_vcodec_vpu_reset_dec_handler(void *priv)
 
 	dev_err(&dev->plat_dev->dev, "Watchdog timeout!!");
 
-	mutex_lock(&dev->dev_mutex);
+	mutex_lock(&dev->dev_ctx_lock);
 	list_for_each_entry(ctx, &dev->ctx_list, list) {
 		ctx->state = MTK_STATE_ABORT;
 		mtk_v4l2_vdec_dbg(0, ctx, "[%d] Change to state MTK_STATE_ABORT", ctx->id);
 	}
-	mutex_unlock(&dev->dev_mutex);
+	mutex_unlock(&dev->dev_ctx_lock);
 }
 
 static void mtk_vcodec_vpu_reset_enc_handler(void *priv)
@@ -65,12 +65,12 @@ static void mtk_vcodec_vpu_reset_enc_handler(void *priv)
 
 	dev_err(&dev->plat_dev->dev, "Watchdog timeout!!");
 
-	mutex_lock(&dev->dev_mutex);
+	mutex_lock(&dev->dev_ctx_lock);
 	list_for_each_entry(ctx, &dev->ctx_list, list) {
 		ctx->state = MTK_STATE_ABORT;
 		mtk_v4l2_vdec_dbg(0, ctx, "[%d] Change to state MTK_STATE_ABORT", ctx->id);
 	}
-	mutex_unlock(&dev->dev_mutex);
+	mutex_unlock(&dev->dev_ctx_lock);
 }
 
 static const struct mtk_vcodec_fw_ops mtk_vcodec_vpu_msg = {
diff --git a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_util.c b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_util.c
index 9ce34a3b5ee6..fc4e34c29192 100644
--- a/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_util.c
+++ b/drivers/media/platform/mediatek/vcodec/common/mtk_vcodec_util.c
@@ -49,7 +49,6 @@ int mtk_vcodec_mem_alloc(void *priv, struct mtk_vcodec_mem *mem)
 {
 	enum mtk_instance_type inst_type = *((unsigned int *)priv);
 	struct platform_device *plat_dev;
-	unsigned long size = mem->size;
 	int id;
 
 	if (inst_type == MTK_INST_ENCODER) {
@@ -64,15 +63,16 @@ int mtk_vcodec_mem_alloc(void *priv, struct mtk_vcodec_mem *mem)
 		id = dec_ctx->id;
 	}
 
-	mem->va = dma_alloc_coherent(&plat_dev->dev, size, &mem->dma_addr, GFP_KERNEL);
+        mem->va = dma_alloc_attrs(&plat_dev->dev, mem->size, &mem->dma_addr,
+                                  GFP_KERNEL, DMA_ATTR_ALLOC_SINGLE_PAGES);
 	if (!mem->va) {
-		mtk_v4l2_err(plat_dev, "%s dma_alloc size=%ld failed!",
-			     dev_name(&plat_dev->dev), size);
+		mtk_v4l2_err(plat_dev, "%s dma_alloc size=0x%zx failed!",
+			     __func__, mem->size);
 		return -ENOMEM;
 	}
 
-	mtk_v4l2_debug(plat_dev, 3, "[%d] - va = %p dma = 0x%lx size = 0x%lx", id, mem->va,
-		       (unsigned long)mem->dma_addr, size);
+	mtk_v4l2_debug(plat_dev, 3, "[%d] - va = %p dma = 0x%lx size = 0x%zx", id, mem->va,
+		       (unsigned long)mem->dma_addr, mem->size);
 
 	return 0;
 }
@@ -82,7 +82,6 @@ void mtk_vcodec_mem_free(void *priv, struct mtk_vcodec_mem *mem)
 {
 	enum mtk_instance_type inst_type = *((unsigned int *)priv);
 	struct platform_device *plat_dev;
-	unsigned long size = mem->size;
 	int id;
 
 	if (inst_type == MTK_INST_ENCODER) {
@@ -98,15 +97,16 @@ void mtk_vcodec_mem_free(void *priv, struct mtk_vcodec_mem *mem)
 	}
 
 	if (!mem->va) {
-		mtk_v4l2_err(plat_dev, "%s dma_free size=%ld failed!",
-			     dev_name(&plat_dev->dev), size);
+		mtk_v4l2_err(plat_dev, "%s: Tried to free a NULL VA", __func__);
+		if (mem->size)
+			mtk_v4l2_err(plat_dev, "Failed to free %zu bytes", mem->size);
 		return;
 	}
 
-	mtk_v4l2_debug(plat_dev, 3, "[%d] - va = %p dma = 0x%lx size = 0x%lx", id, mem->va,
-		       (unsigned long)mem->dma_addr, size);
+	mtk_v4l2_debug(plat_dev, 3, "[%d] - va = %p dma = 0x%lx size = 0x%zx", id, mem->va,
+		       (unsigned long)mem->dma_addr, mem->size);
 
-	dma_free_coherent(&plat_dev->dev, size, mem->va, mem->dma_addr);
+	dma_free_coherent(&plat_dev->dev, mem->size, mem->va, mem->dma_addr);
 	mem->va = NULL;
 	mem->dma_addr = 0;
 	mem->size = 0;
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec.c b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec.c
index ba742f0e391d..98838217b97d 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec.c
@@ -80,21 +80,18 @@ static struct mtk_q_data *mtk_vdec_get_q_data(struct mtk_vcodec_dec_ctx *ctx,
 	return &ctx->q_data[MTK_Q_DATA_DST];
 }
 
-static int vidioc_try_decoder_cmd(struct file *file, void *priv,
-				struct v4l2_decoder_cmd *cmd)
+static int stateful_try_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
 {
 	return v4l2_m2m_ioctl_try_decoder_cmd(file, priv, cmd);
 }
 
-
-static int vidioc_decoder_cmd(struct file *file, void *priv,
-				struct v4l2_decoder_cmd *cmd)
+static int stateful_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
 {
 	struct mtk_vcodec_dec_ctx *ctx = fh_to_dec_ctx(priv);
 	struct vb2_queue *src_vq, *dst_vq;
 	int ret;
 
-	ret = vidioc_try_decoder_cmd(file, priv, cmd);
+	ret = stateful_try_decoder_cmd(file, priv, cmd);
 	if (ret)
 		return ret;
 
@@ -128,6 +125,57 @@ static int vidioc_decoder_cmd(struct file *file, void *priv,
 	return 0;
 }
 
+static int stateless_try_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
+{
+	return v4l2_m2m_ioctl_stateless_try_decoder_cmd(file, priv, cmd);
+}
+
+static int stateless_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
+{
+	struct mtk_vcodec_dec_ctx *ctx = fh_to_dec_ctx(priv);
+	int ret;
+
+	ret = v4l2_m2m_ioctl_stateless_try_decoder_cmd(file, priv, cmd);
+	if (ret)
+		return ret;
+
+	mtk_v4l2_vdec_dbg(3, ctx, "decoder cmd=%u", cmd->cmd);
+	switch (cmd->cmd) {
+	case V4L2_DEC_CMD_FLUSH:
+		/*
+		 * If the flag of the output buffer is equals V4L2_BUF_FLAG_M2M_HOLD_CAPTURE_BUF,
+		 * this command will prevent dequeueing the capture buffer containing the last
+		 * decoded frame. Or do nothing
+		 */
+		break;
+	default:
+		mtk_v4l2_vdec_err(ctx, "invalid stateless decoder cmd=%u", cmd->cmd);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vidioc_try_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
+{
+	struct mtk_vcodec_dec_ctx *ctx = fh_to_dec_ctx(priv);
+
+	if (ctx->dev->vdec_pdata->uses_stateless_api)
+		return stateless_try_decoder_cmd(file, priv, cmd);
+
+	return stateful_try_decoder_cmd(file, priv, cmd);
+}
+
+static int vidioc_decoder_cmd(struct file *file, void *priv, struct v4l2_decoder_cmd *cmd)
+{
+	struct mtk_vcodec_dec_ctx *ctx = fh_to_dec_ctx(priv);
+
+	if (ctx->dev->vdec_pdata->uses_stateless_api)
+		return stateless_decoder_cmd(file, priv, cmd);
+
+	return stateful_decoder_cmd(file, priv, cmd);
+}
+
 void mtk_vdec_unlock(struct mtk_vcodec_dec_ctx *ctx)
 {
 	mutex_unlock(&ctx->dev->dec_mutex[ctx->hw_id]);
@@ -262,7 +310,7 @@ static int vidioc_try_fmt(struct mtk_vcodec_dec_ctx *ctx, struct v4l2_format *f,
 		int tmp_w, tmp_h;
 
 		/*
-		 * Find next closer width align 64, heign align 64, size align
+		 * Find next closer width align 64, height align 64, size align
 		 * 64 rectangle
 		 * Note: This only get default value, the real HW needed value
 		 *       only available when ctx in MTK_STATE_HEADER state
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.c b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.c
index f47c98faf068..9247d92d431d 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.c
@@ -268,7 +268,9 @@ static int fops_vcodec_open(struct file *file)
 
 	ctx->dev->vdec_pdata->init_vdec_params(ctx);
 
+	mutex_lock(&dev->dev_ctx_lock);
 	list_add(&ctx->list, &dev->ctx_list);
+	mutex_unlock(&dev->dev_ctx_lock);
 	mtk_vcodec_dbgfs_create(ctx);
 
 	mutex_unlock(&dev->dev_mutex);
@@ -311,7 +313,9 @@ static int fops_vcodec_release(struct file *file)
 	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
 
 	mtk_vcodec_dbgfs_remove(dev, ctx->id);
+	mutex_lock(&dev->dev_ctx_lock);
 	list_del_init(&ctx->list);
+	mutex_unlock(&dev->dev_ctx_lock);
 	kfree(ctx);
 	mutex_unlock(&dev->dev_mutex);
 	return 0;
@@ -404,6 +408,7 @@ static int mtk_vcodec_probe(struct platform_device *pdev)
 	for (i = 0; i < MTK_VDEC_HW_MAX; i++)
 		mutex_init(&dev->dec_mutex[i]);
 	mutex_init(&dev->dev_mutex);
+	mutex_init(&dev->dev_ctx_lock);
 	spin_lock_init(&dev->irqlock);
 
 	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name), "%s",
@@ -586,7 +591,7 @@ static void mtk_vcodec_dec_remove(struct platform_device *pdev)
 
 static struct platform_driver mtk_vcodec_dec_driver = {
 	.probe	= mtk_vcodec_probe,
-	.remove_new = mtk_vcodec_dec_remove,
+	.remove = mtk_vcodec_dec_remove,
 	.driver	= {
 		.name	= MTK_VCODEC_DEC_NAME,
 		.of_match_table = mtk_vcodec_match,
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.h b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.h
index 849b89dd205c..aececca7ecf8 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.h
+++ b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_drv.h
@@ -17,6 +17,7 @@
 
 #define IS_VDEC_LAT_ARCH(hw_arch) ((hw_arch) >= MTK_VDEC_LAT_SINGLE_CORE)
 #define IS_VDEC_INNER_RACING(capability) ((capability) & MTK_VCODEC_INNER_RACING)
+#define IS_VDEC_SUPPORT_EXT(capability) ((capability) & MTK_VDEC_IS_SUPPORT_EXT)
 
 enum mtk_vcodec_dec_chip_name {
 	MTK_VDEC_INVAL = 0,
@@ -42,6 +43,7 @@ enum mtk_vdec_format_types {
 	MTK_VDEC_FORMAT_HEVC_FRAME = 0x1000,
 	MTK_VCODEC_INNER_RACING = 0x20000,
 	MTK_VDEC_IS_SUPPORT_10BIT = 0x40000,
+	MTK_VDEC_IS_SUPPORT_EXT = 0x80000,
 };
 
 /*
@@ -67,11 +69,11 @@ enum mtk_vdec_hw_arch {
  * @pic_w: picture width
  * @pic_h: picture height
  * @buf_w: picture buffer width (64 aligned up from pic_w)
- * @buf_h: picture buffer heiht (64 aligned up from pic_h)
+ * @buf_h: picture buffer height (64 aligned up from pic_h)
  * @fb_sz: bitstream size of each plane
  * E.g. suppose picture size is 176x144,
  *      buffer size will be aligned to 176x160.
- * @cap_fourcc: fourcc number(may changed when resolution change)
+ * @cap_fourcc: fourcc number(may change on a resolution change)
  * @reserved: align struct to 64-bit in order to adjust 32-bit and 64-bit os.
  */
 struct vdec_pic_info {
@@ -241,6 +243,7 @@ struct mtk_vcodec_dec_ctx {
  *
  * @dec_mutex: decoder hardware lock
  * @dev_mutex: video_device lock
+ * @dev_ctx_lock: the lock of context list
  * @decode_workqueue: decode work queue
  *
  * @irqlock: protect data access by irq handler and work thread
@@ -282,6 +285,7 @@ struct mtk_vcodec_dec_dev {
 	/* decoder hardware mutex lock */
 	struct mutex dec_mutex[MTK_VDEC_HW_MAX];
 	struct mutex dev_mutex;
+	struct mutex dev_ctx_lock;
 	struct workqueue_struct *decode_workqueue;
 
 	spinlock_t irqlock;
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateful.c b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateful.c
index 11ca2c2fbaad..aa9bdee7a96c 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateful.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateful.c
@@ -595,11 +595,9 @@ static void mtk_init_vdec_params(struct mtk_vcodec_dec_ctx *ctx)
 	}
 }
 
-static struct vb2_ops mtk_vdec_frame_vb2_ops = {
+static const struct vb2_ops mtk_vdec_frame_vb2_ops = {
 	.queue_setup = vb2ops_vdec_queue_setup,
 	.buf_prepare = vb2ops_vdec_buf_prepare,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = vb2ops_vdec_start_streaming,
 
 	.buf_queue = vb2ops_vdec_stateful_buf_queue,
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateless.c b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateless.c
index b903e39fee89..d873159b9b30 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateless.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/mtk_vcodec_dec_stateless.c
@@ -152,8 +152,6 @@ static const struct mtk_stateless_control mtk_stateless_controls[] = {
 			.id = V4L2_CID_MPEG_VIDEO_HEVC_PROFILE,
 			.def = V4L2_MPEG_VIDEO_HEVC_PROFILE_MAIN,
 			.max = V4L2_MPEG_VIDEO_HEVC_PROFILE_MAIN_10,
-			.menu_skip_mask =
-				BIT(V4L2_MPEG_VIDEO_HEVC_PROFILE_MAIN_STILL_PICTURE),
 		},
 		.codec_type = V4L2_PIX_FMT_HEVC_SLICE,
 	},
@@ -854,10 +852,8 @@ static int vb2ops_vdec_out_buf_validate(struct vb2_buffer *vb)
 	return 0;
 }
 
-static struct vb2_ops mtk_vdec_request_vb2_ops = {
+static const struct vb2_ops mtk_vdec_request_vb2_ops = {
 	.queue_setup	= vb2ops_vdec_queue_setup,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 	.start_streaming	= vb2ops_vdec_start_streaming,
 	.stop_streaming	= vb2ops_vdec_stop_streaming,
 
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_av1_req_lat_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_av1_req_lat_if.c
index 2b6a5adbc419..bf21f2467a0f 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_av1_req_lat_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_av1_req_lat_if.c
@@ -326,7 +326,7 @@ struct vdec_av1_slice_quantization {
  * @use_lr:                     whether to use loop restoration
  * @use_chroma_lr:              whether to use chroma loop restoration
  * @frame_restoration_type:     specifies the type of restoration used for each plane
- * @loop_restoration_size:      pecifies the size of loop restoration units in units
+ * @loop_restoration_size:      specifies the size of loop restoration units in units
  *                              of samples in the current plane
  */
 struct vdec_av1_slice_lr {
@@ -347,7 +347,7 @@ struct vdec_av1_slice_lr {
  *                              and loop_filter_sharpness together determine when
  *                              a block edge is filtered, and by how much the
  *                              filtering can change the sample values
- * @loop_filter_delta_enabled:  filetr level depends on the mode and reference
+ * @loop_filter_delta_enabled:  filter level depends on the mode and reference
  *                              frame used to predict a block
  */
 struct vdec_av1_slice_loop_filter {
@@ -392,7 +392,7 @@ struct vdec_av1_slice_mfmv {
 /**
  * struct vdec_av1_slice_tile - AV1 Tile info
  * @tile_cols:                  specifies the number of tiles across the frame
- * @tile_rows:                  pecifies the number of tiles down the frame
+ * @tile_rows:                  specifies the number of tiles down the frame
  * @mi_col_starts:              an array specifying the start column
  * @mi_row_starts:              an array specifying the start row
  * @context_update_tile_id:     specifies which tile to use for the CDF update
@@ -423,15 +423,15 @@ struct vdec_av1_slice_tile {
  *                              or the tile sizes are coded
  * @interpolation_filter:       specifies the filter selection used for performing inter prediction
  * @allow_warped_motion:        motion_mode may be present or not
- * @is_motion_mode_switchable : euqlt to 0 specifies that only the SIMPLE motion mode will be used
+ * @is_motion_mode_switchable : equal to 0 specifies that only the SIMPLE motion mode will be used
  * @reference_mode :            frame reference mode selected
  * @allow_high_precision_mv:    specifies that motion vectors are specified to
  *                              quarter pel precision or to eighth pel precision
- * @allow_intra_bc:             ubducates that intra block copy may be used in this frame
+ * @allow_intra_bc:             allows that intra block copy may be used in this frame
  * @force_integer_mv:           specifies motion vectors will always be integers or
  *                              can contain fractional bits
  * @allow_screen_content_tools: intra blocks may use palette encoding
- * @error_resilient_mode:       error resislent mode is enable/disable
+ * @error_resilient_mode:       error resilient mode is enable/disable
  * @frame_type:                 specifies the AV1 frame type
  * @primary_ref_frame:          specifies which reference frame contains the CDF values
  *                              and other state that should be loaded at the start of the frame
@@ -440,8 +440,8 @@ struct vdec_av1_slice_tile {
  * @disable_cdf_update:         specified whether the CDF update in the symbol
  *                              decoding process should be disables
  * @skip_mode:                  av1 skip mode parameters
- * @seg:                        av1 segmentaon parameters
- * @delta_q_lf:                 av1 delta loop fileter
+ * @seg:                        av1 segmentation parameters
+ * @delta_q_lf:                 av1 delta loop filter
  * @quant:                      av1 Quantization params
  * @lr:                         av1 Loop Restauration parameters
  * @superres_denom:             the denominator for the upscaling ratio
@@ -450,8 +450,8 @@ struct vdec_av1_slice_tile {
  * @mfmv:                       av1 mfmv parameters
  * @tile:                       av1 Tile info
  * @frame_is_intra:             intra frame
- * @loss_less_array:            loss less array
- * @coded_loss_less:            coded lsss less
+ * @loss_less_array:            lossless array
+ * @coded_loss_less:            coded lossless
  * @mi_rows:                    size of mi unit in rows
  * @mi_cols:                    size of mi unit in cols
  */
@@ -1023,18 +1023,26 @@ static void vdec_av1_slice_free_working_buffer(struct vdec_av1_slice_instance *i
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(instance->mv); i++)
-		mtk_vcodec_mem_free(ctx, &instance->mv[i]);
+		if (instance->mv[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->mv[i]);
 
 	for (i = 0; i < ARRAY_SIZE(instance->seg); i++)
-		mtk_vcodec_mem_free(ctx, &instance->seg[i]);
+		if (instance->seg[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->seg[i]);
 
 	for (i = 0; i < ARRAY_SIZE(instance->cdf); i++)
-		mtk_vcodec_mem_free(ctx, &instance->cdf[i]);
+		if (instance->cdf[i].va)
+			mtk_vcodec_mem_free(ctx, &instance->cdf[i]);
+
 
-	mtk_vcodec_mem_free(ctx, &instance->tile);
-	mtk_vcodec_mem_free(ctx, &instance->cdf_temp);
-	mtk_vcodec_mem_free(ctx, &instance->cdf_table);
-	mtk_vcodec_mem_free(ctx, &instance->iq_table);
+	if (instance->tile.va)
+		mtk_vcodec_mem_free(ctx, &instance->tile);
+	if (instance->cdf_temp.va)
+		mtk_vcodec_mem_free(ctx, &instance->cdf_temp);
+	if (instance->cdf_table.va)
+		mtk_vcodec_mem_free(ctx, &instance->cdf_table);
+	if (instance->iq_table.va)
+		mtk_vcodec_mem_free(ctx, &instance->iq_table);
 
 	instance->level = AV1_RES_NONE;
 }
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_if.c
index bf7dffe60d07..795cb19b075d 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_if.c
@@ -94,7 +94,7 @@ struct vdec_h264_dec_info {
  *                        AP-W/R : AP is writer/reader on this item
  *                        VPU-W/R: VPU is write/reader on this item
  * @hdr_buf      : Header parsing buffer (AP-W, VPU-R)
- * @pred_buf_dma : HW working predication buffer dma address (AP-W, VPU-R)
+ * @pred_buf_dma : HW working prediction buffer dma address (AP-W, VPU-R)
  * @mv_buf_dma   : HW working motion vector buffer dma address (AP-W, VPU-R)
  * @list_free    : free frame buffer ring list (AP-W/R, VPU-W)
  * @list_disp    : display frame buffer ring list (AP-R, VPU-W)
@@ -117,7 +117,7 @@ struct vdec_h264_vsi {
  * struct vdec_h264_inst - h264 decoder instance
  * @num_nalu : how many nalus be decoded
  * @ctx      : point to mtk_vcodec_dec_ctx
- * @pred_buf : HW working predication buffer
+ * @pred_buf : HW working prediction buffer
  * @mv_buf   : HW working motion vector buffer
  * @vpu      : VPU instance
  * @vsi      : VPU shared information
@@ -136,7 +136,7 @@ static unsigned int get_mv_buf_size(unsigned int width, unsigned int height)
 	return HW_MB_STORE_SZ * (width/MB_UNIT_LEN) * (height/MB_UNIT_LEN);
 }
 
-static int allocate_predication_buf(struct vdec_h264_inst *inst)
+static int allocate_prediction_buf(struct vdec_h264_inst *inst)
 {
 	int err = 0;
 
@@ -151,7 +151,7 @@ static int allocate_predication_buf(struct vdec_h264_inst *inst)
 	return 0;
 }
 
-static void free_predication_buf(struct vdec_h264_inst *inst)
+static void free_prediction_buf(struct vdec_h264_inst *inst)
 {
 	struct mtk_vcodec_mem *mem = NULL;
 
@@ -286,7 +286,7 @@ static int vdec_h264_init(struct mtk_vcodec_dec_ctx *ctx)
 	}
 
 	inst->vsi = (struct vdec_h264_vsi *)inst->vpu.vsi;
-	err = allocate_predication_buf(inst);
+	err = allocate_prediction_buf(inst);
 	if (err)
 		goto error_deinit;
 
@@ -308,7 +308,7 @@ static void vdec_h264_deinit(void *h_vdec)
 	struct vdec_h264_inst *inst = (struct vdec_h264_inst *)h_vdec;
 
 	vpu_dec_deinit(&inst->vpu);
-	free_predication_buf(inst);
+	free_prediction_buf(inst);
 	free_mv_buf(inst);
 
 	kfree(inst);
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_common.h b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_common.h
index ac82be336055..31ffa13160a3 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_common.h
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_common.h
@@ -175,7 +175,7 @@ void mtk_vdec_h264_get_ref_list(u8 *ref_list,
 				int num_valid);
 
 /**
- * mtk_vdec_h264_get_ctrl_ptr - get each CID contrl address.
+ * mtk_vdec_h264_get_ctrl_ptr - get each CID control address.
  *
  * @ctx:	v4l2 ctx
  * @id:	CID control ID
@@ -185,7 +185,7 @@ void mtk_vdec_h264_get_ref_list(u8 *ref_list,
 void *mtk_vdec_h264_get_ctrl_ptr(struct mtk_vcodec_dec_ctx *ctx, int id);
 
 /**
- * mtk_vdec_h264_fill_dpb_info - get each CID contrl address.
+ * mtk_vdec_h264_fill_dpb_info - Fill the decoded picture buffer info
  *
  * @ctx:		v4l2 ctx
  * @decode_params:	slice decode params
@@ -225,10 +225,13 @@ void mtk_vdec_h264_copy_slice_hd_params(struct mtk_h264_slice_hd_param *dst_para
 					const struct v4l2_ctrl_h264_decode_params *dec_param);
 
 /**
- * mtk_vdec_h264_copy_scaling_matrix - get each CID contrl address.
+ * mtk_vdec_h264_copy_scaling_matrix - Copy scaling matrix from a control to the driver
  *
- * @dst_matrix:	scaling list params for hw decoder
- * @src_matrix:	scaling list params from user driver
+ * @dst_matrix:	scaling list params for the HW decoder
+ * @src_matrix:	scaling list params from a V4L2 control
+ *
+ * This function is used to copy the scaling matrix from a
+ * v4l2 control into the slice parameters for a decode.
  */
 void mtk_vdec_h264_copy_scaling_matrix(struct slice_api_h264_scaling_matrix *dst_matrix,
 				       const struct v4l2_ctrl_h264_scaling_matrix *src_matrix);
@@ -246,7 +249,7 @@ mtk_vdec_h264_copy_decode_params(struct slice_api_h264_decode_param *dst_params,
 				 const struct v4l2_h264_dpb_entry dpb[V4L2_H264_NUM_DPB_ENTRIES]);
 
 /**
- * mtk_vdec_h264_update_dpb - updata dpb list.
+ * mtk_vdec_h264_update_dpb - update dpb list.
  *
  * @dec_param:	v4l2 control decode params
  * @dpb:	dpb entry informaton
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_if.c
index 5600f1df653d..1e1b32faac77 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_if.c
@@ -27,7 +27,7 @@ struct mtk_h264_dec_slice_param {
 /**
  * struct vdec_h264_dec_info - decode information
  * @dpb_sz		: decoding picture buffer size
- * @resolution_changed  : resoltion change happen
+ * @resolution_changed  : flag to notify that a resolution change happened
  * @realloc_mv_buf	: flag to notify driver to re-allocate mv buffer
  * @cap_num_planes	: number planes of capture buffer
  * @bs_dma		: Input bit-stream buffer dma address
@@ -54,7 +54,7 @@ struct vdec_h264_dec_info {
  *                        by VPU.
  *                        AP-W/R : AP is writer/reader on this item
  *                        VPU-W/R: VPU is write/reader on this item
- * @pred_buf_dma : HW working predication buffer dma address (AP-W, VPU-R)
+ * @pred_buf_dma : HW working prediction buffer dma address (AP-W, VPU-R)
  * @mv_buf_dma   : HW working motion vector buffer dma address (AP-W, VPU-R)
  * @dec          : decode information (AP-R, VPU-W)
  * @pic          : picture information (AP-R, VPU-W)
@@ -74,7 +74,7 @@ struct vdec_h264_vsi {
  * struct vdec_h264_slice_inst - h264 decoder instance
  * @num_nalu : how many nalus be decoded
  * @ctx      : point to mtk_vcodec_dec_ctx
- * @pred_buf : HW working predication buffer
+ * @pred_buf : HW working prediction buffer
  * @mv_buf   : HW working motion vector buffer
  * @vpu      : VPU instance
  * @vsi_ctx  : Local VSI data for this decoding context
@@ -154,7 +154,7 @@ static int get_vdec_decode_parameters(struct vdec_h264_slice_inst *inst)
 	return 0;
 }
 
-static int allocate_predication_buf(struct vdec_h264_slice_inst *inst)
+static int allocate_prediction_buf(struct vdec_h264_slice_inst *inst)
 {
 	int err;
 
@@ -169,7 +169,7 @@ static int allocate_predication_buf(struct vdec_h264_slice_inst *inst)
 	return 0;
 }
 
-static void free_predication_buf(struct vdec_h264_slice_inst *inst)
+static void free_prediction_buf(struct vdec_h264_slice_inst *inst)
 {
 	struct mtk_vcodec_mem *mem = &inst->pred_buf;
 
@@ -292,7 +292,7 @@ static int vdec_h264_slice_init(struct mtk_vcodec_dec_ctx *ctx)
 	inst->vsi_ctx.dec.resolution_changed = true;
 	inst->vsi_ctx.dec.realloc_mv_buf = true;
 
-	err = allocate_predication_buf(inst);
+	err = allocate_prediction_buf(inst);
 	if (err)
 		goto error_deinit;
 
@@ -320,7 +320,7 @@ static void vdec_h264_slice_deinit(void *h_vdec)
 	struct vdec_h264_slice_inst *inst = h_vdec;
 
 	vpu_dec_deinit(&inst->vpu);
-	free_predication_buf(inst);
+	free_prediction_buf(inst);
 	free_mv_buf(inst);
 
 	kfree(inst);
@@ -347,11 +347,16 @@ static int vdec_h264_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 		return vpu_dec_reset(vpu);
 
 	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	if (!fb) {
+		mtk_vdec_err(inst->ctx, "fb buffer is NULL");
+		return -ENOMEM;
+	}
+
 	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
 	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
 
-	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
-	c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+	y_fb_dma = fb->base_y.dma_addr;
+	c_fb_dma = fb->base_c.dma_addr;
 
 	mtk_vdec_debug(inst->ctx, "+ [%d] FB y_dma=%llx c_dma=%llx va=%p",
 		       inst->num_nalu, y_fb_dma, c_fb_dma, fb);
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_multi_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_multi_if.c
index 0e741e0dc8ba..5b25e1679b51 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_multi_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_h264_req_multi_if.c
@@ -30,6 +30,7 @@ enum vdec_h264_core_dec_err_type {
 
 /**
  * struct vdec_h264_slice_lat_dec_param  - parameters for decode current frame
+ *	(shared data between host and firmware)
  *
  * @sps:		h264 sps syntax parameters
  * @pps:		h264 pps syntax parameters
@@ -48,10 +49,10 @@ struct vdec_h264_slice_lat_dec_param {
 };
 
 /**
- * struct vdec_h264_slice_info - decode information
+ * struct vdec_h264_slice_info - decode information (shared data between host and firmware)
  *
  * @nal_info:		nal info of current picture
- * @timeout:		Decode timeout: 1 timeout, 0 no timeount
+ * @timeout:		Decode timeout: 1 timeout, 0 no timeout
  * @bs_buf_size:	bitstream size
  * @bs_buf_addr:	bitstream buffer dma address
  * @y_fb_dma:		Y frame buffer dma address
@@ -72,23 +73,22 @@ struct vdec_h264_slice_info {
 
 /**
  * struct vdec_h264_slice_vsi - shared memory for decode information exchange
- *        between SCP and Host.
+ *        between SCP and Host (shared data between host and firmware).
  *
- * @wdma_err_addr:		wdma error dma address
- * @wdma_start_addr:		wdma start dma address
- * @wdma_end_addr:		wdma end dma address
- * @slice_bc_start_addr:	slice bc start dma address
- * @slice_bc_end_addr:		slice bc end dma address
- * @row_info_start_addr:	row info start dma address
- * @row_info_end_addr:		row info end dma address
- * @trans_start:		trans start dma address
- * @trans_end:			trans end dma address
- * @wdma_end_addr_offset:	wdma end address offset
+ * @wdma_err_addr:        wdma error dma address
+ * @wdma_start_addr:      wdma start dma address
+ * @wdma_end_addr:        wdma end dma address
+ * @slice_bc_start_addr:  slice bc start dma address
+ * @slice_bc_end_addr:    slice bc end dma address
+ * @row_info_start_addr:  row info start dma address
+ * @row_info_end_addr:    row info end dma address
+ * @trans_start:          trans start dma address
+ * @trans_end:            trans end dma address
+ * @wdma_end_addr_offset: wdma end address offset
  *
- * @mv_buf_dma:		HW working motion vector buffer
- *				dma address (AP-W, VPU-R)
- * @dec:			decode information (AP-R, VPU-W)
- * @h264_slice_params:		decode parameters for hw used
+ * @mv_buf_dma:           HW working motion vector buffer
+ * @dec:                  decode information (AP-R, VPU-W)
+ * @h264_slice_params:    decode parameters for hw used
  */
 struct vdec_h264_slice_vsi {
 	/* LAT dec addr */
@@ -112,12 +112,12 @@ struct vdec_h264_slice_vsi {
  * struct vdec_h264_slice_share_info - shared information used to exchange
  *                                     message between lat and core
  *
- * @sps:		sequence header information from user space
- * @dec_params:	decoder params from user space
- * @h264_slice_params:	decoder params used for hardware
- * @trans_start:	trans start dma address
- * @trans_end:		trans end dma address
- * @nal_info:		nal info of current picture
+ * @sps:               sequence header information from user space
+ * @dec_params:        decoder params from user space
+ * @h264_slice_params: decoder params used for hardware
+ * @trans_start:       trans start dma address
+ * @trans_end:         trans end dma address
+ * @nal_info:          nal info of current picture
  */
 struct vdec_h264_slice_share_info {
 	struct v4l2_ctrl_h264_sps sps;
@@ -128,27 +128,111 @@ struct vdec_h264_slice_share_info {
 	u16 nal_info;
 };
 
+/*
+ * struct vdec_h264_slice_mem - memory address and size
+ *        (shared data between host and firmware)
+ */
+struct vdec_h264_slice_mem {
+	union {
+		u64 buf;
+		u64 dma_addr;
+	};
+	union {
+		size_t size;
+		u64 dma_addr_end;
+	};
+};
+
+/**
+ * struct vdec_h264_slice_fb - frame buffer for decoding
+ *        (shared data between host and firmware)
+ *
+ * @y:  current luma buffer address info
+ * @c:  current chroma buffer address info
+ */
+struct vdec_h264_slice_fb {
+	struct vdec_h264_slice_mem y;
+	struct vdec_h264_slice_mem c;
+};
+
+/**
+ * struct vdec_h264_slice_info_ext - extend decode information
+ *        (shared data between host and firmware)
+ *
+ * @wdma_end_addr_offset: offset from buffer start
+ * @nal_info:             nal info of current picture
+ * @timeout:              toggles whether a decode operation is timeout
+ * @reserved:             reserved
+ * @vdec_fb_va:           vdec frame buffer struct virtual address
+ * @crc:                  displays the hardware status
+ */
+struct vdec_h264_slice_info_ext {
+	u64 wdma_end_addr_offset;
+	u16 nal_info;
+	u16 timeout;
+	u32 reserved;
+	u64 vdec_fb_va;
+	u32 crc[8];
+};
+
+/**
+ * struct vdec_h264_slice_vsi_ext - extend shared memory for decode information exchange
+ *        between SCP and Host (shared data between host and firmware).
+ *
+ * @bs:                input buffer info
+ * @fb:                current y/c buffer
+ *
+ * @ube:               buffer used to share date between lat and core
+ * @trans:             transcoded buffer used for core decode
+ * @row_info:          row info buffer
+ * @err_map:           error map buffer
+ * @slice_bc:          slice buffer
+ *
+ * @mv_buf_dma:        store hardware motion vector data
+ * @dec:               decode information (AP-R, VPU-W)
+ * @h264_slice_params: decode parameters used for the hw
+ */
+struct vdec_h264_slice_vsi_ext {
+	/* LAT dec addr */
+	struct vdec_h264_slice_mem bs;
+	struct vdec_h264_slice_fb fb;
+
+	struct vdec_h264_slice_mem ube;
+	struct vdec_h264_slice_mem trans;
+	struct vdec_h264_slice_mem row_info;
+	struct vdec_h264_slice_mem err_map;
+	struct vdec_h264_slice_mem slice_bc;
+
+	struct vdec_h264_slice_mem mv_buf_dma[H264_MAX_MV_NUM];
+	struct vdec_h264_slice_info_ext dec;
+	struct vdec_h264_slice_lat_dec_param h264_slice_params;
+};
+
 /**
  * struct vdec_h264_slice_inst - h264 decoder instance
  *
- * @slice_dec_num:	how many picture be decoded
+ * @slice_dec_num:	Number of frames to be decoded
  * @ctx:		point to mtk_vcodec_dec_ctx
- * @pred_buf:		HW working predication buffer
+ * @pred_buf:		HW working prediction buffer
  * @mv_buf:		HW working motion vector buffer
  * @vpu:		VPU instance
  * @vsi:		vsi used for lat
  * @vsi_core:		vsi used for core
- *
- * @vsi_ctx:		Local VSI data for this decoding context
+ * @vsi_ctx:		vsi data for this decoding context
+ * @vsi_ext:		extended vsi used for lat
+ * @vsi_core_ext:	extended vsi used for core
+ * @vsi_ctx_ext:	extended vsi data for this decoding context
  * @h264_slice_param:	the parameters that hardware use to decode
  *
- * @resolution_changed:resolution changed
+ * @resolution_changed: resolution changed
  * @realloc_mv_buf:	reallocate mv buffer
  * @cap_num_planes:	number of capture queue plane
  *
  * @dpb:		decoded picture buffer used to store reference
  *			buffer information
- *@is_field_bitstream:	is field bitstream
+ * @is_field_bitstream: not support field bitstream, only support frame
+ *
+ * @decode:		lat decoder pointer for different architectures
  */
 struct vdec_h264_slice_inst {
 	unsigned int slice_dec_num;
@@ -156,10 +240,18 @@ struct vdec_h264_slice_inst {
 	struct mtk_vcodec_mem pred_buf;
 	struct mtk_vcodec_mem mv_buf[H264_MAX_MV_NUM];
 	struct vdec_vpu_inst vpu;
-	struct vdec_h264_slice_vsi *vsi;
-	struct vdec_h264_slice_vsi *vsi_core;
-
-	struct vdec_h264_slice_vsi vsi_ctx;
+	union {
+		struct {
+			struct vdec_h264_slice_vsi *vsi;
+			struct vdec_h264_slice_vsi *vsi_core;
+			struct vdec_h264_slice_vsi vsi_ctx;
+		};
+		struct {
+			struct vdec_h264_slice_vsi_ext *vsi_ext;
+			struct vdec_h264_slice_vsi_ext *vsi_core_ext;
+			struct vdec_h264_slice_vsi_ext vsi_ctx_ext;
+		};
+	};
 	struct vdec_h264_slice_lat_dec_param h264_slice_param;
 
 	unsigned int resolution_changed;
@@ -168,12 +260,15 @@ struct vdec_h264_slice_inst {
 
 	struct v4l2_h264_dpb_entry dpb[16];
 	bool is_field_bitstream;
+
+	int (*decode)(void *h_vdec, struct mtk_vcodec_mem *bs,
+		      struct vdec_fb *unused, bool *res_chg);
 };
 
 static int vdec_h264_slice_fill_decode_parameters(struct vdec_h264_slice_inst *inst,
-						  struct vdec_h264_slice_share_info *share_info)
+						  struct vdec_h264_slice_share_info *share_info,
+						  struct vdec_h264_slice_lat_dec_param *slice_param)
 {
-	struct vdec_h264_slice_lat_dec_param *slice_param = &inst->vsi->h264_slice_params;
 	const struct v4l2_ctrl_h264_decode_params *dec_params;
 	const struct v4l2_ctrl_h264_scaling_matrix *src_matrix;
 	const struct v4l2_ctrl_h264_sps *sps;
@@ -266,9 +361,6 @@ static int get_vdec_sig_decode_parameters(struct vdec_h264_slice_inst *inst)
 	mtk_vdec_h264_get_ref_list(b0_reflist, v4l2_b0_reflist, reflist_builder.num_valid);
 	mtk_vdec_h264_get_ref_list(b1_reflist, v4l2_b1_reflist, reflist_builder.num_valid);
 
-	memcpy(&inst->vsi_ctx.h264_slice_params, slice_param,
-	       sizeof(inst->vsi_ctx.h264_slice_params));
-
 	return 0;
 }
 
@@ -392,68 +484,148 @@ static void vdec_h264_slice_get_crop_info(struct vdec_h264_slice_inst *inst,
 		       cr->left, cr->top, cr->width, cr->height);
 }
 
-static int vdec_h264_slice_init(struct mtk_vcodec_dec_ctx *ctx)
+static void vdec_h264_slice_setup_lat_buffer_ext(struct vdec_h264_slice_inst *inst,
+						 struct mtk_vcodec_mem *bs,
+						 struct vdec_lat_buf *lat_buf)
 {
-	struct vdec_h264_slice_inst *inst;
-	int err, vsi_size;
+	struct mtk_vcodec_mem *mem;
+	int i;
 
-	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
-	if (!inst)
-		return -ENOMEM;
+	inst->vsi_ext->bs.dma_addr = (u64)bs->dma_addr;
+	inst->vsi_ext->bs.size = bs->size;
 
-	inst->ctx = ctx;
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		inst->vsi_ext->mv_buf_dma[i].dma_addr = mem->dma_addr;
+		inst->vsi_ext->mv_buf_dma[i].size = mem->size;
+	}
+	inst->vsi_ext->ube.dma_addr = lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
+	inst->vsi_ext->ube.size = lat_buf->ctx->msg_queue.wdma_addr.size;
 
-	inst->vpu.id = SCP_IPI_VDEC_LAT;
-	inst->vpu.core_id = SCP_IPI_VDEC_CORE;
-	inst->vpu.ctx = ctx;
-	inst->vpu.codec_type = ctx->current_codec;
-	inst->vpu.capture_type = ctx->capture_fourcc;
+	inst->vsi_ext->row_info.dma_addr = 0;
+	inst->vsi_ext->row_info.size = 0;
 
-	err = vpu_dec_init(&inst->vpu);
-	if (err) {
-		mtk_vdec_err(ctx, "vdec_h264 init err=%d", err);
-		goto error_free_inst;
+	inst->vsi_ext->err_map.dma_addr = lat_buf->wdma_err_addr.dma_addr;
+	inst->vsi_ext->err_map.size = lat_buf->wdma_err_addr.size;
+
+	inst->vsi_ext->slice_bc.dma_addr = lat_buf->slice_bc_addr.dma_addr;
+	inst->vsi_ext->slice_bc.size = lat_buf->slice_bc_addr.size;
+
+	inst->vsi_ext->trans.dma_addr_end = inst->ctx->msg_queue.wdma_rptr_addr;
+	inst->vsi_ext->trans.dma_addr = inst->ctx->msg_queue.wdma_wptr_addr;
+}
+
+static int vdec_h264_slice_setup_core_buffer_ext(struct vdec_h264_slice_inst *inst,
+						 struct vdec_h264_slice_share_info *share_info,
+						 struct vdec_lat_buf *lat_buf)
+{
+	struct mtk_vcodec_mem *mem;
+	struct mtk_vcodec_dec_ctx *ctx = inst->ctx;
+	struct vb2_v4l2_buffer *vb2_v4l2;
+	struct vdec_fb *fb;
+	u64 y_fb_dma, c_fb_dma = 0;
+	int i;
+
+	fb = ctx->dev->vdec_pdata->get_cap_buffer(ctx);
+	if (!fb) {
+		mtk_vdec_err(ctx, "Unable to get a CAPTURE buffer for CAPTURE queue is empty.");
+		return -EBUSY;
 	}
 
-	vsi_size = round_up(sizeof(struct vdec_h264_slice_vsi), VCODEC_DEC_ALIGNED_64);
-	inst->vsi = inst->vpu.vsi;
-	inst->vsi_core =
-		(struct vdec_h264_slice_vsi *)(((char *)inst->vpu.vsi) + vsi_size);
-	inst->resolution_changed = true;
-	inst->realloc_mv_buf = true;
+	y_fb_dma = (u64)fb->base_y.dma_addr;
+	if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1)
+		c_fb_dma = y_fb_dma + ctx->picinfo.fb_sz[0];
+	else
+		c_fb_dma = (u64)fb->base_c.dma_addr;
 
-	mtk_vdec_debug(ctx, "lat struct size = %d,%d,%d,%d vsi: %d\n",
-		       (int)sizeof(struct mtk_h264_sps_param),
-		       (int)sizeof(struct mtk_h264_pps_param),
-		       (int)sizeof(struct vdec_h264_slice_lat_dec_param),
-		       (int)sizeof(struct mtk_h264_dpb_info),
-		       vsi_size);
-	mtk_vdec_debug(ctx, "lat H264 instance >> %p, codec_type = 0x%x",
-		       inst, inst->vpu.codec_type);
+	mtk_vdec_debug(ctx, "[h264-core] y/c addr = 0x%llx 0x%llx", y_fb_dma, c_fb_dma);
 
-	ctx->drv_handle = inst;
-	return 0;
+	inst->vsi_core_ext->fb.y.dma_addr = y_fb_dma;
+	inst->vsi_core_ext->fb.y.size = ctx->picinfo.fb_sz[0];
+	inst->vsi_core_ext->fb.c.dma_addr = c_fb_dma;
+	inst->vsi_core_ext->fb.c.size = ctx->picinfo.fb_sz[1];
 
-error_free_inst:
-	kfree(inst);
-	return err;
+	inst->vsi_core_ext->dec.vdec_fb_va = (unsigned long)fb;
+	inst->vsi_core_ext->dec.nal_info = share_info->nal_info;
+
+	inst->vsi_core_ext->ube.dma_addr = lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
+	inst->vsi_core_ext->ube.size = lat_buf->ctx->msg_queue.wdma_addr.size;
+
+	inst->vsi_core_ext->err_map.dma_addr = lat_buf->wdma_err_addr.dma_addr;
+	inst->vsi_core_ext->err_map.size = lat_buf->wdma_err_addr.size;
+
+	inst->vsi_core_ext->slice_bc.dma_addr = lat_buf->slice_bc_addr.dma_addr;
+	inst->vsi_core_ext->slice_bc.size = lat_buf->slice_bc_addr.size;
+
+	inst->vsi_core_ext->row_info.dma_addr = 0;
+	inst->vsi_core_ext->row_info.size = 0;
+
+	inst->vsi_core_ext->trans.dma_addr = share_info->trans_start;
+	inst->vsi_core_ext->trans.dma_addr_end = share_info->trans_end;
+
+	for (i = 0; i < H264_MAX_MV_NUM; i++) {
+		mem = &inst->mv_buf[i];
+		inst->vsi_core_ext->mv_buf_dma[i].dma_addr = mem->dma_addr;
+		inst->vsi_core_ext->mv_buf_dma[i].size = mem->size;
+	}
+
+	vb2_v4l2 = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
+	v4l2_m2m_buf_copy_metadata(&lat_buf->ts_info, vb2_v4l2, true);
+
+	return 0;
 }
 
-static void vdec_h264_slice_deinit(void *h_vdec)
+static int vdec_h264_slice_core_decode_ext(struct vdec_lat_buf *lat_buf)
 {
-	struct vdec_h264_slice_inst *inst = h_vdec;
+	int err, timeout;
+	struct mtk_vcodec_dec_ctx *ctx = lat_buf->ctx;
+	struct vdec_h264_slice_inst *inst = ctx->drv_handle;
+	struct vdec_h264_slice_share_info *share_info = lat_buf->private_data;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
 
-	vpu_dec_deinit(&inst->vpu);
-	vdec_h264_slice_free_mv_buf(inst);
-	vdec_msg_queue_deinit(&inst->ctx->msg_queue, inst->ctx);
+	memcpy(&inst->vsi_core_ext->h264_slice_params, &share_info->h264_slice_params,
+	       sizeof(share_info->h264_slice_params));
 
-	kfree(inst);
+	err = vdec_h264_slice_setup_core_buffer_ext(inst, share_info, lat_buf);
+	if (err)
+		goto vdec_dec_end;
+
+	vdec_h264_slice_fill_decode_reflist(inst, &inst->vsi_core_ext->h264_slice_params,
+					    share_info);
+	err = vpu_dec_core(vpu);
+	if (err) {
+		mtk_vdec_err(ctx, "core decode err=%d", err);
+		goto vdec_dec_end;
+	}
+
+	/* wait decoder done interrupt */
+	timeout = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					       WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+	if (timeout)
+		mtk_vdec_err(ctx, "core decode timeout: pic_%d", ctx->decoded_frame_cnt);
+	inst->vsi_core_ext->dec.timeout = !!timeout;
+
+	vpu_dec_core_end(vpu);
+
+	/* crc is hardware checksum, can be used to check whether the decoder result is right.*/
+	mtk_vdec_debug(ctx, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
+		       ctx->decoded_frame_cnt,
+		       inst->vsi_core_ext->dec.crc[0], inst->vsi_core_ext->dec.crc[1],
+		       inst->vsi_core_ext->dec.crc[2], inst->vsi_core_ext->dec.crc[3],
+		       inst->vsi_core_ext->dec.crc[4], inst->vsi_core_ext->dec.crc[5],
+		       inst->vsi_core_ext->dec.crc[6], inst->vsi_core_ext->dec.crc[7]);
+
+vdec_dec_end:
+	vdec_msg_queue_update_ube_rptr(&lat_buf->ctx->msg_queue, share_info->trans_end);
+	ctx->dev->vdec_pdata->cap_to_disp(ctx, !!err, lat_buf->src_buf_req);
+	mtk_vdec_debug(ctx, "core decode done err=%d", err);
+	ctx->decoded_frame_cnt++;
+	return 0;
 }
 
 static int vdec_h264_slice_core_decode(struct vdec_lat_buf *lat_buf)
 {
 	struct vdec_fb *fb;
-	u64 vdec_fb_va;
 	u64 y_fb_dma, c_fb_dma;
 	int err, timeout, i;
 	struct mtk_vcodec_dec_ctx *ctx = lat_buf->ctx;
@@ -463,22 +635,19 @@ static int vdec_h264_slice_core_decode(struct vdec_lat_buf *lat_buf)
 	struct mtk_vcodec_mem *mem;
 	struct vdec_vpu_inst *vpu = &inst->vpu;
 
-	mtk_vdec_debug(ctx, "[h264-core] vdec_h264 core decode");
 	memcpy(&inst->vsi_core->h264_slice_params, &share_info->h264_slice_params,
 	       sizeof(share_info->h264_slice_params));
 
 	fb = ctx->dev->vdec_pdata->get_cap_buffer(ctx);
 	if (!fb) {
 		err = -EBUSY;
-		mtk_vdec_err(ctx, "fb buffer is NULL");
+		mtk_vdec_err(ctx, "Unable to get a CAPTURE buffer for CAPTURE queue is empty.");
 		goto vdec_dec_end;
 	}
 
-	vdec_fb_va = (unsigned long)fb;
 	y_fb_dma = (u64)fb->base_y.dma_addr;
 	if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1)
-		c_fb_dma =
-			y_fb_dma + inst->ctx->picinfo.buf_w * inst->ctx->picinfo.buf_h;
+		c_fb_dma = y_fb_dma + ctx->picinfo.fb_sz[0];
 	else
 		c_fb_dma = (u64)fb->base_c.dma_addr;
 
@@ -486,7 +655,7 @@ static int vdec_h264_slice_core_decode(struct vdec_lat_buf *lat_buf)
 
 	inst->vsi_core->dec.y_fb_dma = y_fb_dma;
 	inst->vsi_core->dec.c_fb_dma = c_fb_dma;
-	inst->vsi_core->dec.vdec_fb_va = vdec_fb_va;
+	inst->vsi_core->dec.vdec_fb_va = (unsigned long)fb;
 	inst->vsi_core->dec.nal_info = share_info->nal_info;
 	inst->vsi_core->wdma_start_addr =
 		lat_buf->ctx->msg_queue.wdma_addr.dma_addr;
@@ -524,6 +693,8 @@ static int vdec_h264_slice_core_decode(struct vdec_lat_buf *lat_buf)
 	inst->vsi_core->dec.timeout = !!timeout;
 
 	vpu_dec_core_end(vpu);
+
+	/* crc is hardware checksum, can be used to check whether the decoder result is right.*/
 	mtk_vdec_debug(ctx, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
 		       ctx->decoded_frame_cnt,
 		       inst->vsi_core->dec.crc[0], inst->vsi_core->dec.crc[1],
@@ -536,6 +707,7 @@ vdec_dec_end:
 	ctx->dev->vdec_pdata->cap_to_disp(ctx, !!err, lat_buf->src_buf_req);
 	mtk_vdec_debug(ctx, "core decode done err=%d", err);
 	ctx->decoded_frame_cnt++;
+
 	return 0;
 }
 
@@ -562,6 +734,128 @@ static void vdec_h264_insert_startcode(struct mtk_vcodec_dec_dev *vcodec_dev, un
 	(*bs_size) += 4;
 }
 
+static int vdec_h264_slice_lat_decode_ext(void *h_vdec, struct mtk_vcodec_mem *bs,
+					  struct vdec_fb *fb, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info;
+	int err, timeout = 0;
+	unsigned int data[2];
+	struct vdec_lat_buf *lat_buf;
+	struct vdec_h264_slice_share_info *share_info;
+
+	if (vdec_msg_queue_init(&inst->ctx->msg_queue, inst->ctx,
+				vdec_h264_slice_core_decode_ext,
+				sizeof(*share_info)))
+		return -ENOMEM;
+
+	/* bs NULL means flush decoder */
+	if (!bs) {
+		vdec_msg_queue_wait_lat_buf_full(&inst->ctx->msg_queue);
+		return vpu_dec_reset(vpu);
+	}
+
+	if (inst->is_field_bitstream)
+		return -EINVAL;
+
+	lat_buf = vdec_msg_queue_dqbuf(&inst->ctx->msg_queue.lat_ctx);
+	if (!lat_buf) {
+		mtk_vdec_debug(inst->ctx, "failed to get lat buffer");
+		return -EAGAIN;
+	}
+	share_info = lat_buf->private_data;
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+
+	lat_buf->src_buf_req = src_buf_info->m2m_buf.vb.vb2_buf.req_obj.req;
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb, &lat_buf->ts_info, true);
+
+	err = vdec_h264_slice_fill_decode_parameters(inst, share_info,
+						     &inst->vsi_ext->h264_slice_params);
+	if (err)
+		goto err_free_fb_out;
+
+	vdec_h264_insert_startcode(inst->ctx->dev, bs->va, &bs->size,
+				   &share_info->h264_slice_params.pps);
+
+	*res_chg = inst->resolution_changed;
+	if (inst->resolution_changed) {
+		mtk_vdec_debug(inst->ctx, "- resolution changed -");
+		if (inst->realloc_mv_buf) {
+			err = vdec_h264_slice_alloc_mv_buf(inst, &inst->ctx->picinfo);
+			inst->realloc_mv_buf = false;
+			if (err)
+				goto err_free_fb_out;
+		}
+		inst->resolution_changed = false;
+	}
+
+	vdec_h264_slice_setup_lat_buffer_ext(inst, bs, lat_buf);
+	mtk_vdec_debug(inst->ctx, "lat:trans(0x%llx 0x%lx) err:0x%llx",
+		       inst->vsi_ext->ube.dma_addr, (unsigned long)inst->vsi_ext->ube.size,
+		       inst->vsi_ext->err_map.dma_addr);
+
+	mtk_vdec_debug(inst->ctx, "slice(0x%llx 0x%lx) rprt((0x%llx 0x%llx))",
+		       inst->vsi_ext->slice_bc.dma_addr,
+		       (unsigned long)inst->vsi_ext->slice_bc.size,
+		       inst->vsi_ext->trans.dma_addr, inst->vsi_ext->trans.dma_addr_end);
+
+	err = vpu_dec_start(vpu, data, 2);
+	if (err) {
+		mtk_vdec_debug(inst->ctx, "lat decode err: %d", err);
+		goto err_free_fb_out;
+	}
+
+	share_info->trans_end = inst->ctx->msg_queue.wdma_addr.dma_addr +
+		inst->vsi_ext->dec.wdma_end_addr_offset;
+
+	share_info->trans_start = inst->ctx->msg_queue.wdma_wptr_addr;
+	share_info->nal_info = inst->vsi_ext->dec.nal_info;
+
+	if (IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability)) {
+		memcpy(&share_info->h264_slice_params, &inst->vsi_ext->h264_slice_params,
+		       sizeof(share_info->h264_slice_params));
+		vdec_msg_queue_qbuf(&inst->ctx->msg_queue.core_ctx, lat_buf);
+	}
+
+	/* wait decoder done interrupt */
+	timeout = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					       WAIT_INTR_TIMEOUT_MS, MTK_VDEC_LAT0);
+	if (timeout)
+		mtk_vdec_err(inst->ctx, "lat decode timeout: pic_%d", inst->slice_dec_num);
+	inst->vsi_ext->dec.timeout = !!timeout;
+
+	err = vpu_dec_end(vpu);
+	if (err == SLICE_HEADER_FULL || err == TRANS_BUFFER_FULL) {
+		if (!IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability))
+			vdec_msg_queue_qbuf(&inst->ctx->msg_queue.lat_ctx, lat_buf);
+		inst->slice_dec_num++;
+		mtk_vdec_err(inst->ctx, "lat dec fail: pic_%d err:%d", inst->slice_dec_num, err);
+		return -EINVAL;
+	}
+
+	share_info->trans_end = inst->ctx->msg_queue.wdma_addr.dma_addr +
+		inst->vsi_ext->dec.wdma_end_addr_offset;
+
+	vdec_msg_queue_update_ube_wptr(&lat_buf->ctx->msg_queue, share_info->trans_end);
+
+	if (!IS_VDEC_INNER_RACING(inst->ctx->dev->dec_capability)) {
+		memcpy(&share_info->h264_slice_params, &inst->vsi_ext->h264_slice_params,
+		       sizeof(share_info->h264_slice_params));
+		vdec_msg_queue_qbuf(&inst->ctx->msg_queue.core_ctx, lat_buf);
+	}
+	mtk_vdec_debug(inst->ctx, "dec num: %d lat crc: 0x%x 0x%x 0x%x", inst->slice_dec_num,
+		       inst->vsi_ext->dec.crc[0], inst->vsi_ext->dec.crc[1],
+		       inst->vsi_ext->dec.crc[2]);
+
+	inst->slice_dec_num++;
+	return 0;
+err_free_fb_out:
+	vdec_msg_queue_qbuf(&inst->ctx->msg_queue.lat_ctx, lat_buf);
+	mtk_vdec_err(inst->ctx, "slice dec number: %d err: %d", inst->slice_dec_num, err);
+	return err;
+}
+
 static int vdec_h264_slice_lat_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 				      struct vdec_fb *fb, bool *res_chg)
 {
@@ -608,7 +902,8 @@ static int vdec_h264_slice_lat_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 	lat_buf->src_buf_req = src_buf_info->m2m_buf.vb.vb2_buf.req_obj.req;
 	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb, &lat_buf->ts_info, true);
 
-	err = vdec_h264_slice_fill_decode_parameters(inst, share_info);
+	err = vdec_h264_slice_fill_decode_parameters(inst, share_info,
+						     &inst->vsi->h264_slice_params);
 	if (err)
 		goto err_free_fb_out;
 
@@ -706,6 +1001,101 @@ err_free_fb_out:
 	return err;
 }
 
+static int vdec_h264_slice_single_decode_ext(void *h_vdec, struct mtk_vcodec_mem *bs,
+					     struct vdec_fb *unused, bool *res_chg)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+	struct vdec_vpu_inst *vpu = &inst->vpu;
+	struct mtk_video_dec_buf *src_buf_info, *dst_buf_info;
+	struct vdec_fb *fb;
+	unsigned int data[2], i;
+	u64 y_fb_dma, c_fb_dma;
+	struct mtk_vcodec_mem *mem;
+	int err;
+
+	/* bs NULL means flush decoder */
+	if (!bs)
+		return vpu_dec_reset(vpu);
+
+	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	if (!fb) {
+		mtk_vdec_err(inst->ctx,
+			     "Unable to get a CAPTURE buffer for CAPTURE queue is empty.");
+		return -ENOMEM;
+	}
+
+	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
+	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+
+	y_fb_dma = fb->base_y.dma_addr;
+	c_fb_dma = fb->base_c.dma_addr;
+	mtk_vdec_debug(inst->ctx, "[h264-dec] [%d] y_dma=%llx c_dma=%llx",
+		       inst->ctx->decoded_frame_cnt, y_fb_dma, c_fb_dma);
+
+	inst->vsi_ctx_ext.bs.dma_addr = (u64)bs->dma_addr;
+	inst->vsi_ctx_ext.bs.size = bs->size;
+	inst->vsi_ctx_ext.fb.y.dma_addr = y_fb_dma;
+	inst->vsi_ctx_ext.fb.c.dma_addr = c_fb_dma;
+	inst->vsi_ctx_ext.dec.vdec_fb_va = (u64)(uintptr_t)fb;
+
+	v4l2_m2m_buf_copy_metadata(&src_buf_info->m2m_buf.vb,
+				   &dst_buf_info->m2m_buf.vb, true);
+	err = get_vdec_sig_decode_parameters(inst);
+	if (err)
+		goto err_free_fb_out;
+
+	memcpy(&inst->vsi_ctx_ext.h264_slice_params, &inst->h264_slice_param,
+	       sizeof(inst->vsi_ctx_ext.h264_slice_params));
+
+	*res_chg = inst->resolution_changed;
+	if (inst->resolution_changed) {
+		mtk_vdec_debug(inst->ctx, "- resolution changed -");
+		if (inst->realloc_mv_buf) {
+			err = vdec_h264_slice_alloc_mv_buf(inst, &inst->ctx->picinfo);
+			inst->realloc_mv_buf = false;
+			if (err)
+				goto err_free_fb_out;
+		}
+		inst->resolution_changed = false;
+
+		for (i = 0; i < H264_MAX_MV_NUM; i++) {
+			mem = &inst->mv_buf[i];
+			inst->vsi_ctx_ext.mv_buf_dma[i].dma_addr = mem->dma_addr;
+		}
+	}
+
+	memcpy(inst->vpu.vsi, &inst->vsi_ctx_ext, sizeof(inst->vsi_ctx_ext));
+	err = vpu_dec_start(vpu, data, 2);
+	if (err)
+		goto err_free_fb_out;
+
+	/* wait decoder done interrupt */
+	err = mtk_vcodec_wait_for_done_ctx(inst->ctx, MTK_INST_IRQ_RECEIVED,
+					   WAIT_INTR_TIMEOUT_MS, MTK_VDEC_CORE);
+	if (err)
+		mtk_vdec_err(inst->ctx, "decode timeout: pic_%d", inst->ctx->decoded_frame_cnt);
+
+	inst->vsi_ext->dec.timeout = !!err;
+	err = vpu_dec_end(vpu);
+	if (err)
+		goto err_free_fb_out;
+
+	memcpy(&inst->vsi_ctx_ext, inst->vpu.vsi, sizeof(inst->vsi_ctx_ext));
+	mtk_vdec_debug(inst->ctx, "pic[%d] crc: 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x",
+		       inst->ctx->decoded_frame_cnt,
+		       inst->vsi_ctx_ext.dec.crc[0], inst->vsi_ctx_ext.dec.crc[1],
+		       inst->vsi_ctx_ext.dec.crc[2], inst->vsi_ctx_ext.dec.crc[3],
+		       inst->vsi_ctx_ext.dec.crc[4], inst->vsi_ctx_ext.dec.crc[5],
+		       inst->vsi_ctx_ext.dec.crc[6], inst->vsi_ctx_ext.dec.crc[7]);
+
+	inst->ctx->decoded_frame_cnt++;
+	return 0;
+
+err_free_fb_out:
+	mtk_vdec_err(inst->ctx, "dec frame number: %d err: %d", inst->ctx->decoded_frame_cnt, err);
+	return err;
+}
+
 static int vdec_h264_slice_single_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 					 struct vdec_fb *unused, bool *res_chg)
 {
@@ -724,11 +1114,17 @@ static int vdec_h264_slice_single_decode(void *h_vdec, struct mtk_vcodec_mem *bs
 		return vpu_dec_reset(vpu);
 
 	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
+	if (!fb) {
+		mtk_vdec_err(inst->ctx,
+			     "Unable to get a CAPTURE buffer for CAPTURE queue is empty.");
+		return -ENOMEM;
+	}
+
 	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
 	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
 
-	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
-	c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+	y_fb_dma = fb->base_y.dma_addr;
+	c_fb_dma = fb->base_c.dma_addr;
 	mtk_vdec_debug(inst->ctx, "[h264-dec] [%d] y_dma=%llx c_dma=%llx",
 		       inst->ctx->decoded_frame_cnt, y_fb_dma, c_fb_dma);
 
@@ -744,6 +1140,9 @@ static int vdec_h264_slice_single_decode(void *h_vdec, struct mtk_vcodec_mem *bs
 	if (err)
 		goto err_free_fb_out;
 
+	memcpy(&inst->vsi_ctx.h264_slice_params, &inst->h264_slice_param,
+	       sizeof(inst->vsi_ctx.h264_slice_params));
+
 	buf = (unsigned char *)bs->va;
 	nal_start_idx = mtk_vdec_h264_find_start_code(buf, bs->size);
 	if (nal_start_idx < 0) {
@@ -801,21 +1200,95 @@ err_free_fb_out:
 	return err;
 }
 
+static int vdec_h264_slice_init(struct mtk_vcodec_dec_ctx *ctx)
+{
+	struct vdec_h264_slice_inst *inst;
+	int err, vsi_size;
+	unsigned char *temp;
+
+	inst = kzalloc(sizeof(*inst), GFP_KERNEL);
+	if (!inst)
+		return -ENOMEM;
+
+	inst->ctx = ctx;
+
+	inst->vpu.id = SCP_IPI_VDEC_LAT;
+	inst->vpu.core_id = SCP_IPI_VDEC_CORE;
+	inst->vpu.ctx = ctx;
+	inst->vpu.codec_type = ctx->current_codec;
+	inst->vpu.capture_type = ctx->capture_fourcc;
+
+	err = vpu_dec_init(&inst->vpu);
+	if (err) {
+		mtk_vdec_err(ctx, "vdec_h264 init err=%d", err);
+		goto error_free_inst;
+	}
+
+	if (IS_VDEC_SUPPORT_EXT(ctx->dev->dec_capability)) {
+		vsi_size = sizeof(struct vdec_h264_slice_vsi_ext);
+
+		vsi_size = round_up(vsi_size, VCODEC_DEC_ALIGNED_64);
+		inst->vsi_ext = inst->vpu.vsi;
+		temp = (unsigned char *)inst->vsi_ext;
+		inst->vsi_core_ext = (struct vdec_h264_slice_vsi_ext *)(temp + vsi_size);
+
+		if (inst->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_PURE_SINGLE_CORE)
+			inst->decode = vdec_h264_slice_single_decode_ext;
+		else
+			inst->decode = vdec_h264_slice_lat_decode_ext;
+	} else {
+		vsi_size = sizeof(struct vdec_h264_slice_vsi);
+
+		vsi_size = round_up(vsi_size, VCODEC_DEC_ALIGNED_64);
+		inst->vsi = inst->vpu.vsi;
+		temp = (unsigned char *)inst->vsi;
+		inst->vsi_core = (struct vdec_h264_slice_vsi *)(temp + vsi_size);
+
+		if (inst->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_PURE_SINGLE_CORE)
+			inst->decode = vdec_h264_slice_single_decode;
+		else
+			inst->decode = vdec_h264_slice_lat_decode;
+	}
+	inst->resolution_changed = true;
+	inst->realloc_mv_buf = true;
+
+	mtk_vdec_debug(ctx, "lat struct size = %d,%d,%d,%d vsi: %d\n",
+		       (int)sizeof(struct mtk_h264_sps_param),
+		       (int)sizeof(struct mtk_h264_pps_param),
+		       (int)sizeof(struct vdec_h264_slice_lat_dec_param),
+		       (int)sizeof(struct mtk_h264_dpb_info),
+		       vsi_size);
+	mtk_vdec_debug(ctx, "lat H264 instance >> %p, codec_type = 0x%x",
+		       inst, inst->vpu.codec_type);
+
+	ctx->drv_handle = inst;
+	return 0;
+
+error_free_inst:
+	kfree(inst);
+	return err;
+}
+
+static void vdec_h264_slice_deinit(void *h_vdec)
+{
+	struct vdec_h264_slice_inst *inst = h_vdec;
+
+	vpu_dec_deinit(&inst->vpu);
+	vdec_h264_slice_free_mv_buf(inst);
+	vdec_msg_queue_deinit(&inst->ctx->msg_queue, inst->ctx);
+
+	kfree(inst);
+}
+
 static int vdec_h264_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 				  struct vdec_fb *unused, bool *res_chg)
 {
 	struct vdec_h264_slice_inst *inst = h_vdec;
-	int ret;
 
 	if (!h_vdec)
 		return -EINVAL;
 
-	if (inst->ctx->dev->vdec_pdata->hw_arch == MTK_VDEC_PURE_SINGLE_CORE)
-		ret = vdec_h264_slice_single_decode(h_vdec, bs, unused, res_chg);
-	else
-		ret = vdec_h264_slice_lat_decode(h_vdec, bs, unused, res_chg);
-
-	return ret;
+	return inst->decode(h_vdec, bs, unused, res_chg);
 }
 
 static int vdec_h264_slice_get_param(void *h_vdec, enum vdec_get_param_type type,
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_hevc_req_multi_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_hevc_req_multi_if.c
index 06ed47df693b..2725db882e5b 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_hevc_req_multi_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_hevc_req_multi_if.c
@@ -254,7 +254,7 @@ struct vdec_hevc_slice_lat_dec_param {
  * struct vdec_hevc_slice_info - decode information
  *
  * @wdma_end_addr_offset: wdma end address offset
- * @timeout:              Decode timeout: 1 timeout, 0 no timeount
+ * @timeout:              Decode timeout: 1 timeout, 0 no timeout
  * @vdec_fb_va:           VDEC frame buffer struct virtual address
  * @crc:                  Used to check whether hardware's status is right
  */
@@ -342,7 +342,7 @@ struct vdec_hevc_slice_share_info {
 /**
  * struct vdec_hevc_slice_inst - hevc decoder instance
  *
- * @slice_dec_num:      how many picture be decoded
+ * @slice_dec_num:      Number of frames to be decoded
  * @ctx:                point to mtk_vcodec_dec_ctx
  * @mv_buf:             HW working motion vector buffer
  * @vpu:                VPU instance
@@ -821,7 +821,7 @@ static int vdec_hevc_slice_setup_core_buffer(struct vdec_hevc_slice_inst *inst,
 	inst->vsi_core->fb.y.dma_addr = y_fb_dma;
 	inst->vsi_core->fb.y.size = ctx->picinfo.fb_sz[0];
 	inst->vsi_core->fb.c.dma_addr = c_fb_dma;
-	inst->vsi_core->fb.y.size = ctx->picinfo.fb_sz[1];
+	inst->vsi_core->fb.c.size = ctx->picinfo.fb_sz[1];
 
 	inst->vsi_core->dec.vdec_fb_va = (unsigned long)fb;
 
@@ -869,7 +869,6 @@ static int vdec_hevc_slice_init(struct mtk_vcodec_dec_ctx *ctx)
 	inst->vpu.codec_type = ctx->current_codec;
 	inst->vpu.capture_type = ctx->capture_fourcc;
 
-	ctx->drv_handle = inst;
 	err = vpu_dec_init(&inst->vpu);
 	if (err) {
 		mtk_vdec_err(ctx, "vdec_hevc init err=%d", err);
@@ -898,6 +897,7 @@ static int vdec_hevc_slice_init(struct mtk_vcodec_dec_ctx *ctx)
 	mtk_vdec_debug(ctx, "lat hevc instance >> %p, codec_type = 0x%x",
 		       inst, inst->vpu.codec_type);
 
+	ctx->drv_handle = inst;
 	return 0;
 error_free_inst:
 	kfree(inst);
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_if.c
index 19407f9bc773..5f848691cea4 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_if.c
@@ -56,7 +56,7 @@
  * @cur_c_fb_dma      : current plane C frame buffer dma address
  * @bs_dma	      : bitstream dma address
  * @bs_sz	      : bitstream size
- * @resolution_changed: resolution change flag 1 - changed,  0 - not change
+ * @resolution_changed: resolution change flag 1 - changed,  0 - not changed
  * @show_frame	      : display this frame or not
  * @wait_key_frame    : wait key frame coming
  */
@@ -109,7 +109,7 @@ struct vdec_vp8_hw_reg_base {
 /**
  * struct vdec_vp8_vpu_inst - VPU instance for VP8 decode
  * @wq_hd	: Wait queue to wait VPU message ack
- * @signaled	: 1 - Host has received ack message from VPU, 0 - not receive
+ * @signaled	: 1 - Host has received ack message from VPU, 0 - not received
  * @failure	: VPU execution result status 0 - success, others - fail
  * @inst_addr	: VPU decoder instance address
  */
@@ -449,7 +449,7 @@ static int vdec_vp8_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 		       inst->frm_cnt, y_fb_dma, c_fb_dma, fb);
 
 	inst->cur_fb = fb;
-	dec->bs_dma = (unsigned long)bs->dma_addr;
+	dec->bs_dma = bs->dma_addr;
 	dec->bs_sz = bs->size;
 	dec->cur_y_fb_dma = y_fb_dma;
 	dec->cur_c_fb_dma = c_fb_dma;
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_req_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_req_if.c
index f677e499fefa..232ef3bd246a 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_req_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp8_req_if.c
@@ -35,7 +35,7 @@
  * @cur_c_fb_dma:	current plane C frame buffer dma address
  * @bs_dma:		bitstream dma address
  * @bs_sz:		bitstream size
- * @resolution_changed:resolution change flag 1 - changed,  0 - not change
+ * @resolution_changed:resolution change flag 1 - changed,  0 - not changed
  * @frame_header_type:	current frame header type
  * @crc:		used to check whether hardware's status is right
  * @reserved:		reserved, currently unused
@@ -334,14 +334,18 @@ static int vdec_vp8_slice_decode(void *h_vdec, struct mtk_vcodec_mem *bs,
 	src_buf_info = container_of(bs, struct mtk_video_dec_buf, bs_buffer);
 
 	fb = inst->ctx->dev->vdec_pdata->get_cap_buffer(inst->ctx);
-	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+	if (!fb) {
+		mtk_vdec_err(inst->ctx, "fb buffer is NULL");
+		return -ENOMEM;
+	}
 
-	y_fb_dma = fb ? (u64)fb->base_y.dma_addr : 0;
+	dst_buf_info = container_of(fb, struct mtk_video_dec_buf, frame_buffer);
+	y_fb_dma = fb->base_y.dma_addr;
 	if (inst->ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 1)
 		c_fb_dma = y_fb_dma +
 			inst->ctx->picinfo.buf_w * inst->ctx->picinfo.buf_h;
 	else
-		c_fb_dma = fb ? (u64)fb->base_c.dma_addr : 0;
+		c_fb_dma = fb->base_c.dma_addr;
 
 	inst->vsi->dec.bs_dma = (u64)bs->dma_addr;
 	inst->vsi->dec.bs_sz = bs->size;
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_if.c
index 55355fa70090..eb3354192853 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_if.c
@@ -16,6 +16,7 @@
 #include "../vdec_drv_base.h"
 #include "../vdec_vpu_if.h"
 
+#define VP9_MAX_SUPER_FRAMES_NUM 8
 #define VP9_SUPER_FRAME_BS_SZ 64
 #define MAX_VP9_DPB_SIZE	9
 
@@ -41,7 +42,7 @@ struct vp9_dram_buf {
 
 /**
  * struct vp9_fb_info - contains frame buffer info
- * @fb : frmae buffer
+ * @fb : frame buffer
  * @reserved : reserved field used by vpu
  */
 struct vp9_fb_info {
@@ -89,7 +90,7 @@ struct vp9_sf_ref_fb {
  *	AP-W/R : AP is writer/reader on this item
  *	VPU-W/R: VPU is write/reader on this item
  * @sf_bs_buf : super frame backup buffer (AP-W, VPU-R)
- * @sf_ref_fb : record supoer frame reference buffer information
+ * @sf_ref_fb : record super frame reference buffer information
  *	(AP-R/W, VPU-R/W)
  * @sf_next_ref_fb_idx : next available super frame (AP-W, VPU-R)
  * @sf_frm_cnt : super frame count, filled by vpu (AP-R, VPU-W)
@@ -133,11 +134,11 @@ struct vp9_sf_ref_fb {
  */
 struct vdec_vp9_vsi {
 	unsigned char sf_bs_buf[VP9_SUPER_FRAME_BS_SZ];
-	struct vp9_sf_ref_fb sf_ref_fb[VP9_MAX_FRM_BUF_NUM-1];
+	struct vp9_sf_ref_fb sf_ref_fb[VP9_MAX_SUPER_FRAMES_NUM];
 	int sf_next_ref_fb_idx;
 	unsigned int sf_frm_cnt;
-	unsigned int sf_frm_offset[VP9_MAX_FRM_BUF_NUM-1];
-	unsigned int sf_frm_sz[VP9_MAX_FRM_BUF_NUM-1];
+	unsigned int sf_frm_offset[VP9_MAX_SUPER_FRAMES_NUM];
+	unsigned int sf_frm_sz[VP9_MAX_SUPER_FRAMES_NUM];
 	unsigned int sf_frm_idx;
 	unsigned int sf_init;
 	struct vdec_fb fb;
@@ -526,7 +527,7 @@ static void vp9_swap_frm_bufs(struct vdec_vp9_inst *inst)
 	/* if this super frame and it is not last sub-frame, get next fb for
 	 * sub-frame decode
 	 */
-	if (vsi->sf_frm_cnt > 0 && vsi->sf_frm_idx != vsi->sf_frm_cnt - 1)
+	if (vsi->sf_frm_cnt > 0 && vsi->sf_frm_idx != vsi->sf_frm_cnt)
 		vsi->sf_next_ref_fb_idx = vp9_get_sf_ref_fb(inst);
 }
 
@@ -735,7 +736,7 @@ static void get_free_fb(struct vdec_vp9_inst *inst, struct vdec_fb **out_fb)
 
 static int validate_vsi_array_indexes(struct vdec_vp9_inst *inst,
 		struct vdec_vp9_vsi *vsi) {
-	if (vsi->sf_frm_idx >= VP9_MAX_FRM_BUF_NUM - 1) {
+	if (vsi->sf_frm_idx > VP9_MAX_SUPER_FRAMES_NUM) {
 		mtk_vdec_err(inst->ctx, "Invalid vsi->sf_frm_idx=%u.", vsi->sf_frm_idx);
 		return -EIO;
 	}
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_req_lat_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_req_lat_if.c
index cf48d09b78d7..47c302745c1d 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_req_lat_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec/vdec_vp9_req_lat_if.c
@@ -1074,7 +1074,7 @@ static int vdec_vp9_slice_setup_tile_buffer(struct vdec_vp9_slice_instance *inst
 	unsigned int mi_row;
 	unsigned int mi_col;
 	unsigned int offset;
-	unsigned int pa;
+	dma_addr_t pa;
 	unsigned int size;
 	struct vdec_vp9_slice_tiles *tiles;
 	unsigned char *pos;
@@ -1109,7 +1109,7 @@ static int vdec_vp9_slice_setup_tile_buffer(struct vdec_vp9_slice_instance *inst
 	pos = va + offset;
 	end = va + bs->size;
 	/* truncated */
-	pa = (unsigned int)bs->dma_addr + offset;
+	pa = bs->dma_addr + offset;
 	tb = instance->tile.va;
 	for (i = 0; i < rows; i++) {
 		for (j = 0; j < cols; j++) {
@@ -1188,7 +1188,8 @@ err:
 	return ret;
 }
 
-static
+/* clang stack usage explodes if this is inlined */
+static noinline_for_stack
 void vdec_vp9_slice_map_counts_eob_coef(unsigned int i, unsigned int j, unsigned int k,
 					struct vdec_vp9_slice_frame_counts *counts,
 					struct v4l2_vp9_frame_symbol_counts *counts_helper)
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec_msg_queue.h b/drivers/media/platform/mediatek/vcodec/decoder/vdec_msg_queue.h
index 1d9beb9e4a14..b0f576867f4b 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec_msg_queue.h
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec_msg_queue.h
@@ -158,14 +158,14 @@ int vdec_msg_queue_qbuf(struct vdec_msg_queue_ctx *ctx, struct vdec_lat_buf *buf
 struct vdec_lat_buf *vdec_msg_queue_dqbuf(struct vdec_msg_queue_ctx *ctx);
 
 /**
- * vdec_msg_queue_update_ube_rptr - used to updata the ube read point.
+ * vdec_msg_queue_update_ube_rptr - used to update the ube read point.
  * @msg_queue: used to store the lat buffer information
  * @ube_rptr: current ube read point
  */
 void vdec_msg_queue_update_ube_rptr(struct vdec_msg_queue *msg_queue, uint64_t ube_rptr);
 
 /**
- * vdec_msg_queue_update_ube_wptr - used to updata the ube write point.
+ * vdec_msg_queue_update_ube_wptr - used to update the ube write point.
  * @msg_queue: used to store the lat buffer information
  * @ube_wptr: current ube write point
  */
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.c b/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.c
index 82e57ae983d5..145958206e38 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.c
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.c
@@ -77,12 +77,14 @@ static bool vpu_dec_check_ap_inst(struct mtk_vcodec_dec_dev *dec_dev, struct vde
 	struct mtk_vcodec_dec_ctx *ctx;
 	int ret = false;
 
+	mutex_lock(&dec_dev->dev_ctx_lock);
 	list_for_each_entry(ctx, &dec_dev->ctx_list, list) {
 		if (!IS_ERR_OR_NULL(ctx) && ctx->vpu_inst == vpu) {
 			ret = true;
 			break;
 		}
 	}
+	mutex_unlock(&dec_dev->dev_ctx_lock);
 
 	return ret;
 }
@@ -231,6 +233,12 @@ int vpu_dec_init(struct vdec_vpu_inst *vpu)
 	mtk_vdec_debug(vpu->ctx, "vdec_inst=%p", vpu);
 
 	err = vcodec_vpu_send_msg(vpu, (void *)&msg, sizeof(msg));
+
+	if (IS_ERR_OR_NULL(vpu->vsi)) {
+		mtk_vdec_err(vpu->ctx, "invalid vdec vsi, status=%d", err);
+		return -EINVAL;
+	}
+
 	mtk_vdec_debug(vpu->ctx, "- ret=%d", err);
 	return err;
 }
diff --git a/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.h b/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.h
index aa7d08afc2f4..57ed9b1f5eaa 100644
--- a/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.h
+++ b/drivers/media/platform/mediatek/vcodec/decoder/vdec_vpu_if.h
@@ -81,7 +81,7 @@ int vpu_dec_deinit(struct vdec_vpu_inst *vpu);
 
 /**
  * vpu_dec_reset - reset decoder, use for flush decoder when end of stream or
- *                 seek. Remainig non displayed frame will be pushed to display.
+ *                 seek. Remaining non displayed frame will be pushed to display.
  *
  * @vpu: instance for vdec_vpu_inst
  */
@@ -98,7 +98,7 @@ int vpu_dec_core(struct vdec_vpu_inst *vpu);
 /**
  * vpu_dec_core_end - core end decoding, basically the function will be invoked once
  *               when core HW decoding done and receive interrupt successfully. The
- *               decoder in VPU will updata hardware information and deinit hardware
+ *               decoder in VPU will update hardware information and deinit hardware
  *               and check if there is a new decoded frame available to display.
  *
  * @vpu : instance for vdec_vpu_inst
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc.c b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc.c
index 181884e798fd..a01dc25a7699 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc.c
@@ -311,7 +311,7 @@ static int vidioc_try_fmt_out(struct mtk_vcodec_enc_ctx *ctx, struct v4l2_format
 	pix_fmt_mp->height = clamp(pix_fmt_mp->height, MTK_VENC_MIN_H, max_height);
 	pix_fmt_mp->width = clamp(pix_fmt_mp->width, MTK_VENC_MIN_W, max_width);
 
-	/* find next closer width align 16, heign align 32, size align
+	/* find next closer width align 16, height align 32, size align
 	 * 64 rectangle
 	 */
 	tmp_w = pix_fmt_mp->width;
@@ -1009,8 +1009,6 @@ static const struct vb2_ops mtk_venc_vb2_ops = {
 	.buf_out_validate	= vb2ops_venc_buf_out_validate,
 	.buf_prepare		= vb2ops_venc_buf_prepare,
 	.buf_queue		= vb2ops_venc_buf_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= vb2ops_venc_start_streaming,
 	.stop_streaming		= vb2ops_venc_stop_streaming,
 };
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.c b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.c
index 6319f24bc714..a1e4483abcdb 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.c
@@ -177,7 +177,9 @@ static int fops_vcodec_open(struct file *file)
 	mtk_v4l2_venc_dbg(2, ctx, "Create instance [%d]@%p m2m_ctx=%p ",
 			  ctx->id, ctx, ctx->m2m_ctx);
 
+	mutex_lock(&dev->dev_ctx_lock);
 	list_add(&ctx->list, &dev->ctx_list);
+	mutex_unlock(&dev->dev_ctx_lock);
 
 	mutex_unlock(&dev->dev_mutex);
 	mtk_v4l2_venc_dbg(0, ctx, "%s encoder [%d]", dev_name(&dev->plat_dev->dev),
@@ -212,7 +214,9 @@ static int fops_vcodec_release(struct file *file)
 	v4l2_fh_exit(&ctx->fh);
 	v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
 
+	mutex_lock(&dev->dev_ctx_lock);
 	list_del_init(&ctx->list);
+	mutex_unlock(&dev->dev_ctx_lock);
 	kfree(ctx);
 	mutex_unlock(&dev->dev_mutex);
 	return 0;
@@ -294,6 +298,7 @@ static int mtk_vcodec_probe(struct platform_device *pdev)
 
 	mutex_init(&dev->enc_mutex);
 	mutex_init(&dev->dev_mutex);
+	mutex_init(&dev->dev_ctx_lock);
 	spin_lock_init(&dev->irqlock);
 
 	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name), "%s",
@@ -468,7 +473,7 @@ static void mtk_vcodec_enc_remove(struct platform_device *pdev)
 
 static struct platform_driver mtk_vcodec_enc_driver = {
 	.probe	= mtk_vcodec_probe,
-	.remove_new = mtk_vcodec_enc_remove,
+	.remove = mtk_vcodec_enc_remove,
 	.driver	= {
 		.name	= MTK_VCODEC_ENC_NAME,
 		.of_match_table = mtk_vcodec_enc_match,
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.h b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.h
index a042f607ed8d..0bd85d0fb379 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.h
+++ b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_drv.h
@@ -178,6 +178,7 @@ struct mtk_vcodec_enc_ctx {
  *
  * @enc_mutex: encoder hardware lock.
  * @dev_mutex: video_device lock
+ * @dev_ctx_lock: the lock of context list
  * @encode_workqueue: encode work queue
  *
  * @enc_irq: h264 encoder irq resource
@@ -205,6 +206,7 @@ struct mtk_vcodec_enc_dev {
 	/* encoder hardware mutex lock */
 	struct mutex enc_mutex;
 	struct mutex dev_mutex;
+	struct mutex dev_ctx_lock;
 	struct workqueue_struct *encode_workqueue;
 
 	int enc_irq;
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.c b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.c
index a22b7dfc656e..1a2b14a3e219 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.c
@@ -58,13 +58,15 @@ int mtk_vcodec_init_enc_clk(struct mtk_vcodec_enc_dev *mtkdev)
 	return 0;
 }
 
-void mtk_vcodec_enc_pw_on(struct mtk_vcodec_pm *pm)
+int mtk_vcodec_enc_pw_on(struct mtk_vcodec_pm *pm)
 {
 	int ret;
 
 	ret = pm_runtime_resume_and_get(pm->dev);
 	if (ret)
 		dev_err(pm->dev, "pm_runtime_resume_and_get fail: %d", ret);
+
+	return ret;
 }
 
 void mtk_vcodec_enc_pw_off(struct mtk_vcodec_pm *pm)
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.h b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.h
index 157ea08ba9e3..2e28f25e36cc 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.h
+++ b/drivers/media/platform/mediatek/vcodec/encoder/mtk_vcodec_enc_pm.h
@@ -10,7 +10,7 @@
 #include "mtk_vcodec_enc_drv.h"
 
 int mtk_vcodec_init_enc_clk(struct mtk_vcodec_enc_dev *dev);
-void mtk_vcodec_enc_pw_on(struct mtk_vcodec_pm *pm);
+int mtk_vcodec_enc_pw_on(struct mtk_vcodec_pm *pm);
 void mtk_vcodec_enc_pw_off(struct mtk_vcodec_pm *pm);
 void mtk_vcodec_enc_clock_on(struct mtk_vcodec_pm *pm);
 void mtk_vcodec_enc_clock_off(struct mtk_vcodec_pm *pm);
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/venc/venc_h264_if.c b/drivers/media/platform/mediatek/vcodec/encoder/venc/venc_h264_if.c
index a68dac72c4e4..0f63657d8bad 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/venc/venc_h264_if.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/venc/venc_h264_if.c
@@ -301,11 +301,12 @@ static void h264_enc_free_work_buf(struct venc_h264_inst *inst)
 	 * other buffers need to be freed by AP.
 	 */
 	for (i = 0; i < VENC_H264_VPU_WORK_BUF_MAX; i++) {
-		if (i != VENC_H264_VPU_WORK_BUF_SKIP_FRAME)
+		if (i != VENC_H264_VPU_WORK_BUF_SKIP_FRAME && inst->work_bufs[i].va)
 			mtk_vcodec_mem_free(inst->ctx, &inst->work_bufs[i]);
 	}
 
-	mtk_vcodec_mem_free(inst->ctx, &inst->pps_buf);
+	if (inst->pps_buf.va)
+		mtk_vcodec_mem_free(inst->ctx, &inst->pps_buf);
 }
 
 static int h264_enc_alloc_work_buf(struct venc_h264_inst *inst, bool is_34bit)
@@ -514,7 +515,7 @@ static int h264_encode_frame(struct venc_h264_inst *inst,
 	struct venc_frame_info frame_info;
 	struct mtk_vcodec_enc_ctx *ctx = inst->ctx;
 
-	mtk_venc_debug(ctx, "frm_cnt = %d\n ", inst->frm_cnt);
+	mtk_venc_debug(ctx, "frm_cnt = %d\n", inst->frm_cnt);
 
 	if (MTK_ENC_IOVA_IS_34BIT(ctx)) {
 		gop_size = inst->vsi_34->config.gop_size;
@@ -593,7 +594,11 @@ static int h264_enc_init(struct mtk_vcodec_enc_ctx *ctx)
 
 	inst->ctx = ctx;
 	inst->vpu_inst.ctx = ctx;
-	inst->vpu_inst.id = is_ext ? SCP_IPI_VENC_H264 : IPI_VENC_H264;
+	if (is_ext)
+		inst->vpu_inst.id = SCP_IPI_VENC_H264;
+	else
+		inst->vpu_inst.id = IPI_VENC_H264;
+
 	inst->hw_base = mtk_vcodec_get_reg_addr(inst->ctx->dev->reg_base, VENC_SYS);
 
 	ret = vpu_enc_init(&inst->vpu_inst);
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.c b/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.c
index c402a686f3cb..e83747b8d69a 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.c
@@ -64,7 +64,9 @@ int venc_if_encode(struct mtk_vcodec_enc_ctx *ctx,
 	ctx->dev->curr_ctx = ctx;
 	spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
 
-	mtk_vcodec_enc_pw_on(&ctx->dev->pm);
+	ret = mtk_vcodec_enc_pw_on(&ctx->dev->pm);
+	if (ret)
+		goto venc_if_encode_pw_on_err;
 	mtk_vcodec_enc_clock_on(&ctx->dev->pm);
 	ret = ctx->enc_if->encode(ctx->drv_handle, opt, frm_buf,
 				  bs_buf, result);
@@ -75,6 +77,7 @@ int venc_if_encode(struct mtk_vcodec_enc_ctx *ctx,
 	ctx->dev->curr_ctx = NULL;
 	spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
 
+venc_if_encode_pw_on_err:
 	mtk_venc_unlock(ctx);
 	return ret;
 }
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.h b/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.h
index d00fb68b8235..889440a436b6 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.h
+++ b/drivers/media/platform/mediatek/vcodec/encoder/venc_drv_if.h
@@ -156,7 +156,7 @@ int venc_if_set_param(struct mtk_vcodec_enc_ctx *ctx,
  * @ctx: device context
  * @opt: encode frame option
  * @frm_buf: input frame buffer information
- * @bs_buf: output bitstream buffer infomraiton
+ * @bs_buf: output bitstream buffer information
  * @result: encode result
  * Return: 0 if encoding frame successfully, otherwise it is failed.
  */
diff --git a/drivers/media/platform/mediatek/vcodec/encoder/venc_vpu_if.c b/drivers/media/platform/mediatek/vcodec/encoder/venc_vpu_if.c
index 84ad1cc6ad17..51bb7ee141b9 100644
--- a/drivers/media/platform/mediatek/vcodec/encoder/venc_vpu_if.c
+++ b/drivers/media/platform/mediatek/vcodec/encoder/venc_vpu_if.c
@@ -47,12 +47,14 @@ static bool vpu_enc_check_ap_inst(struct mtk_vcodec_enc_dev *enc_dev, struct ven
 	struct mtk_vcodec_enc_ctx *ctx;
 	int ret = false;
 
+	mutex_lock(&enc_dev->dev_ctx_lock);
 	list_for_each_entry(ctx, &enc_dev->ctx_list, list) {
 		if (!IS_ERR_OR_NULL(ctx) && ctx->vpu_inst == vpu) {
 			ret = true;
 			break;
 		}
 	}
+	mutex_unlock(&enc_dev->dev_ctx_lock);
 
 	return ret;
 }
diff --git a/drivers/media/platform/mediatek/vpu/mtk_vpu.c b/drivers/media/platform/mediatek/vpu/mtk_vpu.c
index 724ae7c2ab3b..8d8319f0cd22 100644
--- a/drivers/media/platform/mediatek/vpu/mtk_vpu.c
+++ b/drivers/media/platform/mediatek/vpu/mtk_vpu.c
@@ -1041,7 +1041,7 @@ static const struct dev_pm_ops mtk_vpu_pm = {
 
 static struct platform_driver mtk_vpu_driver = {
 	.probe	= mtk_vpu_probe,
-	.remove_new = mtk_vpu_remove,
+	.remove = mtk_vpu_remove,
 	.driver	= {
 		.name	= "mtk_vpu",
 		.pm = &mtk_vpu_pm,
diff --git a/drivers/media/platform/microchip/microchip-csi2dc.c b/drivers/media/platform/microchip/microchip-csi2dc.c
index fee73260bb1e..70303a0b6919 100644
--- a/drivers/media/platform/microchip/microchip-csi2dc.c
+++ b/drivers/media/platform/microchip/microchip-csi2dc.c
@@ -782,7 +782,7 @@ MODULE_DEVICE_TABLE(of, csi2dc_of_match);
 
 static struct platform_driver csi2dc_driver = {
 	.probe	= csi2dc_probe,
-	.remove_new = csi2dc_remove,
+	.remove = csi2dc_remove,
 	.driver = {
 		.name =			"microchip-csi2dc",
 		.pm =			&csi2dc_dev_pm_ops,
diff --git a/drivers/media/platform/microchip/microchip-isc-base.c b/drivers/media/platform/microchip/microchip-isc-base.c
index f3a5cbacadbe..a7cdc743fda7 100644
--- a/drivers/media/platform/microchip/microchip-isc-base.c
+++ b/drivers/media/platform/microchip/microchip-isc-base.c
@@ -465,8 +465,6 @@ static void isc_buffer_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops isc_vb2_ops = {
 	.queue_setup		= isc_queue_setup,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.buf_prepare		= isc_buffer_prepare,
 	.start_streaming	= isc_start_streaming,
 	.stop_streaming		= isc_stop_streaming,
@@ -902,8 +900,11 @@ static int isc_set_fmt(struct isc_device *isc, struct v4l2_format *f)
 	return 0;
 }
 
-static int isc_validate(struct isc_device *isc)
+static int isc_link_validate(struct media_link *link)
 {
+	struct video_device *vdev =
+		media_entity_to_video_device(link->sink->entity);
+	struct isc_device *isc = video_get_drvdata(vdev);
 	int ret;
 	int i;
 	struct isc_format *sd_fmt = NULL;
@@ -1906,20 +1907,6 @@ int microchip_isc_pipeline_init(struct isc_device *isc)
 }
 EXPORT_SYMBOL_GPL(microchip_isc_pipeline_init);
 
-static int isc_link_validate(struct media_link *link)
-{
-	struct video_device *vdev =
-		media_entity_to_video_device(link->sink->entity);
-	struct isc_device *isc = video_get_drvdata(vdev);
-	int ret;
-
-	ret = v4l2_subdev_link_validate(link);
-	if (ret)
-		return ret;
-
-	return isc_validate(isc);
-}
-
 static const struct media_entity_operations isc_entity_operations = {
 	.link_validate = isc_link_validate,
 };
diff --git a/drivers/media/platform/microchip/microchip-sama5d2-isc.c b/drivers/media/platform/microchip/microchip-sama5d2-isc.c
index 5ac149cf3647..66d3d7891991 100644
--- a/drivers/media/platform/microchip/microchip-sama5d2-isc.c
+++ b/drivers/media/platform/microchip/microchip-sama5d2-isc.c
@@ -353,33 +353,29 @@ static const u32 isc_sama5d2_gamma_table[][GAMMA_ENTRIES] = {
 static int isc_parse_dt(struct device *dev, struct isc_device *isc)
 {
 	struct device_node *np = dev->of_node;
-	struct device_node *epn = NULL;
+	struct device_node *epn;
 	struct isc_subdev_entity *subdev_entity;
 	unsigned int flags;
-	int ret;
 
 	INIT_LIST_HEAD(&isc->subdev_entities);
 
-	while (1) {
+	for_each_endpoint_of_node(np, epn) {
 		struct v4l2_fwnode_endpoint v4l2_epn = { .bus_type = 0 };
-
-		epn = of_graph_get_next_endpoint(np, epn);
-		if (!epn)
-			return 0;
+		int ret;
 
 		ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(epn),
 						 &v4l2_epn);
 		if (ret) {
-			ret = -EINVAL;
+			of_node_put(epn);
 			dev_err(dev, "Could not parse the endpoint\n");
-			break;
+			return -EINVAL;
 		}
 
 		subdev_entity = devm_kzalloc(dev, sizeof(*subdev_entity),
 					     GFP_KERNEL);
 		if (!subdev_entity) {
-			ret = -ENOMEM;
-			break;
+			of_node_put(epn);
+			return -ENOMEM;
 		}
 		subdev_entity->epn = epn;
 
@@ -400,9 +396,8 @@ static int isc_parse_dt(struct device *dev, struct isc_device *isc)
 
 		list_add_tail(&subdev_entity->list, &isc->subdev_entities);
 	}
-	of_node_put(epn);
 
-	return ret;
+	return 0;
 }
 
 static int microchip_isc_probe(struct platform_device *pdev)
@@ -663,7 +658,7 @@ MODULE_DEVICE_TABLE(of, microchip_isc_of_match);
 
 static struct platform_driver microchip_isc_driver = {
 	.probe	= microchip_isc_probe,
-	.remove_new = microchip_isc_remove,
+	.remove = microchip_isc_remove,
 	.driver	= {
 		.name		= "microchip-sama5d2-isc",
 		.pm		= &microchip_isc_dev_pm_ops,
diff --git a/drivers/media/platform/microchip/microchip-sama7g5-isc.c b/drivers/media/platform/microchip/microchip-sama7g5-isc.c
index 73445f33d26b..b0302dfc3278 100644
--- a/drivers/media/platform/microchip/microchip-sama7g5-isc.c
+++ b/drivers/media/platform/microchip/microchip-sama7g5-isc.c
@@ -336,36 +336,32 @@ static const u32 isc_sama7g5_gamma_table[][GAMMA_ENTRIES] = {
 static int xisc_parse_dt(struct device *dev, struct isc_device *isc)
 {
 	struct device_node *np = dev->of_node;
-	struct device_node *epn = NULL;
+	struct device_node *epn;
 	struct isc_subdev_entity *subdev_entity;
 	unsigned int flags;
-	int ret;
 	bool mipi_mode;
 
 	INIT_LIST_HEAD(&isc->subdev_entities);
 
 	mipi_mode = of_property_read_bool(np, "microchip,mipi-mode");
 
-	while (1) {
+	for_each_endpoint_of_node(np, epn) {
 		struct v4l2_fwnode_endpoint v4l2_epn = { .bus_type = 0 };
-
-		epn = of_graph_get_next_endpoint(np, epn);
-		if (!epn)
-			return 0;
+		int ret;
 
 		ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(epn),
 						 &v4l2_epn);
 		if (ret) {
-			ret = -EINVAL;
+			of_node_put(epn);
 			dev_err(dev, "Could not parse the endpoint\n");
-			break;
+			return -EINVAL;
 		}
 
 		subdev_entity = devm_kzalloc(dev, sizeof(*subdev_entity),
 					     GFP_KERNEL);
 		if (!subdev_entity) {
-			ret = -ENOMEM;
-			break;
+			of_node_put(epn);
+			return -ENOMEM;
 		}
 		subdev_entity->epn = epn;
 
@@ -389,9 +385,8 @@ static int xisc_parse_dt(struct device *dev, struct isc_device *isc)
 
 		list_add_tail(&subdev_entity->list, &isc->subdev_entities);
 	}
-	of_node_put(epn);
 
-	return ret;
+	return 0;
 }
 
 static int microchip_xisc_probe(struct platform_device *pdev)
@@ -626,7 +621,7 @@ MODULE_DEVICE_TABLE(of, microchip_xisc_of_match);
 
 static struct platform_driver microchip_xisc_driver = {
 	.probe	= microchip_xisc_probe,
-	.remove_new = microchip_xisc_remove,
+	.remove = microchip_xisc_remove,
 	.driver	= {
 		.name		= "microchip-sama7g5-xisc",
 		.pm		= &microchip_xisc_dev_pm_ops,
diff --git a/drivers/media/platform/nuvoton/npcm-video.c b/drivers/media/platform/nuvoton/npcm-video.c
index 60fbb9140035..44e904e61801 100644
--- a/drivers/media/platform/nuvoton/npcm-video.c
+++ b/drivers/media/platform/nuvoton/npcm-video.c
@@ -578,7 +578,7 @@ static unsigned int npcm_video_hres(struct npcm_video *video)
 	regmap_read(gfxi, HVCNTL, &hvcntl);
 	apb_hor_res = (((hvcnth & HVCNTH_MASK) << 8) + (hvcntl & HVCNTL_MASK) + 1);
 
-	return apb_hor_res;
+	return (apb_hor_res > MAX_WIDTH) ? MAX_WIDTH : apb_hor_res;
 }
 
 static unsigned int npcm_video_vres(struct npcm_video *video)
@@ -591,7 +591,7 @@ static unsigned int npcm_video_vres(struct npcm_video *video)
 
 	apb_ver_res = (((vvcnth & VVCNTH_MASK) << 8) + (vvcntl & VVCNTL_MASK));
 
-	return apb_ver_res;
+	return (apb_ver_res > MAX_HEIGHT) ? MAX_HEIGHT : apb_ver_res;
 }
 
 static int npcm_video_capres(struct npcm_video *video, unsigned int hor_res,
@@ -863,7 +863,6 @@ static void npcm_video_detect_resolution(struct npcm_video *video)
 	struct regmap *gfxi = video->gfx_regmap;
 	unsigned int dispst;
 
-	video->v4l2_input_status = V4L2_IN_ST_NO_SIGNAL;
 	det->width = npcm_video_hres(video);
 	det->height = npcm_video_vres(video);
 
@@ -892,12 +891,16 @@ static void npcm_video_detect_resolution(struct npcm_video *video)
 		clear_bit(VIDEO_RES_CHANGING, &video->flags);
 	}
 
-	if (det->width && det->height)
+	if (det->width && det->height) {
 		video->v4l2_input_status = 0;
-
-	dev_dbg(video->dev, "Got resolution[%dx%d] -> [%dx%d], status %d\n",
-		act->width, act->height, det->width, det->height,
-		video->v4l2_input_status);
+		dev_dbg(video->dev, "Got resolution[%dx%d] -> [%dx%d], status %d\n",
+			act->width, act->height, det->width, det->height,
+			video->v4l2_input_status);
+	} else {
+		video->v4l2_input_status = V4L2_IN_ST_NO_SIGNAL;
+		dev_err(video->dev, "Got invalid resolution[%dx%d]\n", det->width,
+			det->height);
+	}
 }
 
 static int npcm_video_set_resolution(struct npcm_video *video,
@@ -1558,8 +1561,6 @@ static const struct regmap_config npcm_video_ece_regmap_cfg = {
 
 static const struct vb2_ops npcm_video_vb2_ops = {
 	.queue_setup = npcm_video_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.buf_prepare = npcm_video_buf_prepare,
 	.buf_finish = npcm_video_buf_finish,
 	.start_streaming = npcm_video_start_streaming,
@@ -1650,8 +1651,8 @@ rel_ctrl_handler:
 
 static int npcm_video_ece_init(struct npcm_video *video)
 {
+	struct device_node *ece_node __free(device_node) = NULL;
 	struct device *dev = video->dev;
-	struct device_node *ece_node;
 	struct platform_device *ece_pdev;
 	void __iomem *regs;
 
@@ -1667,11 +1668,11 @@ static int npcm_video_ece_init(struct npcm_video *video)
 		dev_info(dev, "Support HEXTILE pixel format\n");
 
 		ece_pdev = of_find_device_by_node(ece_node);
-		if (IS_ERR(ece_pdev)) {
+		if (!ece_pdev) {
 			dev_err(dev, "Failed to find ECE device\n");
-			return PTR_ERR(ece_pdev);
+			return -ENODEV;
 		}
-		of_node_put(ece_node);
+		struct device *ece_dev __free(put_device) = &ece_pdev->dev;
 
 		regs = devm_platform_ioremap_resource(ece_pdev, 0);
 		if (IS_ERR(regs)) {
@@ -1686,7 +1687,7 @@ static int npcm_video_ece_init(struct npcm_video *video)
 			return PTR_ERR(video->ece.regmap);
 		}
 
-		video->ece.reset = devm_reset_control_get(&ece_pdev->dev, NULL);
+		video->ece.reset = devm_reset_control_get(ece_dev, NULL);
 		if (IS_ERR(video->ece.reset)) {
 			dev_err(dev, "Failed to get ECE reset control in DTS\n");
 			return PTR_ERR(video->ece.reset);
@@ -1814,7 +1815,7 @@ static struct platform_driver npcm_video_driver = {
 		.of_match_table = npcm_video_match,
 	},
 	.probe = npcm_video_probe,
-	.remove_new = npcm_video_remove,
+	.remove = npcm_video_remove,
 };
 
 module_platform_driver(npcm_video_driver);
diff --git a/drivers/media/platform/nvidia/tegra-vde/dmabuf-cache.c b/drivers/media/platform/nvidia/tegra-vde/dmabuf-cache.c
index 1c5b94989aec..b34244ea14dd 100644
--- a/drivers/media/platform/nvidia/tegra-vde/dmabuf-cache.c
+++ b/drivers/media/platform/nvidia/tegra-vde/dmabuf-cache.c
@@ -16,7 +16,7 @@
 
 #include "vde.h"
 
-MODULE_IMPORT_NS(DMA_BUF);
+MODULE_IMPORT_NS("DMA_BUF");
 
 struct tegra_vde_cache_entry {
 	enum dma_data_direction dma_dir;
diff --git a/drivers/media/platform/nvidia/tegra-vde/h264.c b/drivers/media/platform/nvidia/tegra-vde/h264.c
index 204e474d57f7..0e56a4331b0d 100644
--- a/drivers/media/platform/nvidia/tegra-vde/h264.c
+++ b/drivers/media/platform/nvidia/tegra-vde/h264.c
@@ -19,11 +19,6 @@
 #define FLAG_B_FRAME		0x1
 #define FLAG_REFERENCE		0x2
 
-struct tegra_vde_h264_frame {
-	unsigned int frame_num;
-	unsigned int flags;
-};
-
 struct tegra_vde_h264_decoder_ctx {
 	unsigned int dpb_frames_nb;
 	unsigned int dpb_ref_frames_with_earlier_poc_nb;
@@ -628,12 +623,14 @@ static int tegra_vde_decode_end(struct tegra_vde *vde)
 	unsigned int read_bytes, macroblocks_nb;
 	struct device *dev = vde->dev;
 	dma_addr_t bsev_ptr;
-	long timeout;
+	long time_left;
 	int ret;
 
-	timeout = wait_for_completion_interruptible_timeout(
+	time_left = wait_for_completion_interruptible_timeout(
 			&vde->decode_completion, msecs_to_jiffies(1000));
-	if (timeout == 0) {
+	if (time_left < 0) {
+		ret = time_left;
+	} else if (time_left == 0) {
 		bsev_ptr = tegra_vde_readl(vde, vde->bsev, 0x10);
 		macroblocks_nb = tegra_vde_readl(vde, vde->sxe, 0xC8) & 0x1FFF;
 		read_bytes = bsev_ptr ? bsev_ptr - vde->bitstream_data_addr : 0;
@@ -642,8 +639,6 @@ static int tegra_vde_decode_end(struct tegra_vde *vde)
 			read_bytes, macroblocks_nb);
 
 		ret = -EIO;
-	} else if (timeout < 0) {
-		ret = timeout;
 	} else {
 		ret = 0;
 	}
diff --git a/drivers/media/platform/nvidia/tegra-vde/iommu.c b/drivers/media/platform/nvidia/tegra-vde/iommu.c
index 5521ed3e465f..b1d9d841d944 100644
--- a/drivers/media/platform/nvidia/tegra-vde/iommu.c
+++ b/drivers/media/platform/nvidia/tegra-vde/iommu.c
@@ -78,9 +78,10 @@ int tegra_vde_iommu_init(struct tegra_vde *vde)
 		arm_iommu_release_mapping(mapping);
 	}
 #endif
-	vde->domain = iommu_domain_alloc(&platform_bus_type);
-	if (!vde->domain) {
-		err = -ENOMEM;
+	vde->domain = iommu_paging_domain_alloc(dev);
+	if (IS_ERR(vde->domain)) {
+		err = PTR_ERR(vde->domain);
+		vde->domain = NULL;
 		goto put_group;
 	}
 
diff --git a/drivers/media/platform/nvidia/tegra-vde/trace.h b/drivers/media/platform/nvidia/tegra-vde/trace.h
index 7853ab095ca4..e8a75a7bd05d 100644
--- a/drivers/media/platform/nvidia/tegra-vde/trace.h
+++ b/drivers/media/platform/nvidia/tegra-vde/trace.h
@@ -20,7 +20,7 @@ DECLARE_EVENT_CLASS(register_access,
 		__field(u32, value)
 	),
 	TP_fast_assign(
-		__assign_str(hw_name, tegra_vde_reg_base_name(vde, base));
+		__assign_str(hw_name);
 		__entry->offset = offset;
 		__entry->value = value;
 	),
diff --git a/drivers/media/platform/nvidia/tegra-vde/v4l2.c b/drivers/media/platform/nvidia/tegra-vde/v4l2.c
index 0f48ce6f243e..e3726cab0c82 100644
--- a/drivers/media/platform/nvidia/tegra-vde/v4l2.c
+++ b/drivers/media/platform/nvidia/tegra-vde/v4l2.c
@@ -328,8 +328,6 @@ static const struct vb2_ops tegra_qops = {
 	.buf_request_complete = tegra_buf_request_complete,
 	.start_streaming = tegra_start_streaming,
 	.stop_streaming = tegra_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int tegra_queue_init(void *priv,
@@ -929,13 +927,13 @@ int tegra_vde_v4l2_init(struct tegra_vde *vde)
 	media_device_init(&vde->mdev);
 	video_set_drvdata(&vde->vdev, vde);
 
-	vde->vdev.lock = &vde->v4l2_lock,
-	vde->vdev.fops = &tegra_v4l2_fops,
-	vde->vdev.vfl_dir = VFL_DIR_M2M,
-	vde->vdev.release = video_device_release_empty,
+	vde->vdev.lock = &vde->v4l2_lock;
+	vde->vdev.fops = &tegra_v4l2_fops;
+	vde->vdev.vfl_dir = VFL_DIR_M2M;
+	vde->vdev.release = video_device_release_empty;
 	vde->vdev.v4l2_dev = &vde->v4l2_dev;
-	vde->vdev.ioctl_ops = &tegra_v4l2_ioctl_ops,
-	vde->vdev.device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING,
+	vde->vdev.ioctl_ops = &tegra_v4l2_ioctl_ops;
+	vde->vdev.device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
 
 	vde->v4l2_dev.mdev = &vde->mdev;
 	vde->mdev.ops = &tegra_media_device_ops;
diff --git a/drivers/media/platform/nvidia/tegra-vde/vde.c b/drivers/media/platform/nvidia/tegra-vde/vde.c
index 81a0d3b76b88..3232392c60e2 100644
--- a/drivers/media/platform/nvidia/tegra-vde/vde.c
+++ b/drivers/media/platform/nvidia/tegra-vde/vde.c
@@ -535,7 +535,7 @@ MODULE_DEVICE_TABLE(of, tegra_vde_of_match);
 
 static struct platform_driver tegra_vde_driver = {
 	.probe		= tegra_vde_probe,
-	.remove_new	= tegra_vde_remove,
+	.remove		= tegra_vde_remove,
 	.shutdown	= tegra_vde_shutdown,
 	.driver		= {
 		.name		= "tegra-vde",
diff --git a/drivers/media/platform/nvidia/tegra-vde/vde.h b/drivers/media/platform/nvidia/tegra-vde/vde.h
index 0fbb1f3d2c88..b2890484b7c3 100644
--- a/drivers/media/platform/nvidia/tegra-vde/vde.h
+++ b/drivers/media/platform/nvidia/tegra-vde/vde.h
@@ -47,7 +47,6 @@ struct iommu_group;
 struct iommu_domain;
 struct reset_control;
 struct dma_buf_attachment;
-struct tegra_vde_h264_frame;
 struct tegra_vde_h264_decoder_ctx;
 
 struct tegra_video_frame {
diff --git a/drivers/media/platform/nxp/dw100/dw100.c b/drivers/media/platform/nxp/dw100/dw100.c
index 0024d6175ad9..3d1db1121bf9 100644
--- a/drivers/media/platform/nxp/dw100/dw100.c
+++ b/drivers/media/platform/nxp/dw100/dw100.c
@@ -558,8 +558,6 @@ static const struct vb2_ops dw100_qops = {
 	.buf_queue	 = dw100_buf_queue,
 	.start_streaming = dw100_start_streaming,
 	.stop_streaming  = dw100_stop_streaming,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int dw100_m2m_queue_init(void *priv, struct vb2_queue *src_vq,
@@ -963,9 +961,9 @@ static int dw100_s_selection(struct file *file, void *fh,
 	src_q_data = dw100_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
 
 	dev_dbg(&ctx->dw_dev->pdev->dev,
-		">>> Buffer Type: %u Target: %u Rect: %ux%u@%d.%d\n",
+		">>> Buffer Type: %u Target: %u Rect: (%d,%d)/%ux%u\n",
 		sel->type, sel->target,
-		sel->r.width, sel->r.height, sel->r.left, sel->r.top);
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height);
 
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP:
@@ -1027,9 +1025,9 @@ static int dw100_s_selection(struct file *file, void *fh,
 	}
 
 	dev_dbg(&ctx->dw_dev->pdev->dev,
-		"<<< Buffer Type: %u Target: %u Rect: %ux%u@%d.%d\n",
+		"<<< Buffer Type: %u Target: %u Rect: (%d,%d)/%ux%u\n",
 		sel->type, sel->target,
-		sel->r.width, sel->r.height, sel->r.left, sel->r.top);
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height);
 
 	return 0;
 }
@@ -1311,7 +1309,7 @@ static void dw100_hw_set_destination(struct dw100_device *dw_dev,
 	}
 
 	dev_dbg(&dw_dev->pdev->dev,
-		"Set HW source registers for %ux%u - stride %u, pixfmt: %p4cc, dma:%pad\n",
+		"Set HW destination registers for %ux%u - stride %u, pixfmt: %p4cc, dma:%pad\n",
 		width, height, stride, &fourcc, &addr_y);
 
 	/* Pixel Format */
@@ -1688,7 +1686,7 @@ MODULE_DEVICE_TABLE(of, dw100_dt_ids);
 
 static struct platform_driver dw100_driver = {
 	.probe		= dw100_probe,
-	.remove_new	= dw100_remove,
+	.remove		= dw100_remove,
 	.driver		= {
 		.name	= DRV_NAME,
 		.pm = &dw100_pm,
diff --git a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg-hw.h b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg-hw.h
index d579c804b047..adb93e977be9 100644
--- a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg-hw.h
+++ b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg-hw.h
@@ -89,6 +89,7 @@
 /* SLOT_STATUS fields for slots 0..3 */
 #define SLOT_STATUS_FRMDONE			(0x1 << 3)
 #define SLOT_STATUS_ENC_CONFIG_ERR		(0x1 << 8)
+#define SLOT_STATUS_ONGOING			(0x1 << 31)
 
 /* SLOT_IRQ_EN fields TBD */
 
diff --git a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.c b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.c
index cc97790ed30f..5c17bc58181e 100644
--- a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.c
+++ b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.c
@@ -535,7 +535,9 @@ static const unsigned char jpeg_sos_maximal[] = {
 };
 
 static const unsigned char jpeg_image_red[] = {
-	0xFC, 0x5F, 0xA2, 0xBF, 0xCA, 0x73, 0xFE, 0xFE,
+	0xF9, 0xFE, 0x8A, 0xFC, 0x34, 0xFD, 0xC4, 0x28,
+	0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A,
+	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0,
 	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
 	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
 	0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28,
@@ -543,9 +545,18 @@ static const unsigned char jpeg_image_red[] = {
 	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0,
 	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
 	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
+	0x8A, 0x00, 0x28, 0xA0, 0x0F, 0xFF, 0xD0, 0xF9,
+	0xFE, 0x8A, 0xFC, 0x34, 0xFD, 0xC4, 0x28, 0xA0,
+	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
+	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
 	0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28,
 	0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A,
-	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00
+	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0,
+	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
+	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
+	0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28,
+	0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A,
+	0x00, 0x28, 0xA0, 0x0F
 };
 
 static const unsigned char jpeg_eoi[] = {
@@ -752,6 +763,39 @@ static int mxc_get_free_slot(struct mxc_jpeg_slot_data *slot_data)
 	return -1;
 }
 
+static void mxc_jpeg_free_slot_data(struct mxc_jpeg_dev *jpeg)
+{
+	/* free descriptor for decoding/encoding phase */
+	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
+			  jpeg->slot_data.desc,
+			  jpeg->slot_data.desc_handle);
+	jpeg->slot_data.desc = NULL;
+	jpeg->slot_data.desc_handle = 0;
+
+	/* free descriptor for encoder configuration phase / decoder DHT */
+	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
+			  jpeg->slot_data.cfg_desc,
+			  jpeg->slot_data.cfg_desc_handle);
+	jpeg->slot_data.cfg_desc_handle = 0;
+	jpeg->slot_data.cfg_desc = NULL;
+
+	/* free configuration stream */
+	dma_free_coherent(jpeg->dev, MXC_JPEG_MAX_CFG_STREAM,
+			  jpeg->slot_data.cfg_stream_vaddr,
+			  jpeg->slot_data.cfg_stream_handle);
+	jpeg->slot_data.cfg_stream_vaddr = NULL;
+	jpeg->slot_data.cfg_stream_handle = 0;
+
+	dma_free_coherent(jpeg->dev, jpeg->slot_data.cfg_dec_size,
+			  jpeg->slot_data.cfg_dec_vaddr,
+			  jpeg->slot_data.cfg_dec_daddr);
+	jpeg->slot_data.cfg_dec_size = 0;
+	jpeg->slot_data.cfg_dec_vaddr = NULL;
+	jpeg->slot_data.cfg_dec_daddr = 0;
+
+	jpeg->slot_data.used = false;
+}
+
 static bool mxc_jpeg_alloc_slot_data(struct mxc_jpeg_dev *jpeg)
 {
 	struct mxc_jpeg_desc *desc;
@@ -788,36 +832,25 @@ static bool mxc_jpeg_alloc_slot_data(struct mxc_jpeg_dev *jpeg)
 		goto err;
 	jpeg->slot_data.cfg_stream_vaddr = cfg_stm;
 
+	jpeg->slot_data.cfg_dec_size = MXC_JPEG_PATTERN_WIDTH * MXC_JPEG_PATTERN_HEIGHT * 2;
+	jpeg->slot_data.cfg_dec_vaddr = dma_alloc_coherent(jpeg->dev,
+							   jpeg->slot_data.cfg_dec_size,
+							   &jpeg->slot_data.cfg_dec_daddr,
+							   GFP_ATOMIC);
+	if (!jpeg->slot_data.cfg_dec_vaddr)
+		goto err;
+
 skip_alloc:
 	jpeg->slot_data.used = true;
 
 	return true;
 err:
 	dev_err(jpeg->dev, "Could not allocate descriptors for slot %d", jpeg->slot_data.slot);
+	mxc_jpeg_free_slot_data(jpeg);
 
 	return false;
 }
 
-static void mxc_jpeg_free_slot_data(struct mxc_jpeg_dev *jpeg)
-{
-	/* free descriptor for decoding/encoding phase */
-	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
-			  jpeg->slot_data.desc,
-			  jpeg->slot_data.desc_handle);
-
-	/* free descriptor for encoder configuration phase / decoder DHT */
-	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
-			  jpeg->slot_data.cfg_desc,
-			  jpeg->slot_data.cfg_desc_handle);
-
-	/* free configuration stream */
-	dma_free_coherent(jpeg->dev, MXC_JPEG_MAX_CFG_STREAM,
-			  jpeg->slot_data.cfg_stream_vaddr,
-			  jpeg->slot_data.cfg_stream_handle);
-
-	jpeg->slot_data.used = false;
-}
-
 static void mxc_jpeg_check_and_set_last_buffer(struct mxc_jpeg_ctx *ctx,
 					       struct vb2_v4l2_buffer *src_buf,
 					       struct vb2_v4l2_buffer *dst_buf)
@@ -877,6 +910,34 @@ static u32 mxc_jpeg_get_plane_size(struct mxc_jpeg_q_data *q_data, u32 plane_no)
 	return size;
 }
 
+static bool mxc_dec_is_ongoing(struct mxc_jpeg_ctx *ctx)
+{
+	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
+	u32 curr_desc;
+	u32 slot_status;
+
+	curr_desc = readl(jpeg->base_reg + MXC_SLOT_OFFSET(ctx->slot, SLOT_CUR_DESCPT_PTR));
+	if (curr_desc == jpeg->slot_data.cfg_desc_handle)
+		return true;
+
+	slot_status = readl(jpeg->base_reg + MXC_SLOT_OFFSET(ctx->slot, SLOT_STATUS));
+	if (slot_status & SLOT_STATUS_ONGOING)
+		return true;
+
+	/*
+	 * The curr_desc register is updated when next_descpt_ptr is loaded,
+	 * the ongoing bit of slot_status is set when the 32 bytes descriptor is loaded.
+	 * So there will be a short time interval in between, which may cause fake false.
+	 * Consider read register is quite slow compared with IP read 32byte from memory,
+	 * read twice slot_status can avoid this situation.
+	 */
+	slot_status = readl(jpeg->base_reg + MXC_SLOT_OFFSET(ctx->slot, SLOT_STATUS));
+	if (slot_status & SLOT_STATUS_ONGOING)
+		return true;
+
+	return false;
+}
+
 static irqreturn_t mxc_jpeg_dec_irq(int irq, void *priv)
 {
 	struct mxc_jpeg_dev *jpeg = priv;
@@ -946,7 +1007,8 @@ static irqreturn_t mxc_jpeg_dec_irq(int irq, void *priv)
 		mxc_jpeg_enc_mode_go(dev, reg, mxc_jpeg_is_extended_sequential(q_data->fmt));
 		goto job_unlock;
 	}
-	if (jpeg->mode == MXC_JPEG_DECODE && jpeg_src_buf->dht_needed) {
+	if (jpeg->mode == MXC_JPEG_DECODE && jpeg_src_buf->dht_needed &&
+	    mxc_dec_is_ongoing(ctx)) {
 		jpeg_src_buf->dht_needed = false;
 		dev_dbg(dev, "Decoder DHT cfg finished. Start decoding...\n");
 		goto job_unlock;
@@ -1209,14 +1271,14 @@ static void mxc_jpeg_config_dec_desc(struct vb2_buffer *out_buf,
 	 */
 	*cfg_size = mxc_jpeg_setup_cfg_stream(cfg_stream_vaddr,
 					      V4L2_PIX_FMT_YUYV,
-					      MXC_JPEG_MIN_WIDTH,
-					      MXC_JPEG_MIN_HEIGHT);
+					      MXC_JPEG_PATTERN_WIDTH,
+					      MXC_JPEG_PATTERN_HEIGHT);
 	cfg_desc->next_descpt_ptr = desc_handle | MXC_NXT_DESCPT_EN;
-	cfg_desc->buf_base0 = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	cfg_desc->buf_base0 = jpeg->slot_data.cfg_dec_daddr;
 	cfg_desc->buf_base1 = 0;
-	cfg_desc->imgsize = MXC_JPEG_MIN_WIDTH << 16;
-	cfg_desc->imgsize |= MXC_JPEG_MIN_HEIGHT;
-	cfg_desc->line_pitch = MXC_JPEG_MIN_WIDTH * 2;
+	cfg_desc->imgsize = MXC_JPEG_PATTERN_WIDTH << 16;
+	cfg_desc->imgsize |= MXC_JPEG_PATTERN_HEIGHT;
+	cfg_desc->line_pitch = MXC_JPEG_PATTERN_WIDTH * 2;
 	cfg_desc->stm_ctrl = STM_CTRL_IMAGE_FORMAT(MXC_JPEG_YUV422);
 	cfg_desc->stm_ctrl |= STM_CTRL_BITBUF_PTR_CLR(1);
 	cfg_desc->stm_bufbase = cfg_stream_handle;
@@ -1634,6 +1696,9 @@ static int mxc_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
 	dev_dbg(ctx->mxc_jpeg->dev, "Start streaming ctx=%p", ctx);
 	q_data->sequence = 0;
 
+	if (V4L2_TYPE_IS_CAPTURE(q->type))
+		ctx->need_initial_source_change_evt = false;
+
 	ret = pm_runtime_resume_and_get(ctx->mxc_jpeg->dev);
 	if (ret < 0) {
 		dev_err(ctx->mxc_jpeg->dev, "Failed to power up jpeg\n");
@@ -1840,17 +1905,6 @@ static int mxc_jpeg_parse(struct mxc_jpeg_ctx *ctx, struct vb2_buffer *vb)
 
 	q_data_out = mxc_jpeg_get_q_data(ctx,
 					 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
-	if (q_data_out->w == 0 && q_data_out->h == 0) {
-		dev_warn(dev, "Invalid user resolution 0x0");
-		dev_warn(dev, "Keeping resolution from JPEG: %dx%d",
-			 header.frame.width, header.frame.height);
-	} else if (header.frame.width != q_data_out->w ||
-		   header.frame.height != q_data_out->h) {
-		dev_err(dev,
-			"Resolution mismatch: %dx%d (JPEG) versus %dx%d(user)",
-			header.frame.width, header.frame.height,
-			q_data_out->w, q_data_out->h);
-	}
 	q_data_out->w = header.frame.width;
 	q_data_out->h = header.frame.height;
 	if (header.frame.width > MXC_JPEG_MAX_WIDTH ||
@@ -1926,9 +1980,19 @@ static void mxc_jpeg_buf_queue(struct vb2_buffer *vb)
 	jpeg_src_buf = vb2_to_mxc_buf(vb);
 	jpeg_src_buf->jpeg_parse_error = false;
 	ret = mxc_jpeg_parse(ctx, vb);
-	if (ret)
+	if (ret) {
 		jpeg_src_buf->jpeg_parse_error = true;
 
+		/*
+		 * if the capture queue is not setup, the device_run() won't be scheduled,
+		 * need to drop the error buffer, so that the decoding can continue
+		 */
+		if (!vb2_is_streaming(v4l2_m2m_get_dst_vq(ctx->fh.m2m_ctx))) {
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+			return;
+		}
+	}
+
 end:
 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
 }
@@ -1973,8 +2037,6 @@ static int mxc_jpeg_buf_prepare(struct vb2_buffer *vb)
 
 static const struct vb2_ops mxc_jpeg_qops = {
 	.queue_setup		= mxc_jpeg_queue_setup,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.buf_out_validate	= mxc_jpeg_buf_out_validate,
 	.buf_prepare		= mxc_jpeg_buf_prepare,
 	.start_streaming	= mxc_jpeg_start_streaming,
@@ -2687,9 +2749,12 @@ static void mxc_jpeg_detach_pm_domains(struct mxc_jpeg_dev *jpeg)
 	int i;
 
 	for (i = 0; i < jpeg->num_domains; i++) {
-		if (jpeg->pd_link[i] && !IS_ERR(jpeg->pd_link[i]))
+		if (!IS_ERR_OR_NULL(jpeg->pd_dev[i]) &&
+		    !pm_runtime_suspended(jpeg->pd_dev[i]))
+			pm_runtime_force_suspend(jpeg->pd_dev[i]);
+		if (!IS_ERR_OR_NULL(jpeg->pd_link[i]))
 			device_link_del(jpeg->pd_link[i]);
-		if (jpeg->pd_dev[i] && !IS_ERR(jpeg->pd_dev[i]))
+		if (!IS_ERR_OR_NULL(jpeg->pd_dev[i]))
 			dev_pm_domain_detach(jpeg->pd_dev[i], true);
 		jpeg->pd_dev[i] = NULL;
 		jpeg->pd_link[i] = NULL;
@@ -2850,6 +2915,7 @@ static int mxc_jpeg_probe(struct platform_device *pdev)
 	jpeg->dec_vdev->vfl_dir = VFL_DIR_M2M;
 	jpeg->dec_vdev->device_caps = V4L2_CAP_STREAMING |
 					V4L2_CAP_VIDEO_M2M_MPLANE;
+	video_set_drvdata(jpeg->dec_vdev, jpeg);
 	if (mode == MXC_JPEG_ENCODE) {
 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_DECODER_CMD);
 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_TRY_DECODER_CMD);
@@ -2862,7 +2928,6 @@ static int mxc_jpeg_probe(struct platform_device *pdev)
 		dev_err(dev, "failed to register video device\n");
 		goto err_vdev_register;
 	}
-	video_set_drvdata(jpeg->dec_vdev, jpeg);
 	if (mode == MXC_JPEG_ENCODE)
 		v4l2_info(&jpeg->v4l2_dev,
 			  "encoder device registered as /dev/video%d (%d,%d)\n",
@@ -2896,7 +2961,6 @@ err_clk:
 	return ret;
 }
 
-#ifdef CONFIG_PM
 static int mxc_jpeg_runtime_resume(struct device *dev)
 {
 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
@@ -2919,9 +2983,7 @@ static int mxc_jpeg_runtime_suspend(struct device *dev)
 
 	return 0;
 }
-#endif
 
-#ifdef CONFIG_PM_SLEEP
 static int mxc_jpeg_suspend(struct device *dev)
 {
 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
@@ -2942,12 +3004,10 @@ static int mxc_jpeg_resume(struct device *dev)
 	v4l2_m2m_resume(jpeg->m2m_dev);
 	return ret;
 }
-#endif
 
 static const struct dev_pm_ops	mxc_jpeg_pm_ops = {
-	SET_RUNTIME_PM_OPS(mxc_jpeg_runtime_suspend,
-			   mxc_jpeg_runtime_resume, NULL)
-	SET_SYSTEM_SLEEP_PM_OPS(mxc_jpeg_suspend, mxc_jpeg_resume)
+	RUNTIME_PM_OPS(mxc_jpeg_runtime_suspend, mxc_jpeg_runtime_resume, NULL)
+	SYSTEM_SLEEP_PM_OPS(mxc_jpeg_suspend, mxc_jpeg_resume)
 };
 
 static void mxc_jpeg_remove(struct platform_device *pdev)
@@ -2967,11 +3027,11 @@ MODULE_DEVICE_TABLE(of, mxc_jpeg_match);
 
 static struct platform_driver mxc_jpeg_driver = {
 	.probe = mxc_jpeg_probe,
-	.remove_new = mxc_jpeg_remove,
+	.remove = mxc_jpeg_remove,
 	.driver = {
 		.name = "mxc-jpeg",
 		.of_match_table = mxc_jpeg_match,
-		.pm = &mxc_jpeg_pm_ops,
+		.pm = pm_ptr(&mxc_jpeg_pm_ops),
 	},
 };
 module_platform_driver(mxc_jpeg_driver);
diff --git a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.h b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.h
index 86e324b21aed..fdde45f7e163 100644
--- a/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.h
+++ b/drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.h
@@ -28,6 +28,8 @@
 #define MXC_JPEG_W_ALIGN		3
 #define MXC_JPEG_MAX_SIZEIMAGE		0xFFFFFC00
 #define MXC_JPEG_MAX_PLANES		2
+#define MXC_JPEG_PATTERN_WIDTH		128
+#define MXC_JPEG_PATTERN_HEIGHT		64
 
 enum mxc_jpeg_enc_state {
 	MXC_JPEG_ENCODING	= 0, /* jpeg encode phase */
@@ -117,6 +119,9 @@ struct mxc_jpeg_slot_data {
 	dma_addr_t desc_handle;
 	dma_addr_t cfg_desc_handle; // configuration descriptor dma address
 	dma_addr_t cfg_stream_handle; // configuration bitstream dma address
+	dma_addr_t cfg_dec_size;
+	void *cfg_dec_vaddr;
+	dma_addr_t cfg_dec_daddr;
 };
 
 struct mxc_jpeg_dev {
diff --git a/drivers/media/platform/nxp/imx-mipi-csis.c b/drivers/media/platform/nxp/imx-mipi-csis.c
index db8ff5f5c4d3..d060eadebc7a 100644
--- a/drivers/media/platform/nxp/imx-mipi-csis.c
+++ b/drivers/media/platform/nxp/imx-mipi-csis.c
@@ -30,6 +30,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
 #include <media/v4l2-fwnode.h>
 #include <media/v4l2-mc.h>
 #include <media/v4l2-subdev.h>
@@ -319,7 +320,11 @@ struct mipi_csis_device {
 	struct v4l2_subdev sd;
 	struct media_pad pads[CSIS_PADS_NUM];
 	struct v4l2_async_notifier notifier;
-	struct v4l2_subdev *src_sd;
+
+	struct {
+		struct v4l2_subdev *sd;
+		const struct media_pad *pad;
+	} source;
 
 	struct v4l2_mbus_config_mipi_csi2 bus;
 	u32 clk_frequency;
@@ -592,12 +597,13 @@ static void __mipi_csis_set_format(struct mipi_csis_device *csis,
 static int mipi_csis_calculate_params(struct mipi_csis_device *csis,
 				      const struct csis_pix_format *csis_fmt)
 {
+	struct media_pad *src_pad =
+		&csis->source.sd->entity.pads[csis->source.pad->index];
 	s64 link_freq;
 	u32 lane_rate;
 
 	/* Calculate the line rate from the pixel rate. */
-	link_freq = v4l2_get_link_freq(csis->src_sd->ctrl_handler,
-				       csis_fmt->width,
+	link_freq = v4l2_get_link_freq(src_pad, csis_fmt->width,
 				       csis->bus.num_data_lanes * 2);
 	if (link_freq < 0) {
 		dev_err(csis->dev, "Unable to obtain link frequency: %d\n",
@@ -742,6 +748,18 @@ static void mipi_csis_stop_stream(struct mipi_csis_device *csis)
 	mipi_csis_system_enable(csis, false);
 }
 
+static void mipi_csis_queue_event_sof(struct mipi_csis_device *csis)
+{
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+	};
+	u32 frame;
+
+	frame = mipi_csis_read(csis, MIPI_CSIS_FRAME_COUNTER_CH(0));
+	event.u.frame_sync.frame_sequence = frame;
+	v4l2_event_queue(csis->sd.devnode, &event);
+}
+
 static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
 {
 	struct mipi_csis_device *csis = dev_id;
@@ -765,6 +783,10 @@ static irqreturn_t mipi_csis_irq_handler(int irq, void *dev_id)
 				event->counter++;
 		}
 	}
+
+	if (status & MIPI_CSIS_INT_SRC_FRAME_START)
+		mipi_csis_queue_event_sof(csis);
+
 	spin_unlock_irqrestore(&csis->slock, flags);
 
 	mipi_csis_write(csis, MIPI_CSIS_INT_SRC, status);
@@ -840,18 +862,21 @@ static void mipi_csis_log_counters(struct mipi_csis_device *csis, bool non_error
 {
 	unsigned int num_events = non_errors ? MIPI_CSIS_NUM_EVENTS
 				: MIPI_CSIS_NUM_EVENTS - 8;
+	unsigned int counters[MIPI_CSIS_NUM_EVENTS];
 	unsigned long flags;
 	unsigned int i;
 
 	spin_lock_irqsave(&csis->slock, flags);
+	for (i = 0; i < num_events; ++i)
+		counters[i] =  csis->events[i].counter;
+	spin_unlock_irqrestore(&csis->slock, flags);
 
 	for (i = 0; i < num_events; ++i) {
-		if (csis->events[i].counter > 0 || csis->debug.enable)
+		if (counters[i] > 0 || csis->debug.enable)
 			dev_info(csis->dev, "%s events: %d\n",
 				 csis->events[i].name,
-				 csis->events[i].counter);
+				 counters[i]);
 	}
-	spin_unlock_irqrestore(&csis->slock, flags);
 }
 
 static int mipi_csis_dump_regs(struct mipi_csis_device *csis)
@@ -941,7 +966,8 @@ static int mipi_csis_s_stream(struct v4l2_subdev *sd, int enable)
 	int ret;
 
 	if (!enable) {
-		v4l2_subdev_call(csis->src_sd, video, s_stream, 0);
+		v4l2_subdev_disable_streams(csis->source.sd,
+					    csis->source.pad->index, BIT(0));
 
 		mipi_csis_stop_stream(csis);
 		if (csis->debug.enable)
@@ -969,7 +995,8 @@ static int mipi_csis_s_stream(struct v4l2_subdev *sd, int enable)
 
 	mipi_csis_start_stream(csis, format, csis_fmt);
 
-	ret = v4l2_subdev_call(csis->src_sd, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(csis->source.sd,
+					 csis->source.pad->index, BIT(0));
 	if (ret < 0)
 		goto err_stop;
 
@@ -1154,8 +1181,23 @@ static int mipi_csis_log_status(struct v4l2_subdev *sd)
 	return 0;
 }
 
+static int mipi_csis_subscribe_event(struct v4l2_subdev *sd, struct v4l2_fh *fh,
+				     struct v4l2_event_subscription *sub)
+{
+	if (sub->type != V4L2_EVENT_FRAME_SYNC)
+		return -EINVAL;
+
+	/* V4L2_EVENT_FRAME_SYNC doesn't require an id, so zero should be set */
+	if (sub->id != 0)
+		return -EINVAL;
+
+	return v4l2_event_subscribe(fh, sub, 0, NULL);
+}
+
 static const struct v4l2_subdev_core_ops mipi_csis_core_ops = {
 	.log_status	= mipi_csis_log_status,
+	.subscribe_event =  mipi_csis_subscribe_event,
+	.unsubscribe_event = v4l2_event_subdev_unsubscribe,
 };
 
 static const struct v4l2_subdev_video_ops mipi_csis_video_ops = {
@@ -1201,12 +1243,14 @@ static int mipi_csis_link_setup(struct media_entity *entity,
 	remote_sd = media_entity_to_v4l2_subdev(remote_pad->entity);
 
 	if (flags & MEDIA_LNK_FL_ENABLED) {
-		if (csis->src_sd)
+		if (csis->source.sd)
 			return -EBUSY;
 
-		csis->src_sd = remote_sd;
+		csis->source.sd = remote_sd;
+		csis->source.pad = remote_pad;
 	} else {
-		csis->src_sd = NULL;
+		csis->source.sd = NULL;
+		csis->source.pad = NULL;
 	}
 
 	return 0;
@@ -1304,7 +1348,7 @@ err_parse:
  * Suspend/resume
  */
 
-static int __maybe_unused mipi_csis_runtime_suspend(struct device *dev)
+static int mipi_csis_runtime_suspend(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
@@ -1319,7 +1363,7 @@ static int __maybe_unused mipi_csis_runtime_suspend(struct device *dev)
 	return 0;
 }
 
-static int __maybe_unused mipi_csis_runtime_resume(struct device *dev)
+static int mipi_csis_runtime_resume(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct mipi_csis_device *csis = sd_to_mipi_csis_device(sd);
@@ -1339,8 +1383,8 @@ static int __maybe_unused mipi_csis_runtime_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops mipi_csis_pm_ops = {
-	SET_RUNTIME_PM_OPS(mipi_csis_runtime_suspend, mipi_csis_runtime_resume,
-			   NULL)
+	RUNTIME_PM_OPS(mipi_csis_runtime_suspend, mipi_csis_runtime_resume,
+		       NULL)
 };
 
 /* -----------------------------------------------------------------------------
@@ -1358,7 +1402,7 @@ static int mipi_csis_subdev_init(struct mipi_csis_device *csis)
 	snprintf(sd->name, sizeof(sd->name), "csis-%s",
 		 dev_name(csis->dev));
 
-	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
 	sd->ctrl_handler = NULL;
 
 	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
@@ -1527,11 +1571,11 @@ MODULE_DEVICE_TABLE(of, mipi_csis_of_match);
 
 static struct platform_driver mipi_csis_driver = {
 	.probe		= mipi_csis_probe,
-	.remove_new	= mipi_csis_remove,
+	.remove		= mipi_csis_remove,
 	.driver		= {
 		.of_match_table = mipi_csis_of_match,
 		.name		= CSIS_DRIVER_NAME,
-		.pm		= &mipi_csis_pm_ops,
+		.pm		= pm_ptr(&mipi_csis_pm_ops),
 	},
 };
 
diff --git a/drivers/media/platform/nxp/imx-pxp.c b/drivers/media/platform/nxp/imx-pxp.c
index e62dc5c1a4ae..7f8ffbac582f 100644
--- a/drivers/media/platform/nxp/imx-pxp.c
+++ b/drivers/media/platform/nxp/imx-pxp.c
@@ -1606,8 +1606,6 @@ static const struct vb2_ops pxp_qops = {
 	.buf_queue	 = pxp_buf_queue,
 	.start_streaming = pxp_start_streaming,
 	.stop_streaming  = pxp_stop_streaming,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -1805,6 +1803,9 @@ static int pxp_probe(struct platform_device *pdev)
 		return PTR_ERR(mmio);
 	dev->regmap = devm_regmap_init_mmio(&pdev->dev, mmio,
 					    &pxp_regmap_config);
+	if (IS_ERR(dev->regmap))
+		return dev_err_probe(&pdev->dev, PTR_ERR(dev->regmap),
+				     "Failed to init regmap\n");
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
@@ -1940,7 +1941,7 @@ MODULE_DEVICE_TABLE(of, pxp_dt_ids);
 
 static struct platform_driver pxp_driver = {
 	.probe		= pxp_probe,
-	.remove_new	= pxp_remove,
+	.remove		= pxp_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
 		.of_match_table = pxp_dt_ids,
diff --git a/drivers/media/platform/nxp/imx-pxp.h b/drivers/media/platform/nxp/imx-pxp.h
index 44f95c749d2e..476f2042fa6f 100644
--- a/drivers/media/platform/nxp/imx-pxp.h
+++ b/drivers/media/platform/nxp/imx-pxp.h
@@ -594,12 +594,17 @@
 	(((v) << 18) & BM_PXP_CSC1_COEF0_C0)
 #define BP_PXP_CSC1_COEF0_UV_OFFSET      9
 #define BM_PXP_CSC1_COEF0_UV_OFFSET 0x0003FE00
+
+/*
+ * We use v * (1 << 9) instead of v << 9, to workaround a gcc5 bug.
+ * The compiler cannot understand that the expression is constant.
+ */
 #define BF_PXP_CSC1_COEF0_UV_OFFSET(v)  \
-	(((v) << 9) & BM_PXP_CSC1_COEF0_UV_OFFSET)
+	(((v) * (1 << 9)) & BM_PXP_CSC1_COEF0_UV_OFFSET)
 #define BP_PXP_CSC1_COEF0_Y_OFFSET      0
 #define BM_PXP_CSC1_COEF0_Y_OFFSET 0x000001FF
 #define BF_PXP_CSC1_COEF0_Y_OFFSET(v)  \
-	(((v) << 0) & BM_PXP_CSC1_COEF0_Y_OFFSET)
+	((v) & BM_PXP_CSC1_COEF0_Y_OFFSET)
 
 #define HW_PXP_CSC1_COEF1	(0x000001b0)
 
diff --git a/drivers/media/platform/nxp/imx7-media-csi.c b/drivers/media/platform/nxp/imx7-media-csi.c
index 9566ff738818..34a92642bbfe 100644
--- a/drivers/media/platform/nxp/imx7-media-csi.c
+++ b/drivers/media/platform/nxp/imx7-media-csi.c
@@ -1507,8 +1507,6 @@ static const struct vb2_ops imx7_csi_video_qops = {
 	.buf_init        = imx7_csi_video_buf_init,
 	.buf_prepare	 = imx7_csi_video_buf_prepare,
 	.buf_queue	 = imx7_csi_video_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = imx7_csi_video_start_streaming,
 	.stop_streaming  = imx7_csi_video_stop_streaming,
 };
@@ -2281,7 +2279,7 @@ MODULE_DEVICE_TABLE(of, imx7_csi_of_match);
 
 static struct platform_driver imx7_csi_driver = {
 	.probe = imx7_csi_probe,
-	.remove_new = imx7_csi_remove,
+	.remove = imx7_csi_remove,
 	.driver = {
 		.of_match_table = imx7_csi_of_match,
 		.name = "imx7-csi",
diff --git a/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.c b/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.c
index c2013995049c..1e79b1211b60 100644
--- a/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.c
+++ b/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.c
@@ -307,6 +307,19 @@ static const struct mxc_isi_plat_data mxc_imx8mp_data = {
 	.has_36bit_dma		= true,
 };
 
+static const struct mxc_isi_plat_data mxc_imx8ulp_data = {
+	.model			= MXC_ISI_IMX8ULP,
+	.num_ports		= 1,
+	.num_channels		= 1,
+	.reg_offset		= 0x0,
+	.ier_reg		= &mxc_imx8_isi_ier_v2,
+	.set_thd		= &mxc_imx8_isi_thd_v1,
+	.clks			= mxc_imx8mn_clks,
+	.num_clks		= ARRAY_SIZE(mxc_imx8mn_clks),
+	.buf_active_reverse	= true,
+	.has_36bit_dma		= false,
+};
+
 static const struct mxc_isi_plat_data mxc_imx93_data = {
 	.model			= MXC_ISI_IMX93,
 	.num_ports		= 1,
@@ -528,6 +541,7 @@ static void mxc_isi_remove(struct platform_device *pdev)
 static const struct of_device_id mxc_isi_of_match[] = {
 	{ .compatible = "fsl,imx8mn-isi", .data = &mxc_imx8mn_data },
 	{ .compatible = "fsl,imx8mp-isi", .data = &mxc_imx8mp_data },
+	{ .compatible = "fsl,imx8ulp-isi", .data = &mxc_imx8ulp_data },
 	{ .compatible = "fsl,imx93-isi", .data = &mxc_imx93_data },
 	{ /* sentinel */ },
 };
@@ -535,7 +549,7 @@ MODULE_DEVICE_TABLE(of, mxc_isi_of_match);
 
 static struct platform_driver mxc_isi_driver = {
 	.probe		= mxc_isi_probe,
-	.remove_new	= mxc_isi_remove,
+	.remove		= mxc_isi_remove,
 	.driver = {
 		.of_match_table = mxc_isi_of_match,
 		.name		= MXC_ISI_DRIVER_NAME,
diff --git a/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.h b/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.h
index 2810ebe9b5f7..9c7fe9e5f941 100644
--- a/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.h
+++ b/drivers/media/platform/nxp/imx8-isi/imx8-isi-core.h
@@ -158,6 +158,7 @@ struct mxc_gasket_ops {
 enum model {
 	MXC_ISI_IMX8MN,
 	MXC_ISI_IMX8MP,
+	MXC_ISI_IMX8ULP,
 	MXC_ISI_IMX93,
 };
 
diff --git a/drivers/media/platform/nxp/imx8-isi/imx8-isi-m2m.c b/drivers/media/platform/nxp/imx8-isi/imx8-isi-m2m.c
index 9745d6219a16..22e49d3a1287 100644
--- a/drivers/media/platform/nxp/imx8-isi/imx8-isi-m2m.c
+++ b/drivers/media/platform/nxp/imx8-isi/imx8-isi-m2m.c
@@ -43,6 +43,7 @@ struct mxc_isi_m2m_ctx_queue_data {
 	struct v4l2_pix_format_mplane format;
 	const struct mxc_isi_format_info *info;
 	u32 sequence;
+	bool streaming;
 };
 
 struct mxc_isi_m2m_ctx {
@@ -269,8 +270,6 @@ static const struct vb2_ops mxc_isi_m2m_vb2_qops = {
 	.buf_init		= mxc_isi_m2m_vb2_buffer_init,
 	.buf_prepare		= mxc_isi_m2m_vb2_buffer_prepare,
 	.buf_queue		= mxc_isi_m2m_vb2_buffer_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= mxc_isi_m2m_vb2_start_streaming,
 	.stop_streaming		= mxc_isi_m2m_vb2_stop_streaming,
 };
@@ -486,15 +485,18 @@ static int mxc_isi_m2m_streamon(struct file *file, void *fh,
 				enum v4l2_buf_type type)
 {
 	struct mxc_isi_m2m_ctx *ctx = to_isi_m2m_ctx(fh);
+	struct mxc_isi_m2m_ctx_queue_data *q = mxc_isi_m2m_ctx_qdata(ctx, type);
 	const struct v4l2_pix_format_mplane *out_pix = &ctx->queues.out.format;
 	const struct v4l2_pix_format_mplane *cap_pix = &ctx->queues.cap.format;
 	const struct mxc_isi_format_info *cap_info = ctx->queues.cap.info;
 	const struct mxc_isi_format_info *out_info = ctx->queues.out.info;
 	struct mxc_isi_m2m *m2m = ctx->m2m;
 	bool bypass;
-
 	int ret;
 
+	if (q->streaming)
+		return 0;
+
 	mutex_lock(&m2m->lock);
 
 	if (m2m->usage_count == INT_MAX) {
@@ -547,6 +549,8 @@ static int mxc_isi_m2m_streamon(struct file *file, void *fh,
 		goto unchain;
 	}
 
+	q->streaming = true;
+
 	return 0;
 
 unchain:
@@ -569,10 +573,14 @@ static int mxc_isi_m2m_streamoff(struct file *file, void *fh,
 				 enum v4l2_buf_type type)
 {
 	struct mxc_isi_m2m_ctx *ctx = to_isi_m2m_ctx(fh);
+	struct mxc_isi_m2m_ctx_queue_data *q = mxc_isi_m2m_ctx_qdata(ctx, type);
 	struct mxc_isi_m2m *m2m = ctx->m2m;
 
 	v4l2_m2m_ioctl_streamoff(file, fh, type);
 
+	if (!q->streaming)
+		return 0;
+
 	mutex_lock(&m2m->lock);
 
 	/*
@@ -598,6 +606,8 @@ static int mxc_isi_m2m_streamoff(struct file *file, void *fh,
 
 	mutex_unlock(&m2m->lock);
 
+	q->streaming = false;
+
 	return 0;
 }
 
diff --git a/drivers/media/platform/nxp/imx8-isi/imx8-isi-video.c b/drivers/media/platform/nxp/imx8-isi/imx8-isi-video.c
index 4091f1c0e78b..8654150728a8 100644
--- a/drivers/media/platform/nxp/imx8-isi/imx8-isi-video.c
+++ b/drivers/media/platform/nxp/imx8-isi/imx8-isi-video.c
@@ -861,6 +861,7 @@ int mxc_isi_video_buffer_prepare(struct mxc_isi_dev *isi, struct vb2_buffer *vb2
 				 const struct mxc_isi_format_info *info,
 				 const struct v4l2_pix_format_mplane *pix)
 {
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb2);
 	unsigned int i;
 
 	for (i = 0; i < info->mem_planes; i++) {
@@ -875,6 +876,8 @@ int mxc_isi_video_buffer_prepare(struct mxc_isi_dev *isi, struct vb2_buffer *vb2
 		vb2_set_plane_payload(vb2, i, size);
 	}
 
+	v4l2_buf->field = pix->field;
+
 	return 0;
 }
 
@@ -987,8 +990,6 @@ static const struct vb2_ops mxc_isi_vb2_qops = {
 	.buf_init		= mxc_isi_vb2_buffer_init,
 	.buf_prepare		= mxc_isi_vb2_buffer_prepare,
 	.buf_queue		= mxc_isi_vb2_buffer_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= mxc_isi_vb2_start_streaming,
 	.stop_streaming		= mxc_isi_vb2_stop_streaming,
 };
diff --git a/drivers/media/platform/nxp/imx8mq-mipi-csi2.c b/drivers/media/platform/nxp/imx8mq-mipi-csi2.c
index ba2e81f24965..a8bcf60e2f37 100644
--- a/drivers/media/platform/nxp/imx8mq-mipi-csi2.c
+++ b/drivers/media/platform/nxp/imx8mq-mipi-csi2.c
@@ -287,6 +287,7 @@ static int imx8mq_mipi_csi_calc_hs_settle(struct csi_state *state,
 					  struct v4l2_subdev_state *sd_state,
 					  u32 *hs_settle)
 {
+	struct media_pad *src_pad;
 	s64 link_freq;
 	u32 lane_rate;
 	unsigned long esc_clk_rate;
@@ -294,13 +295,19 @@ static int imx8mq_mipi_csi_calc_hs_settle(struct csi_state *state,
 	const struct v4l2_mbus_framefmt *fmt;
 	const struct csi2_pix_format *csi2_fmt;
 
+	src_pad = media_entity_remote_source_pad_unique(&sd_state->sd->entity);
+	if (IS_ERR(src_pad)) {
+		dev_err(state->dev, "can't get source pad of %s (%ld)\n",
+			sd_state->sd->name, PTR_ERR(src_pad));
+		return PTR_ERR(src_pad);
+	}
+
 	/* Calculate the line rate from the pixel rate. */
 
 	fmt = v4l2_subdev_state_get_format(sd_state, MIPI_CSI2_PAD_SINK);
 	csi2_fmt = find_csi2_format(fmt->code);
 
-	link_freq = v4l2_get_link_freq(state->src_sd->ctrl_handler,
-				       csi2_fmt->width,
+	link_freq = v4l2_get_link_freq(src_pad, csi2_fmt->width,
 				       state->bus.num_data_lanes * 2);
 	if (link_freq < 0) {
 		dev_err(state->dev, "Unable to obtain link frequency: %d\n",
@@ -693,7 +700,7 @@ unlock:
 	return ret ? -EAGAIN : 0;
 }
 
-static int __maybe_unused imx8mq_mipi_csi_suspend(struct device *dev)
+static int imx8mq_mipi_csi_suspend(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct csi_state *state = mipi_sd_to_csi2_state(sd);
@@ -705,7 +712,7 @@ static int __maybe_unused imx8mq_mipi_csi_suspend(struct device *dev)
 	return 0;
 }
 
-static int __maybe_unused imx8mq_mipi_csi_resume(struct device *dev)
+static int imx8mq_mipi_csi_resume(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct csi_state *state = mipi_sd_to_csi2_state(sd);
@@ -716,7 +723,7 @@ static int __maybe_unused imx8mq_mipi_csi_resume(struct device *dev)
 	return imx8mq_mipi_csi_pm_resume(dev);
 }
 
-static int __maybe_unused imx8mq_mipi_csi_runtime_suspend(struct device *dev)
+static int imx8mq_mipi_csi_runtime_suspend(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct csi_state *state = mipi_sd_to_csi2_state(sd);
@@ -731,7 +738,7 @@ static int __maybe_unused imx8mq_mipi_csi_runtime_suspend(struct device *dev)
 	return ret;
 }
 
-static int __maybe_unused imx8mq_mipi_csi_runtime_resume(struct device *dev)
+static int imx8mq_mipi_csi_runtime_resume(struct device *dev)
 {
 	struct v4l2_subdev *sd = dev_get_drvdata(dev);
 	struct csi_state *state = mipi_sd_to_csi2_state(sd);
@@ -747,10 +754,9 @@ static int __maybe_unused imx8mq_mipi_csi_runtime_resume(struct device *dev)
 }
 
 static const struct dev_pm_ops imx8mq_mipi_csi_pm_ops = {
-	SET_RUNTIME_PM_OPS(imx8mq_mipi_csi_runtime_suspend,
-			   imx8mq_mipi_csi_runtime_resume,
-			   NULL)
-	SET_SYSTEM_SLEEP_PM_OPS(imx8mq_mipi_csi_suspend, imx8mq_mipi_csi_resume)
+	RUNTIME_PM_OPS(imx8mq_mipi_csi_runtime_suspend,
+		       imx8mq_mipi_csi_runtime_resume, NULL)
+	SYSTEM_SLEEP_PM_OPS(imx8mq_mipi_csi_suspend, imx8mq_mipi_csi_resume)
 };
 
 /* -----------------------------------------------------------------------------
@@ -954,11 +960,11 @@ MODULE_DEVICE_TABLE(of, imx8mq_mipi_csi_of_match);
 
 static struct platform_driver imx8mq_mipi_csi_driver = {
 	.probe		= imx8mq_mipi_csi_probe,
-	.remove_new	= imx8mq_mipi_csi_remove,
+	.remove		= imx8mq_mipi_csi_remove,
 	.driver		= {
 		.of_match_table = imx8mq_mipi_csi_of_match,
 		.name		= MIPI_CSI2_DRIVER_NAME,
-		.pm		= &imx8mq_mipi_csi_pm_ops,
+		.pm		= pm_ptr(&imx8mq_mipi_csi_pm_ops),
 	},
 };
 
diff --git a/drivers/media/platform/nxp/mx2_emmaprp.c b/drivers/media/platform/nxp/mx2_emmaprp.c
index 023ed40c6b08..0c6cc120fd2a 100644
--- a/drivers/media/platform/nxp/mx2_emmaprp.c
+++ b/drivers/media/platform/nxp/mx2_emmaprp.c
@@ -677,8 +677,6 @@ static const struct vb2_ops emmaprp_qops = {
 	.queue_setup	 = emmaprp_queue_setup,
 	.buf_prepare	 = emmaprp_buf_prepare,
 	.buf_queue	 = emmaprp_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -902,7 +900,7 @@ static void emmaprp_remove(struct platform_device *pdev)
 
 static struct platform_driver emmaprp_pdrv = {
 	.probe		= emmaprp_probe,
-	.remove_new	= emmaprp_remove,
+	.remove		= emmaprp_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
 	},
diff --git a/drivers/media/platform/qcom/Kconfig b/drivers/media/platform/qcom/Kconfig
index cc5799b9ea00..4f4d3a68e6e5 100644
--- a/drivers/media/platform/qcom/Kconfig
+++ b/drivers/media/platform/qcom/Kconfig
@@ -3,4 +3,5 @@
 comment "Qualcomm media platform drivers"
 
 source "drivers/media/platform/qcom/camss/Kconfig"
+source "drivers/media/platform/qcom/iris/Kconfig"
 source "drivers/media/platform/qcom/venus/Kconfig"
diff --git a/drivers/media/platform/qcom/Makefile b/drivers/media/platform/qcom/Makefile
index 4f055c396e04..ea2221a202c0 100644
--- a/drivers/media/platform/qcom/Makefile
+++ b/drivers/media/platform/qcom/Makefile
@@ -1,3 +1,4 @@
 # SPDX-License-Identifier: GPL-2.0-only
 obj-y += camss/
+obj-y += iris/
 obj-y += venus/
diff --git a/drivers/media/platform/qcom/camss/Makefile b/drivers/media/platform/qcom/camss/Makefile
index 4e2222358973..d26a9c24a430 100644
--- a/drivers/media/platform/qcom/camss/Makefile
+++ b/drivers/media/platform/qcom/camss/Makefile
@@ -6,7 +6,9 @@ qcom-camss-objs += \
 		camss-csid.o \
 		camss-csid-4-1.o \
 		camss-csid-4-7.o \
+		camss-csid-680.o \
 		camss-csid-gen2.o \
+		camss-csid-780.o \
 		camss-csiphy-2ph-1-0.o \
 		camss-csiphy-3ph-1-0.o \
 		camss-csiphy.o \
@@ -14,10 +16,13 @@ qcom-camss-objs += \
 		camss-vfe-4-1.o \
 		camss-vfe-4-7.o \
 		camss-vfe-4-8.o \
-		camss-vfe-170.o \
+		camss-vfe-17x.o \
 		camss-vfe-480.o \
+		camss-vfe-680.o \
+		camss-vfe-780.o \
 		camss-vfe-gen1.o \
 		camss-vfe.o \
 		camss-video.o \
+		camss-format.o \
 
 obj-$(CONFIG_VIDEO_QCOM_CAMSS) += qcom-camss.o
diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-1.c b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
index dd49a40e6a70..6998e1c52895 100644
--- a/drivers/media/platform/qcom/camss/camss-csid-4-1.c
+++ b/drivers/media/platform/qcom/camss/camss-csid-4-1.c
@@ -17,7 +17,6 @@
 #include "camss-csid-gen1.h"
 #include "camss.h"
 
-#define CAMSS_CSID_HW_VERSION		0x0
 #define CAMSS_CSID_CORE_CTRL_0		0x004
 #define CAMSS_CSID_CORE_CTRL_1		0x008
 #define CAMSS_CSID_RST_CMD		0x00c
@@ -45,128 +44,6 @@
 #define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b0 + 0xc * (n))
 #define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0b4 + 0xc * (n))
 
-static const struct csid_format csid_formats[] = {
-	{
-		MEDIA_BUS_FMT_UYVY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_VYUY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YUYV8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YVYU8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-};
-
 static void csid_configure_stream(struct csid_device *csid, u8 enable)
 {
 	struct csid_testgen_config *tg = &csid->testgen;
@@ -174,7 +51,7 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 
 	if (enable) {
 		struct v4l2_mbus_framefmt *input_format;
-		const struct csid_format *format;
+		const struct csid_format_info *format;
 		u8 vc = 0; /* Virtual Channel 0 */
 		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
 		u8 dt_shift;
@@ -184,7 +61,8 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 			u32 num_lines, num_bytes_per_line;
 
 			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
-			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+			format = csid_get_fmt_entry(csid->res->formats->formats,
+						    csid->res->formats->nformats,
 						    input_format->code);
 			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
 			num_lines = input_format->height;
@@ -211,7 +89,8 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 			struct csid_phy_config *phy = &csid->phy;
 
 			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
-			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+			format = csid_get_fmt_entry(csid->res->formats->formats,
+						    csid->res->formats->nformats,
 						    input_format->code);
 
 			val = phy->lane_cnt - 1;
@@ -259,15 +138,6 @@ static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
 	return 0;
 }
 
-static u32 csid_hw_version(struct csid_device *csid)
-{
-	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
-
-	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
-
-	return hw_version;
-}
-
 static irqreturn_t csid_isr(int irq, void *dev)
 {
 	struct csid_device *csid = dev;
@@ -300,19 +170,8 @@ static int csid_reset(struct csid_device *csid)
 	return 0;
 }
 
-static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
-			     unsigned int match_format_idx, u32 match_code)
-{
-	if (match_format_idx > 0)
-		return 0;
-
-	return sink_code;
-}
-
 static void csid_subdev_init(struct csid_device *csid)
 {
-	csid->formats = csid_formats;
-	csid->nformats = ARRAY_SIZE(csid_formats);
 	csid->testgen.modes = csid_testgen_modes;
 	csid->testgen.nmodes = CSID_PAYLOAD_MODE_NUM_SUPPORTED_GEN1;
 }
diff --git a/drivers/media/platform/qcom/camss/camss-csid-4-7.c b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
index 6b26e036294e..66054d4872e6 100644
--- a/drivers/media/platform/qcom/camss/camss-csid-4-7.c
+++ b/drivers/media/platform/qcom/camss/camss-csid-4-7.c
@@ -16,7 +16,6 @@
 #include "camss-csid-gen1.h"
 #include "camss.h"
 
-#define CAMSS_CSID_HW_VERSION		0x0
 #define CAMSS_CSID_CORE_CTRL_0		0x004
 #define CAMSS_CSID_CORE_CTRL_1		0x008
 #define CAMSS_CSID_RST_CMD		0x010
@@ -44,156 +43,6 @@
 #define CAMSS_CSID_TG_DT_n_CGG_1(n)	(0x0b8 + 0xc * (n))
 #define CAMSS_CSID_TG_DT_n_CGG_2(n)	(0x0bc + 0xc * (n))
 
-static const struct csid_format csid_formats[] = {
-	{
-		MEDIA_BUS_FMT_UYVY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_VYUY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YUYV8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YVYU8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-};
-
 static void csid_configure_stream(struct csid_device *csid, u8 enable)
 {
 	struct csid_testgen_config *tg = &csid->testgen;
@@ -203,7 +52,7 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 
 	if (enable) {
 		struct v4l2_mbus_framefmt *input_format;
-		const struct csid_format *format;
+		const struct csid_format_info *format;
 		u8 vc = 0; /* Virtual Channel 0 */
 		u8 cid = vc * 4; /* id of Virtual Channel and Data Type set */
 		u8 dt_shift;
@@ -213,7 +62,8 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 			u32 num_bytes_per_line, num_lines;
 
 			input_format = &csid->fmt[MSM_CSID_PAD_SRC];
-			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+			format = csid_get_fmt_entry(csid->res->formats->formats,
+						    csid->res->formats->nformats,
 						    input_format->code);
 			num_bytes_per_line = input_format->width * format->bpp * format->spp / 8;
 			num_lines = input_format->height;
@@ -240,7 +90,8 @@ static void csid_configure_stream(struct csid_device *csid, u8 enable)
 			struct csid_phy_config *phy = &csid->phy;
 
 			input_format = &csid->fmt[MSM_CSID_PAD_SINK];
-			format = csid_get_fmt_entry(csid->formats, csid->nformats,
+			format = csid_get_fmt_entry(csid->res->formats->formats,
+						    csid->res->formats->nformats,
 						    input_format->code);
 
 			val = phy->lane_cnt - 1;
@@ -299,15 +150,6 @@ static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
 	return 0;
 }
 
-static u32 csid_hw_version(struct csid_device *csid)
-{
-	u32 hw_version = readl_relaxed(csid->base + CAMSS_CSID_HW_VERSION);
-
-	dev_dbg(csid->camss->dev, "CSID HW Version = 0x%08x\n", hw_version);
-
-	return hw_version;
-}
-
 /*
  * isr - CSID module interrupt service routine
  * @irq: Interrupt line
@@ -353,42 +195,8 @@ static int csid_reset(struct csid_device *csid)
 	return 0;
 }
 
-static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
-			     unsigned int match_format_idx, u32 match_code)
-{
-	switch (sink_code) {
-	case MEDIA_BUS_FMT_SBGGR10_1X10:
-	{
-		u32 src_code[] = {
-			MEDIA_BUS_FMT_SBGGR10_1X10,
-			MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
-		};
-
-		return csid_find_code(src_code, ARRAY_SIZE(src_code),
-				      match_format_idx, match_code);
-	}
-	case MEDIA_BUS_FMT_Y10_1X10:
-	{
-		u32 src_code[] = {
-			MEDIA_BUS_FMT_Y10_1X10,
-			MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
-		};
-
-		return csid_find_code(src_code, ARRAY_SIZE(src_code),
-				      match_format_idx, match_code);
-	}
-	default:
-		if (match_format_idx > 0)
-			return 0;
-
-		return sink_code;
-	}
-}
-
 static void csid_subdev_init(struct csid_device *csid)
 {
-	csid->formats = csid_formats;
-	csid->nformats = ARRAY_SIZE(csid_formats);
 	csid->testgen.modes = csid_testgen_modes;
 	csid->testgen.nmodes = CSID_PAYLOAD_MODE_NUM_SUPPORTED_GEN1;
 }
diff --git a/drivers/media/platform/qcom/camss/camss-csid-680.c b/drivers/media/platform/qcom/camss/camss-csid-680.c
new file mode 100644
index 000000000000..3ad3a174bcfb
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-csid-680.c
@@ -0,0 +1,422 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
+ *
+ * Copyright (C) 2020-2025 Linaro Ltd.
+ */
+#include <linux/completion.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+
+#include "camss.h"
+#include "camss-csid.h"
+#include "camss-csid-gen2.h"
+
+#define CSID_TOP_IO_PATH_CFG0(csid)				(0x4 * (csid))
+#define		CSID_TOP_IO_PATH_CFG0_INTERNAL_CSID		BIT(0)
+#define		CSID_TOP_IO_PATH_CFG0_SFE_0			BIT(1)
+#define		CSID_TOP_IO_PATH_CFG0_SFE_1			GENMASK(1, 0)
+#define		CSID_TOP_IO_PATH_CFG0_SBI_0			BIT(4)
+#define		CSID_TOP_IO_PATH_CFG0_SBI_1			GENMASK(3, 0)
+#define		CSID_TOP_IO_PATH_CFG0_SBI_2			GENMASK(3, 1)
+#define		CSID_TOP_IO_PATH_CFG0_OUTPUT_IFE_EN		BIT(8)
+#define		CSID_TOP_IO_PATH_CFG0_SFE_OFFLINE_EN		BIT(12)
+
+#define CSID_RESET_CMD						0x10
+#define		CSID_RESET_CMD_HW_RESET				BIT(0)
+#define		CSID_RESET_CMD_SW_RESET				BIT(1)
+#define		CSID_RESET_CMD_IRQ_CTRL				BIT(2)
+
+#define CSID_IRQ_CMD						0x14
+#define		CSID_IRQ_CMD_CLEAR				BIT(0)
+#define		CSID_IRQ_CMD_SET				BIT(4)
+
+#define CSID_REG_UPDATE_CMD					0x18
+
+#define CSID_CSI2_RDIN_IRQ_STATUS(rdi)					(0xec + 0x10 * (rdi))
+#define		CSID_CSI2_RDIN_CCIF_VIOLATION				BIT(29)
+#define		CSID_CSI2_RDIN_SENSOR_SWITCH_OUT_OF_SYNC_FRAME_DROP	BIT(28)
+#define		CSID_CSI2_RDIN_ERROR_REC_WIDTH_VIOLATION		BIT(27)
+#define		CSID_CSI2_RDIN_ERROR_REC_HEIGHT_VIOLATION		BIT(26)
+#define		CSID_CSI2_RDIN_BATCH_END_MISSING_VIOLATION		BIT(25)
+#define		CSID_CSI2_RDIN_ILLEGAL_BATCH_ID_IRQ			BIT(24)
+#define		CSID_CSI2_RDIN_RUP_DONE					BIT(23)
+#define		CSID_CSI2_RDIN_CAMIF_EPOCH_1_IRQ			BIT(22)
+#define		CSID_CSI2_RDIN_CAMIF_EPOCH_0_IRQ			BIT(21)
+#define		CSID_CSI2_RDIN_ERROR_REC_OVERFLOW_IRQ			BIT(19)
+#define		CSID_CSI2_RDIN_ERROR_REC_FRAME_DROP			BIT(18)
+#define		CSID_CSI2_RDIN_VCDT_GRP_CHANG				BIT(17)
+#define		CSID_CSI2_RDIN_VCDT_GRP_0_SEL				BIT(16)
+#define		CSID_CSI2_RDIN_VCDT_GRP_1_SEL				BIT(15)
+#define		CSID_CSI2_RDIN_ERROR_LINE_COUNT				BIT(14)
+#define		CSID_CSI2_RDIN_ERROR_PIX_COUNT				BIT(13)
+#define		CSID_CSI2_RDIN_INFO_INPUT_SOF				BIT(12)
+#define		CSID_CSI2_RDIN_INFO_INPUT_SOL				BIT(11)
+#define		CSID_CSI2_RDIN_INFO_INPUT_EOL				BIT(10)
+#define		CSID_CSI2_RDIN_INFO_INPUT_EOF				BIT(9)
+#define		CSID_CSI2_RDIN_INFO_FRAME_DROP_SOF			BIT(8)
+#define		CSID_CSI2_RDIN_INFO_FRAME_DROP_SOL			BIT(7)
+#define		CSID_CSI2_RDIN_INFO_FRAME_DROP_EOL			BIT(6)
+#define		CSID_CSI2_RDIN_INFO_FRAME_DROP_EOF			BIT(5)
+#define		CSID_CSI2_RDIN_INFO_CAMIF_SOF				BIT(4)
+#define		CSID_CSI2_RDIN_INFO_CAMIF_EOF				BIT(3)
+#define		CSID_CSI2_RDIN_INFO_FIFO_OVERFLOW			BIT(2)
+#define		CSID_CSI2_RDIN_RES1					BIT(1)
+#define		CSID_CSI2_RDIN_RES0					BIT(0)
+
+#define CSID_CSI2_RDIN_IRQ_MASK(rdi)				(0xf0 + 0x10 * (rdi))
+#define CSID_CSI2_RDIN_IRQ_CLEAR(rdi)				(0xf4 + 0x10 * (rdi))
+#define CSID_CSI2_RDIN_IRQ_SET(rdi)				(0xf8 + 0x10 * (rdi))
+
+#define CSID_TOP_IRQ_STATUS					0x7c
+#define CSID_TOP_IRQ_MASK					0x80
+#define CSID_TOP_IRQ_CLEAR					0x84
+#define		CSID_TOP_IRQ_RESET				BIT(0)
+#define		CSID_TOP_IRQ_RX					BIT(2)
+#define		CSID_TOP_IRQ_LONG_PKT(rdi)			(BIT(8) << (rdi))
+#define		CSID_TOP_IRQ_BUF_DONE				BIT(13)
+
+#define CSID_BUF_DONE_IRQ_STATUS				0x8c
+#define	BUF_DONE_IRQ_STATUS_RDI_OFFSET				(csid_is_lite(csid) ? 1 : 14)
+#define CSID_BUF_DONE_IRQ_MASK					0x90
+#define CSID_BUF_DONE_IRQ_CLEAR					0x94
+
+#define CSID_CSI2_RX_IRQ_STATUS					0x9c
+#define CSID_CSI2_RX_IRQ_MASK					0xa0
+#define CSID_CSI2_RX_IRQ_CLEAR					0xa4
+
+#define CSID_RESET_CFG						0xc
+#define		CSID_RESET_CFG_MODE_IMMEDIATE			BIT(0)
+#define		CSID_RESET_CFG_LOCATION_COMPLETE		BIT(4)
+
+#define CSID_CSI2_RDI_IRQ_STATUS(rdi)				(0xec + 0x10 * (rdi))
+#define CSID_CSI2_RDI_IRQ_MASK(rdi)				(0xf0 + 0x10 * (rdi))
+#define CSID_CSI2_RDI_IRQ_CLEAR(rdi)				(0xf4 + 0x10 * (rdi))
+
+#define CSID_CSI2_RX_CFG0					0x200
+#define		CSI2_RX_CFG0_NUM_ACTIVE_LANES			0
+#define		CSI2_RX_CFG0_DL0_INPUT_SEL			4
+#define		CSI2_RX_CFG0_DL1_INPUT_SEL			8
+#define		CSI2_RX_CFG0_DL2_INPUT_SEL			12
+#define		CSI2_RX_CFG0_DL3_INPUT_SEL			16
+#define		CSI2_RX_CFG0_PHY_NUM_SEL			20
+#define		CSI2_RX_CFG0_PHY_SEL_BASE_IDX			1
+#define		CSI2_RX_CFG0_PHY_TYPE_SEL			24
+
+#define CSID_CSI2_RX_CFG1					0x204
+#define		CSI2_RX_CFG1_PACKET_ECC_CORRECTION_EN		BIT(0)
+#define		CSI2_RX_CFG1_DE_SCRAMBLE_EN			BIT(1)
+#define		CSI2_RX_CFG1_VC_MODE				BIT(2)
+#define		CSI2_RX_CFG1_COMPLETE_STREAM_EN			BIT(4)
+#define		CSI2_RX_CFG1_COMPLETE_STREAM_FRAME_TIMING	BIT(5)
+#define		CSI2_RX_CFG1_MISR_EN				BIT(6)
+#define		CSI2_RX_CFG1_CGC_MODE				BIT(7)
+
+#define CSID_CSI2_RX_CAPTURE_CTRL				0x208
+#define		CSI2_RX_CAPTURE_CTRL_LONG_PKT_EN		BIT(0)
+#define		CSI2_RX_CAPTURE_CTRL_SHORT_PKT_EN		BIT(1)
+#define		CSI2_RX_CAPTURE_CTRL_CPHY_PKT_EN		BIT(2)
+#define		CSI2_RX_CAPTURE_CTRL_LONG_PKT_DT		GENMASK(9, 4)
+#define		CSI2_RX_CAPTURE_CTRL_LONG_PKT_VC		GENMASK(14, 10)
+#define		CSI2_RX_CAPTURE_CTRL_SHORT_PKT_VC		GENMASK(19, 15)
+#define		CSI2_RX_CAPTURE_CTRL_CPHY_PKT_DT		GENMASK(20, 25)
+#define		CSI2_RX_CAPTURE_CTRL_CPHY_PKT_VC		GENMASK(30, 26)
+
+#define CSID_CSI2_RX_TOTAL_PKTS_RCVD				0x240
+#define CSID_CSI2_RX_STATS_ECC					0x244
+#define CSID_CSI2_RX_CRC_ERRORS					0x248
+
+#define CSID_RDI_CFG0(rdi)					(0x500 + 0x100 * (rdi))
+#define		RDI_CFG0_DECODE_FORMAT				12
+#define		RDI_CFG0_DATA_TYPE				16
+#define		RDI_CFG0_VIRTUAL_CHANNEL			22
+#define		RDI_CFG0_DT_ID					27
+#define		RDI_CFG0_ENABLE					BIT(31)
+
+#define CSID_RDI_CTRL(rdi)					(0x504 + 0x100 * (rdi))
+#define		CSID_RDI_CTRL_HALT_CMD_HALT_AT_FRAME_BOUNDARY	0
+#define		CSID_RDI_CTRL_HALT_CMD_RESUME_AT_FRAME_BOUNDARY	1
+
+#define CSID_RDI_CFG1(rdi)					(0x510 + 0x100 * (rdi))
+#define		RDI_CFG1_TIMESTAMP_STB_FRAME			BIT(0)
+#define		RDI_CFG1_TIMESTAMP_STB_IRQ			BIT(1)
+#define		RDI_CFG1_BYTE_CNTR_EN				BIT(2)
+#define		RDI_CFG1_TIMESTAMP_EN				BIT(4)
+#define		RDI_CFG1_DROP_H_EN				BIT(5)
+#define		RDI_CFG1_DROP_V_EN				BIT(6)
+#define		RDI_CFG1_CROP_H_EN				BIT(7)
+#define		RDI_CFG1_CROP_V_EN				BIT(8)
+#define		RDI_CFG1_MISR_EN				BIT(9)
+#define		RDI_CFG1_PLAIN_ALIGN_MSB			BIT(11)
+#define		RDI_CFG1_EARLY_EOF_EN				BIT(14)
+#define		RDI_CFG1_PACKING_MIPI				BIT(15)
+
+#define CSID_RDI_ERR_RECOVERY_CFG0(rdi)				(0x514 + 0x100 * (rdi))
+#define CSID_RDI_EPOCH_IRQ_CFG(rdi)				(0x52c + 0x100 * (rdi))
+#define CSID_RDI_FRM_DROP_PATTERN(rdi)				(0x540 + 0x100 * (rdi))
+#define CSID_RDI_FRM_DROP_PERIOD(rdi)				(0x544 + 0x100 * (rdi))
+#define CSID_RDI_IRQ_SUBSAMPLE_PATTERN(rdi)			(0x548 + 0x100 * (rdi))
+#define CSID_RDI_IRQ_SUBSAMPLE_PERIOD(rdi)			(0x54c + 0x100 * (rdi))
+#define CSID_RDI_PIX_DROP_PATTERN(rdi)				(0x558 + 0x100 * (rdi))
+#define CSID_RDI_PIX_DROP_PERIOD(rdi)				(0x55c + 0x100 * (rdi))
+#define CSID_RDI_LINE_DROP_PATTERN(rdi)				(0x560 + 0x100 * (rdi))
+#define CSID_RDI_LINE_DROP_PERIOD(rdi)				(0x564 + 0x100 * (rdi))
+
+static inline int reg_update_rdi(struct csid_device *csid, int n)
+{
+	return BIT(4 + n) + BIT(20 + n);
+}
+
+static void csid_reg_update(struct csid_device *csid, int port_id)
+{
+	csid->reg_update |= reg_update_rdi(csid, port_id);
+	writel(csid->reg_update, csid->base + CSID_REG_UPDATE_CMD);
+}
+
+static inline void csid_reg_update_clear(struct csid_device *csid,
+					 int port_id)
+{
+	csid->reg_update &= ~reg_update_rdi(csid, port_id);
+	writel(csid->reg_update, csid->base + CSID_REG_UPDATE_CMD);
+}
+
+static void __csid_configure_rx(struct csid_device *csid,
+				struct csid_phy_config *phy, int vc)
+{
+	u32 val;
+
+	val = (phy->lane_cnt - 1) << CSI2_RX_CFG0_NUM_ACTIVE_LANES;
+	val |= phy->lane_assign << CSI2_RX_CFG0_DL0_INPUT_SEL;
+	val |= (phy->csiphy_id + CSI2_RX_CFG0_PHY_SEL_BASE_IDX) << CSI2_RX_CFG0_PHY_NUM_SEL;
+
+	writel(val, csid->base + CSID_CSI2_RX_CFG0);
+
+	val = CSI2_RX_CFG1_PACKET_ECC_CORRECTION_EN;
+	if (vc > 3)
+		val |= CSI2_RX_CFG1_VC_MODE;
+	writel(val, csid->base + CSID_CSI2_RX_CFG1);
+}
+
+static void __csid_ctrl_rdi(struct csid_device *csid, int enable, u8 rdi)
+{
+	u32 val;
+
+	if (enable)
+		val = CSID_RDI_CTRL_HALT_CMD_RESUME_AT_FRAME_BOUNDARY;
+	else
+		val = CSID_RDI_CTRL_HALT_CMD_HALT_AT_FRAME_BOUNDARY;
+
+	writel(val, csid->base + CSID_RDI_CTRL(rdi));
+}
+
+static void __csid_configure_top(struct csid_device *csid)
+{
+	u32 val;
+
+	val = CSID_TOP_IO_PATH_CFG0_OUTPUT_IFE_EN | CSID_TOP_IO_PATH_CFG0_INTERNAL_CSID;
+	writel(val, csid->camss->csid_wrapper_base +
+	    CSID_TOP_IO_PATH_CFG0(csid->id));
+}
+
+static void __csid_configure_rdi_stream(struct csid_device *csid, u8 enable, u8 vc)
+{
+	struct v4l2_mbus_framefmt *input_format = &csid->fmt[MSM_CSID_PAD_FIRST_SRC + vc];
+	const struct csid_format_info *format = csid_get_fmt_entry(csid->res->formats->formats,
+								   csid->res->formats->nformats,
+								   input_format->code);
+	u8 lane_cnt = csid->phy.lane_cnt;
+	u8 dt_id;
+	u32 val;
+
+	if (!lane_cnt)
+		lane_cnt = 4;
+
+	val = 0;
+	writel(val, csid->base + CSID_RDI_FRM_DROP_PERIOD(vc));
+
+	/*
+	 * DT_ID is a two bit bitfield that is concatenated with
+	 * the four least significant bits of the five bit VC
+	 * bitfield to generate an internal CID value.
+	 *
+	 * CSID_RDI_CFG0(vc)
+	 * DT_ID : 28:27
+	 * VC    : 26:22
+	 * DT    : 21:16
+	 *
+	 * CID   : VC 3:0 << 2 | DT_ID 1:0
+	 */
+	dt_id = vc & 0x03;
+
+	/* note: for non-RDI path, this should be format->decode_format */
+	val |= DECODE_FORMAT_PAYLOAD_ONLY << RDI_CFG0_DECODE_FORMAT;
+	val |= format->data_type << RDI_CFG0_DATA_TYPE;
+	val |= vc << RDI_CFG0_VIRTUAL_CHANNEL;
+	val |= dt_id << RDI_CFG0_DT_ID;
+	writel(val, csid->base + CSID_RDI_CFG0(vc));
+
+	val = RDI_CFG1_TIMESTAMP_STB_FRAME;
+	val |= RDI_CFG1_BYTE_CNTR_EN;
+	val |= RDI_CFG1_TIMESTAMP_EN;
+	val |= RDI_CFG1_DROP_H_EN;
+	val |= RDI_CFG1_DROP_V_EN;
+	val |= RDI_CFG1_CROP_H_EN;
+	val |= RDI_CFG1_CROP_V_EN;
+	val |= RDI_CFG1_PACKING_MIPI;
+
+	writel(val, csid->base + CSID_RDI_CFG1(vc));
+
+	val = 0;
+	writel(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PERIOD(vc));
+
+	val = 1;
+	writel(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PATTERN(vc));
+
+	val = 0;
+	writel(val, csid->base + CSID_RDI_CTRL(vc));
+
+	val = readl(csid->base + CSID_RDI_CFG0(vc));
+	if (enable)
+		val |= RDI_CFG0_ENABLE;
+	else
+		val &= ~RDI_CFG0_ENABLE;
+	writel(val, csid->base + CSID_RDI_CFG0(vc));
+}
+
+static void csid_configure_stream(struct csid_device *csid, u8 enable)
+{
+	int i;
+
+	__csid_configure_top(csid);
+
+       /* Loop through all enabled VCs and configure stream for each */
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++) {
+		if (csid->phy.en_vc & BIT(i)) {
+			__csid_configure_rdi_stream(csid, enable, i);
+			__csid_configure_rx(csid, &csid->phy, i);
+			__csid_ctrl_rdi(csid, enable, i);
+		}
+	}
+}
+
+/*
+ * csid_reset - Trigger reset on CSID module and wait to complete
+ * @csid: CSID device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_reset(struct csid_device *csid)
+{
+	unsigned long time;
+	u32 val;
+	int i;
+
+	reinit_completion(&csid->reset_complete);
+
+	writel(CSID_IRQ_CMD_CLEAR, csid->base + CSID_IRQ_CMD);
+
+	/* preserve registers */
+	val = CSID_RESET_CFG_MODE_IMMEDIATE | CSID_RESET_CFG_LOCATION_COMPLETE;
+	writel(val, csid->base + CSID_RESET_CFG);
+
+	val = CSID_RESET_CMD_HW_RESET | CSID_RESET_CMD_SW_RESET;
+	writel(val, csid->base + CSID_RESET_CMD);
+
+	time = wait_for_completion_timeout(&csid->reset_complete,
+					   msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(csid->camss->dev, "CSID reset timeout\n");
+		return -EIO;
+	}
+
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++) {
+		/* Enable RUP done for the client port */
+		writel(CSID_CSI2_RDIN_RUP_DONE, csid->base + CSID_CSI2_RDIN_IRQ_MASK(i));
+	}
+
+	/* Clear RDI status */
+	writel(~0u, csid->base + CSID_BUF_DONE_IRQ_CLEAR);
+
+	/* Enable BUF_DONE bit for all write-master client ports */
+	writel(~0u, csid->base + CSID_BUF_DONE_IRQ_MASK);
+
+	/* Unmask all TOP interrupts */
+	writel(~0u, csid->base + CSID_TOP_IRQ_MASK);
+
+	return 0;
+}
+
+static void csid_rup_complete(struct csid_device *csid, int rdi)
+{
+	csid_reg_update_clear(csid, rdi);
+}
+
+/*
+ * csid_isr - CSID module interrupt service routine
+ * @irq: Interrupt line
+ * @dev: CSID device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t csid_isr(int irq, void *dev)
+{
+	struct csid_device *csid = dev;
+	u32 buf_done_val, val, val_top;
+	int i;
+
+	/* Latch and clear TOP status */
+	val_top = readl(csid->base + CSID_TOP_IRQ_STATUS);
+	writel(val_top, csid->base + CSID_TOP_IRQ_CLEAR);
+
+	/* Latch and clear CSID_CSI2 status */
+	val = readl(csid->base + CSID_CSI2_RX_IRQ_STATUS);
+	writel(val, csid->base + CSID_CSI2_RX_IRQ_CLEAR);
+
+	/* Latch and clear top level BUF_DONE status */
+	buf_done_val = readl(csid->base + CSID_BUF_DONE_IRQ_STATUS);
+	writel(buf_done_val, csid->base + CSID_BUF_DONE_IRQ_CLEAR);
+
+	/* Process state for each RDI channel */
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++) {
+		val = readl(csid->base + CSID_CSI2_RDIN_IRQ_STATUS(i));
+		if (val)
+			writel(val, csid->base + CSID_CSI2_RDIN_IRQ_CLEAR(i));
+
+		if (val & CSID_CSI2_RDIN_RUP_DONE)
+			csid_rup_complete(csid, i);
+
+		if (buf_done_val & BIT(BUF_DONE_IRQ_STATUS_RDI_OFFSET + i))
+			camss_buf_done(csid->camss, csid->id, i);
+	}
+
+	/* Issue clear command */
+	writel(CSID_IRQ_CMD_CLEAR, csid->base + CSID_IRQ_CMD);
+
+	/* Reset complete */
+	if (val_top & CSID_TOP_IRQ_RESET)
+		complete(&csid->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+static void csid_subdev_reg_update(struct csid_device *csid, int port_id, bool is_clear)
+{
+	if (is_clear)
+		csid_reg_update_clear(csid, port_id);
+	else
+		csid_reg_update(csid, port_id);
+}
+
+static void csid_subdev_init(struct csid_device *csid) {}
+
+const struct csid_hw_ops csid_ops_680 = {
+	.configure_testgen_pattern = NULL,
+	.configure_stream = csid_configure_stream,
+	.hw_version = csid_hw_version,
+	.isr = csid_isr,
+	.reset = csid_reset,
+	.src_pad_code = csid_src_pad_code,
+	.subdev_init = csid_subdev_init,
+	.reg_update = csid_subdev_reg_update,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-csid-780.c b/drivers/media/platform/qcom/camss/camss-csid-780.c
new file mode 100644
index 000000000000..4c720d177731
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-csid-780.c
@@ -0,0 +1,337 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module
+ *
+ * Copyright (c) 2024 Qualcomm Technologies, Inc.
+ */
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+
+#include "camss.h"
+#include "camss-csid.h"
+#include "camss-csid-780.h"
+
+#define CSID_IO_PATH_CFG0(csid)		(0x4 * (csid))
+#define		OUTPUT_IFE_EN			0x100
+#define		INTERNAL_CSID			1
+
+#define CSID_RST_CFG			0xC
+#define		RST_MODE			BIT(0)
+#define		RST_LOCATION			BIT(4)
+
+#define CSID_RST_CMD			0x10
+#define		SELECT_HW_RST			BIT(0)
+#define		SELECT_IRQ_RST			BIT(2)
+
+#define CSID_IRQ_CMD			0x14
+#define		IRQ_CMD_CLEAR			BIT(0)
+
+#define CSID_RUP_AUP_CMD		0x18
+#define		CSID_RUP_AUP_RDI(rdi)		((BIT(4) | BIT(20)) << (rdi))
+
+#define CSID_TOP_IRQ_STATUS		0x7C
+#define		 TOP_IRQ_STATUS_RESET_DONE	BIT(0)
+
+#define CSID_TOP_IRQ_MASK		0x80
+#define CSID_TOP_IRQ_CLEAR		0x84
+#define CSID_TOP_IRQ_SET		0x88
+
+#define CSID_CSI2_RX_IRQ_STATUS		0x9C
+#define CSID_CSI2_RX_IRQ_MASK		0xA0
+#define CSID_CSI2_RX_IRQ_CLEAR		0xA4
+#define CSID_CSI2_RX_IRQ_SET		0xA8
+
+#define CSID_BUF_DONE_IRQ_STATUS	0x8C
+#define		BUF_DONE_IRQ_STATUS_RDI_OFFSET	(csid_is_lite(csid) ? 1 : 14)
+#define CSID_BUF_DONE_IRQ_MASK		0x90
+#define CSID_BUF_DONE_IRQ_CLEAR		0x94
+#define CSID_BUF_DONE_IRQ_SET		0x98
+
+#define CSID_CSI2_RDIN_IRQ_STATUS(rdi)	(0xEC + 0x10 * (rdi))
+#define		RUP_DONE_IRQ_STATUS		BIT(23)
+
+#define CSID_CSI2_RDIN_IRQ_CLEAR(rdi)	(0xF4 + 0x10 * (rdi))
+#define CSID_CSI2_RDIN_IRQ_SET(rdi)	(0xF8 + 0x10 * (rdi))
+
+#define CSID_CSI2_RX_CFG0		0x200
+#define		CSI2_RX_CFG0_NUM_ACTIVE_LANES	0
+#define		CSI2_RX_CFG0_DL0_INPUT_SEL	4
+#define		CSI2_RX_CFG0_PHY_NUM_SEL	20
+
+#define CSID_CSI2_RX_CFG1		0x204
+#define		CSI2_RX_CFG1_ECC_CORRECTION_EN	BIT(0)
+#define		CSI2_RX_CFG1_VC_MODE		BIT(2)
+
+#define CSID_RDI_CFG0(rdi)		(0x500 + 0x100 * (rdi))
+#define		RDI_CFG0_TIMESTAMP_EN		BIT(6)
+#define		RDI_CFG0_TIMESTAMP_STB_SEL	BIT(8)
+#define		RDI_CFG0_DECODE_FORMAT		12
+#define		RDI_CFG0_DT			16
+#define		RDI_CFG0_VC			22
+#define		RDI_CFG0_DT_ID			27
+#define		RDI_CFG0_EN			BIT(31)
+
+#define CSID_RDI_CTRL(rdi)		(0x504 + 0x100 * (rdi))
+#define		RDI_CTRL_START_CMD		BIT(0)
+
+#define CSID_RDI_CFG1(rdi)		(0x510 + 0x100 * (rdi))
+#define		RDI_CFG1_DROP_H_EN		BIT(5)
+#define		RDI_CFG1_DROP_V_EN		BIT(6)
+#define		RDI_CFG1_CROP_H_EN		BIT(7)
+#define		RDI_CFG1_CROP_V_EN		BIT(8)
+#define		RDI_CFG1_PIX_STORE		BIT(10)
+#define		RDI_CFG1_PACKING_FORMAT_MIPI	BIT(15)
+
+#define CSID_RDI_IRQ_SUBSAMPLE_PATTERN(rdi)	(0x548 + 0x100 * (rdi))
+#define CSID_RDI_IRQ_SUBSAMPLE_PERIOD(rdi)	(0x54C + 0x100 * (rdi))
+
+#define CSI2_RX_CFG0_PHY_SEL_BASE_IDX	1
+
+static void __csid_configure_rx(struct csid_device *csid,
+				struct csid_phy_config *phy, int vc)
+{
+	int val;
+
+	val = (phy->lane_cnt - 1) << CSI2_RX_CFG0_NUM_ACTIVE_LANES;
+	val |= phy->lane_assign << CSI2_RX_CFG0_DL0_INPUT_SEL;
+	val |= (phy->csiphy_id + CSI2_RX_CFG0_PHY_SEL_BASE_IDX) << CSI2_RX_CFG0_PHY_NUM_SEL;
+
+	writel(val, csid->base + CSID_CSI2_RX_CFG0);
+
+	val = CSI2_RX_CFG1_ECC_CORRECTION_EN;
+	if (vc > 3)
+		val |= CSI2_RX_CFG1_VC_MODE;
+
+	writel(val, csid->base + CSID_CSI2_RX_CFG1);
+}
+
+static void __csid_ctrl_rdi(struct csid_device *csid, int enable, u8 rdi)
+{
+	int val = 0;
+
+	if (enable)
+		val = RDI_CTRL_START_CMD;
+
+	writel(val, csid->base + CSID_RDI_CTRL(rdi));
+}
+
+static void __csid_configure_wrapper(struct csid_device *csid)
+{
+	u32 val;
+
+	/* csid lite doesn't need to configure top register */
+	if (csid->res->is_lite)
+		return;
+
+	val = OUTPUT_IFE_EN | INTERNAL_CSID;
+	writel(val, csid->camss->csid_wrapper_base + CSID_IO_PATH_CFG0(csid->id));
+}
+
+static void __csid_configure_rdi_stream(struct csid_device *csid, u8 enable, u8 vc)
+{
+	u32 val;
+	u8 lane_cnt = csid->phy.lane_cnt;
+	/* Source pads matching RDI channels on hardware. Pad 1 -> RDI0, Pad 2 -> RDI1, etc. */
+	struct v4l2_mbus_framefmt *input_format = &csid->fmt[MSM_CSID_PAD_FIRST_SRC + vc];
+	const struct csid_format_info *format = csid_get_fmt_entry(csid->res->formats->formats,
+								   csid->res->formats->nformats,
+								   input_format->code);
+
+	if (!lane_cnt)
+		lane_cnt = 4;
+
+	/*
+	 * DT_ID is a two bit bitfield that is concatenated with
+	 * the four least significant bits of the five bit VC
+	 * bitfield to generate an internal CID value.
+	 *
+	 * CSID_RDI_CFG0(vc)
+	 * DT_ID : 28:27
+	 * VC    : 26:22
+	 * DT    : 21:16
+	 *
+	 * CID   : VC 3:0 << 2 | DT_ID 1:0
+	 */
+	u8 dt_id = vc & 0x03;
+
+	val = RDI_CFG0_TIMESTAMP_EN;
+	val |= RDI_CFG0_TIMESTAMP_STB_SEL;
+	/* note: for non-RDI path, this should be format->decode_format */
+	val |= DECODE_FORMAT_PAYLOAD_ONLY << RDI_CFG0_DECODE_FORMAT;
+	val |= vc << RDI_CFG0_VC;
+	val |= format->data_type << RDI_CFG0_DT;
+	val |= dt_id << RDI_CFG0_DT_ID;
+
+	writel(val, csid->base + CSID_RDI_CFG0(vc));
+
+	val = RDI_CFG1_PACKING_FORMAT_MIPI;
+	val |= RDI_CFG1_PIX_STORE;
+	val |= RDI_CFG1_DROP_H_EN;
+	val |= RDI_CFG1_DROP_V_EN;
+	val |= RDI_CFG1_CROP_H_EN;
+	val |= RDI_CFG1_CROP_V_EN;
+
+	writel(val, csid->base + CSID_RDI_CFG1(vc));
+
+	val = 0;
+	writel(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PERIOD(vc));
+
+	val = 1;
+	writel(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PATTERN(vc));
+
+	val = 0;
+	writel(val, csid->base + CSID_RDI_CTRL(vc));
+
+	val = readl(csid->base + CSID_RDI_CFG0(vc));
+
+	if (enable)
+		val |= RDI_CFG0_EN;
+	writel(val, csid->base + CSID_RDI_CFG0(vc));
+}
+
+static void csid_configure_stream(struct csid_device *csid, u8 enable)
+{
+	u8 i;
+
+	__csid_configure_wrapper(csid);
+
+	/* Loop through all enabled VCs and configure stream for each */
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++)
+		if (csid->phy.en_vc & BIT(i)) {
+			__csid_configure_rdi_stream(csid, enable, i);
+			__csid_configure_rx(csid, &csid->phy, i);
+			__csid_ctrl_rdi(csid, enable, i);
+		}
+}
+
+static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
+{
+	return 0;
+}
+
+static void csid_subdev_reg_update(struct csid_device *csid, int port_id, bool clear)
+{
+	if (clear) {
+		csid->reg_update &= ~CSID_RUP_AUP_RDI(port_id);
+	} else {
+		csid->reg_update |= CSID_RUP_AUP_RDI(port_id);
+		writel(csid->reg_update, csid->base + CSID_RUP_AUP_CMD);
+	}
+}
+
+/*
+ * csid_isr - CSID module interrupt service routine
+ * @irq: Interrupt line
+ * @dev: CSID device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t csid_isr(int irq, void *dev)
+{
+	struct csid_device *csid = dev;
+	u32 val, buf_done_val;
+	u8 reset_done;
+	int i;
+
+	val = readl(csid->base + CSID_TOP_IRQ_STATUS);
+	writel(val, csid->base + CSID_TOP_IRQ_CLEAR);
+	reset_done = val & TOP_IRQ_STATUS_RESET_DONE;
+
+	val = readl(csid->base + CSID_CSI2_RX_IRQ_STATUS);
+	writel(val, csid->base + CSID_CSI2_RX_IRQ_CLEAR);
+
+	buf_done_val = readl(csid->base + CSID_BUF_DONE_IRQ_STATUS);
+	writel(buf_done_val, csid->base + CSID_BUF_DONE_IRQ_CLEAR);
+
+	/* Read and clear IRQ status for each enabled RDI channel */
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++)
+		if (csid->phy.en_vc & BIT(i)) {
+			val = readl(csid->base + CSID_CSI2_RDIN_IRQ_STATUS(i));
+			writel(val, csid->base + CSID_CSI2_RDIN_IRQ_CLEAR(i));
+
+			if (val & RUP_DONE_IRQ_STATUS)
+				/* clear the reg update bit */
+				csid_subdev_reg_update(csid, i, true);
+
+			if (buf_done_val & BIT(BUF_DONE_IRQ_STATUS_RDI_OFFSET + i)) {
+				/*
+				 * For Titan 780, bus done and RUP IRQ have been moved to
+				 * CSID from VFE. Once CSID received bus done, need notify
+				 * VFE of this event. Trigger VFE to handle bus done process.
+				 */
+				camss_buf_done(csid->camss, csid->id, i);
+			}
+		}
+
+	val = IRQ_CMD_CLEAR;
+	writel(val, csid->base + CSID_IRQ_CMD);
+
+	if (reset_done)
+		complete(&csid->reset_complete);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * csid_reset - Trigger reset on CSID module and wait to complete
+ * @csid: CSID device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int csid_reset(struct csid_device *csid)
+{
+	unsigned long time;
+	u32 val;
+	int i;
+
+	reinit_completion(&csid->reset_complete);
+
+	writel(1, csid->base + CSID_TOP_IRQ_CLEAR);
+	writel(1, csid->base + CSID_IRQ_CMD);
+	writel(1, csid->base + CSID_TOP_IRQ_MASK);
+
+	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++)
+		if (csid->phy.en_vc & BIT(i)) {
+			writel(BIT(BUF_DONE_IRQ_STATUS_RDI_OFFSET + i),
+			       csid->base + CSID_BUF_DONE_IRQ_CLEAR);
+			writel(IRQ_CMD_CLEAR, csid->base + CSID_IRQ_CMD);
+			writel(BIT(BUF_DONE_IRQ_STATUS_RDI_OFFSET + i),
+			       csid->base + CSID_BUF_DONE_IRQ_MASK);
+		}
+
+	/* preserve registers */
+	val = RST_LOCATION | RST_MODE;
+	writel(val, csid->base + CSID_RST_CFG);
+
+	val = SELECT_HW_RST | SELECT_IRQ_RST;
+	writel(val, csid->base + CSID_RST_CMD);
+
+	time = wait_for_completion_timeout(&csid->reset_complete,
+					   msecs_to_jiffies(CSID_RESET_TIMEOUT_MS));
+	if (!time) {
+		dev_err(csid->camss->dev, "CSID reset timeout\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void csid_subdev_init(struct csid_device *csid)
+{
+	csid->testgen.nmodes = CSID_PAYLOAD_MODE_DISABLED;
+}
+
+const struct csid_hw_ops csid_ops_780 = {
+	.configure_stream = csid_configure_stream,
+	.configure_testgen_pattern = csid_configure_testgen_pattern,
+	.hw_version = csid_hw_version,
+	.isr = csid_isr,
+	.reset = csid_reset,
+	.src_pad_code = csid_src_pad_code,
+	.subdev_init = csid_subdev_init,
+	.reg_update = csid_subdev_reg_update,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-csid-780.h b/drivers/media/platform/qcom/camss/camss-csid-780.h
new file mode 100644
index 000000000000..a990c66a60ff
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-csid-780.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * camss-csid-780.h
+ *
+ * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module Generation 3
+ *
+ * Copyright (c) 2024 Qualcomm Technologies, Inc.
+ */
+#ifndef __QC_MSM_CAMSS_CSID_780_H__
+#define __QC_MSM_CAMSS_CSID_780_H__
+
+#define DECODE_FORMAT_UNCOMPRESSED_8_BIT	0x1
+#define DECODE_FORMAT_UNCOMPRESSED_10_BIT	0x2
+#define DECODE_FORMAT_UNCOMPRESSED_12_BIT	0x3
+#define DECODE_FORMAT_UNCOMPRESSED_14_BIT	0x4
+#define DECODE_FORMAT_UNCOMPRESSED_16_BIT	0x5
+#define DECODE_FORMAT_UNCOMPRESSED_20_BIT	0x6
+#define DECODE_FORMAT_UNCOMPRESSED_24_BIT	0x7
+#define DECODE_FORMAT_PAYLOAD_ONLY		0xf
+
+#define PLAIN_FORMAT_PLAIN8	0x0 /* supports DPCM, UNCOMPRESSED_6/8_BIT */
+#define PLAIN_FORMAT_PLAIN16	0x1 /* supports DPCM, UNCOMPRESSED_10/16_BIT */
+#define PLAIN_FORMAT_PLAIN32	0x2 /* supports UNCOMPRESSED_20_BIT */
+
+#endif /* __QC_MSM_CAMSS_CSID_780_H__ */
diff --git a/drivers/media/platform/qcom/camss/camss-csid-gen2.c b/drivers/media/platform/qcom/camss/camss-csid-gen2.c
index b11de4797cca..2a1746dcc1c5 100644
--- a/drivers/media/platform/qcom/camss/camss-csid-gen2.c
+++ b/drivers/media/platform/qcom/camss/camss-csid-gen2.c
@@ -22,11 +22,6 @@
  * alternate register layout.
  */
 
-#define CSID_HW_VERSION		0x0
-#define		HW_VERSION_STEPPING	0
-#define		HW_VERSION_REVISION	16
-#define		HW_VERSION_GENERATION	28
-
 #define CSID_RST_STROBES	0x10
 #define		RST_STROBES	0
 
@@ -176,306 +171,171 @@
 #define		TPG_COLOR_BOX_CFG_MODE		0
 #define		TPG_COLOR_BOX_PATTERN_SEL	2
 
-static const struct csid_format csid_formats[] = {
-	{
-		MEDIA_BUS_FMT_UYVY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_VYUY8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YUYV8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_YVYU8_1X16,
-		DATA_TYPE_YUV422_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		2,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y8_1X8,
-		DATA_TYPE_RAW_8BIT,
-		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
-		8,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_Y10_1X10,
-		DATA_TYPE_RAW_10BIT,
-		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
-		10,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB12_1X12,
-		DATA_TYPE_RAW_12BIT,
-		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
-		12,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SBGGR14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGBRG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SGRBG14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-	{
-		MEDIA_BUS_FMT_SRGGB14_1X14,
-		DATA_TYPE_RAW_14BIT,
-		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
-		14,
-		1,
-	},
-};
-
-static void __csid_configure_stream(struct csid_device *csid, u8 enable, u8 vc)
+static void __csid_configure_rx(struct csid_device *csid,
+				struct csid_phy_config *phy, int vc)
 {
-	struct csid_testgen_config *tg = &csid->testgen;
-	u32 val;
-	u32 phy_sel = 0;
 	u8 lane_cnt = csid->phy.lane_cnt;
-	/* Source pads matching RDI channels on hardware. Pad 1 -> RDI0, Pad 2 -> RDI1, etc. */
-	struct v4l2_mbus_framefmt *input_format = &csid->fmt[MSM_CSID_PAD_FIRST_SRC + vc];
-	const struct csid_format *format = csid_get_fmt_entry(csid->formats, csid->nformats,
-							      input_format->code);
+	int val;
 
 	if (!lane_cnt)
 		lane_cnt = 4;
 
-	if (!tg->enabled)
-		phy_sel = csid->phy.csiphy_id;
-
-	if (enable) {
-		/*
-		 * DT_ID is a two bit bitfield that is concatenated with
-		 * the four least significant bits of the five bit VC
-		 * bitfield to generate an internal CID value.
-		 *
-		 * CSID_RDI_CFG0(vc)
-		 * DT_ID : 28:27
-		 * VC    : 26:22
-		 * DT    : 21:16
-		 *
-		 * CID   : VC 3:0 << 2 | DT_ID 1:0
-		 */
-		u8 dt_id = vc & 0x03;
-
-		if (tg->enabled) {
-			/* configure one DT, infinite frames */
-			val = vc << TPG_VC_CFG0_VC_NUM;
-			val |= INTELEAVING_MODE_ONE_SHOT << TPG_VC_CFG0_LINE_INTERLEAVING_MODE;
-			val |= 0 << TPG_VC_CFG0_NUM_FRAMES;
-			writel_relaxed(val, csid->base + CSID_TPG_VC_CFG0);
-
-			val = 0x740 << TPG_VC_CFG1_H_BLANKING_COUNT;
-			val |= 0x3ff << TPG_VC_CFG1_V_BLANKING_COUNT;
-			writel_relaxed(val, csid->base + CSID_TPG_VC_CFG1);
-
-			writel_relaxed(0x12345678, csid->base + CSID_TPG_LFSR_SEED);
-
-			val = (input_format->height & 0x1fff) << TPG_DT_n_CFG_0_FRAME_HEIGHT;
-			val |= (input_format->width & 0x1fff) << TPG_DT_n_CFG_0_FRAME_WIDTH;
-			writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_0(0));
-
-			val = format->data_type << TPG_DT_n_CFG_1_DATA_TYPE;
-			writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_1(0));
-
-			val = (tg->mode - 1) << TPG_DT_n_CFG_2_PAYLOAD_MODE;
-			val |= 0xBE << TPG_DT_n_CFG_2_USER_SPECIFIED_PAYLOAD;
-			val |= format->decode_format << TPG_DT_n_CFG_2_ENCODE_FORMAT;
-			writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_2(0));
-
-			writel_relaxed(0, csid->base + CSID_TPG_COLOR_BARS_CFG);
-
-			writel_relaxed(0, csid->base + CSID_TPG_COLOR_BOX_CFG);
-		}
+	val = (lane_cnt - 1) << CSI2_RX_CFG0_NUM_ACTIVE_LANES;
+	val |= phy->lane_assign << CSI2_RX_CFG0_DL0_INPUT_SEL;
+	val |= phy->csiphy_id << CSI2_RX_CFG0_PHY_NUM_SEL;
+	writel_relaxed(val, csid->base + CSID_CSI2_RX_CFG0);
 
-		val = 1 << RDI_CFG0_BYTE_CNTR_EN;
-		val |= 1 << RDI_CFG0_FORMAT_MEASURE_EN;
-		val |= 1 << RDI_CFG0_TIMESTAMP_EN;
-		/* note: for non-RDI path, this should be format->decode_format */
-		val |= DECODE_FORMAT_PAYLOAD_ONLY << RDI_CFG0_DECODE_FORMAT;
-		val |= format->data_type << RDI_CFG0_DATA_TYPE;
-		val |= vc << RDI_CFG0_VIRTUAL_CHANNEL;
-		val |= dt_id << RDI_CFG0_DT_ID;
-		writel_relaxed(val, csid->base + CSID_RDI_CFG0(vc));
+	val = 1 << CSI2_RX_CFG1_PACKET_ECC_CORRECTION_EN;
+	if (vc > 3)
+		val |= 1 << CSI2_RX_CFG1_VC_MODE;
+	val |= 1 << CSI2_RX_CFG1_MISR_EN;
+	writel_relaxed(val, csid->base + CSID_CSI2_RX_CFG1);
+}
 
-		/* CSID_TIMESTAMP_STB_POST_IRQ */
-		val = 2 << RDI_CFG1_TIMESTAMP_STB_SEL;
-		writel_relaxed(val, csid->base + CSID_RDI_CFG1(vc));
+static void __csid_ctrl_rdi(struct csid_device *csid, int enable, u8 rdi)
+{
+	int val;
 
-		val = 1;
-		writel_relaxed(val, csid->base + CSID_RDI_FRM_DROP_PERIOD(vc));
+	if (enable)
+		val = HALT_CMD_RESUME_AT_FRAME_BOUNDARY << RDI_CTRL_HALT_CMD;
+	else
+		val = HALT_CMD_HALT_AT_FRAME_BOUNDARY << RDI_CTRL_HALT_CMD;
+	writel_relaxed(val, csid->base + CSID_RDI_CTRL(rdi));
+}
 
-		val = 0;
-		writel_relaxed(val, csid->base + CSID_RDI_FRM_DROP_PATTERN(vc));
+static void __csid_configure_testgen(struct csid_device *csid, u8 enable, u8 vc)
+{
+	struct csid_testgen_config *tg = &csid->testgen;
+	struct v4l2_mbus_framefmt *input_format = &csid->fmt[MSM_CSID_PAD_FIRST_SRC + vc];
+	const struct csid_format_info *format = csid_get_fmt_entry(csid->res->formats->formats,
+								   csid->res->formats->nformats,
+								   input_format->code);
+	u8 lane_cnt = csid->phy.lane_cnt;
+	u32 val;
 
-		val = 1;
-		writel_relaxed(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PERIOD(vc));
+	if (!lane_cnt)
+		lane_cnt = 4;
 
-		val = 0;
-		writel_relaxed(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PATTERN(vc));
+	/* configure one DT, infinite frames */
+	val = vc << TPG_VC_CFG0_VC_NUM;
+	val |= INTELEAVING_MODE_ONE_SHOT << TPG_VC_CFG0_LINE_INTERLEAVING_MODE;
+	val |= 0 << TPG_VC_CFG0_NUM_FRAMES;
+	writel_relaxed(val, csid->base + CSID_TPG_VC_CFG0);
 
-		val = 1;
-		writel_relaxed(val, csid->base + CSID_RDI_RPP_PIX_DROP_PERIOD(vc));
+	val = 0x740 << TPG_VC_CFG1_H_BLANKING_COUNT;
+	val |= 0x3ff << TPG_VC_CFG1_V_BLANKING_COUNT;
+	writel_relaxed(val, csid->base + CSID_TPG_VC_CFG1);
 
-		val = 0;
-		writel_relaxed(val, csid->base + CSID_RDI_RPP_PIX_DROP_PATTERN(vc));
+	writel_relaxed(0x12345678, csid->base + CSID_TPG_LFSR_SEED);
 
-		val = 1;
-		writel_relaxed(val, csid->base + CSID_RDI_RPP_LINE_DROP_PERIOD(vc));
+	val = (input_format->height & 0x1fff) << TPG_DT_n_CFG_0_FRAME_HEIGHT;
+	val |= (input_format->width & 0x1fff) << TPG_DT_n_CFG_0_FRAME_WIDTH;
+	writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_0(0));
 
-		val = 0;
-		writel_relaxed(val, csid->base + CSID_RDI_RPP_LINE_DROP_PATTERN(vc));
+	val = format->data_type << TPG_DT_n_CFG_1_DATA_TYPE;
+	writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_1(0));
 
-		val = 0;
-		writel_relaxed(val, csid->base + CSID_RDI_CTRL(vc));
+	val = (tg->mode - 1) << TPG_DT_n_CFG_2_PAYLOAD_MODE;
+	val |= 0xBE << TPG_DT_n_CFG_2_USER_SPECIFIED_PAYLOAD;
+	val |= format->decode_format << TPG_DT_n_CFG_2_ENCODE_FORMAT;
+	writel_relaxed(val, csid->base + CSID_TPG_DT_n_CFG_2(0));
 
-		val = readl_relaxed(csid->base + CSID_RDI_CFG0(vc));
-		val |=  1 << RDI_CFG0_ENABLE;
-		writel_relaxed(val, csid->base + CSID_RDI_CFG0(vc));
-	}
+	writel_relaxed(0, csid->base + CSID_TPG_COLOR_BARS_CFG);
 
-	if (tg->enabled) {
-		val = enable << TPG_CTRL_TEST_EN;
-		val |= 1 << TPG_CTRL_FS_PKT_EN;
-		val |= 1 << TPG_CTRL_FE_PKT_EN;
-		val |= (lane_cnt - 1) << TPG_CTRL_NUM_ACTIVE_LANES;
-		val |= 0x64 << TPG_CTRL_CYCLES_BETWEEN_PKTS;
-		val |= 0xA << TPG_CTRL_NUM_TRAIL_BYTES;
-		writel_relaxed(val, csid->base + CSID_TPG_CTRL);
-	}
+	writel_relaxed(0, csid->base + CSID_TPG_COLOR_BOX_CFG);
 
-	val = (lane_cnt - 1) << CSI2_RX_CFG0_NUM_ACTIVE_LANES;
-	val |= csid->phy.lane_assign << CSI2_RX_CFG0_DL0_INPUT_SEL;
-	val |= phy_sel << CSI2_RX_CFG0_PHY_NUM_SEL;
-	writel_relaxed(val, csid->base + CSID_CSI2_RX_CFG0);
+	val = enable << TPG_CTRL_TEST_EN;
+	val |= 1 << TPG_CTRL_FS_PKT_EN;
+	val |= 1 << TPG_CTRL_FE_PKT_EN;
+	val |= (lane_cnt - 1) << TPG_CTRL_NUM_ACTIVE_LANES;
+	val |= 0x64 << TPG_CTRL_CYCLES_BETWEEN_PKTS;
+	val |= 0xA << TPG_CTRL_NUM_TRAIL_BYTES;
+	writel_relaxed(val, csid->base + CSID_TPG_CTRL);
+}
 
-	val = 1 << CSI2_RX_CFG1_PACKET_ECC_CORRECTION_EN;
-	if (vc > 3)
-		val |= 1 << CSI2_RX_CFG1_VC_MODE;
-	val |= 1 << CSI2_RX_CFG1_MISR_EN;
-	writel_relaxed(val, csid->base + CSID_CSI2_RX_CFG1);
+static void __csid_configure_rdi_stream(struct csid_device *csid, u8 enable, u8 vc)
+{
+	/* Source pads matching RDI channels on hardware. Pad 1 -> RDI0, Pad 2 -> RDI1, etc. */
+	struct v4l2_mbus_framefmt *input_format = &csid->fmt[MSM_CSID_PAD_FIRST_SRC + vc];
+	const struct csid_format_info *format = csid_get_fmt_entry(csid->res->formats->formats,
+								   csid->res->formats->nformats,
+								   input_format->code);
+	u32 val;
 
-	if (enable)
-		val = HALT_CMD_RESUME_AT_FRAME_BOUNDARY << RDI_CTRL_HALT_CMD;
-	else
-		val = HALT_CMD_HALT_AT_FRAME_BOUNDARY << RDI_CTRL_HALT_CMD;
+	/*
+	 * DT_ID is a two bit bitfield that is concatenated with
+	 * the four least significant bits of the five bit VC
+	 * bitfield to generate an internal CID value.
+	 *
+	 * CSID_RDI_CFG0(vc)
+	 * DT_ID : 28:27
+	 * VC    : 26:22
+	 * DT    : 21:16
+	 *
+	 * CID   : VC 3:0 << 2 | DT_ID 1:0
+	 */
+	u8 dt_id = vc & 0x03;
+
+	val = 1 << RDI_CFG0_BYTE_CNTR_EN;
+	val |= 1 << RDI_CFG0_FORMAT_MEASURE_EN;
+	val |= 1 << RDI_CFG0_TIMESTAMP_EN;
+	/* note: for non-RDI path, this should be format->decode_format */
+	val |= DECODE_FORMAT_PAYLOAD_ONLY << RDI_CFG0_DECODE_FORMAT;
+	val |= format->data_type << RDI_CFG0_DATA_TYPE;
+	val |= vc << RDI_CFG0_VIRTUAL_CHANNEL;
+	val |= dt_id << RDI_CFG0_DT_ID;
+	writel_relaxed(val, csid->base + CSID_RDI_CFG0(vc));
+
+	/* CSID_TIMESTAMP_STB_POST_IRQ */
+	val = 2 << RDI_CFG1_TIMESTAMP_STB_SEL;
+	writel_relaxed(val, csid->base + CSID_RDI_CFG1(vc));
+
+	val = 1;
+	writel_relaxed(val, csid->base + CSID_RDI_FRM_DROP_PERIOD(vc));
+
+	val = 0;
+	writel_relaxed(val, csid->base + CSID_RDI_FRM_DROP_PATTERN(vc));
+
+	val = 1;
+	writel_relaxed(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PERIOD(vc));
+
+	val = 0;
+	writel_relaxed(val, csid->base + CSID_RDI_IRQ_SUBSAMPLE_PATTERN(vc));
+
+	val = 1;
+	writel_relaxed(val, csid->base + CSID_RDI_RPP_PIX_DROP_PERIOD(vc));
+
+	val = 0;
+	writel_relaxed(val, csid->base + CSID_RDI_RPP_PIX_DROP_PATTERN(vc));
+
+	val = 1;
+	writel_relaxed(val, csid->base + CSID_RDI_RPP_LINE_DROP_PERIOD(vc));
+
+	val = 0;
+	writel_relaxed(val, csid->base + CSID_RDI_RPP_LINE_DROP_PATTERN(vc));
+
+	val = 0;
 	writel_relaxed(val, csid->base + CSID_RDI_CTRL(vc));
+
+	val = readl_relaxed(csid->base + CSID_RDI_CFG0(vc));
+	val |=  enable << RDI_CFG0_ENABLE;
+	writel_relaxed(val, csid->base + CSID_RDI_CFG0(vc));
 }
 
 static void csid_configure_stream(struct csid_device *csid, u8 enable)
 {
+	struct csid_testgen_config *tg = &csid->testgen;
 	u8 i;
 	/* Loop through all enabled VCs and configure stream for each */
 	for (i = 0; i < MSM_CSID_MAX_SRC_STREAMS; i++)
-		if (csid->phy.en_vc & BIT(i))
-			__csid_configure_stream(csid, enable, i);
+		if (csid->phy.en_vc & BIT(i)) {
+			if (tg->enabled)
+				__csid_configure_testgen(csid, enable, i);
+
+			__csid_configure_rdi_stream(csid, enable, i);
+			__csid_configure_rx(csid, &csid->phy, i);
+			__csid_ctrl_rdi(csid, enable, i);
+		}
 }
 
 static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
@@ -487,29 +347,6 @@ static int csid_configure_testgen_pattern(struct csid_device *csid, s32 val)
 }
 
 /*
- * csid_hw_version - CSID hardware version query
- * @csid: CSID device
- *
- * Return HW version or error
- */
-static u32 csid_hw_version(struct csid_device *csid)
-{
-	u32 hw_version;
-	u32 hw_gen;
-	u32 hw_rev;
-	u32 hw_step;
-
-	hw_version = readl_relaxed(csid->base + CSID_HW_VERSION);
-	hw_gen = (hw_version >> HW_VERSION_GENERATION) & 0xF;
-	hw_rev = (hw_version >> HW_VERSION_REVISION) & 0xFFF;
-	hw_step = (hw_version >> HW_VERSION_STEPPING) & 0xFFFF;
-	dev_dbg(csid->camss->dev, "CSID HW Version = %u.%u.%u\n",
-		hw_gen, hw_rev, hw_step);
-
-	return hw_version;
-}
-
-/*
  * csid_isr - CSID module interrupt service routine
  * @irq: Interrupt line
  * @dev: CSID device
@@ -578,42 +415,8 @@ static int csid_reset(struct csid_device *csid)
 	return 0;
 }
 
-static u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
-			     unsigned int match_format_idx, u32 match_code)
-{
-	switch (sink_code) {
-	case MEDIA_BUS_FMT_SBGGR10_1X10:
-	{
-		u32 src_code[] = {
-			MEDIA_BUS_FMT_SBGGR10_1X10,
-			MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
-		};
-
-		return csid_find_code(src_code, ARRAY_SIZE(src_code),
-				      match_format_idx, match_code);
-	}
-	case MEDIA_BUS_FMT_Y10_1X10:
-	{
-		u32 src_code[] = {
-			MEDIA_BUS_FMT_Y10_1X10,
-			MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
-		};
-
-		return csid_find_code(src_code, ARRAY_SIZE(src_code),
-				      match_format_idx, match_code);
-	}
-	default:
-		if (match_format_idx > 0)
-			return 0;
-
-		return sink_code;
-	}
-}
-
 static void csid_subdev_init(struct csid_device *csid)
 {
-	csid->formats = csid_formats;
-	csid->nformats = ARRAY_SIZE(csid_formats);
 	csid->testgen.modes = csid_testgen_modes;
 	csid->testgen.nmodes = CSID_PAYLOAD_MODE_NUM_SUPPORTED_GEN2;
 }
diff --git a/drivers/media/platform/qcom/camss/camss-csid.c b/drivers/media/platform/qcom/camss/camss-csid.c
index eb27d69e89a1..5284b5857368 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.c
+++ b/drivers/media/platform/qcom/camss/camss-csid.c
@@ -17,6 +17,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
 #include <media/media-entity.h>
+#include <media/mipi-csi2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-subdev.h>
@@ -29,6 +30,11 @@
 #define VFE_480_CSID_OFFSET 0x1200
 #define VFE_480_LITE_CSID_OFFSET 0x200
 
+#define CSID_HW_VERSION		0x0
+#define		HW_VERSION_STEPPING	0
+#define		HW_VERSION_REVISION	16
+#define		HW_VERSION_GENERATION	28
+
 #define MSM_CSID_NAME "msm_csid"
 
 const char * const csid_testgen_modes[] = {
@@ -45,6 +51,450 @@ const char * const csid_testgen_modes[] = {
 	NULL
 };
 
+static const struct csid_format_info formats_4_1[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+};
+
+static const struct csid_format_info formats_4_7[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+};
+
+static const struct csid_format_info formats_gen2[] = {
+	{
+		MEDIA_BUS_FMT_UYVY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_VYUY8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YUYV8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_YVYU8_1X16,
+		MIPI_CSI2_DT_YUV422_8B,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		2,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y8_1X8,
+		MIPI_CSI2_DT_RAW8,
+		DECODE_FORMAT_UNCOMPRESSED_8_BIT,
+		8,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_Y10_1X10,
+		MIPI_CSI2_DT_RAW10,
+		DECODE_FORMAT_UNCOMPRESSED_10_BIT,
+		10,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB12_1X12,
+		MIPI_CSI2_DT_RAW12,
+		DECODE_FORMAT_UNCOMPRESSED_12_BIT,
+		12,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SBGGR14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGBRG14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SGRBG14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+	{
+		MEDIA_BUS_FMT_SRGGB14_1X14,
+		MIPI_CSI2_DT_RAW14,
+		DECODE_FORMAT_UNCOMPRESSED_14_BIT,
+		14,
+		1,
+	},
+};
+
+const struct csid_formats csid_formats_4_1 = {
+	.nformats = ARRAY_SIZE(formats_4_1),
+	.formats = formats_4_1
+};
+
+const struct csid_formats csid_formats_4_7 = {
+	.nformats = ARRAY_SIZE(formats_4_7),
+	.formats = formats_4_7
+};
+
+const struct csid_formats csid_formats_gen2 = {
+	.nformats = ARRAY_SIZE(formats_gen2),
+	.formats = formats_gen2
+};
+
 u32 csid_find_code(u32 *codes, unsigned int ncodes,
 		   unsigned int match_format_idx, u32 match_code)
 {
@@ -65,9 +515,9 @@ u32 csid_find_code(u32 *codes, unsigned int ncodes,
 	return codes[0];
 }
 
-const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
-					     unsigned int nformats,
-					     u32 code)
+const struct csid_format_info *csid_get_fmt_entry(const struct csid_format_info *formats,
+						  unsigned int nformats,
+						  u32 code)
 {
 	unsigned int i;
 
@@ -87,12 +537,12 @@ const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
 static int csid_set_clock_rates(struct csid_device *csid)
 {
 	struct device *dev = csid->camss->dev;
-	const struct csid_format *fmt;
+	const struct csid_format_info *fmt;
 	s64 link_freq;
 	int i, j;
 	int ret;
 
-	fmt = csid_get_fmt_entry(csid->formats, csid->nformats,
+	fmt = csid_get_fmt_entry(csid->res->formats->formats, csid->res->formats->nformats,
 				 csid->fmt[MSM_CSIPHY_PAD_SINK].code);
 	link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,
 					csid->phy.lane_cnt);
@@ -147,6 +597,78 @@ static int csid_set_clock_rates(struct csid_device *csid)
 }
 
 /*
+ * csid_hw_version - CSID hardware version query
+ * @csid: CSID device
+ *
+ * Return HW version or error
+ */
+u32 csid_hw_version(struct csid_device *csid)
+{
+	u32 hw_version;
+	u32 hw_gen;
+	u32 hw_rev;
+	u32 hw_step;
+
+	hw_version = readl_relaxed(csid->base + CSID_HW_VERSION);
+	hw_gen = (hw_version >> HW_VERSION_GENERATION) & 0xF;
+	hw_rev = (hw_version >> HW_VERSION_REVISION) & 0xFFF;
+	hw_step = (hw_version >> HW_VERSION_STEPPING) & 0xFFFF;
+	dev_dbg(csid->camss->dev, "CSID:%d HW Version = %u.%u.%u\n",
+		csid->id, hw_gen, hw_rev, hw_step);
+
+	return hw_version;
+}
+
+/*
+ * csid_src_pad_code - Pick an output/src format based on the input/sink format
+ * @csid: CSID device
+ * @sink_code: The sink format of the input
+ * @match_format_idx: Request preferred index, as defined by subdevice csid
+ *                    format. Set @match_code to 0 if used.
+ * @match_code: Request preferred code, set @match_format_idx to 0 if used
+ *
+ * Return 0 on failure or src format code otherwise
+ */
+u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
+		      unsigned int match_format_idx, u32 match_code)
+{
+	if (csid->camss->res->version == CAMSS_8x16) {
+		if (match_format_idx > 0)
+			return 0;
+
+		return sink_code;
+	}
+
+	switch (sink_code) {
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	{
+		u32 src_code[] = {
+			MEDIA_BUS_FMT_SBGGR10_1X10,
+			MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE,
+		};
+
+		return csid_find_code(src_code, ARRAY_SIZE(src_code),
+				      match_format_idx, match_code);
+	}
+	case MEDIA_BUS_FMT_Y10_1X10:
+	{
+		u32 src_code[] = {
+			MEDIA_BUS_FMT_Y10_1X10,
+			MEDIA_BUS_FMT_Y10_2X8_PADHI_LE,
+		};
+
+		return csid_find_code(src_code, ARRAY_SIZE(src_code),
+				      match_format_idx, match_code);
+	}
+	default:
+		if (match_format_idx > 0)
+			return 0;
+
+		return sink_code;
+	}
+}
+
+/*
  * csid_set_power - Power on/off CSID module
  * @sd: CSID V4L2 subdevice
  * @on: Requested power state
@@ -158,7 +680,6 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
 	struct camss *camss = csid->camss;
 	struct device *dev = camss->dev;
-	struct vfe_device *vfe = &camss->vfe[csid->id];
 	int ret = 0;
 
 	if (on) {
@@ -167,7 +688,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 		 * switching on the CSID. Do so unconditionally, as there is no
 		 * drawback in following the same powering order on older SoCs.
 		 */
-		ret = vfe_get(vfe);
+		ret = csid->res->parent_dev_ops->get(camss, csid->id);
 		if (ret < 0)
 			return ret;
 
@@ -202,7 +723,7 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 
 		enable_irq(csid->irq);
 
-		ret = csid->ops->reset(csid);
+		ret = csid->res->hw_ops->reset(csid);
 		if (ret < 0) {
 			disable_irq(csid->irq);
 			camss_disable_clocks(csid->nclocks, csid->clock);
@@ -212,14 +733,14 @@ static int csid_set_power(struct v4l2_subdev *sd, int on)
 			return ret;
 		}
 
-		csid->ops->hw_version(csid);
+		csid->res->hw_ops->hw_version(csid);
 	} else {
 		disable_irq(csid->irq);
 		camss_disable_clocks(csid->nclocks, csid->clock);
 		regulator_bulk_disable(csid->num_supplies,
 				       csid->supplies);
 		pm_runtime_put_sync(dev);
-		vfe_put(vfe);
+		csid->res->parent_dev_ops->put(camss, csid->id);
 	}
 
 	return ret;
@@ -240,11 +761,13 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 	int ret;
 
 	if (enable) {
-		ret = v4l2_ctrl_handler_setup(&csid->ctrls);
-		if (ret < 0) {
-			dev_err(csid->camss->dev,
-				"could not sync v4l2 controls: %d\n", ret);
-			return ret;
+		if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
+			ret = v4l2_ctrl_handler_setup(&csid->ctrls);
+			if (ret < 0) {
+				dev_err(csid->camss->dev,
+					"could not sync v4l2 controls: %d\n", ret);
+				return ret;
+			}
 		}
 
 		if (!csid->testgen.enabled &&
@@ -253,7 +776,7 @@ static int csid_set_stream(struct v4l2_subdev *sd, int enable)
 	}
 
 	if (csid->phy.need_vc_update) {
-		csid->ops->configure_stream(csid, enable);
+		csid->res->hw_ops->configure_stream(csid, enable);
 		csid->phy.need_vc_update = false;
 	}
 
@@ -301,12 +824,12 @@ static void csid_try_format(struct csid_device *csid,
 	case MSM_CSID_PAD_SINK:
 		/* Set format on sink pad */
 
-		for (i = 0; i < csid->nformats; i++)
-			if (fmt->code == csid->formats[i].code)
+		for (i = 0; i < csid->res->formats->nformats; i++)
+			if (fmt->code == csid->res->formats->formats[i].code)
 				break;
 
 		/* If not found, use UYVY as default */
-		if (i >= csid->nformats)
+		if (i >= csid->res->formats->nformats)
 			fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
 
 		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
@@ -318,24 +841,25 @@ static void csid_try_format(struct csid_device *csid,
 		break;
 
 	case MSM_CSID_PAD_SRC:
-		if (csid->testgen_mode->cur.val == 0) {
+		if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
+		    csid->testgen_mode->cur.val == 0) {
 			/* Test generator is disabled, */
 			/* keep pad formats in sync */
 			u32 code = fmt->code;
 
 			*fmt = *__csid_get_format(csid, sd_state,
 						      MSM_CSID_PAD_SINK, which);
-			fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);
+			fmt->code = csid->res->hw_ops->src_pad_code(csid, fmt->code, 0, code);
 		} else {
 			/* Test generator is enabled, set format on source */
 			/* pad to allow test generator usage */
 
-			for (i = 0; i < csid->nformats; i++)
-				if (csid->formats[i].code == fmt->code)
+			for (i = 0; i < csid->res->formats->nformats; i++)
+				if (csid->res->formats->formats[i].code == fmt->code)
 					break;
 
 			/* If not found, use UYVY as default */
-			if (i >= csid->nformats)
+			if (i >= csid->res->formats->nformats)
 				fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
 
 			fmt->width = clamp_t(u32, fmt->width, 1, 8191);
@@ -363,27 +887,28 @@ static int csid_enum_mbus_code(struct v4l2_subdev *sd,
 	struct csid_device *csid = v4l2_get_subdevdata(sd);
 
 	if (code->pad == MSM_CSID_PAD_SINK) {
-		if (code->index >= csid->nformats)
+		if (code->index >= csid->res->formats->nformats)
 			return -EINVAL;
 
-		code->code = csid->formats[code->index].code;
+		code->code = csid->res->formats->formats[code->index].code;
 	} else {
-		if (csid->testgen_mode->cur.val == 0) {
+		if (csid->testgen.nmodes == CSID_PAYLOAD_MODE_DISABLED ||
+		    csid->testgen_mode->cur.val == 0) {
 			struct v4l2_mbus_framefmt *sink_fmt;
 
 			sink_fmt = __csid_get_format(csid, sd_state,
 						     MSM_CSID_PAD_SINK,
 						     code->which);
 
-			code->code = csid->ops->src_pad_code(csid, sink_fmt->code,
-						       code->index, 0);
+			code->code = csid->res->hw_ops->src_pad_code(csid, sink_fmt->code,
+								     code->index, 0);
 			if (!code->code)
 				return -EINVAL;
 		} else {
-			if (code->index >= csid->nformats)
+			if (code->index >= csid->res->formats->nformats)
 				return -EINVAL;
 
-			code->code = csid->formats[code->index].code;
+			code->code = csid->res->formats->formats[code->index].code;
 		}
 	}
 
@@ -529,7 +1054,7 @@ static int csid_set_test_pattern(struct csid_device *csid, s32 value)
 
 	tg->enabled = !!value;
 
-	return csid->ops->configure_testgen_pattern(csid, value);
+	return csid->res->hw_ops->configure_testgen_pattern(csid, value);
 }
 
 /*
@@ -575,9 +1100,14 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 
 	csid->camss = camss;
 	csid->id = id;
-	csid->ops = res->ops;
+	csid->res = &res->csid;
 
-	csid->ops->subdev_init(csid);
+	if (dev_WARN_ONCE(dev, !csid->res->parent_dev_ops,
+			  "Error: CSID depends on VFE/IFE device ops!\n")) {
+		return -EINVAL;
+	}
+
+	csid->res->hw_ops->subdev_init(csid);
 
 	/* Memory */
 
@@ -587,9 +1117,11 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 		 * VFE to be initialized before CSID
 		 */
 		if (id >= 2) /* VFE/CSID lite */
-			csid->base = camss->vfe[id].base + VFE_480_LITE_CSID_OFFSET;
+			csid->base = csid->res->parent_dev_ops->get_base_address(camss, id)
+				+ VFE_480_LITE_CSID_OFFSET;
 		else
-			csid->base = camss->vfe[id].base + VFE_480_CSID_OFFSET;
+			csid->base = csid->res->parent_dev_ops->get_base_address(camss, id)
+				 + VFE_480_CSID_OFFSET;
 	} else {
 		csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
 		if (IS_ERR(csid->base))
@@ -605,7 +1137,7 @@ int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 	csid->irq = ret;
 	snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",
 		 dev_name(dev), MSM_CSID_NAME, csid->id);
-	ret = devm_request_irq(dev, csid->irq, csid->ops->isr,
+	ret = devm_request_irq(dev, csid->irq, csid->res->hw_ops->isr,
 			       IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
 			       csid->irq_name, csid);
 	if (ret < 0) {
@@ -740,7 +1272,8 @@ static int csid_link_setup(struct media_entity *entity,
 
 		/* If test generator is enabled */
 		/* do not allow a link from CSIPHY to CSID */
-		if (csid->testgen_mode->cur.val != 0)
+		if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED &&
+		    csid->testgen_mode->cur.val != 0)
 			return -EBUSY;
 
 		sd = media_entity_to_v4l2_subdev(remote->entity);
@@ -833,24 +1366,27 @@ int msm_csid_register_entity(struct csid_device *csid,
 		 MSM_CSID_NAME, csid->id);
 	v4l2_set_subdevdata(sd, csid);
 
-	ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
-	if (ret < 0) {
-		dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
-		return ret;
-	}
+	if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED) {
+		ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);
+		if (ret < 0) {
+			dev_err(dev, "Failed to init ctrl handler: %d\n", ret);
+			return ret;
+		}
 
-	csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,
-				&csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
-				csid->testgen.nmodes, 0, 0,
-				csid->testgen.modes);
+		csid->testgen_mode =
+			v4l2_ctrl_new_std_menu_items(&csid->ctrls,
+						     &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,
+						     csid->testgen.nmodes, 0, 0,
+						     csid->testgen.modes);
 
-	if (csid->ctrls.error) {
-		dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
-		ret = csid->ctrls.error;
-		goto free_ctrl;
-	}
+		if (csid->ctrls.error) {
+			dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);
+			ret = csid->ctrls.error;
+			goto free_ctrl;
+		}
 
-	csid->subdev.ctrl_handler = &csid->ctrls;
+		csid->subdev.ctrl_handler = &csid->ctrls;
+	}
 
 	ret = csid_init_formats(sd, NULL);
 	if (ret < 0) {
@@ -881,7 +1417,8 @@ int msm_csid_register_entity(struct csid_device *csid,
 media_cleanup:
 	media_entity_cleanup(&sd->entity);
 free_ctrl:
-	v4l2_ctrl_handler_free(&csid->ctrls);
+	if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
+		v4l2_ctrl_handler_free(&csid->ctrls);
 
 	return ret;
 }
@@ -894,10 +1431,11 @@ void msm_csid_unregister_entity(struct csid_device *csid)
 {
 	v4l2_device_unregister_subdev(&csid->subdev);
 	media_entity_cleanup(&csid->subdev.entity);
-	v4l2_ctrl_handler_free(&csid->ctrls);
+	if (csid->testgen.nmodes != CSID_PAYLOAD_MODE_DISABLED)
+		v4l2_ctrl_handler_free(&csid->ctrls);
 }
 
 inline bool csid_is_lite(struct csid_device *csid)
 {
-	return csid->camss->res->csid_res[csid->id].is_lite;
+	return csid->camss->res->csid_res[csid->id].csid.is_lite;
 }
diff --git a/drivers/media/platform/qcom/camss/camss-csid.h b/drivers/media/platform/qcom/camss/camss-csid.h
index fddccb69da13..9dc826d8c8f6 100644
--- a/drivers/media/platform/qcom/camss/camss-csid.h
+++ b/drivers/media/platform/qcom/camss/camss-csid.h
@@ -27,29 +27,6 @@
 /* CSID hardware can demultiplex up to 4 outputs */
 #define MSM_CSID_MAX_SRC_STREAMS	4
 
-#define DATA_TYPE_EMBEDDED_DATA_8BIT	0x12
-#define DATA_TYPE_YUV420_8BIT		0x18
-#define DATA_TYPE_YUV420_10BIT		0x19
-#define DATA_TYPE_YUV420_8BIT_LEGACY	0x1a
-#define DATA_TYPE_YUV420_8BIT_SHIFTED	0x1c /* Chroma Shifted Pixel Sampling */
-#define DATA_TYPE_YUV420_10BIT_SHIFTED	0x1d /* Chroma Shifted Pixel Sampling */
-#define DATA_TYPE_YUV422_8BIT		0x1e
-#define DATA_TYPE_YUV422_10BIT		0x1f
-#define DATA_TYPE_RGB444		0x20
-#define DATA_TYPE_RGB555		0x21
-#define DATA_TYPE_RGB565		0x22
-#define DATA_TYPE_RGB666		0x23
-#define DATA_TYPE_RGB888		0x24
-#define DATA_TYPE_RAW_24BIT		0x27
-#define DATA_TYPE_RAW_6BIT		0x28
-#define DATA_TYPE_RAW_7BIT		0x29
-#define DATA_TYPE_RAW_8BIT		0x2a
-#define DATA_TYPE_RAW_10BIT		0x2b
-#define DATA_TYPE_RAW_12BIT		0x2c
-#define DATA_TYPE_RAW_14BIT		0x2d
-#define DATA_TYPE_RAW_16BIT		0x2e
-#define DATA_TYPE_RAW_20BIT		0x2f
-
 #define CSID_RESET_TIMEOUT_MS 500
 
 enum csid_testgen_mode {
@@ -67,7 +44,7 @@ enum csid_testgen_mode {
 	CSID_PAYLOAD_MODE_NUM_SUPPORTED_GEN2 = 9, /* excluding disabled */
 };
 
-struct csid_format {
+struct csid_format_info {
 	u32 code;
 	u8 data_type;
 	u8 decode_format;
@@ -75,6 +52,11 @@ struct csid_format {
 	u8 spp; /* bus samples per pixel */
 };
 
+struct csid_formats {
+	unsigned int nformats;
+	const struct csid_format_info *formats;
+};
+
 struct csid_testgen_config {
 	enum csid_testgen_mode mode;
 	const char * const*modes;
@@ -147,6 +129,21 @@ struct csid_hw_ops {
 	 * @csid: CSID device
 	 */
 	void (*subdev_init)(struct csid_device *csid);
+
+	/*
+	 * reg_update - receive message from other sub device
+	 * @csid: CSID device
+	 * @port_id: Port id
+	 * @is_clear: Indicate if it is clearing reg update or setting reg update
+	 */
+	void (*reg_update)(struct csid_device *csid, int port_id, bool is_clear);
+};
+
+struct csid_subdev_resources {
+	bool is_lite;
+	const struct csid_hw_ops *hw_ops;
+	const struct parent_dev_ops *parent_dev_ops;
+	const struct csid_formats *formats;
 };
 
 struct csid_device {
@@ -157,6 +154,7 @@ struct csid_device {
 	void __iomem *base;
 	u32 irq;
 	char irq_name[30];
+	u32 reg_update;
 	struct camss_clock *clock;
 	int nclocks;
 	struct regulator_bulk_data *supplies;
@@ -167,9 +165,7 @@ struct csid_device {
 	struct v4l2_mbus_framefmt fmt[MSM_CSID_PADS_NUM];
 	struct v4l2_ctrl_handler ctrls;
 	struct v4l2_ctrl *testgen_mode;
-	const struct csid_format *formats;
-	unsigned int nformats;
-	const struct csid_hw_ops *ops;
+	const struct csid_subdev_resources *res;
 };
 
 struct camss_subdev_resources;
@@ -188,16 +184,16 @@ u32 csid_find_code(u32 *codes, unsigned int ncode,
 		   unsigned int match_format_idx, u32 match_code);
 
 /*
- * csid_get_fmt_entry - Find csid_format entry with matching format code
- * @formats: Array of format csid_format entries
+ * csid_get_fmt_entry - Find csid_format_info entry with matching format code
+ * @formats: Array of format csid_format_info entries
  * @nformats: Length of @nformats array
  * @code: Desired format code
  *
  * Return formats[0] on failure to find code
  */
-const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,
-					     unsigned int nformats,
-					     u32 code);
+const struct csid_format_info *csid_get_fmt_entry(const struct csid_format_info *formats,
+						  unsigned int nformats,
+						  u32 code);
 
 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,
 			 const struct camss_subdev_resources *res, u8 id);
@@ -211,9 +207,15 @@ void msm_csid_get_csid_id(struct media_entity *entity, u8 *id);
 
 extern const char * const csid_testgen_modes[];
 
+extern const struct csid_formats csid_formats_4_1;
+extern const struct csid_formats csid_formats_4_7;
+extern const struct csid_formats csid_formats_gen2;
+
 extern const struct csid_hw_ops csid_ops_4_1;
 extern const struct csid_hw_ops csid_ops_4_7;
+extern const struct csid_hw_ops csid_ops_680;
 extern const struct csid_hw_ops csid_ops_gen2;
+extern const struct csid_hw_ops csid_ops_780;
 
 /*
  * csid_is_lite - Check if CSID is CSID lite.
@@ -223,4 +225,25 @@ extern const struct csid_hw_ops csid_ops_gen2;
  */
 bool csid_is_lite(struct csid_device *csid);
 
+/*
+ * csid_hw_version - CSID hardware version query
+ * @csid: CSID device
+ *
+ * Return HW version or error
+ */
+u32 csid_hw_version(struct csid_device *csid);
+
+/*
+ * csid_src_pad_code - Pick an output/src format based on the input/sink format
+ * @csid: CSID device
+ * @sink_code: The sink format of the input
+ * @match_format_idx: Request preferred index, as defined by subdevice csid
+ *                    format. Set @match_code to 0 if used.
+ * @match_code: Request preferred code, set @match_format_idx to 0 if used
+ *
+ * Return 0 on failure or src format code otherwise
+ */
+u32 csid_src_pad_code(struct csid_device *csid, u32 sink_code,
+		      unsigned int match_format_idx, u32 match_code);
+
 #endif /* QC_MSM_CAMSS_CSID_H */
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
index cd4a8c369234..9d67e7fa6366 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy-2ph-1-0.c
@@ -180,6 +180,11 @@ static irqreturn_t csiphy_isr(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
+static int csiphy_init(struct csiphy_device *csiphy)
+{
+	return 0;
+}
+
 const struct csiphy_hw_ops csiphy_ops_2ph_1_0 = {
 	.get_lane_mask = csiphy_get_lane_mask,
 	.hw_version_read = csiphy_hw_version_read,
@@ -187,4 +192,5 @@ const struct csiphy_hw_ops csiphy_ops_2ph_1_0 = {
 	.lanes_enable = csiphy_lanes_enable,
 	.lanes_disable = csiphy_lanes_disable,
 	.isr = csiphy_isr,
+	.init = csiphy_init,
 };
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
index f50e2235c37f..f732a76de93e 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy-3ph-1-0.c
@@ -42,243 +42,522 @@
 #define CSIPHY_3PH_LNn_CSI_LANE_CTRL15(n)	(0x03c + 0x100 * (n))
 #define CSIPHY_3PH_LNn_CSI_LANE_CTRL15_SWI_SOT_SYMBOL	0xb8
 
-#define CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(n)	(0x800 + 0x4 * (n))
+#define CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(offset, n)	((offset) + 0x4 * (n))
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL5_CLK_ENABLE	BIT(7)
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_COMMON_PWRDN_B	BIT(0)
 #define CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID	BIT(1)
-#define CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(n)	(0x8b0 + 0x4 * (n))
-
-#define CSIPHY_DEFAULT_PARAMS            0
-#define CSIPHY_LANE_ENABLE               1
-#define CSIPHY_SETTLE_CNT_LOWER_BYTE     2
-#define CSIPHY_SETTLE_CNT_HIGHER_BYTE    3
-#define CSIPHY_DNP_PARAMS                4
-#define CSIPHY_2PH_REGS                  5
-#define CSIPHY_3PH_REGS                  6
-
-struct csiphy_reg_t {
+#define CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(offset, n)	((offset) + 0xb0 + 0x4 * (n))
+
+#define CSIPHY_DEFAULT_PARAMS		0
+#define CSIPHY_LANE_ENABLE		1
+#define CSIPHY_SETTLE_CNT_LOWER_BYTE	2
+#define CSIPHY_SETTLE_CNT_HIGHER_BYTE	3
+#define CSIPHY_DNP_PARAMS		4
+#define CSIPHY_2PH_REGS			5
+#define CSIPHY_3PH_REGS			6
+#define CSIPHY_SKEW_CAL			7
+
+struct csiphy_lane_regs {
 	s32 reg_addr;
 	s32 reg_data;
-	s32 delay;
+	u32 delay_us;
 	u32 csiphy_param_type;
 };
 
 /* GEN2 1.0 2PH */
 static const struct
-csiphy_reg_t lane_regs_sdm845[5][14] = {
-	{
-		{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0034, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x001C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0028, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0000, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0008, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x000c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0010, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0060, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0064, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0704, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x072C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0734, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x071C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0714, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0728, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x073C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0700, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0708, 0x14, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x070C, 0xA5, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0710, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0738, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0760, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0764, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0204, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x022C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0234, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x021C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0214, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0228, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x023C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0200, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0208, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x020C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0210, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0238, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0260, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0264, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0434, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x041C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0428, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0400, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0408, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x040C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0410, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0460, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0464, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0604, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x062C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0634, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x061C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0614, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0628, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x063C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0600, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0608, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x060C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0610, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0638, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0660, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0664, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
+csiphy_lane_regs lane_regs_sdm845[] = {
+	{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0034, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x001C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0028, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0000, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0008, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x000c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0010, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0060, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0064, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0704, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x072C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0734, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x071C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0714, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0728, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x073C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0700, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0708, 0x14, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x070C, 0xA5, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0710, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0738, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0760, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0764, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0204, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x022C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0234, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x021C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0214, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0228, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x023C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0200, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0208, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x020C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0210, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0238, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0260, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0264, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0434, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x041C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0428, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0400, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0408, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x040C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0410, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0460, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0464, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0604, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x062C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0634, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x061C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0614, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0628, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x063C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0600, 0x91, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0608, 0x00, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x060C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0610, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0638, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0660, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0664, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+};
+
+/* GEN2 1.1 2PH */
+static const struct
+csiphy_lane_regs lane_regs_sc8280xp[] = {
+	{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0034, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x001C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0028, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0000, 0x90, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0008, 0x0E, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x000C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0010, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0060, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0064, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0704, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x072C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0734, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x071C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0714, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0728, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x073C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0700, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0708, 0x0E, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x070C, 0xA5, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0710, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0738, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0760, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0764, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0204, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x022C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0234, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x021C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0214, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0228, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x023C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0200, 0x90, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0208, 0x0E, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x020C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0210, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0238, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0260, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0264, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0434, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x041C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0428, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0400, 0x90, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0408, 0x0E, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x040C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0410, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0460, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0464, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0604, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x062C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0634, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x061C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0614, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0628, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x063C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0600, 0x90, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0608, 0x0E, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x060C, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0610, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0638, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0660, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0664, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
 };
 
 /* GEN2 1.2.1 2PH */
 static const struct
-csiphy_reg_t lane_regs_sm8250[5][20] = {
-	{
-		{0x0030, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0900, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0908, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0904, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0904, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0034, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0010, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x001C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0008, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x0000, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x000c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0028, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0024, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0730, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C80, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C88, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C84, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C84, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0704, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x072C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0734, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0710, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x071C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x073C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0708, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x0700, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x070c, 0xA5, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0738, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0714, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0728, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0724, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0230, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0A00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0A08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0A04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0A04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0204, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x022C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0234, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0210, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x021C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x023C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0208, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x0200, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x020c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0238, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0214, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0228, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0224, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0430, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0B00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0B08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0B04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0B04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0434, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0410, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x041C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0408, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x0400, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x040c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0428, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0424, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
-	{
-		{0x0630, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0C04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0604, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x062C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0634, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0610, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x061C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x063C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0608, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
-		{0x0600, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x060c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0638, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0614, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0628, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0624, 0x00, 0x00, CSIPHY_DNP_PARAMS},
-		{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
-		{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
-	},
+csiphy_lane_regs lane_regs_sm8250[] = {
+	{0x0030, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0900, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0908, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0904, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0904, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0034, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0010, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x001C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0008, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0000, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x000c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0028, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0024, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0730, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C80, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C88, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C84, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C84, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0704, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x072C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0734, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0710, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x071C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x073C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0708, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0700, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x070c, 0xA5, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0738, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0714, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0728, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0724, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0230, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0A00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0A08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0A04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0A04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0204, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x022C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0234, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0210, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x021C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x023C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0208, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0200, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x020c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0238, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0214, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0228, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0224, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0430, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0B00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0B08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0B04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0B04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0434, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0410, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x041C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0408, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0400, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x040c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0428, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0424, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0630, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C00, 0x05, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C08, 0x10, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C04, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C04, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0604, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x062C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0634, 0x07, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0610, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x061C, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x063C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0608, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0600, 0x8D, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x060c, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0638, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0614, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0628, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0624, 0x00, 0x00, CSIPHY_DNP_PARAMS},
+	{0x0800, 0x02, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0884, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+};
+
+/* GEN2 2.1.2 2PH DPHY mode */
+static const struct
+csiphy_lane_regs lane_regs_sm8550[] = {
+	{0x0E90, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E98, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E94, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x00A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0090, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0098, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0094, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0494, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x04A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0490, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0498, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0494, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0894, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x08A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0890, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0898, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0894, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0C94, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0CA0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C90, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C98, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C94, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0E30, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E28, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E00, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E0C, 0xFF, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E38, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E2C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E34, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E1C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E14, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E3C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E04, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E20, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E08, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0E10, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0030, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0000, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0034, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x001C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0020, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0008, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0010, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0430, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0400, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0434, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x041C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0420, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0408, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0410, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0830, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0800, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0838, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x082C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0834, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x081C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0814, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x083C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0804, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0820, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0808, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0810, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C30, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C00, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C38, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C2C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C34, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C1C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C14, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C3C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C04, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C20, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C08, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0C10, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0094, 0xD7, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x005C, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0060, 0xBD, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0064, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0494, 0xD7, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x045C, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0460, 0xBD, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0464, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0894, 0xD7, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x085C, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0860, 0xBD, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0864, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C94, 0xD7, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C5C, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C60, 0xBD, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C64, 0x7F, 0x00, CSIPHY_DEFAULT_PARAMS},
+};
+
+/* 4nm 2PH v 2.1.2 2p5Gbps 4 lane DPHY mode */
+static const struct
+csiphy_lane_regs lane_regs_x1e80100[] = {
+	/* Power up lanes 2ph mode */
+	{0x1014, 0xD5, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x101C, 0x7A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x1018, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+
+	{0x0094, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x00A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0090, 0x0f, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0098, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0094, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0030, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0000, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0038, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x002C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0034, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x001C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0014, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x003C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0004, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0020, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0008, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0010, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0094, 0xD7, 0x00, CSIPHY_SKEW_CAL},
+	{0x005C, 0x00, 0x00, CSIPHY_SKEW_CAL},
+	{0x0060, 0xBD, 0x00, CSIPHY_SKEW_CAL},
+	{0x0064, 0x7F, 0x00, CSIPHY_SKEW_CAL},
+
+	{0x0E94, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0EA0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E90, 0x0f, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E98, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E94, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0E30, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E28, 0x04, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E00, 0x80, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E0C, 0xFF, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E38, 0x1F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E2C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E34, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E1C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E14, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E3C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E04, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E20, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0E08, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0E10, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+
+	{0x0494, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x04A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0490, 0x0f, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0498, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0494, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0430, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0400, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0438, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x042C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0434, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x041C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0414, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x043C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0404, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0420, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0408, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0410, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0494, 0xD7, 0x00, CSIPHY_SKEW_CAL},
+	{0x045C, 0x00, 0x00, CSIPHY_SKEW_CAL},
+	{0x0460, 0xBD, 0x00, CSIPHY_SKEW_CAL},
+	{0x0464, 0x7F, 0x00, CSIPHY_SKEW_CAL},
+
+	{0x0894, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x08A0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0890, 0x0f, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0898, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0894, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0830, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0800, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0838, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x082C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0834, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x081C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0814, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x083C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0804, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0820, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0808, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0810, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0894, 0xD7, 0x00, CSIPHY_SKEW_CAL},
+	{0x085C, 0x00, 0x00, CSIPHY_SKEW_CAL},
+	{0x0860, 0xBD, 0x00, CSIPHY_SKEW_CAL},
+	{0x0864, 0x7F, 0x00, CSIPHY_SKEW_CAL},
+
+	{0x0C94, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0CA0, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C90, 0x0f, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C98, 0x08, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C94, 0x07, 0x01, CSIPHY_DEFAULT_PARAMS},
+	{0x0C30, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C00, 0x8E, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C38, 0xFE, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C2C, 0x01, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C34, 0x0F, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C1C, 0x0A, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C14, 0x60, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C3C, 0xB8, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C04, 0x0C, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C20, 0x00, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C08, 0x10, 0x00, CSIPHY_SETTLE_CNT_LOWER_BYTE},
+	{0x0C10, 0x52, 0x00, CSIPHY_DEFAULT_PARAMS},
+	{0x0C94, 0xD7, 0x00, CSIPHY_SKEW_CAL},
+	{0x0C5C, 0x00, 0x00, CSIPHY_SKEW_CAL},
+	{0x0C60, 0xBD, 0x00, CSIPHY_SKEW_CAL},
+	{0x0C64, 0x7F, 0x00, CSIPHY_SKEW_CAL},
 };
 
 static void csiphy_hw_version_read(struct csiphy_device *csiphy,
 				   struct device *dev)
 {
+	struct csiphy_device_regs *regs = csiphy->regs;
 	u32 hw_version;
 
-	writel(CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID,
-	       csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(6));
+	writel(CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_SHOW_REV_ID, csiphy->base +
+	       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 6));
 
 	hw_version = readl_relaxed(csiphy->base +
-				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(12));
+				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(regs->offset, 12));
 	hw_version |= readl_relaxed(csiphy->base +
-				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(13)) << 8;
+				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(regs->offset, 13)) << 8;
 	hw_version |= readl_relaxed(csiphy->base +
-				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(14)) << 16;
+				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(regs->offset, 14)) << 16;
 	hw_version |= readl_relaxed(csiphy->base +
-				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(15)) << 24;
+				   CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(regs->offset, 15)) << 24;
 
 	dev_dbg(dev, "CSIPHY 3PH HW Version = 0x%08x\n", hw_version);
 }
@@ -289,31 +568,39 @@ static void csiphy_hw_version_read(struct csiphy_device *csiphy,
  */
 static void csiphy_reset(struct csiphy_device *csiphy)
 {
-	writel_relaxed(0x1, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(0));
+	struct csiphy_device_regs *regs = csiphy->regs;
+
+	writel_relaxed(0x1, csiphy->base +
+		      CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 0));
 	usleep_range(5000, 8000);
-	writel_relaxed(0x0, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(0));
+	writel_relaxed(0x0, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 0));
 }
 
 static irqreturn_t csiphy_isr(int irq, void *dev)
 {
 	struct csiphy_device *csiphy = dev;
+	struct csiphy_device_regs *regs = csiphy->regs;
 	int i;
 
 	for (i = 0; i < 11; i++) {
 		int c = i + 22;
 		u8 val = readl_relaxed(csiphy->base +
-				       CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(i));
+				       CSIPHY_3PH_CMN_CSI_COMMON_STATUSn(regs->offset, i));
 
 		writel_relaxed(val, csiphy->base +
-				    CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(c));
+			       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, c));
 	}
 
-	writel_relaxed(0x1, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(10));
-	writel_relaxed(0x0, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(10));
+	writel_relaxed(0x1, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 10));
+	writel_relaxed(0x0, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 10));
 
-	for (i = 22; i < 33; i++)
+	for (i = 22; i < 33; i++) {
 		writel_relaxed(0x0, csiphy->base +
-				    CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(i));
+			       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, i));
+	}
 
 	return IRQ_HANDLED;
 }
@@ -415,38 +702,27 @@ static void csiphy_gen1_config_lanes(struct csiphy_device *csiphy,
 static void csiphy_gen2_config_lanes(struct csiphy_device *csiphy,
 				     u8 settle_cnt)
 {
-	const struct csiphy_reg_t *r;
-	int i, l, array_size;
+	const struct csiphy_lane_regs *r = csiphy->regs->lane_regs;
+	int i, array_size = csiphy->regs->lane_array_size;
 	u32 val;
 
-	switch (csiphy->camss->res->version) {
-	case CAMSS_845:
-		r = &lane_regs_sdm845[0][0];
-		array_size = ARRAY_SIZE(lane_regs_sdm845[0]);
-		break;
-	case CAMSS_8250:
-		r = &lane_regs_sm8250[0][0];
-		array_size = ARRAY_SIZE(lane_regs_sm8250[0]);
-		break;
-	default:
-		WARN(1, "unknown cspi version\n");
-		return;
-	}
-
-	for (l = 0; l < 5; l++) {
-		for (i = 0; i < array_size; i++, r++) {
-			switch (r->csiphy_param_type) {
-			case CSIPHY_SETTLE_CNT_LOWER_BYTE:
-				val = settle_cnt & 0xff;
-				break;
-			case CSIPHY_DNP_PARAMS:
-				continue;
-			default:
-				val = r->reg_data;
-				break;
-			}
-			writel_relaxed(val, csiphy->base + r->reg_addr);
+	for (i = 0; i < array_size; i++, r++) {
+		switch (r->csiphy_param_type) {
+		case CSIPHY_SETTLE_CNT_LOWER_BYTE:
+			val = settle_cnt & 0xff;
+			break;
+		case CSIPHY_SKEW_CAL:
+			/* TODO: support application of skew from dt flag */
+			continue;
+		case CSIPHY_DNP_PARAMS:
+			continue;
+		default:
+			val = r->reg_data;
+			break;
 		}
+		writel_relaxed(val, csiphy->base + r->reg_addr);
+		if (r->delay_us)
+			udelay(r->delay_us);
 	}
 }
 
@@ -463,13 +739,30 @@ static u8 csiphy_get_lane_mask(struct csiphy_lanes_cfg *lane_cfg)
 	return lane_mask;
 }
 
+static bool csiphy_is_gen2(u32 version)
+{
+	bool ret = false;
+
+	switch (version) {
+	case CAMSS_7280:
+	case CAMSS_8250:
+	case CAMSS_8280XP:
+	case CAMSS_845:
+	case CAMSS_8550:
+	case CAMSS_X1E80100:
+		ret = true;
+		break;
+	}
+
+	return ret;
+}
+
 static void csiphy_lanes_enable(struct csiphy_device *csiphy,
 				struct csiphy_config *cfg,
 				s64 link_freq, u8 lane_mask)
 {
 	struct csiphy_lanes_cfg *c = &cfg->csi2->lane_cfg;
-	bool is_gen2 = (csiphy->camss->res->version == CAMSS_845 ||
-			csiphy->camss->res->version == CAMSS_8250);
+	struct csiphy_device_regs *regs = csiphy->regs;
 	u8 settle_cnt;
 	u8 val;
 	int i;
@@ -480,35 +773,87 @@ static void csiphy_lanes_enable(struct csiphy_device *csiphy,
 	for (i = 0; i < c->num_data; i++)
 		val |= BIT(c->data[i].pos * 2);
 
-	writel_relaxed(val, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(5));
+	writel_relaxed(val, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 5));
 
 	val = CSIPHY_3PH_CMN_CSI_COMMON_CTRL6_COMMON_PWRDN_B;
-	writel_relaxed(val, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(6));
+	writel_relaxed(val, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 6));
 
 	val = 0x02;
-	writel_relaxed(val, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(7));
+	writel_relaxed(val, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 7));
 
 	val = 0x00;
-	writel_relaxed(val, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(0));
+	writel_relaxed(val, csiphy->base +
+		       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 0));
 
-	if (is_gen2)
+	if (csiphy_is_gen2(csiphy->camss->res->version))
 		csiphy_gen2_config_lanes(csiphy, settle_cnt);
 	else
 		csiphy_gen1_config_lanes(csiphy, cfg, settle_cnt);
 
 	/* IRQ_MASK registers - disable all interrupts */
-	for (i = 11; i < 22; i++)
-		writel_relaxed(0, csiphy->base + CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(i));
+	for (i = 11; i < 22; i++) {
+		writel_relaxed(0, csiphy->base +
+			       CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, i));
+	}
 }
 
 static void csiphy_lanes_disable(struct csiphy_device *csiphy,
 				 struct csiphy_config *cfg)
 {
+	struct csiphy_device_regs *regs = csiphy->regs;
+
 	writel_relaxed(0, csiphy->base +
-			  CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(5));
+			  CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 5));
 
 	writel_relaxed(0, csiphy->base +
-			  CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(6));
+			  CSIPHY_3PH_CMN_CSI_COMMON_CTRLn(regs->offset, 6));
+}
+
+static int csiphy_init(struct csiphy_device *csiphy)
+{
+	struct device *dev = csiphy->camss->dev;
+	struct csiphy_device_regs *regs;
+
+	regs = devm_kmalloc(dev, sizeof(*regs), GFP_KERNEL);
+	if (!regs)
+		return -ENOMEM;
+
+	csiphy->regs = regs;
+	regs->offset = 0x800;
+
+	switch (csiphy->camss->res->version) {
+	case CAMSS_845:
+		regs->lane_regs = &lane_regs_sdm845[0];
+		regs->lane_array_size = ARRAY_SIZE(lane_regs_sdm845);
+		break;
+	case CAMSS_7280:
+	case CAMSS_8250:
+		regs->lane_regs = &lane_regs_sm8250[0];
+		regs->lane_array_size = ARRAY_SIZE(lane_regs_sm8250);
+		break;
+	case CAMSS_8280XP:
+		regs->lane_regs = &lane_regs_sc8280xp[0];
+		regs->lane_array_size = ARRAY_SIZE(lane_regs_sc8280xp);
+		break;
+	case CAMSS_X1E80100:
+		regs->lane_regs = &lane_regs_x1e80100[0];
+		regs->lane_array_size = ARRAY_SIZE(lane_regs_x1e80100);
+		regs->offset = 0x1000;
+		break;
+	case CAMSS_8550:
+		regs->lane_regs = &lane_regs_sm8550[0];
+		regs->lane_array_size = ARRAY_SIZE(lane_regs_sm8550);
+		regs->offset = 0x1000;
+		break;
+	default:
+		WARN(1, "unknown csiphy version\n");
+		return -ENODEV;
+	}
+
+	return 0;
 }
 
 const struct csiphy_hw_ops csiphy_ops_3ph_1_0 = {
@@ -518,4 +863,5 @@ const struct csiphy_hw_ops csiphy_ops_3ph_1_0 = {
 	.lanes_enable = csiphy_lanes_enable,
 	.lanes_disable = csiphy_lanes_disable,
 	.isr = csiphy_isr,
+	.init = csiphy_init,
 };
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.c b/drivers/media/platform/qcom/camss/camss-csiphy.c
index 264c99efeae8..c622efcc92ff 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.c
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.c
@@ -24,12 +24,7 @@
 
 #define MSM_CSIPHY_NAME "msm_csiphy"
 
-struct csiphy_format {
-	u32 code;
-	u8 bpp;
-};
-
-static const struct csiphy_format csiphy_formats_8x16[] = {
+static const struct csiphy_format_info formats_8x16[] = {
 	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
@@ -49,7 +44,7 @@ static const struct csiphy_format csiphy_formats_8x16[] = {
 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
 };
 
-static const struct csiphy_format csiphy_formats_8x96[] = {
+static const struct csiphy_format_info formats_8x96[] = {
 	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
@@ -73,7 +68,7 @@ static const struct csiphy_format csiphy_formats_8x96[] = {
 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
 };
 
-static const struct csiphy_format csiphy_formats_sdm845[] = {
+static const struct csiphy_format_info formats_sdm845[] = {
 	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
 	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
@@ -98,6 +93,26 @@ static const struct csiphy_format csiphy_formats_sdm845[] = {
 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
 };
 
+const struct csiphy_formats csiphy_formats_8x16 = {
+	.nformats = ARRAY_SIZE(formats_8x16),
+	.formats = formats_8x16
+};
+
+const struct csiphy_formats csiphy_formats_8x96 = {
+	.nformats = ARRAY_SIZE(formats_8x96),
+	.formats = formats_8x96
+};
+
+const struct csiphy_formats csiphy_formats_sc7280 = {
+	.nformats = ARRAY_SIZE(formats_sdm845),
+	.formats = formats_sdm845
+};
+
+const struct csiphy_formats csiphy_formats_sdm845 = {
+	.nformats = ARRAY_SIZE(formats_sdm845),
+	.formats = formats_sdm845
+};
+
 /*
  * csiphy_get_bpp - map media bus format to bits per pixel
  * @formats: supported media bus formats array
@@ -106,7 +121,7 @@ static const struct csiphy_format csiphy_formats_sdm845[] = {
  *
  * Return number of bits per pixel
  */
-static u8 csiphy_get_bpp(const struct csiphy_format *formats,
+static u8 csiphy_get_bpp(const struct csiphy_format_info *formats,
 			 unsigned int nformats, u32 code)
 {
 	unsigned int i;
@@ -131,7 +146,7 @@ static int csiphy_set_clock_rates(struct csiphy_device *csiphy)
 	int i, j;
 	int ret;
 
-	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
+	u8 bpp = csiphy_get_bpp(csiphy->res->formats->formats, csiphy->res->formats->nformats,
 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
 
@@ -202,28 +217,41 @@ static int csiphy_set_power(struct v4l2_subdev *sd, int on)
 		if (ret < 0)
 			return ret;
 
+		ret = regulator_bulk_enable(csiphy->num_supplies,
+					    csiphy->supplies);
+		if (ret < 0) {
+			pm_runtime_put_sync(dev);
+			return ret;
+		}
+
 		ret = csiphy_set_clock_rates(csiphy);
 		if (ret < 0) {
+			regulator_bulk_disable(csiphy->num_supplies,
+					       csiphy->supplies);
 			pm_runtime_put_sync(dev);
 			return ret;
 		}
 
 		ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev);
 		if (ret < 0) {
+			regulator_bulk_disable(csiphy->num_supplies,
+					       csiphy->supplies);
 			pm_runtime_put_sync(dev);
 			return ret;
 		}
 
 		enable_irq(csiphy->irq);
 
-		csiphy->ops->reset(csiphy);
+		csiphy->res->hw_ops->reset(csiphy);
 
-		csiphy->ops->hw_version_read(csiphy, dev);
+		csiphy->res->hw_ops->hw_version_read(csiphy, dev);
 	} else {
 		disable_irq(csiphy->irq);
 
 		camss_disable_clocks(csiphy->nclocks, csiphy->clock);
 
+		regulator_bulk_disable(csiphy->num_supplies, csiphy->supplies);
+
 		pm_runtime_put_sync(dev);
 	}
 
@@ -243,8 +271,8 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
 {
 	struct csiphy_config *cfg = &csiphy->cfg;
 	s64 link_freq;
-	u8 lane_mask = csiphy->ops->get_lane_mask(&cfg->csi2->lane_cfg);
-	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
+	u8 lane_mask = csiphy->res->hw_ops->get_lane_mask(&cfg->csi2->lane_cfg);
+	u8 bpp = csiphy_get_bpp(csiphy->res->formats->formats, csiphy->res->formats->nformats,
 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
 	u8 val;
@@ -272,7 +300,7 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
 		wmb();
 	}
 
-	csiphy->ops->lanes_enable(csiphy, cfg, link_freq, lane_mask);
+	csiphy->res->hw_ops->lanes_enable(csiphy, cfg, link_freq, lane_mask);
 
 	return 0;
 }
@@ -285,7 +313,7 @@ static int csiphy_stream_on(struct csiphy_device *csiphy)
  */
 static void csiphy_stream_off(struct csiphy_device *csiphy)
 {
-	csiphy->ops->lanes_disable(csiphy, &csiphy->cfg);
+	csiphy->res->hw_ops->lanes_disable(csiphy, &csiphy->cfg);
 }
 
 
@@ -350,12 +378,12 @@ static void csiphy_try_format(struct csiphy_device *csiphy,
 	case MSM_CSIPHY_PAD_SINK:
 		/* Set format on sink pad */
 
-		for (i = 0; i < csiphy->nformats; i++)
-			if (fmt->code == csiphy->formats[i].code)
+		for (i = 0; i < csiphy->res->formats->nformats; i++)
+			if (fmt->code == csiphy->res->formats->formats[i].code)
 				break;
 
 		/* If not found, use UYVY as default */
-		if (i >= csiphy->nformats)
+		if (i >= csiphy->res->formats->nformats)
 			fmt->code = MEDIA_BUS_FMT_UYVY8_1X16;
 
 		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
@@ -392,10 +420,10 @@ static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
 	struct v4l2_mbus_framefmt *format;
 
 	if (code->pad == MSM_CSIPHY_PAD_SINK) {
-		if (code->index >= csiphy->nformats)
+		if (code->index >= csiphy->res->formats->nformats)
 			return -EINVAL;
 
-		code->code = csiphy->formats[code->index].code;
+		code->code = csiphy->res->formats->formats[code->index].code;
 	} else {
 		if (code->index > 0)
 			return -EINVAL;
@@ -558,30 +586,17 @@ int msm_csiphy_subdev_init(struct camss *camss,
 {
 	struct device *dev = camss->dev;
 	struct platform_device *pdev = to_platform_device(dev);
-	int i, j, k;
+	int i, j;
 	int ret;
 
 	csiphy->camss = camss;
 	csiphy->id = id;
 	csiphy->cfg.combo_mode = 0;
-	csiphy->ops = res->ops;
+	csiphy->res = &res->csiphy;
 
-	switch (camss->res->version) {
-	case CAMSS_8x16:
-		csiphy->formats = csiphy_formats_8x16;
-		csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x16);
-		break;
-	case CAMSS_8x96:
-	case CAMSS_660:
-		csiphy->formats = csiphy_formats_8x96;
-		csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x96);
-		break;
-	case CAMSS_845:
-	case CAMSS_8250:
-		csiphy->formats = csiphy_formats_sdm845;
-		csiphy->nformats = ARRAY_SIZE(csiphy_formats_sdm845);
-		break;
-	}
+	ret = csiphy->res->hw_ops->init(csiphy);
+	if (ret)
+		return ret;
 
 	/* Memory */
 
@@ -590,6 +605,7 @@ int msm_csiphy_subdev_init(struct camss *camss,
 		return PTR_ERR(csiphy->base);
 
 	if (camss->res->version == CAMSS_8x16 ||
+	    camss->res->version == CAMSS_8x53 ||
 	    camss->res->version == CAMSS_8x96) {
 		csiphy->base_clk_mux =
 			devm_platform_ioremap_resource_byname(pdev, res->reg[1]);
@@ -609,7 +625,7 @@ int msm_csiphy_subdev_init(struct camss *camss,
 	snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d",
 		 dev_name(dev), MSM_CSIPHY_NAME, csiphy->id);
 
-	ret = devm_request_irq(dev, csiphy->irq, csiphy->ops->isr,
+	ret = devm_request_irq(dev, csiphy->irq, csiphy->res->hw_ops->isr,
 			       IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
 			       csiphy->irq_name, csiphy);
 	if (ret < 0) {
@@ -664,26 +680,44 @@ int msm_csiphy_subdev_init(struct camss *camss,
 		for (j = 0; j < clock->nfreqs; j++)
 			clock->freq[j] = res->clock_rate[i][j];
 
-		for (k = 0; k < camss->res->csiphy_num; k++) {
+		csiphy->rate_set[i] = csiphy_match_clock_name(clock->name,
+							      "csiphy%d_timer",
+							      csiphy->id);
+		if (csiphy->rate_set[i])
+			continue;
+
+		if (camss->res->version == CAMSS_660) {
 			csiphy->rate_set[i] = csiphy_match_clock_name(clock->name,
-								      "csiphy%d_timer", k);
+								      "csi%d_phy",
+								       csiphy->id);
 			if (csiphy->rate_set[i])
-				break;
+				continue;
+		}
 
-			if (camss->res->version == CAMSS_660) {
-				csiphy->rate_set[i] = csiphy_match_clock_name(clock->name,
-									      "csi%d_phy", k);
-				if (csiphy->rate_set[i])
-					break;
-			}
+		csiphy->rate_set[i] = csiphy_match_clock_name(clock->name, "csiphy%d", csiphy->id);
+	}
 
-			csiphy->rate_set[i] = csiphy_match_clock_name(clock->name, "csiphy%d", k);
-			if (csiphy->rate_set[i])
-				break;
-		}
+	/* CSIPHY supplies */
+	for (i = 0; i < ARRAY_SIZE(res->regulators); i++) {
+		if (res->regulators[i])
+			csiphy->num_supplies++;
 	}
 
-	return 0;
+	if (csiphy->num_supplies) {
+		csiphy->supplies = devm_kmalloc_array(camss->dev,
+						      csiphy->num_supplies,
+						      sizeof(*csiphy->supplies),
+						      GFP_KERNEL);
+		if (!csiphy->supplies)
+			return -ENOMEM;
+	}
+
+	for (i = 0; i < csiphy->num_supplies; i++)
+		csiphy->supplies[i].supply = res->regulators[i];
+
+	ret = devm_regulator_bulk_get(camss->dev, csiphy->num_supplies,
+				      csiphy->supplies);
+	return ret;
 }
 
 /*
diff --git a/drivers/media/platform/qcom/camss/camss-csiphy.h b/drivers/media/platform/qcom/camss/camss-csiphy.h
index c9b7fe82b1f0..ab91273303b9 100644
--- a/drivers/media/platform/qcom/camss/camss-csiphy.h
+++ b/drivers/media/platform/qcom/camss/camss-csiphy.h
@@ -26,6 +26,12 @@ struct csiphy_lane {
 	u8 pol;
 };
 
+/**
+ * struct csiphy_lanes_cfg - CSIPHY lanes configuration
+ * @num_data: number of data lanes
+ * @data:     data lanes configuration
+ * @clk:      clock lane configuration (only for D-PHY)
+ */
 struct csiphy_lanes_cfg {
 	int num_data;
 	struct csiphy_lane *data;
@@ -42,6 +48,16 @@ struct csiphy_config {
 	struct csiphy_csi2_cfg *csi2;
 };
 
+struct csiphy_format_info {
+	u32 code;
+	u8 bpp;
+};
+
+struct csiphy_formats {
+	unsigned int nformats;
+	const struct csiphy_format_info *formats;
+};
+
 struct csiphy_device;
 
 struct csiphy_hw_ops {
@@ -61,6 +77,19 @@ struct csiphy_hw_ops {
 	void (*lanes_disable)(struct csiphy_device *csiphy,
 			      struct csiphy_config *cfg);
 	irqreturn_t (*isr)(int irq, void *dev);
+	int (*init)(struct csiphy_device *csiphy);
+};
+
+struct csiphy_subdev_resources {
+	u8 id;
+	const struct csiphy_hw_ops *hw_ops;
+	const struct csiphy_formats *formats;
+};
+
+struct csiphy_device_regs {
+	const struct csiphy_lane_regs *lane_regs;
+	int lane_array_size;
+	u32 offset;
 };
 
 struct csiphy_device {
@@ -76,11 +105,12 @@ struct csiphy_device {
 	bool *rate_set;
 	int nclocks;
 	u32 timer_clk_rate;
+	struct regulator_bulk_data *supplies;
+	int num_supplies;
 	struct csiphy_config cfg;
 	struct v4l2_mbus_framefmt fmt[MSM_CSIPHY_PADS_NUM];
-	const struct csiphy_hw_ops *ops;
-	const struct csiphy_format *formats;
-	unsigned int nformats;
+	const struct csiphy_subdev_resources *res;
+	struct csiphy_device_regs *regs;
 };
 
 struct camss_subdev_resources;
@@ -94,6 +124,11 @@ int msm_csiphy_register_entity(struct csiphy_device *csiphy,
 
 void msm_csiphy_unregister_entity(struct csiphy_device *csiphy);
 
+extern const struct csiphy_formats csiphy_formats_8x16;
+extern const struct csiphy_formats csiphy_formats_8x96;
+extern const struct csiphy_formats csiphy_formats_sc7280;
+extern const struct csiphy_formats csiphy_formats_sdm845;
+
 extern const struct csiphy_hw_ops csiphy_ops_2ph_1_0;
 extern const struct csiphy_hw_ops csiphy_ops_3ph_1_0;
 
diff --git a/drivers/media/platform/qcom/camss/camss-format.c b/drivers/media/platform/qcom/camss/camss-format.c
new file mode 100644
index 000000000000..4a3d5549615c
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-format.c
@@ -0,0 +1,91 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * camss-format.c
+ *
+ * Qualcomm MSM Camera Subsystem - Format helpers
+ *
+ * Copyright (c) 2023, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023 Qualcomm Technologies, Inc.
+ */
+#include <linux/bug.h>
+#include <linux/errno.h>
+
+#include "camss-format.h"
+
+/*
+ * camss_format_get_bpp - Map media bus format to bits per pixel
+ * @formats: supported media bus formats array
+ * @nformats: size of @formats array
+ * @code: media bus format code
+ *
+ * Return number of bits per pixel
+ */
+u8 camss_format_get_bpp(const struct camss_format_info *formats, unsigned int nformats, u32 code)
+{
+	unsigned int i;
+
+	for (i = 0; i < nformats; i++)
+		if (code == formats[i].code)
+			return formats[i].mbus_bpp;
+
+	WARN(1, "Unknown format\n");
+
+	return formats[0].mbus_bpp;
+}
+
+/*
+ * camss_format_find_code - Find a format code in an array
+ * @code: a pointer to media bus format codes array
+ * @n_code: size of @code array
+ * @index: index of code in the array
+ * @req_code: required code
+ *
+ * Return media bus format code
+ */
+u32 camss_format_find_code(u32 *code, unsigned int n_code, unsigned int index, u32 req_code)
+{
+	unsigned int i;
+
+	if (!req_code && index >= n_code)
+		return 0;
+
+	for (i = 0; i < n_code; i++) {
+		if (req_code) {
+			if (req_code == code[i])
+				return req_code;
+		} else {
+			if (i == index)
+				return code[i];
+		}
+	}
+
+	return code[0];
+}
+
+/*
+ * camss_format_find_format - Find a format in an array
+ * @code: media bus format code
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @formats: a pointer to formats array
+ * @nformats: size of @formats array
+ *
+ * Return index of a format or a negative error code otherwise
+ */
+int camss_format_find_format(u32 code, u32 pixelformat, const struct camss_format_info *formats,
+			     unsigned int nformats)
+{
+	unsigned int i;
+
+	for (i = 0; i < nformats; i++) {
+		if (formats[i].code == code &&
+		    formats[i].pixelformat == pixelformat)
+			return i;
+	}
+
+	for (i = 0; i < nformats; i++) {
+		if (formats[i].code == code)
+			return i;
+	}
+
+	return -EINVAL;
+}
diff --git a/drivers/media/platform/qcom/camss/camss-format.h b/drivers/media/platform/qcom/camss/camss-format.h
new file mode 100644
index 000000000000..923a48c9c3fb
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-format.h
@@ -0,0 +1,62 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * camss-format.h
+ *
+ * Qualcomm MSM Camera Subsystem - Format helpers
+ *
+ * Copyright (c) 2023, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023 Qualcomm Technologies, Inc.
+ */
+#ifndef __CAMSS_FORMAT_H__
+#define __CAMSS_FORMAT_H__
+
+#include <linux/types.h>
+
+#define PER_PLANE_DATA(plane, h_fract_num, h_fract_den, v_fract_num, v_fract_den, _bpp)         \
+	.hsub[(plane)].numerator	= (h_fract_num),                                        \
+	.hsub[(plane)].denominator	= (h_fract_den),                                        \
+	.vsub[(plane)].numerator	= (v_fract_num),                                        \
+	.vsub[(plane)].denominator	= (v_fract_den),                                        \
+	.bpp[(plane)]			= (_bpp)
+
+/*
+ * struct fract - Represents a fraction
+ * @numerator: Store the numerator part of the fraction
+ * @denominator: Store the denominator part of the fraction
+ */
+struct fract {
+	u8 numerator;
+	u8 denominator;
+};
+
+/*
+ * struct camss_format_info - ISP media bus format information
+ * @code: V4L2 media bus format code
+ * @mbus_bpp: Media bus bits per pixel
+ * @pixelformat: V4L2 pixel format FCC identifier
+ * @planes: Number of planes
+ * @hsub: Horizontal subsampling (for each plane)
+ * @vsub: Vertical subsampling (for each plane)
+ * @bpp: Bits per pixel when stored in memory (for each plane)
+ */
+struct camss_format_info {
+	u32 code;
+	u32 mbus_bpp;
+	u32 pixelformat;
+	u8 planes;
+	struct fract hsub[3];
+	struct fract vsub[3];
+	unsigned int bpp[3];
+};
+
+struct camss_formats {
+	unsigned int nformats;
+	const struct camss_format_info *formats;
+};
+
+u8 camss_format_get_bpp(const struct camss_format_info *formats, unsigned int nformats, u32 code);
+u32 camss_format_find_code(u32 *code, unsigned int n_code, unsigned int index, u32 req_code);
+int camss_format_find_format(u32 code, u32 pixelformat, const struct camss_format_info *formats,
+			     unsigned int nformats);
+
+#endif /* __CAMSS_FORMAT_H__ */
diff --git a/drivers/media/platform/qcom/camss/camss-ispif.c b/drivers/media/platform/qcom/camss/camss-ispif.c
index a12dcc7ff438..2dc585c6123d 100644
--- a/drivers/media/platform/qcom/camss/camss-ispif.c
+++ b/drivers/media/platform/qcom/camss/camss-ispif.c
@@ -830,6 +830,7 @@ static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
 		ispif_select_cid(ispif, intf, cid, vfe, 1);
 		ispif_config_irq(ispif, intf, vfe, 1);
 		if (camss->res->version == CAMSS_8x96 ||
+		    camss->res->version == CAMSS_8x53 ||
 		    camss->res->version == CAMSS_660)
 			ispif_config_pack(ispif,
 					  line->fmt[MSM_ISPIF_PAD_SINK].code,
@@ -848,6 +849,7 @@ static int ispif_set_stream(struct v4l2_subdev *sd, int enable)
 
 		mutex_lock(&ispif->config_lock);
 		if (camss->res->version == CAMSS_8x96 ||
+		    camss->res->version == CAMSS_8x53 ||
 		    camss->res->version == CAMSS_660)
 			ispif_config_pack(ispif,
 					  line->fmt[MSM_ISPIF_PAD_SINK].code,
@@ -1111,6 +1113,7 @@ int msm_ispif_subdev_init(struct camss *camss,
 	if (camss->res->version == CAMSS_8x16)
 		ispif->line_num = 2;
 	else if (camss->res->version == CAMSS_8x96 ||
+		 camss->res->version == CAMSS_8x53 ||
 		 camss->res->version == CAMSS_660)
 		ispif->line_num = 4;
 	else
@@ -1130,6 +1133,7 @@ int msm_ispif_subdev_init(struct camss *camss,
 			ispif->line[i].nformats =
 					ARRAY_SIZE(ispif_formats_8x16);
 		} else if (camss->res->version == CAMSS_8x96 ||
+			   camss->res->version == CAMSS_8x53 ||
 			   camss->res->version == CAMSS_660) {
 			ispif->line[i].formats = ispif_formats_8x96;
 			ispif->line[i].nformats =
@@ -1162,6 +1166,7 @@ int msm_ispif_subdev_init(struct camss *camss,
 		ret = devm_request_irq(dev, ispif->irq, ispif_isr_8x16,
 			       IRQF_TRIGGER_RISING, ispif->irq_name, ispif);
 	else if (camss->res->version == CAMSS_8x96 ||
+		 camss->res->version == CAMSS_8x53 ||
 		 camss->res->version == CAMSS_660)
 		ret = devm_request_irq(dev, ispif->irq, ispif_isr_8x96,
 			       IRQF_TRIGGER_RISING, ispif->irq_name, ispif);
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-170.c b/drivers/media/platform/qcom/camss/camss-vfe-17x.c
index 795ac3815339..e5ee7e717b3b 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-170.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-17x.c
@@ -14,8 +14,6 @@
 #include "camss.h"
 #include "camss-vfe.h"
 
-#define VFE_HW_VERSION				(0x000)
-
 #define VFE_GLOBAL_RESET_CMD			(0x018)
 #define		GLOBAL_RESET_CMD_CORE		BIT(0)
 #define		GLOBAL_RESET_CMD_CAMIF		BIT(1)
@@ -176,20 +174,6 @@
 #define VFE_BUS_WM_FRAME_INC(n)			(0x2258 + (n) * 0x100)
 #define VFE_BUS_WM_BURST_LIMIT(n)		(0x225c + (n) * 0x100)
 
-static u32 vfe_hw_version(struct vfe_device *vfe)
-{
-	u32 hw_version = readl_relaxed(vfe->base + VFE_HW_VERSION);
-
-	u32 gen = (hw_version >> 28) & 0xF;
-	u32 rev = (hw_version >> 16) & 0xFFF;
-	u32 step = hw_version & 0xFFFF;
-
-	dev_dbg(vfe->camss->dev, "VFE HW Version = %u.%u.%u\n",
-		gen, rev, step);
-
-	return hw_version;
-}
-
 static inline void vfe_reg_set(struct vfe_device *vfe, u32 reg, u32 set_bits)
 {
 	u32 bits = readl_relaxed(vfe->base + reg);
@@ -353,7 +337,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 	writel_relaxed(status0, vfe->base + VFE_IRQ_CLEAR_0);
 	writel_relaxed(status1, vfe->base + VFE_IRQ_CLEAR_1);
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++) {
 		vfe_bus_status[i] = readl_relaxed(vfe->base + VFE_BUS_IRQ_STATUS(i));
 		writel_relaxed(vfe_bus_status[i], vfe->base + VFE_BUS_IRQ_CLEAR(i));
 	}
@@ -367,11 +351,11 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 	if (status0 & STATUS_0_RESET_ACK)
 		vfe->isr_ops.reset_ack(vfe);
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++)
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++)
 		if (status0 & STATUS_0_RDI_REG_UPDATE(i))
 			vfe->isr_ops.reg_update(vfe, i);
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++)
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++)
 		if (status0 & STATUS_1_RDI_SOF(i))
 			vfe->isr_ops.sof(vfe, i);
 
@@ -438,62 +422,6 @@ error:
 	return -EINVAL;
 }
 
-static int vfe_enable_output(struct vfe_line *line)
-{
-	struct vfe_device *vfe = to_vfe(line);
-	struct vfe_output *output = &line->output;
-	const struct vfe_hw_ops *ops = vfe->ops;
-	struct media_entity *sensor;
-	unsigned long flags;
-	unsigned int frame_skip = 0;
-	unsigned int i;
-
-	sensor = camss_find_sensor(&line->subdev.entity);
-	if (sensor) {
-		struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(sensor);
-
-		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
-		/* Max frame skip is 29 frames */
-		if (frame_skip > VFE_FRAME_DROP_VAL - 1)
-			frame_skip = VFE_FRAME_DROP_VAL - 1;
-	}
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	ops->reg_update_clear(vfe, line->id);
-
-	if (output->state > VFE_OUTPUT_RESERVED) {
-		dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
-			output->state);
-		spin_unlock_irqrestore(&vfe->output_lock, flags);
-		return -EINVAL;
-	}
-
-	WARN_ON(output->gen2.active_num);
-
-	output->state = VFE_OUTPUT_ON;
-
-	output->sequence = 0;
-	output->wait_reg_update = 0;
-	reinit_completion(&output->reg_update);
-
-	vfe_wm_start(vfe, output->wm_idx[0], line);
-
-	for (i = 0; i < 2; i++) {
-		output->buf[i] = vfe_buf_get_pending(output);
-		if (!output->buf[i])
-			break;
-		output->gen2.active_num++;
-		vfe_wm_update(vfe, output->wm_idx[0], output->buf[i]->addr[0], line);
-	}
-
-	ops->reg_update(vfe, line->id);
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	return 0;
-}
-
 /*
  * vfe_enable - Enable streaming on VFE line
  * @line: VFE line
@@ -518,7 +446,7 @@ static int vfe_enable(struct vfe_line *line)
 	if (ret < 0)
 		goto error_get_output;
 
-	ret = vfe_enable_output(line);
+	ret = vfe_enable_output_v2(line);
 	if (ret < 0)
 		goto error_enable_output;
 
@@ -560,7 +488,7 @@ static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 	unsigned long flags;
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
-	vfe->ops->reg_update_clear(vfe, line_id);
+	vfe->res->hw_ops->reg_update_clear(vfe, line_id);
 
 	output = &vfe->line[line_id].output;
 
@@ -627,40 +555,6 @@ out_unlock:
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 }
 
-/*
- * vfe_queue_buffer - Add empty buffer
- * @vid: Video device structure
- * @buf: Buffer to be enqueued
- *
- * Add an empty buffer - depending on the current number of buffers it will be
- * put in pending buffer queue or directly given to the hardware to be filled.
- *
- * Return 0 on success or a negative error code otherwise
- */
-static int vfe_queue_buffer(struct camss_video *vid,
-			    struct camss_buffer *buf)
-{
-	struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
-	struct vfe_device *vfe = to_vfe(line);
-	struct vfe_output *output;
-	unsigned long flags;
-
-	output = &line->output;
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	if (output->state == VFE_OUTPUT_ON && output->gen2.active_num < 2) {
-		output->buf[output->gen2.active_num++] = buf;
-		vfe_wm_update(vfe, output->wm_idx[0], buf->addr[0], line);
-	} else {
-		vfe_buf_add_pending(output, buf);
-	}
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	return 0;
-}
-
 static const struct vfe_isr_ops vfe_isr_ops_170 = {
 	.reset_ack = vfe_isr_reset_ack,
 	.halt_ack = vfe_isr_halt_ack,
@@ -671,7 +565,7 @@ static const struct vfe_isr_ops vfe_isr_ops_170 = {
 };
 
 static const struct camss_video_ops vfe_video_ops_170 = {
-	.queue_buffer = vfe_queue_buffer,
+	.queue_buffer = vfe_queue_buffer_v2,
 	.flush_buffers = vfe_flush_buffers,
 };
 
@@ -695,5 +589,7 @@ const struct vfe_hw_ops vfe_ops_170 = {
 	.vfe_enable = vfe_enable,
 	.vfe_halt = vfe_halt,
 	.violation_read = vfe_violation_read,
+	.vfe_wm_start = vfe_wm_start,
 	.vfe_wm_stop = vfe_wm_stop,
+	.vfe_wm_update = vfe_wm_update,
 };
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-4-1.c b/drivers/media/platform/qcom/camss/camss-vfe-4-1.c
index ef6b34c915df..901677293d97 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-4-1.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-4-1.c
@@ -210,15 +210,6 @@
 #define MSM_VFE_VFE0_UB_SIZE 1023
 #define MSM_VFE_VFE0_UB_SIZE_RDI (MSM_VFE_VFE0_UB_SIZE / 3)
 
-static u32 vfe_hw_version(struct vfe_device *vfe)
-{
-	u32 hw_version = readl_relaxed(vfe->base + VFE_0_HW_VERSION);
-
-	dev_dbg(vfe->camss->dev, "VFE HW Version = 0x%08x\n", hw_version);
-
-	return hw_version;
-}
-
 static u16 vfe_get_ub_size(u8 vfe_id)
 {
 	if (vfe_id == 0)
@@ -892,7 +883,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 	u32 value0, value1;
 	int i, j;
 
-	vfe->ops->isr_read(vfe, &value0, &value1);
+	vfe->res->hw_ops->isr_read(vfe, &value0, &value1);
 
 	dev_dbg(vfe->camss->dev, "VFE: status0 = 0x%08x, status1 = 0x%08x\n",
 		value0, value1);
@@ -901,7 +892,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 		vfe->isr_ops.reset_ack(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_VIOLATION)
-		vfe->ops->violation_read(vfe);
+		vfe->res->hw_ops->violation_read(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK)
 		vfe->isr_ops.halt_ack(vfe);
@@ -938,7 +929,10 @@ static irqreturn_t vfe_isr(int irq, void *dev)
  */
 static void vfe_4_1_pm_domain_off(struct vfe_device *vfe)
 {
-	/* nop */
+	if (!vfe->res->has_pd)
+		return;
+
+	vfe_pm_domain_off(vfe);
 }
 
 /*
@@ -947,7 +941,10 @@ static void vfe_4_1_pm_domain_off(struct vfe_device *vfe)
  */
 static int vfe_4_1_pm_domain_on(struct vfe_device *vfe)
 {
-	return 0;
+	if (!vfe->res->has_pd)
+		return 0;
+
+	return vfe_pm_domain_on(vfe);
 }
 
 static const struct vfe_hw_ops_gen1 vfe_ops_gen1_4_1 = {
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-4-7.c b/drivers/media/platform/qcom/camss/camss-vfe-4-7.c
index 7655d22a9fda..76729607db02 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-4-7.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-4-7.c
@@ -18,8 +18,6 @@
 #include "camss-vfe-gen1.h"
 
 
-#define VFE_0_HW_VERSION		0x000
-
 #define VFE_0_GLOBAL_RESET_CMD		0x018
 #define VFE_0_GLOBAL_RESET_CMD_CORE	BIT(0)
 #define VFE_0_GLOBAL_RESET_CMD_CAMIF	BIT(1)
@@ -254,15 +252,6 @@
 #define MSM_VFE_VFE1_UB_SIZE 1535
 #define MSM_VFE_VFE1_UB_SIZE_RDI (MSM_VFE_VFE1_UB_SIZE / 3)
 
-static u32 vfe_hw_version(struct vfe_device *vfe)
-{
-	u32 hw_version = readl_relaxed(vfe->base + VFE_0_HW_VERSION);
-
-	dev_dbg(vfe->camss->dev, "VFE HW Version = 0x%08x\n", hw_version);
-
-	return hw_version;
-}
-
 static u16 vfe_get_ub_size(u8 vfe_id)
 {
 	if (vfe_id == 0)
@@ -1050,7 +1039,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 	u32 value0, value1;
 	int i, j;
 
-	vfe->ops->isr_read(vfe, &value0, &value1);
+	vfe->res->hw_ops->isr_read(vfe, &value0, &value1);
 
 	dev_dbg(vfe->camss->dev, "VFE: status0 = 0x%08x, status1 = 0x%08x\n",
 		value0, value1);
@@ -1059,12 +1048,12 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 		vfe->isr_ops.reset_ack(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_VIOLATION)
-		vfe->ops->violation_read(vfe);
+		vfe->res->hw_ops->violation_read(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK)
 		vfe->isr_ops.halt_ack(vfe);
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++)
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++)
 		if (value0 & VFE_0_IRQ_STATUS_0_line_n_REG_UPDATE(i))
 			vfe->isr_ops.reg_update(vfe, i);
 
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-4-8.c b/drivers/media/platform/qcom/camss/camss-vfe-4-8.c
index f52fa30f3853..b2f7d855d8dd 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-4-8.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-4-8.c
@@ -17,8 +17,6 @@
 #include "camss-vfe.h"
 #include "camss-vfe-gen1.h"
 
-#define VFE_0_HW_VERSION		0x000
-
 #define VFE_0_GLOBAL_RESET_CMD		0x018
 #define VFE_0_GLOBAL_RESET_CMD_CORE	BIT(0)
 #define VFE_0_GLOBAL_RESET_CMD_CAMIF	BIT(1)
@@ -247,15 +245,6 @@
 #define MSM_VFE_VFE1_UB_SIZE 1535
 #define MSM_VFE_VFE1_UB_SIZE_RDI (MSM_VFE_VFE1_UB_SIZE / 3)
 
-static u32 vfe_hw_version(struct vfe_device *vfe)
-{
-	u32 hw_version = readl_relaxed(vfe->base + VFE_0_HW_VERSION);
-
-	dev_dbg(vfe->camss->dev, "VFE HW Version = 0x%08x\n", hw_version);
-
-	return hw_version;
-}
-
 static inline void vfe_reg_clr(struct vfe_device *vfe, u32 reg, u32 clr_bits)
 {
 	u32 bits = readl_relaxed(vfe->base + reg);
@@ -980,7 +969,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 	u32 value0, value1;
 	int i, j;
 
-	vfe->ops->isr_read(vfe, &value0, &value1);
+	vfe->res->hw_ops->isr_read(vfe, &value0, &value1);
 
 	dev_dbg(vfe->camss->dev, "VFE: status0 = 0x%08x, status1 = 0x%08x\n",
 		value0, value1);
@@ -989,12 +978,12 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 		vfe->isr_ops.reset_ack(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_VIOLATION)
-		vfe->ops->violation_read(vfe);
+		vfe->res->hw_ops->violation_read(vfe);
 
 	if (value1 & VFE_0_IRQ_STATUS_1_BUS_BDG_HALT_ACK)
 		vfe->isr_ops.halt_ack(vfe);
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++)
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++)
 		if (value0 & VFE_0_IRQ_STATUS_0_line_n_REG_UPDATE(i))
 			vfe->isr_ops.reg_update(vfe, i);
 
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-480.c b/drivers/media/platform/qcom/camss/camss-vfe-480.c
index dc2735476c82..4feea590a47b 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-480.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-480.c
@@ -15,8 +15,6 @@
 #include "camss.h"
 #include "camss-vfe.h"
 
-#define VFE_HW_VERSION			(0x00)
-
 #define VFE_GLOBAL_RESET_CMD		(vfe_is_lite(vfe) ? 0x0c : 0x1c)
 #define	    GLOBAL_RESET_HW_AND_REG	(vfe_is_lite(vfe) ? BIT(1) : BIT(0))
 
@@ -92,19 +90,6 @@ static inline int bus_irq_mask_0_comp_done(struct vfe_device *vfe, int n)
 
 #define MAX_VFE_OUTPUT_LINES	4
 
-static u32 vfe_hw_version(struct vfe_device *vfe)
-{
-	u32 hw_version = readl_relaxed(vfe->base + VFE_HW_VERSION);
-
-	u32 gen = (hw_version >> 28) & 0xF;
-	u32 rev = (hw_version >> 16) & 0xFFF;
-	u32 step = hw_version & 0xFFFF;
-
-	dev_dbg(vfe->camss->dev, "VFE HW Version = %u.%u.%u\n", gen, rev, step);
-
-	return hw_version;
-}
-
 static void vfe_global_reset(struct vfe_device *vfe)
 {
 	writel_relaxed(IRQ_MASK_0_RESET_ACK, vfe->base + VFE_IRQ_MASK(0));
@@ -167,18 +152,16 @@ static inline void vfe_reg_update_clear(struct vfe_device *vfe,
 	vfe->reg_update &= ~REG_UPDATE_RDI(vfe, line_id);
 }
 
-static void vfe_enable_irq_common(struct vfe_device *vfe)
-{
-	/* enable reset ack IRQ and top BUS status IRQ */
-	writel_relaxed(IRQ_MASK_0_RESET_ACK | IRQ_MASK_0_BUS_TOP_IRQ,
-		       vfe->base + VFE_IRQ_MASK(0));
-}
-
-static void vfe_enable_lines_irq(struct vfe_device *vfe)
+static void vfe_enable_irq(struct vfe_device *vfe)
 {
 	int i;
 	u32 bus_irq_mask = 0;
 
+	if (!vfe->stream_count)
+		/* enable reset ack IRQ and top BUS status IRQ */
+		writel(IRQ_MASK_0_RESET_ACK | IRQ_MASK_0_BUS_TOP_IRQ,
+		       vfe->base + VFE_IRQ_MASK(0));
+
 	for (i = 0; i < MAX_VFE_OUTPUT_LINES; i++) {
 		/* Enable IRQ for newly added lines, but also keep already running lines's IRQ */
 		if (vfe->line[i].output.state == VFE_OUTPUT_RESERVED ||
@@ -188,11 +171,10 @@ static void vfe_enable_lines_irq(struct vfe_device *vfe)
 			}
 	}
 
-	writel_relaxed(bus_irq_mask, vfe->base + VFE_BUS_IRQ_MASK(0));
+	writel(bus_irq_mask, vfe->base + VFE_BUS_IRQ_MASK(0));
 }
 
 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id);
-static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm);
 
 /*
  * vfe_isr - VFE module interrupt handler
@@ -226,7 +208,7 @@ static irqreturn_t vfe_isr(int irq, void *dev)
 				vfe_isr_reg_update(vfe, i);
 
 			if (status & BUS_IRQ_MASK_0_COMP_DONE(vfe, RDI_COMP_GROUP(i)))
-				vfe_isr_wm_done(vfe, i);
+				vfe_buf_done(vfe, i);
 		}
 	}
 
@@ -245,132 +227,6 @@ static int vfe_halt(struct vfe_device *vfe)
 	return 0;
 }
 
-static int vfe_get_output(struct vfe_line *line)
-{
-	struct vfe_device *vfe = to_vfe(line);
-	struct vfe_output *output;
-	unsigned long flags;
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	output = &line->output;
-	if (output->state > VFE_OUTPUT_RESERVED) {
-		dev_err(vfe->camss->dev, "Output is running\n");
-		goto error;
-	}
-
-	output->wm_num = 1;
-
-	/* Correspondence between VFE line number and WM number.
-	 * line 0 -> RDI 0, line 1 -> RDI1, line 2 -> RDI2, line 3 -> PIX/RDI3
-	 * Note this 1:1 mapping will not work for PIX streams.
-	 */
-	output->wm_idx[0] = line->id;
-	vfe->wm_output_map[line->id] = line->id;
-
-	output->drop_update_idx = 0;
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	return 0;
-
-error:
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-	output->state = VFE_OUTPUT_OFF;
-
-	return -EINVAL;
-}
-
-static int vfe_enable_output(struct vfe_line *line)
-{
-	struct vfe_device *vfe = to_vfe(line);
-	struct vfe_output *output = &line->output;
-	unsigned long flags;
-	unsigned int i;
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	vfe_reg_update_clear(vfe, line->id);
-
-	if (output->state > VFE_OUTPUT_RESERVED) {
-		dev_err(vfe->camss->dev, "Output is not in reserved state %d\n",
-			output->state);
-		spin_unlock_irqrestore(&vfe->output_lock, flags);
-		return -EINVAL;
-	}
-
-	WARN_ON(output->gen2.active_num);
-
-	output->state = VFE_OUTPUT_ON;
-
-	output->sequence = 0;
-	output->wait_reg_update = 0;
-	reinit_completion(&output->reg_update);
-
-	vfe_wm_start(vfe, output->wm_idx[0], line);
-
-	for (i = 0; i < 2; i++) {
-		output->buf[i] = vfe_buf_get_pending(output);
-		if (!output->buf[i])
-			break;
-		output->gen2.active_num++;
-		vfe_wm_update(vfe, output->wm_idx[0], output->buf[i]->addr[0], line);
-	}
-
-	vfe_reg_update(vfe, line->id);
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	return 0;
-}
-
-/*
- * vfe_enable - Enable streaming on VFE line
- * @line: VFE line
- *
- * Return 0 on success or a negative error code otherwise
- */
-static int vfe_enable(struct vfe_line *line)
-{
-	struct vfe_device *vfe = to_vfe(line);
-	int ret;
-
-	mutex_lock(&vfe->stream_lock);
-
-	if (!vfe->stream_count)
-		vfe_enable_irq_common(vfe);
-
-	vfe->stream_count++;
-
-	vfe_enable_lines_irq(vfe);
-
-	mutex_unlock(&vfe->stream_lock);
-
-	ret = vfe_get_output(line);
-	if (ret < 0)
-		goto error_get_output;
-
-	ret = vfe_enable_output(line);
-	if (ret < 0)
-		goto error_enable_output;
-
-	vfe->was_streaming = 1;
-
-	return 0;
-
-error_enable_output:
-	vfe_put_output(line);
-
-error_get_output:
-	mutex_lock(&vfe->stream_lock);
-
-	vfe->stream_count--;
-
-	mutex_unlock(&vfe->stream_lock);
-
-	return ret;
-}
-
 /*
  * vfe_isr_reg_update - Process reg update interrupt
  * @vfe: VFE Device
@@ -394,114 +250,48 @@ static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 }
 
-/*
- * vfe_isr_wm_done - Process write master done interrupt
- * @vfe: VFE Device
- * @wm: Write master id
- */
-static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
-{
-	struct vfe_line *line = &vfe->line[vfe->wm_output_map[wm]];
-	struct camss_buffer *ready_buf;
-	struct vfe_output *output;
-	unsigned long flags;
-	u32 index;
-	u64 ts = ktime_get_ns();
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
-		dev_err_ratelimited(vfe->camss->dev,
-				    "Received wm done for unmapped index\n");
-		goto out_unlock;
-	}
-	output = &vfe->line[vfe->wm_output_map[wm]].output;
-
-	ready_buf = output->buf[0];
-	if (!ready_buf) {
-		dev_err_ratelimited(vfe->camss->dev,
-				    "Missing ready buf %d!\n", output->state);
-		goto out_unlock;
-	}
-
-	ready_buf->vb.vb2_buf.timestamp = ts;
-	ready_buf->vb.sequence = output->sequence++;
-
-	index = 0;
-	output->buf[0] = output->buf[1];
-	if (output->buf[0])
-		index = 1;
-
-	output->buf[index] = vfe_buf_get_pending(output);
-
-	if (output->buf[index])
-		vfe_wm_update(vfe, output->wm_idx[0], output->buf[index]->addr[0], line);
-	else
-		output->gen2.active_num--;
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
-
-	return;
+static const struct camss_video_ops vfe_video_ops_480 = {
+	.queue_buffer = vfe_queue_buffer_v2,
+	.flush_buffers = vfe_flush_buffers,
+};
 
-out_unlock:
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
+static void vfe_subdev_init(struct device *dev, struct vfe_device *vfe)
+{
+	vfe->video_ops = vfe_video_ops_480;
 }
 
-/*
- * vfe_queue_buffer - Add empty buffer
- * @vid: Video device structure
- * @buf: Buffer to be enqueued
- *
- * Add an empty buffer - depending on the current number of buffers it will be
- * put in pending buffer queue or directly given to the hardware to be filled.
- *
- * Return 0 on success or a negative error code otherwise
- */
-static int vfe_queue_buffer(struct camss_video *vid,
-			    struct camss_buffer *buf)
+static void vfe_isr_read(struct vfe_device *vfe, u32 *value0, u32 *value1)
 {
-	struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
-	struct vfe_device *vfe = to_vfe(line);
-	struct vfe_output *output;
-	unsigned long flags;
-
-	output = &line->output;
-
-	spin_lock_irqsave(&vfe->output_lock, flags);
-
-	if (output->state == VFE_OUTPUT_ON && output->gen2.active_num < 2) {
-		output->buf[output->gen2.active_num++] = buf;
-		vfe_wm_update(vfe, output->wm_idx[0], buf->addr[0], line);
-	} else {
-		vfe_buf_add_pending(output, buf);
-	}
-
-	spin_unlock_irqrestore(&vfe->output_lock, flags);
-
-	return 0;
+	/* nop */
 }
 
-static const struct camss_video_ops vfe_video_ops_480 = {
-	.queue_buffer = vfe_queue_buffer,
-	.flush_buffers = vfe_flush_buffers,
-};
+static void vfe_violation_read(struct vfe_device *vfe)
+{
+	/* nop */
+}
 
-static void vfe_subdev_init(struct device *dev, struct vfe_device *vfe)
+static void vfe_buf_done_480(struct vfe_device *vfe, int port_id)
 {
-	vfe->video_ops = vfe_video_ops_480;
+	/* nop */
 }
 
 const struct vfe_hw_ops vfe_ops_480 = {
+	.enable_irq = vfe_enable_irq,
 	.global_reset = vfe_global_reset,
 	.hw_version = vfe_hw_version,
 	.isr = vfe_isr,
+	.isr_read = vfe_isr_read,
+	.reg_update = vfe_reg_update,
+	.reg_update_clear = vfe_reg_update_clear,
 	.pm_domain_off = vfe_pm_domain_off,
 	.pm_domain_on = vfe_pm_domain_on,
 	.subdev_init = vfe_subdev_init,
 	.vfe_disable = vfe_disable,
-	.vfe_enable = vfe_enable,
+	.vfe_enable = vfe_enable_v2,
 	.vfe_halt = vfe_halt,
+	.violation_read = vfe_violation_read,
+	.vfe_wm_start = vfe_wm_start,
 	.vfe_wm_stop = vfe_wm_stop,
+	.vfe_buf_done = vfe_buf_done_480,
+	.vfe_wm_update = vfe_wm_update,
 };
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-680.c b/drivers/media/platform/qcom/camss/camss-vfe-680.c
new file mode 100644
index 000000000000..99036e7c1e76
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-vfe-680.c
@@ -0,0 +1,244 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * camss-vfe-680.c
+ *
+ * Qualcomm MSM Camera Subsystem - VFE (Video Front End) Module v680
+ *
+ * Copyright (C) 2025 Linaro Ltd.
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+
+#include "camss.h"
+#include "camss-vfe.h"
+
+#define VFE_TOP_IRQn_STATUS(vfe, n)		((vfe_is_lite(vfe) ? 0x1c : 0x44) + (n) * 4)
+#define VFE_TOP_IRQn_MASK(vfe, n)		((vfe_is_lite(vfe) ? 0x24 : 0x34) + (n) * 4)
+#define VFE_TOP_IRQn_CLEAR(vfe, n)		((vfe_is_lite(vfe) ? 0x2c : 0x3c) + (n) * 4)
+#define		VFE_IRQ1_SOF(vfe, n)		((vfe_is_lite(vfe) ? BIT(2) : BIT(8)) << ((n) * 2))
+#define		VFE_IRQ1_EOF(vfe, n)		((vfe_is_lite(vfe) ? BIT(3) : BIT(9)) << ((n) * 2))
+#define VFE_TOP_IRQ_CMD(vfe)			(vfe_is_lite(vfe) ? 0x38 : 0x30)
+#define		VFE_TOP_IRQ_CMD_GLOBAL_CLEAR	BIT(0)
+#define VFE_TOP_DIAG_CONFIG			(vfe_is_lite(vfe) ? 0x40 : 0x50)
+
+#define VFE_TOP_DEBUG_11(vfe)			(vfe_is_lite(vfe) ? 0x40 : 0xcc)
+#define VFE_TOP_DEBUG_12(vfe)			(vfe_is_lite(vfe) ? 0x40 : 0xd0)
+#define VFE_TOP_DEBUG_13(vfe)			(vfe_is_lite(vfe) ? 0x40 : 0xd4)
+
+#define VFE_BUS_IRQn_MASK(vfe, n)		((vfe_is_lite(vfe) ? 0x218 : 0xc18) + (n) * 4)
+#define VFE_BUS_IRQn_CLEAR(vfe, n)		((vfe_is_lite(vfe) ? 0x220 : 0xc20) + (n) * 4)
+#define VFE_BUS_IRQn_STATUS(vfe, n)		((vfe_is_lite(vfe) ? 0x228 : 0xc28) + (n) * 4)
+#define VFE_BUS_IRQ_GLOBAL_CLEAR(vfe)		(vfe_is_lite(vfe) ? 0x230 : 0xc30)
+#define VFE_BUS_WR_VIOLATION_STATUS(vfe)	(vfe_is_lite(vfe) ? 0x264 : 0xc64)
+#define VFE_BUS_WR_OVERFLOW_STATUS(vfe)		(vfe_is_lite(vfe) ? 0x268 : 0xc68)
+#define VFE_BUS_WR_IMAGE_VIOLATION_STATUS(vfe)	(vfe_is_lite(vfe) ? 0x270 : 0xc70)
+
+#define VFE_BUS_WRITE_CLIENT_CFG(vfe, c)	((vfe_is_lite(vfe) ? 0x400 : 0xe00) + (c) * 0x100)
+#define		VFE_BUS_WRITE_CLIENT_CFG_EN	BIT(0)
+#define VFE_BUS_IMAGE_ADDR(vfe, c)		((vfe_is_lite(vfe) ? 0x404 : 0xe04) + (c) * 0x100)
+#define VFE_BUS_FRAME_INCR(vfe, c)		((vfe_is_lite(vfe) ? 0x408 : 0xe08) + (c) * 0x100)
+#define VFE_BUS_IMAGE_CFG0(vfe, c)		((vfe_is_lite(vfe) ? 0x40c : 0xe0c) + (c) * 0x100)
+#define		VFE_BUS_IMAGE_CFG0_DATA(h, s)	(((h) << 16) | ((s) >> 4))
+#define WM_IMAGE_CFG_0_DEFAULT_WIDTH		(0xFFFF)
+
+#define VFE_BUS_IMAGE_CFG1(vfe, c)		((vfe_is_lite(vfe) ? 0x410 : 0xe10) + (c) * 0x100)
+#define VFE_BUS_IMAGE_CFG2(vfe, c)		((vfe_is_lite(vfe) ? 0x414 : 0xe14) + (c) * 0x100)
+#define VFE_BUS_PACKER_CFG(vfe, c)		((vfe_is_lite(vfe) ? 0x418 : 0xe18) + (c) * 0x100)
+#define VFE_BUS_IRQ_SUBSAMPLE_PERIOD(vfe, c)	((vfe_is_lite(vfe) ? 0x430 : 0xe30) + (c) * 0x100)
+#define VFE_BUS_IRQ_SUBSAMPLE_PATTERN(vfe, c)	((vfe_is_lite(vfe) ? 0x434 : 0xe34) + (c) * 0x100)
+#define VFE_BUS_FRAMEDROP_PERIOD(vfe, c)	((vfe_is_lite(vfe) ? 0x438 : 0xe38) + (c) * 0x100)
+#define VFE_BUS_FRAMEDROP_PATTERN(vfe, c)	((vfe_is_lite(vfe) ? 0x43c : 0xe3c) + (c) * 0x100)
+#define VFE_BUS_MMU_PREFETCH_CFG(vfe, c)	((vfe_is_lite(vfe) ? 0x460 : 0xe60) + (c) * 0x100)
+#define		VFE_BUS_MMU_PREFETCH_CFG_EN	BIT(0)
+#define VFE_BUS_MMU_PREFETCH_MAX_OFFSET(vfe, c)	((vfe_is_lite(vfe) ? 0x464 : 0xe64) + (c) * 0x100)
+#define VFE_BUS_ADDR_STATUS0(vfe, c)		((vfe_is_lite(vfe) ? 0x470 : 0xe70) + (c) * 0x100)
+
+/*
+ * TODO: differentiate the port id based on requested type of RDI, BHIST etc
+ *
+ * IFE write master IDs
+ *
+ * VIDEO_FULL_Y		0
+ * VIDEO_FULL_C		1
+ * VIDEO_DS_4:1		2
+ * VIDEO_DS_16:1	3
+ * DISPLAY_FULL_Y	4
+ * DISPLAY_FULL_C	5
+ * DISPLAY_DS_4:1	6
+ * DISPLAY_DS_16:1	7
+ * FD_Y			8
+ * FD_C			9
+ * PIXEL_RAW		10
+ * STATS_BE0		11
+ * STATS_BHIST0		12
+ * STATS_TINTLESS_BG	13
+ * STATS_AWB_BG		14
+ * STATS_AWB_BFW	15
+ * STATS_BAF		16
+ * STATS_BHIST		17
+ * STATS_RS		18
+ * STATS_IHIST		19
+ * SPARSE_PD		20
+ * PDAF_V2.0_PD_DATA	21
+ * PDAF_V2.0_SAD	22
+ * LCR			23
+ * RDI0			24
+ * RDI1			25
+ * RDI2			26
+ * LTM_STATS		27
+ *
+ * IFE Lite write master IDs
+ *
+ * RDI0			0
+ * RDI1			1
+ * RDI2			2
+ * RDI3			3
+ * GAMMA		4
+ * BE			5
+ */
+
+/* TODO: assign an ENUM in resources and use the provided master
+ *       id directly for RDI, STATS, AWB_BG, BHIST.
+ *       This macro only works because RDI is all we support right now.
+ */
+#define RDI_WM(n)			((vfe_is_lite(vfe) ? 0 : 24) + (n))
+
+static void vfe_global_reset(struct vfe_device *vfe)
+{
+	/* VFE680 has no global reset, simply report a completion */
+	complete(&vfe->reset_complete);
+}
+
+/*
+ * vfe_isr - VFE module interrupt handler
+ * @irq: Interrupt line
+ * @dev: VFE device
+ *
+ * Return IRQ_HANDLED on success
+ */
+static irqreturn_t vfe_isr(int irq, void *dev)
+{
+	return IRQ_HANDLED;
+}
+
+/*
+ * vfe_halt - Trigger halt on VFE module and wait to complete
+ * @vfe: VFE device
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+static int vfe_halt(struct vfe_device *vfe)
+{
+	/* rely on vfe_disable_output() to stop the VFE */
+	return 0;
+}
+
+static void vfe_disable_irq(struct vfe_device *vfe)
+{
+	writel(0u, vfe->base + VFE_TOP_IRQn_MASK(vfe, 0));
+	writel(0u, vfe->base + VFE_TOP_IRQn_MASK(vfe, 1));
+	writel(0u, vfe->base + VFE_BUS_IRQn_MASK(vfe, 0));
+	writel(0u, vfe->base + VFE_BUS_IRQn_MASK(vfe, 1));
+}
+
+static void vfe_wm_update(struct vfe_device *vfe, u8 rdi, u32 addr,
+			  struct vfe_line *line)
+{
+	u8 wm = RDI_WM(rdi);
+
+	writel(addr, vfe->base + VFE_BUS_IMAGE_ADDR(vfe, wm));
+}
+
+static void vfe_wm_start(struct vfe_device *vfe, u8 rdi, struct vfe_line *line)
+{
+	struct v4l2_pix_format_mplane *pix =
+		&line->video_out.active_fmt.fmt.pix_mp;
+	u32 stride = pix->plane_fmt[0].bytesperline;
+	u32 cfg;
+	u8 wm;
+
+	cfg = VFE_BUS_IMAGE_CFG0_DATA(pix->height, stride);
+	wm = RDI_WM(rdi);
+
+	writel(cfg, vfe->base + VFE_BUS_IMAGE_CFG0(vfe, wm));
+	writel(0, vfe->base + VFE_BUS_IMAGE_CFG1(vfe, wm));
+	writel(stride, vfe->base + VFE_BUS_IMAGE_CFG2(vfe, wm));
+	writel(0, vfe->base + VFE_BUS_PACKER_CFG(vfe, wm));
+
+	/* Set total frame increment value */
+	writel(pix->plane_fmt[0].bytesperline * pix->height,
+	       vfe->base + VFE_BUS_FRAME_INCR(vfe, wm));
+
+	/* MMU */
+	writel(VFE_BUS_MMU_PREFETCH_CFG_EN, vfe->base + VFE_BUS_MMU_PREFETCH_CFG(vfe, wm));
+	writel(~0u, vfe->base + VFE_BUS_MMU_PREFETCH_MAX_OFFSET(vfe, wm));
+
+	/* no dropped frames, one irq per frame */
+	writel(1, vfe->base + VFE_BUS_FRAMEDROP_PATTERN(vfe, wm));
+	writel(0, vfe->base + VFE_BUS_FRAMEDROP_PERIOD(vfe, wm));
+	writel(1, vfe->base + VFE_BUS_IRQ_SUBSAMPLE_PATTERN(vfe, wm));
+	writel(0, vfe->base + VFE_BUS_IRQ_SUBSAMPLE_PERIOD(vfe, wm));
+
+	/* We don't process IRQs for VFE in RDI mode at the moment */
+	vfe_disable_irq(vfe);
+
+	/* Enable WM */
+	writel(VFE_BUS_WRITE_CLIENT_CFG_EN,
+	       vfe->base + VFE_BUS_WRITE_CLIENT_CFG(vfe, wm));
+
+	dev_dbg(vfe->camss->dev, "RDI%d WM:%d width %d height %d stride %d\n",
+		rdi, wm, pix->width, pix->height, stride);
+}
+
+static void vfe_wm_stop(struct vfe_device *vfe, u8 rdi)
+{
+	u8 wm = RDI_WM(rdi);
+
+	writel(0, vfe->base + VFE_BUS_WRITE_CLIENT_CFG(vfe, wm));
+}
+
+static const struct camss_video_ops vfe_video_ops_680 = {
+	.queue_buffer = vfe_queue_buffer_v2,
+	.flush_buffers = vfe_flush_buffers,
+};
+
+static void vfe_subdev_init(struct device *dev, struct vfe_device *vfe)
+{
+	vfe->video_ops = vfe_video_ops_680;
+}
+
+static void vfe_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
+{
+	int port_id = line_id;
+
+	camss_reg_update(vfe->camss, vfe->id, port_id, false);
+}
+
+static inline void vfe_reg_update_clear(struct vfe_device *vfe,
+					enum vfe_line_id line_id)
+{
+	int port_id = line_id;
+
+	camss_reg_update(vfe->camss, vfe->id, port_id, true);
+}
+
+const struct vfe_hw_ops vfe_ops_680 = {
+	.global_reset = vfe_global_reset,
+	.hw_version = vfe_hw_version,
+	.isr = vfe_isr,
+	.pm_domain_off = vfe_pm_domain_off,
+	.pm_domain_on = vfe_pm_domain_on,
+	.subdev_init = vfe_subdev_init,
+	.vfe_disable = vfe_disable,
+	.vfe_enable = vfe_enable_v2,
+	.vfe_halt = vfe_halt,
+	.vfe_wm_start = vfe_wm_start,
+	.vfe_wm_stop = vfe_wm_stop,
+	.vfe_buf_done = vfe_buf_done,
+	.vfe_wm_update = vfe_wm_update,
+	.reg_update = vfe_reg_update,
+	.reg_update_clear = vfe_reg_update_clear,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-780.c b/drivers/media/platform/qcom/camss/camss-vfe-780.c
new file mode 100644
index 000000000000..b9812d70f91b
--- /dev/null
+++ b/drivers/media/platform/qcom/camss/camss-vfe-780.c
@@ -0,0 +1,159 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Qualcomm MSM Camera Subsystem - VFE (Video Front End) Module v780 (SM8550)
+ *
+ * Copyright (c) 2024 Qualcomm Technologies, Inc.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+
+#include "camss.h"
+#include "camss-vfe.h"
+
+#define BUS_REG_BASE			(vfe_is_lite(vfe) ? 0x200 : 0xC00)
+
+#define VFE_BUS_WM_CGC_OVERRIDE		(BUS_REG_BASE + 0x08)
+#define		WM_CGC_OVERRIDE_ALL		(0x7FFFFFF)
+
+#define VFE_BUS_WM_TEST_BUS_CTRL	(BUS_REG_BASE + 0xDC)
+
+#define VFE_BUS_WM_CFG(n)		(BUS_REG_BASE + 0x200 + (n) * 0x100)
+#define		WM_CFG_EN			BIT(0)
+#define		WM_VIR_FRM_EN			BIT(1)
+#define		WM_CFG_MODE			BIT(16)
+#define VFE_BUS_WM_IMAGE_ADDR(n)	(BUS_REG_BASE + 0x204 + (n) * 0x100)
+#define VFE_BUS_WM_FRAME_INCR(n)	(BUS_REG_BASE + 0x208 + (n) * 0x100)
+#define VFE_BUS_WM_IMAGE_CFG_0(n)	(BUS_REG_BASE + 0x20c + (n) * 0x100)
+#define		WM_IMAGE_CFG_0_DEFAULT_WIDTH	(0xFFFF)
+#define VFE_BUS_WM_IMAGE_CFG_2(n)	(BUS_REG_BASE + 0x214 + (n) * 0x100)
+#define		WM_IMAGE_CFG_2_DEFAULT_STRIDE	(0xFFFF)
+#define VFE_BUS_WM_PACKER_CFG(n)	(BUS_REG_BASE + 0x218 + (n) * 0x100)
+
+#define VFE_BUS_WM_IRQ_SUBSAMPLE_PERIOD(n)	(BUS_REG_BASE + 0x230 + (n) * 0x100)
+#define VFE_BUS_WM_IRQ_SUBSAMPLE_PATTERN(n)	(BUS_REG_BASE + 0x234 + (n) * 0x100)
+#define VFE_BUS_WM_FRAMEDROP_PERIOD(n)		(BUS_REG_BASE + 0x238 + (n) * 0x100)
+#define VFE_BUS_WM_FRAMEDROP_PATTERN(n)		(BUS_REG_BASE + 0x23c + (n) * 0x100)
+
+#define VFE_BUS_WM_MMU_PREFETCH_CFG(n)		(BUS_REG_BASE + 0x260 + (n) * 0x100)
+#define VFE_BUS_WM_MMU_PREFETCH_MAX_OFFSET(n)	(BUS_REG_BASE + 0x264 + (n) * 0x100)
+
+/*
+ * Bus client mapping:
+ *
+ * Full VFE:
+ * 23 = RDI0, 24 = RDI1, 25 = RDI2
+ *
+ * VFE LITE:
+ * 0 = RDI0, 1 = RDI1, 2 = RDI3, 4 = RDI4
+ */
+#define RDI_WM(n)			((vfe_is_lite(vfe) ? 0x0 : 0x17) + (n))
+
+static void vfe_wm_start(struct vfe_device *vfe, u8 wm, struct vfe_line *line)
+{
+	struct v4l2_pix_format_mplane *pix =
+		&line->video_out.active_fmt.fmt.pix_mp;
+
+	wm = RDI_WM(wm);
+
+	/* no clock gating at bus input */
+	writel(WM_CGC_OVERRIDE_ALL, vfe->base + VFE_BUS_WM_CGC_OVERRIDE);
+
+	writel(0x0, vfe->base + VFE_BUS_WM_TEST_BUS_CTRL);
+
+	writel(ALIGN(pix->plane_fmt[0].bytesperline, 16) * pix->height >> 8,
+	       vfe->base + VFE_BUS_WM_FRAME_INCR(wm));
+	writel((WM_IMAGE_CFG_0_DEFAULT_WIDTH & 0xFFFF),
+	       vfe->base + VFE_BUS_WM_IMAGE_CFG_0(wm));
+	writel(WM_IMAGE_CFG_2_DEFAULT_STRIDE,
+	       vfe->base + VFE_BUS_WM_IMAGE_CFG_2(wm));
+	writel(0, vfe->base + VFE_BUS_WM_PACKER_CFG(wm));
+
+	/* no dropped frames, one irq per frame */
+	writel(0, vfe->base + VFE_BUS_WM_FRAMEDROP_PERIOD(wm));
+	writel(1, vfe->base + VFE_BUS_WM_FRAMEDROP_PATTERN(wm));
+	writel(0, vfe->base + VFE_BUS_WM_IRQ_SUBSAMPLE_PERIOD(wm));
+	writel(1, vfe->base + VFE_BUS_WM_IRQ_SUBSAMPLE_PATTERN(wm));
+
+	writel(1, vfe->base + VFE_BUS_WM_MMU_PREFETCH_CFG(wm));
+	writel(0xFFFFFFFF, vfe->base + VFE_BUS_WM_MMU_PREFETCH_MAX_OFFSET(wm));
+
+	writel(WM_CFG_EN | WM_CFG_MODE, vfe->base + VFE_BUS_WM_CFG(wm));
+}
+
+static void vfe_wm_stop(struct vfe_device *vfe, u8 wm)
+{
+	wm = RDI_WM(wm);
+	writel(0, vfe->base + VFE_BUS_WM_CFG(wm));
+}
+
+static void vfe_wm_update(struct vfe_device *vfe, u8 wm, u32 addr,
+			  struct vfe_line *line)
+{
+	wm = RDI_WM(wm);
+	writel((addr >> 8) & 0xFFFFFFFF, vfe->base + VFE_BUS_WM_IMAGE_ADDR(wm));
+
+	dev_dbg(vfe->camss->dev, "wm:%d, image buf addr:0x%x\n",
+		wm, addr);
+}
+
+static void vfe_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
+{
+	int port_id = line_id;
+
+	camss_reg_update(vfe->camss, vfe->id, port_id, false);
+}
+
+static inline void vfe_reg_update_clear(struct vfe_device *vfe,
+					enum vfe_line_id line_id)
+{
+	int port_id = line_id;
+
+	camss_reg_update(vfe->camss, vfe->id, port_id, true);
+}
+
+static const struct camss_video_ops vfe_video_ops_780 = {
+	.queue_buffer = vfe_queue_buffer_v2,
+	.flush_buffers = vfe_flush_buffers,
+};
+
+static void vfe_subdev_init(struct device *dev, struct vfe_device *vfe)
+{
+	vfe->video_ops = vfe_video_ops_780;
+}
+
+static void vfe_global_reset(struct vfe_device *vfe)
+{
+	vfe_isr_reset_ack(vfe);
+}
+
+static irqreturn_t vfe_isr(int irq, void *dev)
+{
+	/* nop */
+	return IRQ_HANDLED;
+}
+
+static int vfe_halt(struct vfe_device *vfe)
+{
+	/* rely on vfe_disable_output() to stop the VFE */
+	return 0;
+}
+
+const struct vfe_hw_ops vfe_ops_780 = {
+	.global_reset = vfe_global_reset,
+	.hw_version = vfe_hw_version,
+	.isr = vfe_isr,
+	.pm_domain_off = vfe_pm_domain_off,
+	.pm_domain_on = vfe_pm_domain_on,
+	.reg_update = vfe_reg_update,
+	.reg_update_clear = vfe_reg_update_clear,
+	.subdev_init = vfe_subdev_init,
+	.vfe_disable = vfe_disable,
+	.vfe_enable = vfe_enable_v2,
+	.vfe_halt = vfe_halt,
+	.vfe_wm_start = vfe_wm_start,
+	.vfe_wm_stop = vfe_wm_stop,
+	.vfe_buf_done = vfe_buf_done,
+	.vfe_wm_update = vfe_wm_update,
+};
diff --git a/drivers/media/platform/qcom/camss/camss-vfe-gen1.c b/drivers/media/platform/qcom/camss/camss-vfe-gen1.c
index 239d3d4ac666..d84a375e3318 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe-gen1.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe-gen1.c
@@ -37,7 +37,7 @@ static int vfe_disable_output(struct vfe_line *line)
 {
 	struct vfe_device *vfe = to_vfe(line);
 	struct vfe_output *output = &line->output;
-	const struct vfe_hw_ops *ops = vfe->ops;
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
 	unsigned long flags;
 	unsigned long time;
 	unsigned int i;
@@ -162,15 +162,15 @@ static void vfe_output_frame_drop(struct vfe_device *vfe,
 		vfe->ops_gen1->wm_set_framedrop_pattern(vfe, output->wm_idx[i], drop_pattern);
 	}
 
-	vfe->ops->reg_update(vfe, container_of(output, struct vfe_line, output)->id);
+	vfe->res->hw_ops->reg_update(vfe, container_of(output, struct vfe_line, output)->id);
 }
 
 static int vfe_enable_output(struct vfe_line *line)
 {
 	struct vfe_device *vfe = to_vfe(line);
 	struct vfe_output *output = &line->output;
-	const struct vfe_hw_ops *ops = vfe->ops;
-	struct media_entity *sensor;
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
+	struct media_pad *sensor_pad;
 	unsigned long flags;
 	unsigned int frame_skip = 0;
 	unsigned int i;
@@ -180,9 +180,10 @@ static int vfe_enable_output(struct vfe_line *line)
 	if (!ub_size)
 		return -EINVAL;
 
-	sensor = camss_find_sensor(&line->subdev.entity);
-	if (sensor) {
-		struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(sensor);
+	sensor_pad = camss_find_sensor_pad(&line->subdev.entity);
+	if (sensor_pad) {
+		struct v4l2_subdev *subdev =
+			media_entity_to_v4l2_subdev(sensor_pad->entity);
 
 		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
 		/* Max frame skip is 29 frames */
@@ -545,7 +546,7 @@ static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 	unsigned long flags;
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
-	vfe->ops->reg_update_clear(vfe, line_id);
+	vfe->res->hw_ops->reg_update_clear(vfe, line_id);
 
 	output = &line->output;
 
diff --git a/drivers/media/platform/qcom/camss/camss-vfe.c b/drivers/media/platform/qcom/camss/camss-vfe.c
index 2062be668f49..4bca6c3abaff 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe.c
+++ b/drivers/media/platform/qcom/camss/camss-vfe.c
@@ -32,139 +32,256 @@
 
 #define SCALER_RATIO_MAX 16
 
-struct vfe_format {
-	u32 code;
-	u8 bpp;
+#define VFE_HW_VERSION		0x0
+#define		HW_VERSION_STEPPING	0
+#define		HW_VERSION_REVISION	16
+#define		HW_VERSION_GENERATION	28
+
+static const struct camss_format_info formats_rdi_8x16[] = {
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_UYVY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_VYUY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_YUYV, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_YVYU, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8, V4L2_PIX_FMT_SBGGR8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, V4L2_PIX_FMT_SGBRG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, V4L2_PIX_FMT_SGRBG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, V4L2_PIX_FMT_SRGGB8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, V4L2_PIX_FMT_SBGGR10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, V4L2_PIX_FMT_SGBRG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, V4L2_PIX_FMT_SGRBG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, V4L2_PIX_FMT_SRGGB10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, V4L2_PIX_FMT_SBGGR12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, V4L2_PIX_FMT_SGBRG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, V4L2_PIX_FMT_SGRBG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, V4L2_PIX_FMT_SRGGB12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_Y10_1X10, 10, V4L2_PIX_FMT_Y10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
 };
 
-static const struct vfe_format formats_rdi_8x16[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, 8 },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
-	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
+static const struct camss_format_info formats_rdi_8x96[] = {
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_UYVY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_VYUY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_YUYV, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_YVYU, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8, V4L2_PIX_FMT_SBGGR8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, V4L2_PIX_FMT_SGBRG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, V4L2_PIX_FMT_SGRBG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, V4L2_PIX_FMT_SRGGB8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, V4L2_PIX_FMT_SBGGR10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, V4L2_PIX_FMT_SGBRG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, V4L2_PIX_FMT_SGRBG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, V4L2_PIX_FMT_SRGGB10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16, V4L2_PIX_FMT_SBGGR10, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, V4L2_PIX_FMT_SBGGR12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, V4L2_PIX_FMT_SGBRG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, V4L2_PIX_FMT_SGRBG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, V4L2_PIX_FMT_SRGGB12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14, V4L2_PIX_FMT_SBGGR14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14, V4L2_PIX_FMT_SGBRG14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14, V4L2_PIX_FMT_SGRBG14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14, V4L2_PIX_FMT_SRGGB14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_Y10_1X10, 10, V4L2_PIX_FMT_Y10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16, V4L2_PIX_FMT_Y10, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
 };
 
-static const struct vfe_format formats_pix_8x16[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, 8 },
+static const struct camss_format_info formats_rdi_845[] = {
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_UYVY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_VYUY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_YUYV, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_YVYU, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8, V4L2_PIX_FMT_SBGGR8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8, V4L2_PIX_FMT_SGBRG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8, V4L2_PIX_FMT_SGRBG8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8, V4L2_PIX_FMT_SRGGB8, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10, V4L2_PIX_FMT_SBGGR10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10, V4L2_PIX_FMT_SGBRG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10, V4L2_PIX_FMT_SGRBG10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10, V4L2_PIX_FMT_SRGGB10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16, V4L2_PIX_FMT_SBGGR10, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12, V4L2_PIX_FMT_SBGGR12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12, V4L2_PIX_FMT_SGBRG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12, V4L2_PIX_FMT_SGRBG12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12, V4L2_PIX_FMT_SRGGB12P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 12) },
+	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14, V4L2_PIX_FMT_SBGGR14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14, V4L2_PIX_FMT_SGBRG14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14, V4L2_PIX_FMT_SGRBG14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14, V4L2_PIX_FMT_SRGGB14P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 14) },
+	{ MEDIA_BUS_FMT_Y8_1X8, 8, V4L2_PIX_FMT_GREY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 8) },
+	{ MEDIA_BUS_FMT_Y10_1X10, 10, V4L2_PIX_FMT_Y10P, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 10) },
+	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16, V4L2_PIX_FMT_Y10, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
 };
 
-static const struct vfe_format formats_rdi_8x96[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, 8 },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
-	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
-	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
+static const struct camss_format_info formats_pix_8x16[] = {
+	{ MEDIA_BUS_FMT_YUYV8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
 };
 
-static const struct vfe_format formats_pix_8x96[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, 8 },
+static const struct camss_format_info formats_pix_8x96[] = {
+	{ MEDIA_BUS_FMT_YUYV8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1_5X8, 8, V4L2_PIX_FMT_NV12, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1_5X8, 8, V4L2_PIX_FMT_NV21, 1,
+	  PER_PLANE_DATA(0, 1, 1, 2, 3, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_NV16, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_NV61, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 2, 8) },
+	{ MEDIA_BUS_FMT_UYVY8_1X16, 8, V4L2_PIX_FMT_UYVY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_VYUY8_1X16, 8, V4L2_PIX_FMT_VYUY, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YUYV8_1X16, 8, V4L2_PIX_FMT_YUYV, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
+	{ MEDIA_BUS_FMT_YVYU8_1X16, 8, V4L2_PIX_FMT_YVYU, 1,
+	  PER_PLANE_DATA(0, 1, 1, 1, 1, 16) },
 };
 
-static const struct vfe_format formats_rdi_845[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, 8 },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, 8 },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
-	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, 16 },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
-	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
-	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
-	{ MEDIA_BUS_FMT_Y8_1X8, 8 },
-	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
-	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, 16 },
+const struct camss_formats vfe_formats_rdi_8x16 = {
+	.nformats = ARRAY_SIZE(formats_rdi_8x16),
+	.formats = formats_rdi_8x16
 };
 
-/*
- * vfe_get_bpp - map media bus format to bits per pixel
- * @formats: supported media bus formats array
- * @nformats: size of @formats array
- * @code: media bus format code
- *
- * Return number of bits per pixel
- */
-static u8 vfe_get_bpp(const struct vfe_format *formats,
-		      unsigned int nformats, u32 code)
-{
-	unsigned int i;
-
-	for (i = 0; i < nformats; i++)
-		if (code == formats[i].code)
-			return formats[i].bpp;
-
-	WARN(1, "Unknown format\n");
-
-	return formats[0].bpp;
-}
+const struct camss_formats vfe_formats_pix_8x16 = {
+	.nformats = ARRAY_SIZE(formats_pix_8x16),
+	.formats = formats_pix_8x16
+};
 
-static u32 vfe_find_code(u32 *code, unsigned int n_code,
-			 unsigned int index, u32 req_code)
-{
-	int i;
+const struct camss_formats vfe_formats_rdi_8x96 = {
+	.nformats = ARRAY_SIZE(formats_rdi_8x96),
+	.formats = formats_rdi_8x96
+};
 
-	if (!req_code && (index >= n_code))
-		return 0;
+const struct camss_formats vfe_formats_pix_8x96 = {
+	.nformats = ARRAY_SIZE(formats_pix_8x96),
+	.formats = formats_pix_8x96
+};
 
-	for (i = 0; i < n_code; i++)
-		if (req_code) {
-			if (req_code == code[i])
-				return req_code;
-		} else {
-			if (i == index)
-				return code[i];
-		}
+const struct camss_formats vfe_formats_rdi_845 = {
+	.nformats = ARRAY_SIZE(formats_rdi_845),
+	.formats = formats_rdi_845
+};
 
-	return code[0];
-}
+/* TODO: Replace with pix formats */
+const struct camss_formats vfe_formats_pix_845 = {
+	.nformats = ARRAY_SIZE(formats_rdi_845),
+	.formats = formats_rdi_845
+};
 
 static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 			    unsigned int index, u32 src_req_code)
@@ -173,6 +290,7 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 
 	switch (vfe->camss->res->version) {
 	case CAMSS_8x16:
+	case CAMSS_8x53:
 		switch (sink_code) {
 		case MEDIA_BUS_FMT_YUYV8_1X16:
 		{
@@ -181,8 +299,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_YUYV8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_YVYU8_1X16:
 		{
@@ -191,8 +309,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_YVYU8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_UYVY8_1X16:
 		{
@@ -201,8 +319,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_UYVY8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_VYUY8_1X16:
 		{
@@ -211,8 +329,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_VYUY8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		default:
 			if (index > 0)
@@ -221,10 +339,14 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 			return sink_code;
 		}
 		break;
-	case CAMSS_8x96:
 	case CAMSS_660:
-	case CAMSS_845:
+	case CAMSS_7280:
+	case CAMSS_8x96:
 	case CAMSS_8250:
+	case CAMSS_8280XP:
+	case CAMSS_845:
+	case CAMSS_8550:
+	case CAMSS_X1E80100:
 		switch (sink_code) {
 		case MEDIA_BUS_FMT_YUYV8_1X16:
 		{
@@ -236,8 +358,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_YUYV8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_YVYU8_1X16:
 		{
@@ -249,8 +371,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_YVYU8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_UYVY8_1X16:
 		{
@@ -262,8 +384,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_UYVY8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		case MEDIA_BUS_FMT_VYUY8_1X16:
 		{
@@ -275,8 +397,8 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 				MEDIA_BUS_FMT_VYUY8_1_5X8,
 			};
 
-			return vfe_find_code(src_code, ARRAY_SIZE(src_code),
-					     index, src_req_code);
+			return camss_format_find_code(src_code, ARRAY_SIZE(src_code),
+						      index, src_req_code);
 		}
 		default:
 			if (index > 0)
@@ -285,17 +407,285 @@ static u32 vfe_src_pad_code(struct vfe_line *line, u32 sink_code,
 			return sink_code;
 		}
 		break;
+	default:
+		WARN(1, "Unsupported HW version: %x\n",
+		     vfe->camss->res->version);
+		break;
+	}
+	return 0;
+}
+
+/*
+ * vfe_hw_version - Process write master done interrupt
+ * @vfe: VFE Device
+ *
+ * Return vfe hw version
+ */
+u32 vfe_hw_version(struct vfe_device *vfe)
+{
+	u32 hw_version = readl_relaxed(vfe->base + VFE_HW_VERSION);
+
+	u32 gen = (hw_version >> HW_VERSION_GENERATION) & 0xF;
+	u32 rev = (hw_version >> HW_VERSION_REVISION) & 0xFFF;
+	u32 step = (hw_version >> HW_VERSION_STEPPING) & 0xFFFF;
+
+	dev_dbg(vfe->camss->dev, "VFE:%d HW Version = %u.%u.%u\n",
+		vfe->id, gen, rev, step);
+
+	return hw_version;
+}
+
+/*
+ * vfe_buf_done - Process write master done interrupt
+ * @vfe: VFE Device
+ * @wm: Write master id
+ */
+void vfe_buf_done(struct vfe_device *vfe, int wm)
+{
+	struct vfe_line *line = &vfe->line[vfe->wm_output_map[wm]];
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
+	struct camss_buffer *ready_buf;
+	struct vfe_output *output;
+	unsigned long flags;
+	u32 index;
+	u64 ts = ktime_get_ns();
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
+		dev_err_ratelimited(vfe->camss->dev,
+				    "Received wm done for unmapped index\n");
+		goto out_unlock;
+	}
+	output = &vfe->line[vfe->wm_output_map[wm]].output;
+
+	ready_buf = output->buf[0];
+	if (!ready_buf) {
+		dev_err_ratelimited(vfe->camss->dev,
+				    "Missing ready buf %d!\n", output->state);
+		goto out_unlock;
+	}
+
+	ready_buf->vb.vb2_buf.timestamp = ts;
+	ready_buf->vb.sequence = output->sequence++;
+
+	index = 0;
+	output->buf[0] = output->buf[1];
+	if (output->buf[0])
+		index = 1;
+
+	output->buf[index] = vfe_buf_get_pending(output);
+
+	if (output->buf[index]) {
+		ops->vfe_wm_update(vfe, output->wm_idx[0],
+				   output->buf[index]->addr[0],
+				   line);
+		ops->reg_update(vfe, line->id);
+	} else {
+		output->gen2.active_num--;
+	}
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
+
+	return;
+
+out_unlock:
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+}
+
+int vfe_enable_output_v2(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output = &line->output;
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
+	struct media_pad *sensor_pad;
+	unsigned long flags;
+	unsigned int frame_skip = 0;
+	unsigned int i;
+
+	sensor_pad = camss_find_sensor_pad(&line->subdev.entity);
+	if (sensor_pad) {
+		struct v4l2_subdev *subdev =
+			media_entity_to_v4l2_subdev(sensor_pad->entity);
+
+		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
+		/* Max frame skip is 29 frames */
+		if (frame_skip > VFE_FRAME_DROP_VAL - 1)
+			frame_skip = VFE_FRAME_DROP_VAL - 1;
+	}
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	ops->reg_update_clear(vfe, line->id);
+
+	if (output->state > VFE_OUTPUT_RESERVED) {
+		dev_err(vfe->camss->dev,
+			"Output is not in reserved state %d\n",
+			output->state);
+		spin_unlock_irqrestore(&vfe->output_lock, flags);
+		return -EINVAL;
+	}
+
+	WARN_ON(output->gen2.active_num);
+
+	output->state = VFE_OUTPUT_ON;
+
+	output->sequence = 0;
+	output->wait_reg_update = 0;
+	reinit_completion(&output->reg_update);
+
+	ops->vfe_wm_start(vfe, output->wm_idx[0], line);
+
+	for (i = 0; i < 2; i++) {
+		output->buf[i] = vfe_buf_get_pending(output);
+		if (!output->buf[i])
+			break;
+		output->gen2.active_num++;
+		ops->vfe_wm_update(vfe, output->wm_idx[0],
+				   output->buf[i]->addr[0], line);
+		ops->reg_update(vfe, line->id);
+	}
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+}
+
+/*
+ * vfe_queue_buffer_v2 - Add empty buffer
+ * @vid: Video device structure
+ * @buf: Buffer to be enqueued
+ *
+ * Add an empty buffer - depending on the current number of buffers it will be
+ * put in pending buffer queue or directly given to the hardware to be filled.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_queue_buffer_v2(struct camss_video *vid,
+			struct camss_buffer *buf)
+{
+	struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
+	struct vfe_device *vfe = to_vfe(line);
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
+	struct vfe_output *output;
+	unsigned long flags;
+
+	output = &line->output;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	if (output->state == VFE_OUTPUT_ON &&
+	    output->gen2.active_num < 2) {
+		output->buf[output->gen2.active_num++] = buf;
+		ops->vfe_wm_update(vfe, output->wm_idx[0],
+				   buf->addr[0], line);
+		ops->reg_update(vfe, line->id);
+	} else {
+		vfe_buf_add_pending(output, buf);
 	}
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
 	return 0;
 }
 
+/*
+ * vfe_enable_v2 - Enable streaming on VFE line
+ * @line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_enable_v2(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	const struct vfe_hw_ops *ops = vfe->res->hw_ops;
+	int ret;
+
+	mutex_lock(&vfe->stream_lock);
+
+	if (vfe->res->hw_ops->enable_irq)
+		ops->enable_irq(vfe);
+
+	vfe->stream_count++;
+
+	mutex_unlock(&vfe->stream_lock);
+
+	ret = vfe_get_output_v2(line);
+	if (ret < 0)
+		goto error_get_output;
+
+	ret = vfe_enable_output_v2(line);
+	if (ret < 0)
+		goto error_enable_output;
+
+	vfe->was_streaming = 1;
+
+	return 0;
+
+error_enable_output:
+	vfe_put_output(line);
+
+error_get_output:
+	mutex_lock(&vfe->stream_lock);
+
+	vfe->stream_count--;
+
+	mutex_unlock(&vfe->stream_lock);
+
+	return ret;
+}
+
+/*
+ * vfe_get_output_v2 - Get vfe output port for corresponding VFE line
+ * @line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_get_output_v2(struct vfe_line *line)
+{
+	struct vfe_device *vfe = to_vfe(line);
+	struct vfe_output *output;
+	unsigned long flags;
+
+	spin_lock_irqsave(&vfe->output_lock, flags);
+
+	output = &line->output;
+	if (output->state > VFE_OUTPUT_RESERVED) {
+		dev_err(vfe->camss->dev, "Output is running\n");
+		goto error;
+	}
+
+	output->wm_num = 1;
+
+	/* Correspondence between VFE line number and WM number.
+	 * line 0 -> RDI 0, line 1 -> RDI1, line 2 -> RDI2, line 3 -> PIX/RDI3
+	 * Note this 1:1 mapping will not work for PIX streams.
+	 */
+	output->wm_idx[0] = line->id;
+	vfe->wm_output_map[line->id] = line->id;
+
+	output->drop_update_idx = 0;
+
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+
+	return 0;
+
+error:
+	spin_unlock_irqrestore(&vfe->output_lock, flags);
+	output->state = VFE_OUTPUT_OFF;
+
+	return -EINVAL;
+}
+
 int vfe_reset(struct vfe_device *vfe)
 {
 	unsigned long time;
 
 	reinit_completion(&vfe->reset_complete);
 
-	vfe->ops->global_reset(vfe);
+	vfe->res->hw_ops->global_reset(vfe);
 
 	time = wait_for_completion_timeout(&vfe->reset_complete,
 		msecs_to_jiffies(VFE_RESET_TIMEOUT_MS));
@@ -311,7 +701,7 @@ static void vfe_init_outputs(struct vfe_device *vfe)
 {
 	int i;
 
-	for (i = 0; i < vfe->line_num; i++) {
+	for (i = 0; i < vfe->res->line_num; i++) {
 		struct vfe_output *output = &vfe->line[i].output;
 
 		output->state = VFE_OUTPUT_OFF;
@@ -420,7 +810,7 @@ static int vfe_disable_output(struct vfe_line *line)
 
 	spin_lock_irqsave(&vfe->output_lock, flags);
 	for (i = 0; i < output->wm_num; i++)
-		vfe->ops->vfe_wm_stop(vfe, output->wm_idx[i]);
+		vfe->res->hw_ops->vfe_wm_stop(vfe, output->wm_idx[i]);
 	output->gen2.active_num = 0;
 	spin_unlock_irqrestore(&vfe->output_lock, flags);
 
@@ -536,7 +926,7 @@ static int vfe_set_clock_rates(struct vfe_device *vfe)
 	int i, j;
 	int ret;
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++) {
 		ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
 					    &pixel_clock[i]);
 		if (ret)
@@ -550,7 +940,7 @@ static int vfe_set_clock_rates(struct vfe_device *vfe)
 			u64 min_rate = 0;
 			long rate;
 
-			for (j = VFE_LINE_RDI0; j < vfe->line_num; j++) {
+			for (j = VFE_LINE_RDI0; j < vfe->res->line_num; j++) {
 				u32 tmp;
 				u8 bpp;
 
@@ -559,9 +949,9 @@ static int vfe_set_clock_rates(struct vfe_device *vfe)
 				} else {
 					struct vfe_line *l = &vfe->line[j];
 
-					bpp = vfe_get_bpp(l->formats,
-						l->nformats,
-						l->fmt[MSM_VFE_PAD_SINK].code);
+					bpp = camss_format_get_bpp(l->formats,
+								   l->nformats,
+								   l->fmt[MSM_VFE_PAD_SINK].code);
 					tmp = pixel_clock[j] * bpp / 64;
 				}
 
@@ -617,7 +1007,7 @@ static int vfe_check_clock_rates(struct vfe_device *vfe)
 	int i, j;
 	int ret;
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++) {
 		ret = camss_get_pixel_clock(&vfe->line[i].subdev.entity,
 					    &pixel_clock[i]);
 		if (ret)
@@ -631,7 +1021,7 @@ static int vfe_check_clock_rates(struct vfe_device *vfe)
 			u64 min_rate = 0;
 			unsigned long rate;
 
-			for (j = VFE_LINE_RDI0; j < vfe->line_num; j++) {
+			for (j = VFE_LINE_RDI0; j < vfe->res->line_num; j++) {
 				u32 tmp;
 				u8 bpp;
 
@@ -640,9 +1030,9 @@ static int vfe_check_clock_rates(struct vfe_device *vfe)
 				} else {
 					struct vfe_line *l = &vfe->line[j];
 
-					bpp = vfe_get_bpp(l->formats,
-						l->nformats,
-						l->fmt[MSM_VFE_PAD_SINK].code);
+					bpp = camss_format_get_bpp(l->formats,
+								   l->nformats,
+								   l->fmt[MSM_VFE_PAD_SINK].code);
 					tmp = pixel_clock[j] * bpp / 64;
 				}
 
@@ -674,7 +1064,7 @@ int vfe_get(struct vfe_device *vfe)
 	mutex_lock(&vfe->power_lock);
 
 	if (vfe->power_count == 0) {
-		ret = vfe->ops->pm_domain_on(vfe);
+		ret = vfe->res->hw_ops->pm_domain_on(vfe);
 		if (ret < 0)
 			goto error_pm_domain;
 
@@ -699,7 +1089,7 @@ int vfe_get(struct vfe_device *vfe)
 
 		vfe_init_outputs(vfe);
 
-		vfe->ops->hw_version(vfe);
+		vfe->res->hw_ops->hw_version(vfe);
 	} else {
 		ret = vfe_check_clock_rates(vfe);
 		if (ret < 0)
@@ -717,7 +1107,7 @@ error_reset:
 error_pm_runtime_get:
 	pm_runtime_put_sync(vfe->camss->dev);
 error_domain_off:
-	vfe->ops->pm_domain_off(vfe);
+	vfe->res->hw_ops->pm_domain_off(vfe);
 
 error_pm_domain:
 	mutex_unlock(&vfe->power_lock);
@@ -739,11 +1129,11 @@ void vfe_put(struct vfe_device *vfe)
 	} else if (vfe->power_count == 1) {
 		if (vfe->was_streaming) {
 			vfe->was_streaming = 0;
-			vfe->ops->vfe_halt(vfe);
+			vfe->res->hw_ops->vfe_halt(vfe);
 		}
 		camss_disable_clocks(vfe->nclocks, vfe->clock);
 		pm_runtime_put_sync(vfe->camss->dev);
-		vfe->ops->pm_domain_off(vfe);
+		vfe->res->hw_ops->pm_domain_off(vfe);
 	}
 
 	vfe->power_count--;
@@ -833,12 +1223,12 @@ static int vfe_set_stream(struct v4l2_subdev *sd, int enable)
 
 	if (enable) {
 		line->output.state = VFE_OUTPUT_RESERVED;
-		ret = vfe->ops->vfe_enable(line);
+		ret = vfe->res->hw_ops->vfe_enable(line);
 		if (ret < 0)
 			dev_err(vfe->camss->dev,
 				"Failed to enable vfe outputs\n");
 	} else {
-		ret = vfe->ops->vfe_disable(line);
+		ret = vfe->res->hw_ops->vfe_disable(line);
 		if (ret < 0)
 			dev_err(vfe->camss->dev,
 				"Failed to disable vfe outputs\n");
@@ -1375,23 +1765,24 @@ int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
 	int i, j;
 	int ret;
 
-	vfe->ops = res->ops;
-
-	if (!res->line_num)
+	if (!res->vfe.line_num)
 		return -EINVAL;
 
+	vfe->res = &res->vfe;
+	vfe->res->hw_ops->subdev_init(dev, vfe);
+
 	/* Power domain */
 
-	if (res->pd_name) {
+	if (res->vfe.pd_name) {
 		vfe->genpd = dev_pm_domain_attach_by_name(camss->dev,
-							  res->pd_name);
+							  res->vfe.pd_name);
 		if (IS_ERR(vfe->genpd)) {
 			ret = PTR_ERR(vfe->genpd);
 			return ret;
 		}
 	}
 
-	if (!vfe->genpd && res->has_pd) {
+	if (!vfe->genpd && res->vfe.has_pd) {
 		/*
 		 * Legacy magic index.
 		 * Requires
@@ -1408,9 +1799,6 @@ int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
 			return PTR_ERR(vfe->genpd);
 	}
 
-	vfe->line_num = res->line_num;
-	vfe->ops->subdev_init(dev, vfe);
-
 	/* Memory */
 
 	vfe->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
@@ -1428,7 +1816,7 @@ int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
 	vfe->irq = ret;
 	snprintf(vfe->irq_name, sizeof(vfe->irq_name), "%s_%s%d",
 		 dev_name(dev), MSM_VFE_NAME, id);
-	ret = devm_request_irq(dev, vfe->irq, vfe->ops->isr,
+	ret = devm_request_irq(dev, vfe->irq, vfe->res->hw_ops->isr,
 			       IRQF_TRIGGER_RISING, vfe->irq_name, vfe);
 	if (ret < 0) {
 		dev_err(dev, "request_irq failed: %d\n", ret);
@@ -1487,7 +1875,7 @@ int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
 	vfe->id = id;
 	vfe->reg_update = 0;
 
-	for (i = VFE_LINE_RDI0; i < vfe->line_num; i++) {
+	for (i = VFE_LINE_RDI0; i < vfe->res->line_num; i++) {
 		struct vfe_line *l = &vfe->line[i];
 
 		l->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
@@ -1496,31 +1884,12 @@ int msm_vfe_subdev_init(struct camss *camss, struct vfe_device *vfe,
 		init_completion(&l->output.sof);
 		init_completion(&l->output.reg_update);
 
-		switch (camss->res->version) {
-		case CAMSS_8x16:
-			if (i == VFE_LINE_PIX) {
-				l->formats = formats_pix_8x16;
-				l->nformats = ARRAY_SIZE(formats_pix_8x16);
-			} else {
-				l->formats = formats_rdi_8x16;
-				l->nformats = ARRAY_SIZE(formats_rdi_8x16);
-			}
-			break;
-		case CAMSS_8x96:
-		case CAMSS_660:
-			if (i == VFE_LINE_PIX) {
-				l->formats = formats_pix_8x96;
-				l->nformats = ARRAY_SIZE(formats_pix_8x96);
-			} else {
-				l->formats = formats_rdi_8x96;
-				l->nformats = ARRAY_SIZE(formats_rdi_8x96);
-			}
-			break;
-		case CAMSS_845:
-		case CAMSS_8250:
-			l->formats = formats_rdi_845;
-			l->nformats = ARRAY_SIZE(formats_rdi_845);
-			break;
+		if (i == VFE_LINE_PIX) {
+			l->nformats = res->vfe.formats_pix->nformats;
+			l->formats = res->vfe.formats_pix->formats;
+		} else {
+			l->nformats = res->vfe.formats_rdi->nformats;
+			l->formats = res->vfe.formats_rdi->formats;
 		}
 	}
 
@@ -1595,6 +1964,26 @@ static const struct media_entity_operations vfe_media_ops = {
 	.link_validate = v4l2_subdev_link_validate,
 };
 
+static int vfe_bpl_align(struct vfe_device *vfe)
+{
+	int ret = 8;
+
+	switch (vfe->camss->res->version) {
+	case CAMSS_7280:
+	case CAMSS_8250:
+	case CAMSS_8280XP:
+	case CAMSS_845:
+	case CAMSS_8550:
+	case CAMSS_X1E80100:
+		ret = 16;
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
 /*
  * msm_vfe_register_entities - Register subdev node for VFE module
  * @vfe: VFE device
@@ -1617,7 +2006,7 @@ int msm_vfe_register_entities(struct vfe_device *vfe,
 	int ret;
 	int i;
 
-	for (i = 0; i < vfe->line_num; i++) {
+	for (i = 0; i < vfe->res->line_num; i++) {
 		char name[32];
 
 		sd = &vfe->line[i].subdev;
@@ -1661,20 +2050,19 @@ int msm_vfe_register_entities(struct vfe_device *vfe,
 		}
 
 		video_out->ops = &vfe->video_ops;
-		if (vfe->camss->res->version == CAMSS_845 ||
-		    vfe->camss->res->version == CAMSS_8250)
-			video_out->bpl_alignment = 16;
-		else
-			video_out->bpl_alignment = 8;
+		video_out->bpl_alignment = vfe_bpl_align(vfe);
 		video_out->line_based = 0;
 		if (i == VFE_LINE_PIX) {
 			video_out->bpl_alignment = 16;
 			video_out->line_based = 1;
 		}
+
+		video_out->nformats = vfe->line[i].nformats;
+		video_out->formats = vfe->line[i].formats;
+
 		snprintf(name, ARRAY_SIZE(name), "%s%d_%s%d",
 			 MSM_VFE_NAME, vfe->id, "video", i);
-		ret = msm_video_register(video_out, v4l2_dev, name,
-					 i == VFE_LINE_PIX ? 1 : 0);
+		ret = msm_video_register(video_out, v4l2_dev, name);
 		if (ret < 0) {
 			dev_err(dev, "Failed to register video node: %d\n",
 				ret);
@@ -1728,7 +2116,7 @@ void msm_vfe_unregister_entities(struct vfe_device *vfe)
 	mutex_destroy(&vfe->power_lock);
 	mutex_destroy(&vfe->stream_lock);
 
-	for (i = 0; i < vfe->line_num; i++) {
+	for (i = 0; i < vfe->res->line_num; i++) {
 		struct v4l2_subdev *sd = &vfe->line[i].subdev;
 		struct camss_video *video_out = &vfe->line[i].video_out;
 
@@ -1740,5 +2128,5 @@ void msm_vfe_unregister_entities(struct vfe_device *vfe)
 
 bool vfe_is_lite(struct vfe_device *vfe)
 {
-	return vfe->camss->res->vfe_res[vfe->id].is_lite;
+	return vfe->camss->res->vfe_res[vfe->id].vfe.is_lite;
 }
diff --git a/drivers/media/platform/qcom/camss/camss-vfe.h b/drivers/media/platform/qcom/camss/camss-vfe.h
index 0572c9b08e11..a23f666be753 100644
--- a/drivers/media/platform/qcom/camss/camss-vfe.h
+++ b/drivers/media/platform/qcom/camss/camss-vfe.h
@@ -92,14 +92,14 @@ struct vfe_line {
 	struct v4l2_rect crop;
 	struct camss_video video_out;
 	struct vfe_output output;
-	const struct vfe_format *formats;
+	const struct camss_format_info *formats;
 	unsigned int nformats;
 };
 
 struct vfe_device;
 
 struct vfe_hw_ops {
-	void (*enable_irq_common)(struct vfe_device *vfe);
+	void (*enable_irq)(struct vfe_device *vfe);
 	void (*global_reset)(struct vfe_device *vfe);
 	u32 (*hw_version)(struct vfe_device *vfe);
 	irqreturn_t (*isr)(int irq, void *dev);
@@ -114,7 +114,12 @@ struct vfe_hw_ops {
 	int (*vfe_enable)(struct vfe_line *line);
 	int (*vfe_halt)(struct vfe_device *vfe);
 	void (*violation_read)(struct vfe_device *vfe);
+	void (*vfe_wm_start)(struct vfe_device *vfe, u8 wm,
+			     struct vfe_line *line);
 	void (*vfe_wm_stop)(struct vfe_device *vfe, u8 wm);
+	void (*vfe_buf_done)(struct vfe_device *vfe, int port_id);
+	void (*vfe_wm_update)(struct vfe_device *vfe, u8 wm, u32 addr,
+			      struct vfe_line *line);
 };
 
 struct vfe_isr_ops {
@@ -126,6 +131,16 @@ struct vfe_isr_ops {
 	void (*wm_done)(struct vfe_device *vfe, u8 wm);
 };
 
+struct vfe_subdev_resources {
+	bool is_lite;
+	u8 line_num;
+	bool has_pd;
+	char *pd_name;
+	const struct vfe_hw_ops *hw_ops;
+	const struct camss_formats *formats_rdi;
+	const struct camss_formats *formats_pix;
+};
+
 struct vfe_device {
 	struct camss *camss;
 	u8 id;
@@ -143,10 +158,9 @@ struct vfe_device {
 	spinlock_t output_lock;
 	enum vfe_line_id wm_output_map[MSM_VFE_IMAGE_MASTERS_NUM];
 	struct vfe_line line[VFE_LINE_NUM_MAX];
-	u8 line_num;
 	u32 reg_update;
 	u8 was_streaming;
-	const struct vfe_hw_ops *ops;
+	const struct vfe_subdev_resources *res;
 	const struct vfe_hw_ops_gen1 *ops_gen1;
 	struct vfe_isr_ops isr_ops;
 	struct camss_video_ops video_ops;
@@ -217,11 +231,20 @@ void vfe_pm_domain_off(struct vfe_device *vfe);
  */
 int vfe_pm_domain_on(struct vfe_device *vfe);
 
+extern const struct camss_formats vfe_formats_rdi_8x16;
+extern const struct camss_formats vfe_formats_pix_8x16;
+extern const struct camss_formats vfe_formats_rdi_8x96;
+extern const struct camss_formats vfe_formats_pix_8x96;
+extern const struct camss_formats vfe_formats_rdi_845;
+extern const struct camss_formats vfe_formats_pix_845;
+
 extern const struct vfe_hw_ops vfe_ops_4_1;
 extern const struct vfe_hw_ops vfe_ops_4_7;
 extern const struct vfe_hw_ops vfe_ops_4_8;
 extern const struct vfe_hw_ops vfe_ops_170;
 extern const struct vfe_hw_ops vfe_ops_480;
+extern const struct vfe_hw_ops vfe_ops_680;
+extern const struct vfe_hw_ops vfe_ops_780;
 
 int vfe_get(struct vfe_device *vfe);
 void vfe_put(struct vfe_device *vfe);
@@ -236,4 +259,55 @@ void vfe_put(struct vfe_device *vfe);
  */
 bool vfe_is_lite(struct vfe_device *vfe);
 
+/*
+ * vfe_hw_version - Process write master done interrupt
+ * @vfe: VFE Device
+ *
+ * Return vfe hw version
+ */
+u32 vfe_hw_version(struct vfe_device *vfe);
+/*
+ * vfe_enable - Enable streaming on VFE line
+ * @line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_enable_v2(struct vfe_line *line);
+
+/*
+ * vfe_buf_done - Process write master done interrupt
+ * @vfe: VFE Device
+ * @wm: Write master id
+ */
+void vfe_buf_done(struct vfe_device *vfe, int wm);
+
+/*
+ * vfe_get_output_v2 - Get vfe output line
+ * line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_get_output_v2(struct vfe_line *line);
+
+/*
+ * vfe_enable_output_v2 - Enable vfe output line
+ * line: VFE line
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_enable_output_v2(struct vfe_line *line);
+
+/*
+ * vfe_queue_buffer_v2 - Add empty buffer
+ * @vid: Video device structure
+ * @buf: Buffer to be enqueued
+ *
+ * Add an empty buffer - depending on the current number of buffers it will be
+ * put in pending buffer queue or directly given to the hardware to be filled.
+ *
+ * Return 0 on success or a negative error code otherwise
+ */
+int vfe_queue_buffer_v2(struct camss_video *vid,
+			struct camss_buffer *buf);
+
 #endif /* QC_MSM_CAMSS_VFE_H */
diff --git a/drivers/media/platform/qcom/camss/camss-video.c b/drivers/media/platform/qcom/camss/camss-video.c
index a89da5ef4710..aa021fd5e123 100644
--- a/drivers/media/platform/qcom/camss/camss-video.c
+++ b/drivers/media/platform/qcom/camss/camss-video.c
@@ -24,269 +24,10 @@
 #define CAMSS_FRAME_MAX_HEIGHT_RDI	8191
 #define CAMSS_FRAME_MAX_HEIGHT_PIX	4096
 
-struct fract {
-	u8 numerator;
-	u8 denominator;
-};
-
-/*
- * struct camss_format_info - ISP media bus format information
- * @code: V4L2 media bus format code
- * @pixelformat: V4L2 pixel format FCC identifier
- * @planes: Number of planes
- * @hsub: Horizontal subsampling (for each plane)
- * @vsub: Vertical subsampling (for each plane)
- * @bpp: Bits per pixel when stored in memory (for each plane)
- */
-struct camss_format_info {
-	u32 code;
-	u32 pixelformat;
-	u8 planes;
-	struct fract hsub[3];
-	struct fract vsub[3];
-	unsigned int bpp[3];
-};
-
-static const struct camss_format_info formats_rdi_8x16[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_UYVY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_VYUY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_YUYV, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_YVYU, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-};
-
-static const struct camss_format_info formats_rdi_8x96[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_UYVY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_VYUY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_YUYV, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_YVYU, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_PIX_FMT_SBGGR10, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SBGGR14_1X14, V4L2_PIX_FMT_SBGGR14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SGBRG14_1X14, V4L2_PIX_FMT_SGBRG14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SGRBG14_1X14, V4L2_PIX_FMT_SGRBG14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SRGGB14_1X14, V4L2_PIX_FMT_SRGGB14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, V4L2_PIX_FMT_Y10, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-};
-
-static const struct camss_format_info formats_rdi_845[] = {
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_UYVY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_VYUY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_YUYV, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_YVYU, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_PIX_FMT_SBGGR10, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 12 } },
-	{ MEDIA_BUS_FMT_SBGGR14_1X14, V4L2_PIX_FMT_SBGGR14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SGBRG14_1X14, V4L2_PIX_FMT_SGBRG14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SGRBG14_1X14, V4L2_PIX_FMT_SGRBG14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_SRGGB14_1X14, V4L2_PIX_FMT_SRGGB14P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 14 } },
-	{ MEDIA_BUS_FMT_Y8_1X8, V4L2_PIX_FMT_GREY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 8 } },
-	{ MEDIA_BUS_FMT_Y10_1X10, V4L2_PIX_FMT_Y10P, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 10 } },
-	{ MEDIA_BUS_FMT_Y10_2X8_PADHI_LE, V4L2_PIX_FMT_Y10, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-};
-
-static const struct camss_format_info formats_pix_8x16[] = {
-	{ MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-};
-
-static const struct camss_format_info formats_pix_8x96[] = {
-	{ MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV12, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1_5X8, V4L2_PIX_FMT_NV21, 1,
-	  { { 1, 1 } }, { { 2, 3 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_NV16, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_NV61, 1,
-	  { { 1, 1 } }, { { 1, 2 } }, { 8 } },
-	{ MEDIA_BUS_FMT_UYVY8_1X16, V4L2_PIX_FMT_UYVY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_VYUY8_1X16, V4L2_PIX_FMT_VYUY, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YUYV8_1X16, V4L2_PIX_FMT_YUYV, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-	{ MEDIA_BUS_FMT_YVYU8_1X16, V4L2_PIX_FMT_YVYU, 1,
-	  { { 1, 1 } }, { { 1, 1 } }, { 16 } },
-};
-
 /* -----------------------------------------------------------------------------
  * Helper functions
  */
 
-static int video_find_format(u32 code, u32 pixelformat,
-			     const struct camss_format_info *formats,
-			     unsigned int nformats)
-{
-	int i;
-
-	for (i = 0; i < nformats; i++) {
-		if (formats[i].code == code &&
-		    formats[i].pixelformat == pixelformat)
-			return i;
-	}
-
-	for (i = 0; i < nformats; i++)
-		if (formats[i].code == code)
-			return i;
-
-	WARN_ON(1);
-
-	return -EINVAL;
-}
-
 /*
  * video_mbus_to_pix_mp - Convert v4l2_mbus_framefmt to v4l2_pix_format_mplane
  * @mbus: v4l2_mbus_framefmt format (input)
@@ -359,9 +100,8 @@ static int video_get_subdev_format(struct camss_video *video,
 	if (ret)
 		return ret;
 
-	ret = video_find_format(fmt.format.code,
-				format->fmt.pix_mp.pixelformat,
-				video->formats, video->nformats);
+	ret = camss_format_find_format(fmt.format.code, format->fmt.pix_mp.pixelformat,
+				       video->formats, video->nformats);
 	if (ret < 0)
 		return ret;
 
@@ -557,12 +297,6 @@ static void video_stop_streaming(struct vb2_queue *q)
 
 		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
 
-		if (entity->use_count > 1) {
-			/* Don't stop if other instances of the pipeline are still running */
-			dev_dbg(video->camss->dev, "Video pipeline still used, don't stop streaming.\n");
-			return;
-		}
-
 		if (ret) {
 			dev_err(video->camss->dev, "Video pipeline stop failed: %d\n", ret);
 			return;
@@ -576,8 +310,6 @@ static void video_stop_streaming(struct vb2_queue *q)
 
 static const struct vb2_ops msm_video_vb2_q_ops = {
 	.queue_setup     = video_queue_setup,
-	.wait_prepare    = vb2_ops_wait_prepare,
-	.wait_finish     = vb2_ops_wait_finish,
 	.buf_init        = video_buf_init,
 	.buf_prepare     = video_buf_prepare,
 	.buf_queue       = video_buf_queue,
@@ -969,7 +701,7 @@ static int msm_video_init_format(struct camss_video *video)
  */
 
 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
-		       const char *name, int is_pix)
+		       const char *name)
 {
 	struct media_pad *pad = &video->pad;
 	struct video_device *vdev;
@@ -1006,33 +738,6 @@ int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
 
 	mutex_init(&video->lock);
 
-	switch (video->camss->res->version) {
-	case CAMSS_8x16:
-		if (is_pix) {
-			video->formats = formats_pix_8x16;
-			video->nformats = ARRAY_SIZE(formats_pix_8x16);
-		} else {
-			video->formats = formats_rdi_8x16;
-			video->nformats = ARRAY_SIZE(formats_rdi_8x16);
-		}
-		break;
-	case CAMSS_8x96:
-	case CAMSS_660:
-		if (is_pix) {
-			video->formats = formats_pix_8x96;
-			video->nformats = ARRAY_SIZE(formats_pix_8x96);
-		} else {
-			video->formats = formats_rdi_8x96;
-			video->nformats = ARRAY_SIZE(formats_rdi_8x96);
-		}
-		break;
-	case CAMSS_845:
-	case CAMSS_8250:
-		video->formats = formats_rdi_845;
-		video->nformats = ARRAY_SIZE(formats_rdi_845);
-		break;
-	}
-
 	ret = msm_video_init_format(video);
 	if (ret < 0) {
 		dev_err(v4l2_dev->dev, "Failed to init format: %d\n", ret);
diff --git a/drivers/media/platform/qcom/camss/camss-video.h b/drivers/media/platform/qcom/camss/camss-video.h
index bdbae8424140..d3e56e240a88 100644
--- a/drivers/media/platform/qcom/camss/camss-video.h
+++ b/drivers/media/platform/qcom/camss/camss-video.h
@@ -33,8 +33,6 @@ struct camss_video_ops {
 			     enum vb2_buffer_state state);
 };
 
-struct camss_format_info;
-
 struct camss_video {
 	struct camss *camss;
 	struct vb2_queue vb2_q;
@@ -53,7 +51,7 @@ struct camss_video {
 };
 
 int msm_video_register(struct camss_video *video, struct v4l2_device *v4l2_dev,
-		       const char *name, int is_pix);
+		       const char *name);
 
 void msm_video_unregister(struct camss_video *video);
 
diff --git a/drivers/media/platform/qcom/camss/camss.c b/drivers/media/platform/qcom/camss/camss.c
index 58f4be660290..06f42875702f 100644
--- a/drivers/media/platform/qcom/camss/camss.c
+++ b/drivers/media/platform/qcom/camss/camss.c
@@ -32,6 +32,8 @@
 #define CAMSS_CLOCK_MARGIN_NUMERATOR 105
 #define CAMSS_CLOCK_MARGIN_DENOMINATOR 100
 
+static const struct parent_dev_ops vfe_parent_dev_ops;
+
 static const struct camss_subdev_resources csiphy_res_8x16[] = {
 	/* CSIPHY0 */
 	{
@@ -43,7 +45,11 @@ static const struct camss_subdev_resources csiphy_res_8x16[] = {
 				{ 100000000, 200000000 } },
 		.reg = { "csiphy0", "csiphy0_clk_mux" },
 		.interrupt = { "csiphy0" },
-		.ops = &csiphy_ops_2ph_1_0
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_2ph_1_0,
+			.formats = &csiphy_formats_8x16
+		}
 	},
 
 	/* CSIPHY1 */
@@ -56,7 +62,11 @@ static const struct camss_subdev_resources csiphy_res_8x16[] = {
 				{ 100000000, 200000000 } },
 		.reg = { "csiphy1", "csiphy1_clk_mux" },
 		.interrupt = { "csiphy1" },
-		.ops = &csiphy_ops_2ph_1_0
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_2ph_1_0,
+			.formats = &csiphy_formats_8x16
+		}
 	}
 };
 
@@ -76,7 +86,11 @@ static const struct camss_subdev_resources csid_res_8x16[] = {
 				{ 0 } },
 		.reg = { "csid0" },
 		.interrupt = { "csid0" },
-		.ops = &csid_ops_4_1,
+		.csid = {
+			.hw_ops = &csid_ops_4_1,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_1
+		}
 	},
 
 	/* CSID1 */
@@ -94,7 +108,11 @@ static const struct camss_subdev_resources csid_res_8x16[] = {
 				{ 0 } },
 		.reg = { "csid1" },
 		.interrupt = { "csid1" },
-		.ops = &csid_ops_4_1,
+		.csid = {
+			.hw_ops = &csid_ops_4_1,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_1
+		}
 	},
 };
 
@@ -105,8 +123,7 @@ static const struct camss_subdev_resources ispif_res_8x16 = {
 		   "csi1", "csi1_pix", "csi1_rdi" },
 	.clock_for_reset = { "vfe0", "csi_vfe0" },
 	.reg = { "ispif", "csi_clk_mux" },
-	.interrupt = { "ispif" }
-
+	.interrupt = { "ispif" },
 };
 
 static const struct camss_subdev_resources vfe_res_8x16[] = {
@@ -128,11 +145,169 @@ static const struct camss_subdev_resources vfe_res_8x16[] = {
 				{ 0 } },
 		.reg = { "vfe0" },
 		.interrupt = { "vfe0" },
-		.line_num = 3,
-		.ops = &vfe_ops_4_1
+		.vfe = {
+			.line_num = 3,
+			.hw_ops = &vfe_ops_4_1,
+			.formats_rdi = &vfe_formats_rdi_8x16,
+			.formats_pix = &vfe_formats_pix_8x16
+		}
 	}
 };
 
+static const struct camss_subdev_resources csid_res_8x53[] = {
+	/* CSID0 */
+	{
+		.regulators = { "vdda" },
+		.clock = { "top_ahb", "ispif_ahb", "csi0_ahb", "ahb",
+			   "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 200000000, 310000000,
+				  400000000, 465000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
+	},
+
+	/* CSID1 */
+	{
+		.regulators = { "vdda" },
+		.clock = { "top_ahb", "ispif_ahb", "csi1_ahb", "ahb",
+			   "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 200000000, 310000000,
+				  400000000, 465000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
+	},
+
+	/* CSID2 */
+	{
+		.regulators = { "vdda" },
+		.clock = { "top_ahb", "ispif_ahb", "csi2_ahb", "ahb",
+			   "csi2", "csi2_phy", "csi2_pix", "csi2_rdi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 200000000, 310000000,
+				  400000000, 465000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
+	},
+};
+
+static const struct camss_subdev_resources ispif_res_8x53 = {
+	/* ISPIF */
+	.clock = { "top_ahb", "ahb", "ispif_ahb",
+		   "csi0", "csi0_pix", "csi0_rdi",
+		   "csi1", "csi1_pix", "csi1_rdi",
+		   "csi2", "csi2_pix", "csi2_rdi" },
+	.clock_for_reset = { "vfe0", "csi_vfe0", "vfe1", "csi_vfe1" },
+	.reg = { "ispif", "csi_clk_mux" },
+	.interrupt = { "ispif" },
+};
+
+static const struct camss_subdev_resources vfe_res_8x53[] = {
+	/* VFE0 */
+	{
+		.regulators = {},
+		.clock = { "top_ahb", "ahb", "ispif_ahb",
+			   "vfe0", "csi_vfe0", "vfe0_ahb", "vfe0_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 50000000, 100000000, 133330000,
+				  160000000, 200000000, 266670000,
+				  310000000, 400000000, 465000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.pd_name = "vfe0",
+			.hw_ops = &vfe_ops_4_1,
+			.formats_rdi = &vfe_formats_rdi_8x16,
+			.formats_pix = &vfe_formats_pix_8x16
+		}
+	},
+
+	/* VFE1 */
+	{
+		.regulators = {},
+		.clock = { "top_ahb", "ahb", "ispif_ahb",
+			   "vfe1", "csi_vfe1", "vfe1_ahb", "vfe1_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 50000000, 100000000, 133330000,
+				  160000000, 200000000, 266670000,
+				  310000000, 400000000, 465000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.pd_name = "vfe1",
+			.hw_ops = &vfe_ops_4_1,
+			.formats_rdi = &vfe_formats_rdi_8x16,
+			.formats_pix = &vfe_formats_pix_8x16
+		}
+	}
+};
+
+static const struct resources_icc icc_res_8x53[] = {
+	{
+		.name = "cam_ahb",
+		.icc_bw_tbl.avg = 38400,
+		.icc_bw_tbl.peak = 76800,
+	},
+	{
+		.name = "cam_vfe0_mem",
+		.icc_bw_tbl.avg = 939524,
+		.icc_bw_tbl.peak = 1342177,
+	},
+	{
+		.name = "cam_vfe1_mem",
+		.icc_bw_tbl.avg = 939524,
+		.icc_bw_tbl.peak = 1342177,
+	},
+};
+
 static const struct camss_subdev_resources csiphy_res_8x96[] = {
 	/* CSIPHY0 */
 	{
@@ -144,7 +319,11 @@ static const struct camss_subdev_resources csiphy_res_8x96[] = {
 				{ 100000000, 200000000, 266666667 } },
 		.reg = { "csiphy0", "csiphy0_clk_mux" },
 		.interrupt = { "csiphy0" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	},
 
 	/* CSIPHY1 */
@@ -157,7 +336,11 @@ static const struct camss_subdev_resources csiphy_res_8x96[] = {
 				{ 100000000, 200000000, 266666667 } },
 		.reg = { "csiphy1", "csiphy1_clk_mux" },
 		.interrupt = { "csiphy1" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	},
 
 	/* CSIPHY2 */
@@ -170,7 +353,11 @@ static const struct camss_subdev_resources csiphy_res_8x96[] = {
 				{ 100000000, 200000000, 266666667 } },
 		.reg = { "csiphy2", "csiphy2_clk_mux" },
 		.interrupt = { "csiphy2" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	}
 };
 
@@ -190,7 +377,11 @@ static const struct camss_subdev_resources csid_res_8x96[] = {
 				{ 0 } },
 		.reg = { "csid0" },
 		.interrupt = { "csid0" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID1 */
@@ -208,7 +399,11 @@ static const struct camss_subdev_resources csid_res_8x96[] = {
 				{ 0 } },
 		.reg = { "csid1" },
 		.interrupt = { "csid1" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID2 */
@@ -226,7 +421,11 @@ static const struct camss_subdev_resources csid_res_8x96[] = {
 				{ 0 } },
 		.reg = { "csid2" },
 		.interrupt = { "csid2" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID3 */
@@ -244,7 +443,11 @@ static const struct camss_subdev_resources csid_res_8x96[] = {
 				{ 0 } },
 		.reg = { "csid3" },
 		.interrupt = { "csid3" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	}
 };
 
@@ -257,7 +460,7 @@ static const struct camss_subdev_resources ispif_res_8x96 = {
 		   "csi3", "csi3_pix", "csi3_rdi" },
 	.clock_for_reset = { "vfe0", "csi_vfe0", "vfe1", "csi_vfe1" },
 	.reg = { "ispif", "csi_clk_mux" },
-	.interrupt = { "ispif" }
+	.interrupt = { "ispif" },
 };
 
 static const struct camss_subdev_resources vfe_res_8x96[] = {
@@ -277,9 +480,13 @@ static const struct camss_subdev_resources vfe_res_8x96[] = {
 				{ 0 } },
 		.reg = { "vfe0" },
 		.interrupt = { "vfe0" },
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_4_7
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.hw_ops = &vfe_ops_4_7,
+			.formats_rdi = &vfe_formats_rdi_8x96,
+			.formats_pix = &vfe_formats_pix_8x96
+		}
 	},
 
 	/* VFE1 */
@@ -298,9 +505,13 @@ static const struct camss_subdev_resources vfe_res_8x96[] = {
 				{ 0 } },
 		.reg = { "vfe1" },
 		.interrupt = { "vfe1" },
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_4_7
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.hw_ops = &vfe_ops_4_7,
+			.formats_rdi = &vfe_formats_rdi_8x96,
+			.formats_pix = &vfe_formats_pix_8x96
+		}
 	}
 };
 
@@ -317,7 +528,11 @@ static const struct camss_subdev_resources csiphy_res_660[] = {
 				{ 0 } },
 		.reg = { "csiphy0", "csiphy0_clk_mux" },
 		.interrupt = { "csiphy0" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	},
 
 	/* CSIPHY1 */
@@ -332,7 +547,11 @@ static const struct camss_subdev_resources csiphy_res_660[] = {
 				{ 0 } },
 		.reg = { "csiphy1", "csiphy1_clk_mux" },
 		.interrupt = { "csiphy1" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	},
 
 	/* CSIPHY2 */
@@ -347,7 +566,11 @@ static const struct camss_subdev_resources csiphy_res_660[] = {
 				{ 0 } },
 		.reg = { "csiphy2", "csiphy2_clk_mux" },
 		.interrupt = { "csiphy2" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_8x96
+		}
 	}
 };
 
@@ -370,7 +593,11 @@ static const struct camss_subdev_resources csid_res_660[] = {
 				{ 0 } },
 		.reg = { "csid0" },
 		.interrupt = { "csid0" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID1 */
@@ -391,7 +618,11 @@ static const struct camss_subdev_resources csid_res_660[] = {
 				{ 0 } },
 		.reg = { "csid1" },
 		.interrupt = { "csid1" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID2 */
@@ -412,7 +643,11 @@ static const struct camss_subdev_resources csid_res_660[] = {
 				{ 0 } },
 		.reg = { "csid2" },
 		.interrupt = { "csid2" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	},
 
 	/* CSID3 */
@@ -433,7 +668,11 @@ static const struct camss_subdev_resources csid_res_660[] = {
 				{ 0 } },
 		.reg = { "csid3" },
 		.interrupt = { "csid3" },
-		.ops = &csid_ops_4_7,
+		.csid = {
+			.hw_ops = &csid_ops_4_7,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_4_7
+		}
 	}
 };
 
@@ -446,7 +685,7 @@ static const struct camss_subdev_resources ispif_res_660 = {
 		   "csi3", "csi3_pix", "csi3_rdi" },
 	.clock_for_reset = { "vfe0", "csi_vfe0", "vfe1", "csi_vfe1" },
 	.reg = { "ispif", "csi_clk_mux" },
-	.interrupt = { "ispif" }
+	.interrupt = { "ispif" },
 };
 
 static const struct camss_subdev_resources vfe_res_660[] = {
@@ -469,9 +708,13 @@ static const struct camss_subdev_resources vfe_res_660[] = {
 				{ 0 } },
 		.reg = { "vfe0" },
 		.interrupt = { "vfe0" },
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_4_8
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.hw_ops = &vfe_ops_4_8,
+			.formats_rdi = &vfe_formats_rdi_8x96,
+			.formats_pix = &vfe_formats_pix_8x96
+		}
 	},
 
 	/* VFE1 */
@@ -493,9 +736,195 @@ static const struct camss_subdev_resources vfe_res_660[] = {
 				{ 0 } },
 		.reg = { "vfe1" },
 		.interrupt = { "vfe1" },
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_4_8
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.hw_ops = &vfe_ops_4_8,
+			.formats_rdi = &vfe_formats_rdi_8x96,
+			.formats_pix = &vfe_formats_pix_8x96
+		}
+	}
+};
+
+static const struct camss_subdev_resources csiphy_res_670[] = {
+	/* CSIPHY0 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "soc_ahb", "cpas_ahb",
+			   "csiphy0", "csiphy0_timer" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 19200000, 240000000, 269333333 } },
+		.reg = { "csiphy0" },
+		.interrupt = { "csiphy0" },
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+
+	/* CSIPHY1 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "soc_ahb", "cpas_ahb",
+			   "csiphy1", "csiphy1_timer" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 19200000, 240000000, 269333333 } },
+		.reg = { "csiphy1" },
+		.interrupt = { "csiphy1" },
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+
+	/* CSIPHY2 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "soc_ahb", "cpas_ahb",
+			   "csiphy2", "csiphy2_timer" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 19200000, 240000000, 269333333 } },
+		.reg = { "csiphy2" },
+		.interrupt = { "csiphy2" },
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	}
+};
+
+static const struct camss_subdev_resources csid_res_670[] = {
+	/* CSID0 */
+	{
+		.regulators = {},
+		.clock = { "cpas_ahb", "soc_ahb", "vfe0",
+			   "vfe0_cphy_rx", "csi0" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 },
+				{ 384000000 },
+				{ 19200000, 75000000, 384000000, 538666667 } },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+
+	/* CSID1 */
+	{
+		.regulators = {},
+		.clock = { "cpas_ahb", "soc_ahb", "vfe1",
+			   "vfe1_cphy_rx", "csi1" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 },
+				{ 384000000 },
+				{ 19200000, 75000000, 384000000, 538666667 } },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+
+	/* CSID2 */
+	{
+		.regulators = {},
+		.clock = { "cpas_ahb", "soc_ahb", "vfe_lite",
+			   "vfe_lite_cphy_rx", "csi2" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 },
+				{ 384000000 },
+				{ 19200000, 75000000, 384000000, 538666667 } },
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	}
+};
+
+static const struct camss_subdev_resources vfe_res_670[] = {
+	/* VFE0 */
+	{
+		.regulators = {},
+		.clock = { "camnoc_axi", "cpas_ahb", "soc_ahb",
+			   "vfe0", "vfe0_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 },
+				{ 0 } },
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 4,
+			.has_pd = true,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+
+	/* VFE1 */
+	{
+		.regulators = {},
+		.clock = { "camnoc_axi", "cpas_ahb", "soc_ahb",
+			   "vfe1", "vfe1_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 },
+				{ 0 } },
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 4,
+			.has_pd = true,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+
+	/* VFE-lite */
+	{
+		.regulators = {},
+		.clock = { "camnoc_axi", "cpas_ahb", "soc_ahb",
+			   "vfe_lite" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 100000000, 320000000, 404000000, 480000000, 600000000 } },
+		.reg = { "vfe_lite" },
+		.interrupt = { "vfe_lite" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	}
 };
 
@@ -516,7 +945,11 @@ static const struct camss_subdev_resources csiphy_res_845[] = {
 				{ 19200000, 240000000, 269333333 } },
 		.reg = { "csiphy0" },
 		.interrupt = { "csiphy0" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 
 	/* CSIPHY1 */
@@ -535,7 +968,11 @@ static const struct camss_subdev_resources csiphy_res_845[] = {
 				{ 19200000, 240000000, 269333333 } },
 		.reg = { "csiphy1" },
 		.interrupt = { "csiphy1" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 
 	/* CSIPHY2 */
@@ -554,7 +991,11 @@ static const struct camss_subdev_resources csiphy_res_845[] = {
 				{ 19200000, 240000000, 269333333 } },
 		.reg = { "csiphy2" },
 		.interrupt = { "csiphy2" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 
 	/* CSIPHY3 */
@@ -573,7 +1014,11 @@ static const struct camss_subdev_resources csiphy_res_845[] = {
 				{ 19200000, 240000000, 269333333 } },
 		.reg = { "csiphy3" },
 		.interrupt = { "csiphy3" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 3,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	}
 };
 
@@ -596,7 +1041,11 @@ static const struct camss_subdev_resources csid_res_845[] = {
 				{ 384000000 } },
 		.reg = { "csid0" },
 		.interrupt = { "csid0" },
-		.ops = &csid_ops_gen2
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	},
 
 	/* CSID1 */
@@ -617,7 +1066,11 @@ static const struct camss_subdev_resources csid_res_845[] = {
 				{ 384000000 } },
 		.reg = { "csid1" },
 		.interrupt = { "csid1" },
-		.ops = &csid_ops_gen2
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	},
 
 	/* CSID2 */
@@ -638,8 +1091,12 @@ static const struct camss_subdev_resources csid_res_845[] = {
 				{ 384000000 } },
 		.reg = { "csid2" },
 		.interrupt = { "csid2" },
-		.is_lite = true,
-		.ops = &csid_ops_gen2
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	}
 };
 
@@ -662,9 +1119,14 @@ static const struct camss_subdev_resources vfe_res_845[] = {
 				{ 384000000 } },
 		.reg = { "vfe0" },
 		.interrupt = { "vfe0" },
-		.line_num = 4,
-		.has_pd = true,
-		.ops = &vfe_ops_170
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife0",
+			.has_pd = true,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 
 	/* VFE1 */
@@ -685,9 +1147,14 @@ static const struct camss_subdev_resources vfe_res_845[] = {
 				{ 384000000 } },
 		.reg = { "vfe1" },
 		.interrupt = { "vfe1" },
-		.line_num = 4,
-		.has_pd = true,
-		.ops = &vfe_ops_170
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife1",
+			.has_pd = true,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 
 	/* VFE-lite */
@@ -707,79 +1174,107 @@ static const struct camss_subdev_resources vfe_res_845[] = {
 				{ 384000000 } },
 		.reg = { "vfe_lite" },
 		.interrupt = { "vfe_lite" },
-		.is_lite = true,
-		.line_num = 4,
-		.ops = &vfe_ops_170
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	}
 };
 
 static const struct camss_subdev_resources csiphy_res_8250[] = {
 	/* CSIPHY0 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy0", "csiphy0_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy0" },
 		.interrupt = { "csiphy0" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 	/* CSIPHY1 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy1", "csiphy1_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy1" },
 		.interrupt = { "csiphy1" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 	/* CSIPHY2 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy2", "csiphy2_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy2" },
 		.interrupt = { "csiphy2" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 	/* CSIPHY3 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy3", "csiphy3_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy3" },
 		.interrupt = { "csiphy3" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 3,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 	/* CSIPHY4 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy4", "csiphy4_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy4" },
 		.interrupt = { "csiphy4" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 4,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	},
 	/* CSIPHY5 */
 	{
-		.regulators = {},
+		.regulators = { "vdda-phy", "vdda-pll" },
 		.clock = { "csiphy5", "csiphy5_timer" },
 		.clock_rate = { { 400000000 },
 				{ 300000000 } },
 		.reg = { "csiphy5" },
 		.interrupt = { "csiphy5" },
-		.ops = &csiphy_ops_3ph_1_0
+		.csiphy = {
+			.id = 5,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
 	}
 };
 
 static const struct camss_subdev_resources csid_res_8250[] = {
 	/* CSID0 */
 	{
-		.regulators = { "vdda-phy", "vdda-pll" },
+		.regulators = {},
 		.clock = { "vfe0_csid", "vfe0_cphy_rx", "vfe0", "vfe0_areg", "vfe0_ahb" },
 		.clock_rate = { { 400000000 },
 				{ 400000000 },
@@ -788,11 +1283,15 @@ static const struct camss_subdev_resources csid_res_8250[] = {
 				{ 0 } },
 		.reg = { "csid0" },
 		.interrupt = { "csid0" },
-		.ops = &csid_ops_gen2
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	},
 	/* CSID1 */
 	{
-		.regulators = { "vdda-phy", "vdda-pll" },
+		.regulators = {},
 		.clock = { "vfe1_csid", "vfe1_cphy_rx", "vfe1", "vfe1_areg", "vfe1_ahb" },
 		.clock_rate = { { 400000000 },
 				{ 400000000 },
@@ -801,11 +1300,15 @@ static const struct camss_subdev_resources csid_res_8250[] = {
 				{ 0 } },
 		.reg = { "csid1" },
 		.interrupt = { "csid1" },
-		.ops = &csid_ops_gen2
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	},
 	/* CSID2 */
 	{
-		.regulators = { "vdda-phy", "vdda-pll" },
+		.regulators = {},
 		.clock = { "vfe_lite_csid", "vfe_lite_cphy_rx", "vfe_lite",  "vfe_lite_ahb" },
 		.clock_rate = { { 400000000 },
 				{ 400000000 },
@@ -813,12 +1316,16 @@ static const struct camss_subdev_resources csid_res_8250[] = {
 				{ 0 } },
 		.reg = { "csid2" },
 		.interrupt = { "csid2" },
-		.is_lite = true,
-		.ops = &csid_ops_gen2
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	},
 	/* CSID3 */
 	{
-		.regulators = { "vdda-phy", "vdda-pll" },
+		.regulators = {},
 		.clock = { "vfe_lite_csid", "vfe_lite_cphy_rx", "vfe_lite",  "vfe_lite_ahb" },
 		.clock_rate = { { 400000000 },
 				{ 400000000 },
@@ -826,8 +1333,12 @@ static const struct camss_subdev_resources csid_res_8250[] = {
 				{ 0 } },
 		.reg = { "csid3" },
 		.interrupt = { "csid3" },
-		.is_lite = true,
-		.ops = &csid_ops_gen2
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
 	}
 };
 
@@ -849,10 +1360,14 @@ static const struct camss_subdev_resources vfe_res_8250[] = {
 				{ 0 } },
 		.reg = { "vfe0" },
 		.interrupt = { "vfe0" },
-		.pd_name = "ife0",
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_480
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_480,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 	/* VFE1 */
 	{
@@ -871,10 +1386,14 @@ static const struct camss_subdev_resources vfe_res_8250[] = {
 				{ 0 } },
 		.reg = { "vfe1" },
 		.interrupt = { "vfe1" },
-		.pd_name = "ife1",
-		.line_num = 3,
-		.has_pd = true,
-		.ops = &vfe_ops_480
+		.vfe = {
+			.line_num = 3,
+			.has_pd = true,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_480,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 	/* VFE2 (lite) */
 	{
@@ -892,9 +1411,13 @@ static const struct camss_subdev_resources vfe_res_8250[] = {
 				{ 0 } },
 		.reg = { "vfe_lite0" },
 		.interrupt = { "vfe_lite0" },
-		.is_lite = true,
-		.line_num = 4,
-		.ops = &vfe_ops_480
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_480,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 	/* VFE3 (lite) */
 	{
@@ -912,9 +1435,13 @@ static const struct camss_subdev_resources vfe_res_8250[] = {
 				{ 0 } },
 		.reg = { "vfe_lite1" },
 		.interrupt = { "vfe_lite1" },
-		.is_lite = true,
-		.line_num = 4,
-		.ops = &vfe_ops_480
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_480,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
 	},
 };
 
@@ -941,6 +1468,1314 @@ static const struct resources_icc icc_res_sm8250[] = {
 	},
 };
 
+static const struct camss_subdev_resources csiphy_res_7280[] = {
+	/* CSIPHY0 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+
+		.clock = { "csiphy0", "csiphy0_timer" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy0" },
+		.interrupt = { "csiphy0" },
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sc7280
+		}
+	},
+	/* CSIPHY1 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+
+		.clock = { "csiphy1", "csiphy1_timer" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy1" },
+		.interrupt = { "csiphy1" },
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sc7280
+		}
+	},
+	/* CSIPHY2 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+
+		.clock = { "csiphy2", "csiphy2_timer" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy2" },
+		.interrupt = { "csiphy2" },
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sc7280
+		}
+	},
+	/* CSIPHY3 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+
+		.clock = { "csiphy3", "csiphy3_timer" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy3" },
+		.interrupt = { "csiphy3" },
+		.csiphy = {
+			.id = 3,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sc7280
+		}
+	},
+	/* CSIPHY4 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+
+		.clock = { "csiphy4", "csiphy4_timer" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy4" },
+		.interrupt = { "csiphy4" },
+		.csiphy = {
+			.id = 4,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sc7280
+		}
+	},
+};
+
+static const struct camss_subdev_resources csid_res_7280[] = {
+	/* CSID0 */
+	{
+		.regulators = {},
+
+		.clock = { "vfe0_csid", "vfe0_cphy_rx", "vfe0" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 }
+		},
+
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.is_lite = false,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID1 */
+	{
+		.regulators = {},
+
+		.clock = { "vfe1_csid", "vfe1_cphy_rx", "vfe1" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 }
+		},
+
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.is_lite = false,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID2 */
+	{
+		.regulators = {},
+
+		.clock = { "vfe2_csid", "vfe2_cphy_rx", "vfe2" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 }
+		},
+
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.is_lite = false,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID3 */
+	{
+		.regulators = {},
+
+		.clock = { "vfe_lite0_csid", "vfe_lite0_cphy_rx", "vfe_lite0" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 0 },
+				{ 320000000, 400000000, 480000000, 600000000 }
+		},
+
+		.reg = { "csid_lite0" },
+		.interrupt = { "csid_lite0" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID4 */
+	{
+		.regulators = {},
+
+		.clock = { "vfe_lite1_csid", "vfe_lite1_cphy_rx", "vfe_lite1" },
+		.clock_rate = { { 300000000, 400000000 },
+				{ 0 },
+				{ 320000000, 400000000, 480000000, 600000000 }
+		},
+
+		.reg = { "csid_lite1" },
+		.interrupt = { "csid_lite1" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+};
+
+static const struct camss_subdev_resources vfe_res_7280[] = {
+	/* VFE0 */
+	{
+		.regulators = {},
+
+		.clock = { "camnoc_axi", "cpas_ahb", "icp_ahb", "vfe0",
+			   "vfe0_axi", "gcc_axi_hf", "gcc_axi_sf" },
+		.clock_rate = { { 150000000, 240000000, 320000000, 400000000, 480000000 },
+				{ 80000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.has_pd = true,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE1 */
+	{
+		.regulators = {},
+
+		.clock = { "camnoc_axi", "cpas_ahb", "icp_ahb", "vfe1",
+			   "vfe1_axi", "gcc_axi_hf", "gcc_axi_sf" },
+		.clock_rate = { { 150000000, 240000000, 320000000, 400000000, 480000000 },
+				{ 80000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.has_pd = true,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE2 */
+	{
+		.regulators = {},
+
+		.clock = { "camnoc_axi", "cpas_ahb", "icp_ahb", "vfe2",
+			   "vfe2_axi", "gcc_axi_hf", "gcc_axi_sf" },
+		.clock_rate = { { 150000000, 240000000, 320000000, 400000000, 480000000 },
+				{ 80000000 },
+				{ 0 },
+				{ 380000000, 510000000, 637000000, 760000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+
+		.reg = { "vfe2" },
+		.interrupt = { "vfe2" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.hw_ops = &vfe_ops_170,
+			.has_pd = true,
+			.pd_name = "ife2",
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE3 (lite) */
+	{
+		.clock = { "camnoc_axi", "cpas_ahb", "icp_ahb",
+			   "vfe_lite0", "gcc_axi_hf", "gcc_axi_sf" },
+		.clock_rate = { { 150000000, 240000000, 320000000, 400000000, 480000000 },
+				{ 80000000 },
+				{ 0 },
+				{ 320000000, 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 } },
+
+		.regulators = {},
+		.reg = { "vfe_lite0" },
+		.interrupt = { "vfe_lite0" },
+		.vfe = {
+			.line_num = 4,
+			.is_lite = true,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE4 (lite) */
+	{
+		.clock = { "camnoc_axi", "cpas_ahb", "icp_ahb",
+			   "vfe_lite1", "gcc_axi_hf", "gcc_axi_sf" },
+		.clock_rate = { { 150000000, 240000000, 320000000, 400000000, 480000000 },
+				{ 80000000 },
+				{ 0 },
+				{ 320000000, 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 } },
+
+		.regulators = {},
+		.reg = { "vfe_lite1" },
+		.interrupt = { "vfe_lite1" },
+		.vfe = {
+			.line_num = 4,
+			.is_lite = true,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+};
+
+static const struct resources_icc icc_res_sc7280[] = {
+	{
+		.name = "ahb",
+		.icc_bw_tbl.avg = 38400,
+		.icc_bw_tbl.peak = 76800,
+	},
+	{
+		.name = "hf_0",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+};
+
+static const struct camss_subdev_resources csiphy_res_sc8280xp[] = {
+	/* CSIPHY0 */
+	{
+		.regulators = {},
+		.clock = { "csiphy0", "csiphy0_timer" },
+		.clock_rate = { { 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy0" },
+		.interrupt = { "csiphy0" },
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY1 */
+	{
+		.regulators = {},
+		.clock = { "csiphy1", "csiphy1_timer" },
+		.clock_rate = { { 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy1" },
+		.interrupt = { "csiphy1" },
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY2 */
+	{
+		.regulators = {},
+		.clock = { "csiphy2", "csiphy2_timer" },
+		.clock_rate = { { 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy2" },
+		.interrupt = { "csiphy2" },
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY3 */
+	{
+		.regulators = {},
+		.clock = { "csiphy3", "csiphy3_timer" },
+		.clock_rate = { { 400000000 },
+				{ 300000000 } },
+		.reg = { "csiphy3" },
+		.interrupt = { "csiphy3" },
+		.csiphy = {
+			.id = 3,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+};
+
+static const struct camss_subdev_resources csid_res_sc8280xp[] = {
+	/* CSID0 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe0_csid", "vfe0_cphy_rx", "vfe0", "vfe0_axi" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID1 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe1_csid", "vfe1_cphy_rx", "vfe1", "vfe1_axi" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID2 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe2_csid", "vfe2_cphy_rx", "vfe2", "vfe2_axi" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID3 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe3_csid", "vfe3_cphy_rx", "vfe3", "vfe3_axi" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 },
+				{ 0 } },
+		.reg = { "csid3" },
+		.interrupt = { "csid3" },
+		.csid = {
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID_LITE0 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe_lite0_csid", "vfe_lite0_cphy_rx", "vfe_lite0" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 }, },
+		.reg = { "csid0_lite" },
+		.interrupt = { "csid0_lite" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID_LITE1 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe_lite1_csid", "vfe_lite1_cphy_rx", "vfe_lite1" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 }, },
+		.reg = { "csid1_lite" },
+		.interrupt = { "csid1_lite" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID_LITE2 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe_lite2_csid", "vfe_lite2_cphy_rx", "vfe_lite2" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 }, },
+		.reg = { "csid2_lite" },
+		.interrupt = { "csid2_lite" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID_LITE3 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "vfe_lite3_csid", "vfe_lite3_cphy_rx", "vfe_lite3" },
+		.clock_rate = { { 400000000, 480000000, 600000000 },
+				{ 0 },
+				{ 0 }, },
+		.reg = { "csid3_lite" },
+		.interrupt = { "csid3_lite" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_gen2,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	}
+};
+
+static const struct camss_subdev_resources vfe_res_sc8280xp[] = {
+	/* VFE0 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe0", "vfe0_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 400000000, 558000000, 637000000, 760000000 },
+				{ 0 }, },
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE1 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe1", "vfe1_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 400000000, 558000000, 637000000, 760000000 },
+				{ 0 }, },
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE2 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe2", "vfe2_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 400000000, 558000000, 637000000, 760000000 },
+				{ 0 }, },
+		.reg = { "vfe2" },
+		.interrupt = { "vfe2" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife2",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE3 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe3", "vfe3_axi" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 400000000, 558000000, 637000000, 760000000 },
+				{ 0 }, },
+		.reg = { "vfe3" },
+		.interrupt = { "vfe3" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife3",
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE_LITE_0 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe_lite0" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 320000000, 400000000, 480000000, 600000000 }, },
+		.reg = { "vfe_lite0" },
+		.interrupt = { "vfe_lite0" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE_LITE_1 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe_lite1" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 320000000, 400000000, 480000000, 600000000 }, },
+		.reg = { "vfe_lite1" },
+		.interrupt = { "vfe_lite1" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE_LITE_2 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe_lite2" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 320000000, 400000000, 480000000, 600000000, }, },
+		.reg = { "vfe_lite2" },
+		.interrupt = { "vfe_lite2" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE_LITE_3 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb", "camnoc_axi", "vfe_lite3" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 19200000, 80000000},
+				{ 19200000, 150000000, 266666667, 320000000, 400000000, 480000000 },
+				{ 320000000, 400000000, 480000000, 600000000 }, },
+		.reg = { "vfe_lite3" },
+		.interrupt = { "vfe_lite3" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_170,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+};
+
+static const struct resources_icc icc_res_sc8280xp[] = {
+	{
+		.name = "cam_ahb",
+		.icc_bw_tbl.avg = 150000,
+		.icc_bw_tbl.peak = 300000,
+	},
+	{
+		.name = "cam_hf_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+	{
+		.name = "cam_sf_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+	{
+		.name = "cam_sf_icp_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+};
+
+static const struct camss_subdev_resources csiphy_res_8550[] = {
+	/* CSIPHY0 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy0", "csiphy0_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy0" },
+		.interrupt = { "csiphy0" },
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY1 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy1", "csiphy1_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy1" },
+		.interrupt = { "csiphy1" },
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY2 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy2", "csiphy2_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy2" },
+		.interrupt = { "csiphy2" },
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY3 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy3", "csiphy3_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy3" },
+		.interrupt = { "csiphy3" },
+		.csiphy = {
+			.id = 3,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY4 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy4", "csiphy4_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy4" },
+		.interrupt = { "csiphy4" },
+		.csiphy = {
+			.id = 4,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY5 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy5", "csiphy5_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy5" },
+		.interrupt = { "csiphy5" },
+		.csiphy = {
+			.id = 5,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY6 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy6", "csiphy6_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy6" },
+		.interrupt = { "csiphy6" },
+		.csiphy = {
+			.id = 6,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	},
+	/* CSIPHY7 */
+	{
+		.regulators = { "vdda-phy", "vdda-pll" },
+		.clock = { "csiphy7", "csiphy7_timer" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000 } },
+		.reg = { "csiphy7" },
+		.interrupt = { "csiphy7" },
+		.csiphy = {
+			.id = 7,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		}
+	}
+};
+
+static const struct resources_wrapper csid_wrapper_res_sm8550 = {
+	.reg = "csid_wrapper",
+};
+
+static const struct camss_subdev_resources csid_res_8550[] = {
+	/* CSID0 */
+	{
+		.regulators = {},
+		.clock = { "csid", "csiphy_rx" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000, 480000000 } },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.is_lite = false,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.hw_ops = &csid_ops_780,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID1 */
+	{
+		.regulators = {},
+		.clock = { "csid", "csiphy_rx" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000, 480000000 } },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.is_lite = false,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.hw_ops = &csid_ops_780,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID2 */
+	{
+		.regulators = {},
+		.clock = { "csid", "csiphy_rx" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000, 480000000 } },
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.is_lite = false,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.hw_ops = &csid_ops_780,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID3 */
+	{
+		.regulators = {},
+		.clock = { "vfe_lite_csid", "vfe_lite_cphy_rx" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000, 480000000 } },
+		.reg = { "csid_lite0" },
+		.interrupt = { "csid_lite0" },
+		.csid = {
+			.is_lite = true,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.hw_ops = &csid_ops_780,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID4 */
+	{
+		.regulators = {},
+		.clock = { "vfe_lite_csid", "vfe_lite_cphy_rx" },
+		.clock_rate = { { 400000000, 480000000 },
+				{ 400000000, 480000000 } },
+		.reg = { "csid_lite1" },
+		.interrupt = { "csid_lite1" },
+		.csid = {
+			.is_lite = true,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.hw_ops = &csid_ops_780,
+			.formats = &csid_formats_gen2
+		}
+	}
+};
+
+static const struct camss_subdev_resources vfe_res_8550[] = {
+	/* VFE0 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "cpas_ahb", "cpas_fast_ahb_clk", "vfe0_fast_ahb",
+			   "vfe0", "cpas_vfe0", "camnoc_axi" },
+		.clock_rate = { { 0 },
+				{ 80000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 },
+				{ 466000000, 594000000, 675000000, 785000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 } },
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.has_pd = true,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_780,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE1 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "cpas_ahb", "cpas_fast_ahb_clk", "vfe1_fast_ahb",
+			   "vfe1", "cpas_vfe1", "camnoc_axi" },
+		.clock_rate = {	{ 0 },
+				{ 80000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 },
+				{ 466000000, 594000000, 675000000, 785000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 } },
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.has_pd = true,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_780,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE2 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "cpas_ahb", "cpas_fast_ahb_clk", "vfe2_fast_ahb",
+			   "vfe2", "cpas_vfe2", "camnoc_axi" },
+		.clock_rate = {	{ 0 },
+				{ 80000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 },
+				{ 466000000, 594000000, 675000000, 785000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 } },
+		.reg = { "vfe2" },
+		.interrupt = { "vfe2" },
+		.vfe = {
+			.line_num = 3,
+			.is_lite = false,
+			.has_pd = true,
+			.pd_name = "ife2",
+			.hw_ops = &vfe_ops_780,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE3 lite */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "cpas_ahb", "cpas_fast_ahb_clk", "vfe_lite_ahb",
+			   "vfe_lite", "cpas_ife_lite", "camnoc_axi" },
+		.clock_rate = {	{ 0 },
+				{ 80000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 },
+				{ 400000000, 480000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 } },
+		.reg = { "vfe_lite0" },
+		.interrupt = { "vfe_lite0" },
+		.vfe = {
+			.line_num = 4,
+			.is_lite = true,
+			.hw_ops = &vfe_ops_780,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+	/* VFE4 lite */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "cpas_ahb", "cpas_fast_ahb_clk", "vfe_lite_ahb",
+			   "vfe_lite", "cpas_ife_lite", "camnoc_axi" },
+		.clock_rate = {	{ 0 },
+				{ 80000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 },
+				{ 400000000, 480000000 },
+				{ 300000000, 400000000 },
+				{ 300000000, 400000000 } },
+		.reg = { "vfe_lite1" },
+		.interrupt = { "vfe_lite1" },
+		.vfe = {
+			.line_num = 4,
+			.is_lite = true,
+			.hw_ops = &vfe_ops_780,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		}
+	},
+};
+
+static const struct resources_icc icc_res_sm8550[] = {
+	{
+		.name = "ahb",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+	{
+		.name = "hf_0_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+};
+
+static const struct camss_subdev_resources csiphy_res_x1e80100[] = {
+	/* CSIPHY0 */
+	{
+		.regulators = { "vdd-csiphy-0p8-supply",
+				"vdd-csiphy-1p2-supply" },
+		.clock = { "csiphy0", "csiphy0_timer" },
+		.clock_rate = { { 300000000, 400000000, 480000000 },
+				{ 266666667, 400000000 } },
+		.reg = { "csiphy0" },
+		.interrupt = { "csiphy0" },
+		.csiphy = {
+			.id = 0,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		},
+	},
+	/* CSIPHY1 */
+	{
+		.regulators = { "vdd-csiphy-0p8-supply",
+				"vdd-csiphy-1p2-supply" },
+		.clock = { "csiphy1", "csiphy1_timer" },
+		.clock_rate = { { 300000000, 400000000, 480000000 },
+				{ 266666667, 400000000 } },
+		.reg = { "csiphy1" },
+		.interrupt = { "csiphy1" },
+		.csiphy = {
+			.id = 1,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		},
+	},
+	/* CSIPHY2 */
+	{
+		.regulators = { "vdd-csiphy-0p8-supply",
+				"vdd-csiphy-1p2-supply" },
+		.clock = { "csiphy2", "csiphy2_timer" },
+		.clock_rate = { { 300000000, 400000000, 480000000 },
+				{ 266666667, 400000000 } },
+		.reg = { "csiphy2" },
+		.interrupt = { "csiphy2" },
+		.csiphy = {
+			.id = 2,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		},
+	},
+	/* CSIPHY4 */
+	{
+		.regulators = { "vdd-csiphy-0p8-supply",
+				"vdd-csiphy-1p2-supply" },
+		.clock = { "csiphy4", "csiphy4_timer" },
+		.clock_rate = { { 300000000, 400000000, 480000000 },
+				{ 266666667, 400000000 } },
+		.reg = { "csiphy4" },
+		.interrupt = { "csiphy4" },
+		.csiphy = {
+			.id = 4,
+			.hw_ops = &csiphy_ops_3ph_1_0,
+			.formats = &csiphy_formats_sdm845
+		},
+	},
+};
+
+static const struct camss_subdev_resources csid_res_x1e80100[] = {
+	/* CSID0 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb",
+			   "cpas_fast_ahb", "csid", "csid_csiphy_rx" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 64000000, 80000000 },
+				{ 80000000,  100000000, 200000000,
+				  300000000, 400000000 },
+				{ 300000000, 400000000, 480000000 },
+				{ 300000000, 400000000, 480000000 }, },
+		.reg = { "csid0" },
+		.interrupt = { "csid0" },
+		.csid = {
+			.hw_ops = &csid_ops_680,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		},
+	},
+	/* CSID1 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb",
+			   "cpas_fast_ahb", "csid", "csid_csiphy_rx" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 64000000, 80000000 },
+				{ 80000000,  100000000, 200000000,
+				  300000000, 400000000 },
+				{ 300000000, 400000000, 480000000 },
+				{ 300000000, 400000000, 480000000 }, },
+		.reg = { "csid1" },
+		.interrupt = { "csid1" },
+		.csid = {
+			.hw_ops = &csid_ops_680,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		},
+	},
+	/* CSID2 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb",
+			   "cpas_fast_ahb", "csid", "csid_csiphy_rx" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 64000000, 80000000 },
+				{ 80000000,  100000000, 200000000,
+				  300000000, 400000000 },
+				{ 300000000, 400000000, 480000000 },
+				{ 300000000, 400000000, 480000000 }, },
+		.reg = { "csid2" },
+		.interrupt = { "csid2" },
+		.csid = {
+			.hw_ops = &csid_ops_680,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		},
+	},
+	/* CSID_LITE0 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb",
+			   "cpas_fast_ahb", "csid", "csid_csiphy_rx" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 64000000, 80000000 },
+				{ 80000000,  100000000, 200000000,
+				  300000000, 400000000 },
+				{ 300000000, 400000000, 480000000 },
+				{ 300000000, 400000000, 480000000 }, },
+		.reg = { "csid_lite0" },
+		.interrupt = { "csid_lite0" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_680,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+	/* CSID_LITE1 */
+	{
+		.regulators = {},
+		.clock = { "gcc_axi_hf", "gcc_axi_sf", "cpas_ahb",
+			   "cpas_fast_ahb", "csid", "csid_csiphy_rx" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 64000000, 80000000 },
+				{ 80000000,  100000000, 200000000,
+				  300000000, 400000000 },
+				{ 300000000, 400000000, 480000000 },
+				{ 300000000, 400000000, 480000000 }, },
+
+		.reg = { "csid_lite1" },
+		.interrupt = { "csid_lite1" },
+		.csid = {
+			.is_lite = true,
+			.hw_ops = &csid_ops_680,
+			.parent_dev_ops = &vfe_parent_dev_ops,
+			.formats = &csid_formats_gen2
+		}
+	},
+};
+
+static const struct camss_subdev_resources vfe_res_x1e80100[] = {
+	/* IFE0 */
+	{
+		.regulators = {},
+		.clock = {"camnoc_rt_axi", "camnoc_nrt_axi", "cpas_ahb",
+			  "cpas_fast_ahb", "cpas_vfe0", "vfe0_fast_ahb",
+			  "vfe0" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 345600000, 432000000, 594000000, 675000000,
+				  727000000 }, },
+		.reg = { "vfe0" },
+		.interrupt = { "vfe0" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife0",
+			.hw_ops = &vfe_ops_680,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		},
+	},
+	/* IFE1 */
+	{
+		.regulators = {},
+		.clock = { "camnoc_rt_axi", "camnoc_nrt_axi", "cpas_ahb",
+			   "cpas_fast_ahb", "cpas_vfe1", "vfe1_fast_ahb",
+			   "vfe1"  },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 345600000, 432000000, 594000000, 675000000,
+				  727000000 }, },
+		.reg = { "vfe1" },
+		.interrupt = { "vfe1" },
+		.vfe = {
+			.line_num = 4,
+			.pd_name = "ife1",
+			.hw_ops = &vfe_ops_680,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		},
+	},
+	/* IFE_LITE_0 */
+	{
+		.regulators = {},
+		.clock = { "camnoc_rt_axi", "camnoc_nrt_axi", "cpas_ahb",
+			   "vfe_lite_ahb", "cpas_vfe_lite", "vfe_lite",
+			   "vfe_lite_csid" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 266666667, 400000000, 480000000 },
+				{ 266666667, 400000000, 480000000 }, },
+		.reg = { "vfe_lite0" },
+		.interrupt = { "vfe_lite0" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_680,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		},
+	},
+	/* IFE_LITE_1 */
+	{
+		.regulators = {},
+		.clock = { "camnoc_rt_axi", "camnoc_nrt_axi", "cpas_ahb",
+			   "vfe_lite_ahb", "cpas_vfe_lite", "vfe_lite",
+			   "vfe_lite_csid" },
+		.clock_rate = { { 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 0 },
+				{ 266666667, 400000000, 480000000 },
+				{ 266666667, 400000000, 480000000 }, },
+		.reg = { "vfe_lite1" },
+		.interrupt = { "vfe_lite1" },
+		.vfe = {
+			.is_lite = true,
+			.line_num = 4,
+			.hw_ops = &vfe_ops_680,
+			.formats_rdi = &vfe_formats_rdi_845,
+			.formats_pix = &vfe_formats_pix_845
+		},
+	},
+};
+
+static const struct resources_icc icc_res_x1e80100[] = {
+	{
+		.name = "ahb",
+		.icc_bw_tbl.avg = 150000,
+		.icc_bw_tbl.peak = 300000,
+	},
+	{
+		.name = "hf_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+	{
+		.name = "sf_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+	{
+		.name = "sf_icp_mnoc",
+		.icc_bw_tbl.avg = 2097152,
+		.icc_bw_tbl.peak = 2097152,
+	},
+};
+
+static const struct resources_wrapper csid_wrapper_res_x1e80100 = {
+	.reg = "csid_wrapper",
+};
+
 /*
  * camss_add_clock_margin - Add margin to clock frequency rate
  * @rate: Clock frequency rate
@@ -999,12 +2834,12 @@ void camss_disable_clocks(int nclocks, struct camss_clock *clock)
 }
 
 /*
- * camss_find_sensor - Find a linked media entity which represents a sensor
+ * camss_find_sensor_pad - Find the media pad via which the sensor is linked
  * @entity: Media entity to start searching from
  *
- * Return a pointer to sensor media entity or NULL if not found
+ * Return a pointer to sensor media pad or NULL if not found
  */
-struct media_entity *camss_find_sensor(struct media_entity *entity)
+struct media_pad *camss_find_sensor_pad(struct media_entity *entity)
 {
 	struct media_pad *pad;
 
@@ -1020,7 +2855,7 @@ struct media_entity *camss_find_sensor(struct media_entity *entity)
 		entity = pad->entity;
 
 		if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
-			return entity;
+			return pad;
 	}
 }
 
@@ -1035,16 +2870,13 @@ struct media_entity *camss_find_sensor(struct media_entity *entity)
 s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
 			unsigned int lanes)
 {
-	struct media_entity *sensor;
-	struct v4l2_subdev *subdev;
+	struct media_pad *sensor_pad;
 
-	sensor = camss_find_sensor(entity);
-	if (!sensor)
+	sensor_pad = camss_find_sensor_pad(entity);
+	if (!sensor_pad)
 		return -ENODEV;
 
-	subdev = media_entity_to_v4l2_subdev(sensor);
-
-	return v4l2_get_link_freq(subdev->ctrl_handler, bpp, 2 * lanes);
+	return v4l2_get_link_freq(sensor_pad, bpp, 2 * lanes);
 }
 
 /*
@@ -1056,15 +2888,15 @@ s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
  */
 int camss_get_pixel_clock(struct media_entity *entity, u64 *pixel_clock)
 {
-	struct media_entity *sensor;
+	struct media_pad *sensor_pad;
 	struct v4l2_subdev *subdev;
 	struct v4l2_ctrl *ctrl;
 
-	sensor = camss_find_sensor(entity);
-	if (!sensor)
+	sensor_pad = camss_find_sensor_pad(entity);
+	if (!sensor_pad)
 		return -ENODEV;
 
-	subdev = media_entity_to_v4l2_subdev(sensor);
+	subdev = media_entity_to_v4l2_subdev(sensor_pad->entity);
 
 	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
 
@@ -1083,7 +2915,7 @@ int camss_pm_domain_on(struct camss *camss, int id)
 	if (id < camss->res->vfe_num) {
 		struct vfe_device *vfe = &camss->vfe[id];
 
-		ret = vfe->ops->pm_domain_on(vfe);
+		ret = vfe->res->hw_ops->pm_domain_on(vfe);
 	}
 
 	return ret;
@@ -1094,10 +2926,52 @@ void camss_pm_domain_off(struct camss *camss, int id)
 	if (id < camss->res->vfe_num) {
 		struct vfe_device *vfe = &camss->vfe[id];
 
-		vfe->ops->pm_domain_off(vfe);
+		vfe->res->hw_ops->pm_domain_off(vfe);
+	}
+}
+
+static int vfe_parent_dev_ops_get(struct camss *camss, int id)
+{
+	int ret = -EINVAL;
+
+	if (id < camss->res->vfe_num) {
+		struct vfe_device *vfe = &camss->vfe[id];
+
+		ret = vfe_get(vfe);
+	}
+
+	return ret;
+}
+
+static int vfe_parent_dev_ops_put(struct camss *camss, int id)
+{
+	if (id < camss->res->vfe_num) {
+		struct vfe_device *vfe = &camss->vfe[id];
+
+		vfe_put(vfe);
+	}
+
+	return 0;
+}
+
+static void __iomem
+*vfe_parent_dev_ops_get_base_address(struct camss *camss, int id)
+{
+	if (id < camss->res->vfe_num) {
+		struct vfe_device *vfe = &camss->vfe[id];
+
+		return vfe->base;
 	}
+
+	return NULL;
 }
 
+static const struct parent_dev_ops vfe_parent_dev_ops = {
+	.get = vfe_parent_dev_ops_get,
+	.put = vfe_parent_dev_ops_put,
+	.get_base_address = vfe_parent_dev_ops_get_base_address
+};
+
 /*
  * camss_of_parse_endpoint_node - Parse port endpoint node
  * @dev: Device
@@ -1114,8 +2988,20 @@ static int camss_of_parse_endpoint_node(struct device *dev,
 	struct v4l2_mbus_config_mipi_csi2 *mipi_csi2;
 	struct v4l2_fwnode_endpoint vep = { { 0 } };
 	unsigned int i;
+	int ret;
+
+	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+	if (ret)
+		return ret;
 
-	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
+	/*
+	 * Most SoCs support both D-PHY and C-PHY standards, but currently only
+	 * D-PHY is supported in the driver.
+	 */
+	if (vep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		dev_err(dev, "Unsupported bus type %d\n", vep.bus_type);
+		return -EINVAL;
+	}
 
 	csd->interface.csiphy_id = vep.base.port;
 
@@ -1196,13 +3082,15 @@ err_cleanup:
  */
 static int camss_init_subdevices(struct camss *camss)
 {
+	struct platform_device *pdev = to_platform_device(camss->dev);
 	const struct camss_resources *res = camss->res;
 	unsigned int i;
 	int ret;
 
 	for (i = 0; i < camss->res->csiphy_num; i++) {
 		ret = msm_csiphy_subdev_init(camss, &camss->csiphy[i],
-					     &res->csiphy_res[i], i);
+					     &res->csiphy_res[i],
+					     res->csiphy_res[i].csiphy.id);
 		if (ret < 0) {
 			dev_err(camss->dev,
 				"Failed to init csiphy%d sub-device: %d\n",
@@ -1222,6 +3110,17 @@ static int camss_init_subdevices(struct camss *camss)
 		}
 	}
 
+	/* Get optional CSID wrapper regs shared between CSID devices */
+	if (res->csid_wrapper_res) {
+		char *reg = res->csid_wrapper_res->reg;
+		void __iomem *base;
+
+		base = devm_platform_ioremap_resource_byname(pdev, reg);
+		if (IS_ERR(base))
+			return PTR_ERR(base);
+		camss->csid_wrapper_base = base;
+	}
+
 	for (i = 0; i < camss->res->csid_num; i++) {
 		ret = msm_csid_subdev_init(camss, &camss->csid[i],
 					   &res->csid_res[i], i);
@@ -1244,72 +3143,47 @@ static int camss_init_subdevices(struct camss *camss)
 }
 
 /*
- * camss_register_entities - Register subdev nodes and create links
+ * camss_link_entities - Register subdev nodes and create links
+ * camss_link_err - print error in case link creation fails
+ * @src_name: name for source of the link
+ * @sink_name: name for sink of the link
+ */
+inline void camss_link_err(struct camss *camss,
+			   const char *src_name,
+			   const char *sink_name,
+			   int ret)
+{
+	dev_err(camss->dev,
+		"Failed to link %s->%s entities: %d\n",
+		src_name,
+		sink_name,
+		ret);
+}
+
+/*
+ * camss_link_entities - Register subdev nodes and create links
  * @camss: CAMSS device
  *
  * Return 0 on success or a negative error code on failure
  */
-static int camss_register_entities(struct camss *camss)
+static int camss_link_entities(struct camss *camss)
 {
 	int i, j, k;
 	int ret;
 
 	for (i = 0; i < camss->res->csiphy_num; i++) {
-		ret = msm_csiphy_register_entity(&camss->csiphy[i],
-						 &camss->v4l2_dev);
-		if (ret < 0) {
-			dev_err(camss->dev,
-				"Failed to register csiphy%d entity: %d\n",
-				i, ret);
-			goto err_reg_csiphy;
-		}
-	}
-
-	for (i = 0; i < camss->res->csid_num; i++) {
-		ret = msm_csid_register_entity(&camss->csid[i],
-					       &camss->v4l2_dev);
-		if (ret < 0) {
-			dev_err(camss->dev,
-				"Failed to register csid%d entity: %d\n",
-				i, ret);
-			goto err_reg_csid;
-		}
-	}
-
-	ret = msm_ispif_register_entities(camss->ispif,
-					  &camss->v4l2_dev);
-	if (ret < 0) {
-		dev_err(camss->dev, "Failed to register ispif entities: %d\n",
-		ret);
-		goto err_reg_ispif;
-	}
-
-	for (i = 0; i < camss->res->vfe_num; i++) {
-		ret = msm_vfe_register_entities(&camss->vfe[i],
-						&camss->v4l2_dev);
-		if (ret < 0) {
-			dev_err(camss->dev,
-				"Failed to register vfe%d entities: %d\n",
-				i, ret);
-			goto err_reg_vfe;
-		}
-	}
-
-	for (i = 0; i < camss->res->csiphy_num; i++) {
 		for (j = 0; j < camss->res->csid_num; j++) {
-			ret = media_create_pad_link(
-				&camss->csiphy[i].subdev.entity,
-				MSM_CSIPHY_PAD_SRC,
-				&camss->csid[j].subdev.entity,
-				MSM_CSID_PAD_SINK,
-				0);
+			ret = media_create_pad_link(&camss->csiphy[i].subdev.entity,
+						    MSM_CSIPHY_PAD_SRC,
+						    &camss->csid[j].subdev.entity,
+						    MSM_CSID_PAD_SINK,
+						    0);
 			if (ret < 0) {
-				dev_err(camss->dev,
-					"Failed to link %s->%s entities: %d\n",
-					camss->csiphy[i].subdev.entity.name,
-					camss->csid[j].subdev.entity.name,
-					ret);
-				goto err_link;
+				camss_link_err(camss,
+					       camss->csiphy[i].subdev.entity.name,
+					       camss->csid[j].subdev.entity.name,
+					       ret);
+				return ret;
 			}
 		}
 	}
@@ -1317,26 +3191,24 @@ static int camss_register_entities(struct camss *camss)
 	if (camss->ispif) {
 		for (i = 0; i < camss->res->csid_num; i++) {
 			for (j = 0; j < camss->ispif->line_num; j++) {
-				ret = media_create_pad_link(
-					&camss->csid[i].subdev.entity,
-					MSM_CSID_PAD_SRC,
-					&camss->ispif->line[j].subdev.entity,
-					MSM_ISPIF_PAD_SINK,
-					0);
+				ret = media_create_pad_link(&camss->csid[i].subdev.entity,
+							    MSM_CSID_PAD_SRC,
+							    &camss->ispif->line[j].subdev.entity,
+							    MSM_ISPIF_PAD_SINK,
+							    0);
 				if (ret < 0) {
-					dev_err(camss->dev,
-						"Failed to link %s->%s entities: %d\n",
-						camss->csid[i].subdev.entity.name,
-						camss->ispif->line[j].subdev.entity.name,
-						ret);
-					goto err_link;
+					camss_link_err(camss,
+						       camss->csid[i].subdev.entity.name,
+						       camss->ispif->line[j].subdev.entity.name,
+						       ret);
+					return ret;
 				}
 			}
 		}
 
 		for (i = 0; i < camss->ispif->line_num; i++)
 			for (k = 0; k < camss->res->vfe_num; k++)
-				for (j = 0; j < camss->vfe[k].line_num; j++) {
+				for (j = 0; j < camss->vfe[k].res->line_num; j++) {
 					struct v4l2_subdev *ispif = &camss->ispif->line[i].subdev;
 					struct v4l2_subdev *vfe = &camss->vfe[k].line[j].subdev;
 
@@ -1346,18 +3218,16 @@ static int camss_register_entities(struct camss *camss)
 								    MSM_VFE_PAD_SINK,
 								    0);
 					if (ret < 0) {
-						dev_err(camss->dev,
-							"Failed to link %s->%s entities: %d\n",
-							ispif->entity.name,
-							vfe->entity.name,
-							ret);
-						goto err_link;
+						camss_link_err(camss, ispif->entity.name,
+							       vfe->entity.name,
+							       ret);
+						return ret;
 					}
 				}
 	} else {
 		for (i = 0; i < camss->res->csid_num; i++)
 			for (k = 0; k < camss->res->vfe_num; k++)
-				for (j = 0; j < camss->vfe[k].line_num; j++) {
+				for (j = 0; j < camss->vfe[k].res->line_num; j++) {
 					struct v4l2_subdev *csid = &camss->csid[i].subdev;
 					struct v4l2_subdev *vfe = &camss->vfe[k].line[j].subdev;
 
@@ -1367,20 +3237,92 @@ static int camss_register_entities(struct camss *camss)
 								    MSM_VFE_PAD_SINK,
 								    0);
 					if (ret < 0) {
-						dev_err(camss->dev,
-							"Failed to link %s->%s entities: %d\n",
-							csid->entity.name,
-							vfe->entity.name,
-							ret);
-						goto err_link;
+						camss_link_err(camss, csid->entity.name,
+							       vfe->entity.name,
+							       ret);
+						return ret;
 					}
 				}
 	}
 
 	return 0;
+}
+
+void camss_reg_update(struct camss *camss, int hw_id, int port_id, bool is_clear)
+{
+	struct csid_device *csid;
+
+	if (hw_id < camss->res->csid_num) {
+		csid = &camss->csid[hw_id];
+
+		csid->res->hw_ops->reg_update(csid, port_id, is_clear);
+	}
+}
+
+void camss_buf_done(struct camss *camss, int hw_id, int port_id)
+{
+	struct vfe_device *vfe;
+
+	if (hw_id < camss->res->vfe_num) {
+		vfe = &camss->vfe[hw_id];
+
+		vfe->res->hw_ops->vfe_buf_done(vfe, port_id);
+	}
+}
+
+/*
+ * camss_register_entities - Register subdev nodes and create links
+ * @camss: CAMSS device
+ *
+ * Return 0 on success or a negative error code on failure
+ */
+static int camss_register_entities(struct camss *camss)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < camss->res->csiphy_num; i++) {
+		ret = msm_csiphy_register_entity(&camss->csiphy[i],
+						 &camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register csiphy%d entity: %d\n",
+				i, ret);
+			goto err_reg_csiphy;
+		}
+	}
+
+	for (i = 0; i < camss->res->csid_num; i++) {
+		ret = msm_csid_register_entity(&camss->csid[i],
+					       &camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register csid%d entity: %d\n",
+				i, ret);
+			goto err_reg_csid;
+		}
+	}
+
+	ret = msm_ispif_register_entities(camss->ispif,
+					  &camss->v4l2_dev);
+	if (ret < 0) {
+		dev_err(camss->dev, "Failed to register ispif entities: %d\n", ret);
+		goto err_reg_ispif;
+	}
+
+	for (i = 0; i < camss->res->vfe_num; i++) {
+		ret = msm_vfe_register_entities(&camss->vfe[i],
+						&camss->v4l2_dev);
+		if (ret < 0) {
+			dev_err(camss->dev,
+				"Failed to register vfe%d entities: %d\n",
+				i, ret);
+			goto err_reg_vfe;
+		}
+	}
+
+	return 0;
 
-err_link:
-	i = camss->res->vfe_num;
 err_reg_vfe:
 	for (i--; i >= 0; i--)
 		msm_vfe_unregister_entities(&camss->vfe[i]);
@@ -1468,9 +3410,9 @@ static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
 				input, MSM_CSIPHY_PAD_SINK,
 				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 			if (ret < 0) {
-				dev_err(camss->dev,
-					"Failed to link %s->%s entities: %d\n",
-					sensor->name, input->name, ret);
+				camss_link_err(camss, sensor->name,
+					       input->name,
+					       ret);
 				return ret;
 			}
 		}
@@ -1518,7 +3460,7 @@ static int camss_configure_pd(struct camss *camss)
 
 	/* count the # of VFEs which have flagged power-domain */
 	for (vfepd_num = i = 0; i < camss->res->vfe_num; i++) {
-		if (res->vfe_res[i].has_pd)
+		if (res->vfe_res[i].vfe.has_pd)
 			vfepd_num++;
 	}
 
@@ -1537,10 +3479,8 @@ static int camss_configure_pd(struct camss *camss)
 	if (camss->res->pd_name) {
 		camss->genpd = dev_pm_domain_attach_by_name(camss->dev,
 							    camss->res->pd_name);
-		if (IS_ERR(camss->genpd)) {
-			ret = PTR_ERR(camss->genpd);
-			goto fail_pm;
-		}
+		if (IS_ERR(camss->genpd))
+			return PTR_ERR(camss->genpd);
 	}
 
 	if (!camss->genpd) {
@@ -1550,14 +3490,13 @@ static int camss_configure_pd(struct camss *camss)
 		 */
 		camss->genpd = dev_pm_domain_attach_by_id(camss->dev,
 							  camss->genpd_num - 1);
+		if (IS_ERR(camss->genpd))
+			return PTR_ERR(camss->genpd);
 	}
-	if (IS_ERR_OR_NULL(camss->genpd)) {
-		if (!camss->genpd)
-			ret = -ENODEV;
-		else
-			ret = PTR_ERR(camss->genpd);
-		goto fail_pm;
-	}
+
+	if (!camss->genpd)
+		return -ENODEV;
+
 	camss->genpd_link = device_link_add(camss->dev, camss->genpd,
 					    DL_FLAG_STATELESS | DL_FLAG_PM_RUNTIME |
 					    DL_FLAG_RPM_ACTIVE);
@@ -1646,6 +3585,7 @@ static int camss_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	if (camss->res->version == CAMSS_8x16 ||
+	    camss->res->version == CAMSS_8x53 ||
 	    camss->res->version == CAMSS_8x96) {
 		camss->ispif = devm_kcalloc(dev, 1, sizeof(*camss->ispif), GFP_KERNEL);
 		if (!camss->ispif)
@@ -1690,6 +3630,8 @@ static int camss_probe(struct platform_device *pdev)
 
 	v4l2_async_nf_init(&camss->notifier, &camss->v4l2_dev);
 
+	pm_runtime_enable(dev);
+
 	num_subdevs = camss_of_parse_ports(camss);
 	if (num_subdevs < 0) {
 		ret = num_subdevs;
@@ -1700,6 +3642,10 @@ static int camss_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_v4l2_device_unregister;
 
+	ret = camss->res->link_entities(camss);
+	if (ret < 0)
+		goto err_register_subdevs;
+
 	if (num_subdevs) {
 		camss->notifier.ops = &camss_subdev_notifier_ops;
 
@@ -1726,8 +3672,6 @@ static int camss_probe(struct platform_device *pdev)
 		}
 	}
 
-	pm_runtime_enable(dev);
-
 	return 0;
 
 err_register_subdevs:
@@ -1735,6 +3679,7 @@ err_register_subdevs:
 err_v4l2_device_unregister:
 	v4l2_device_unregister(&camss->v4l2_dev);
 	v4l2_async_nf_cleanup(&camss->notifier);
+	pm_runtime_disable(dev);
 err_genpd_cleanup:
 	camss_genpd_cleanup(camss);
 
@@ -1779,6 +3724,21 @@ static const struct camss_resources msm8916_resources = {
 	.csiphy_num = ARRAY_SIZE(csiphy_res_8x16),
 	.csid_num = ARRAY_SIZE(csid_res_8x16),
 	.vfe_num = ARRAY_SIZE(vfe_res_8x16),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources msm8953_resources = {
+	.version = CAMSS_8x53,
+	.icc_res = icc_res_8x53,
+	.icc_path_num = ARRAY_SIZE(icc_res_8x53),
+	.csiphy_res = csiphy_res_8x96,
+	.csid_res = csid_res_8x53,
+	.ispif_res = &ispif_res_8x53,
+	.vfe_res = vfe_res_8x53,
+	.csiphy_num = ARRAY_SIZE(csiphy_res_8x96),
+	.csid_num = ARRAY_SIZE(csid_res_8x53),
+	.vfe_num = ARRAY_SIZE(vfe_res_8x53),
+	.link_entities = camss_link_entities
 };
 
 static const struct camss_resources msm8996_resources = {
@@ -1790,6 +3750,7 @@ static const struct camss_resources msm8996_resources = {
 	.csiphy_num = ARRAY_SIZE(csiphy_res_8x96),
 	.csid_num = ARRAY_SIZE(csid_res_8x96),
 	.vfe_num = ARRAY_SIZE(vfe_res_8x96),
+	.link_entities = camss_link_entities
 };
 
 static const struct camss_resources sdm660_resources = {
@@ -1801,16 +3762,30 @@ static const struct camss_resources sdm660_resources = {
 	.csiphy_num = ARRAY_SIZE(csiphy_res_660),
 	.csid_num = ARRAY_SIZE(csid_res_660),
 	.vfe_num = ARRAY_SIZE(vfe_res_660),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources sdm670_resources = {
+	.version = CAMSS_845,
+	.csiphy_res = csiphy_res_670,
+	.csid_res = csid_res_670,
+	.vfe_res = vfe_res_670,
+	.csiphy_num = ARRAY_SIZE(csiphy_res_670),
+	.csid_num = ARRAY_SIZE(csid_res_670),
+	.vfe_num = ARRAY_SIZE(vfe_res_670),
+	.link_entities = camss_link_entities
 };
 
 static const struct camss_resources sdm845_resources = {
 	.version = CAMSS_845,
+	.pd_name = "top",
 	.csiphy_res = csiphy_res_845,
 	.csid_res = csid_res_845,
 	.vfe_res = vfe_res_845,
 	.csiphy_num = ARRAY_SIZE(csiphy_res_845),
 	.csid_num = ARRAY_SIZE(csid_res_845),
 	.vfe_num = ARRAY_SIZE(vfe_res_845),
+	.link_entities = camss_link_entities
 };
 
 static const struct camss_resources sm8250_resources = {
@@ -1824,14 +3799,80 @@ static const struct camss_resources sm8250_resources = {
 	.csiphy_num = ARRAY_SIZE(csiphy_res_8250),
 	.csid_num = ARRAY_SIZE(csid_res_8250),
 	.vfe_num = ARRAY_SIZE(vfe_res_8250),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources sc8280xp_resources = {
+	.version = CAMSS_8280XP,
+	.pd_name = "top",
+	.csiphy_res = csiphy_res_sc8280xp,
+	.csid_res = csid_res_sc8280xp,
+	.ispif_res = NULL,
+	.vfe_res = vfe_res_sc8280xp,
+	.icc_res = icc_res_sc8280xp,
+	.icc_path_num = ARRAY_SIZE(icc_res_sc8280xp),
+	.csiphy_num = ARRAY_SIZE(csiphy_res_sc8280xp),
+	.csid_num = ARRAY_SIZE(csid_res_sc8280xp),
+	.vfe_num = ARRAY_SIZE(vfe_res_sc8280xp),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources sc7280_resources = {
+	.version = CAMSS_7280,
+	.pd_name = "top",
+	.csiphy_res = csiphy_res_7280,
+	.csid_res = csid_res_7280,
+	.vfe_res = vfe_res_7280,
+	.icc_res = icc_res_sc7280,
+	.icc_path_num = ARRAY_SIZE(icc_res_sc7280),
+	.csiphy_num = ARRAY_SIZE(csiphy_res_7280),
+	.csid_num = ARRAY_SIZE(csid_res_7280),
+	.vfe_num = ARRAY_SIZE(vfe_res_7280),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources sm8550_resources = {
+	.version = CAMSS_8550,
+	.pd_name = "top",
+	.csiphy_res = csiphy_res_8550,
+	.csid_res = csid_res_8550,
+	.vfe_res = vfe_res_8550,
+	.csid_wrapper_res = &csid_wrapper_res_sm8550,
+	.icc_res = icc_res_sm8550,
+	.icc_path_num = ARRAY_SIZE(icc_res_sm8550),
+	.csiphy_num = ARRAY_SIZE(csiphy_res_8550),
+	.csid_num = ARRAY_SIZE(csid_res_8550),
+	.vfe_num = ARRAY_SIZE(vfe_res_8550),
+	.link_entities = camss_link_entities
+};
+
+static const struct camss_resources x1e80100_resources = {
+	.version = CAMSS_X1E80100,
+	.pd_name = "top",
+	.csiphy_res = csiphy_res_x1e80100,
+	.csid_res = csid_res_x1e80100,
+	.vfe_res = vfe_res_x1e80100,
+	.csid_wrapper_res = &csid_wrapper_res_x1e80100,
+	.icc_res = icc_res_x1e80100,
+	.icc_path_num = ARRAY_SIZE(icc_res_x1e80100),
+	.csiphy_num = ARRAY_SIZE(csiphy_res_x1e80100),
+	.csid_num = ARRAY_SIZE(csid_res_x1e80100),
+	.vfe_num = ARRAY_SIZE(vfe_res_x1e80100),
+	.link_entities = camss_link_entities
 };
 
 static const struct of_device_id camss_dt_match[] = {
 	{ .compatible = "qcom,msm8916-camss", .data = &msm8916_resources },
+	{ .compatible = "qcom,msm8953-camss", .data = &msm8953_resources },
 	{ .compatible = "qcom,msm8996-camss", .data = &msm8996_resources },
+	{ .compatible = "qcom,sc7280-camss", .data = &sc7280_resources },
+	{ .compatible = "qcom,sc8280xp-camss", .data = &sc8280xp_resources },
 	{ .compatible = "qcom,sdm660-camss", .data = &sdm660_resources },
+	{ .compatible = "qcom,sdm670-camss", .data = &sdm670_resources },
 	{ .compatible = "qcom,sdm845-camss", .data = &sdm845_resources },
 	{ .compatible = "qcom,sm8250-camss", .data = &sm8250_resources },
+	{ .compatible = "qcom,sm8550-camss", .data = &sm8550_resources },
+	{ .compatible = "qcom,x1e80100-camss", .data = &x1e80100_resources },
 	{ }
 };
 
@@ -1878,7 +3919,7 @@ static const struct dev_pm_ops camss_pm_ops = {
 
 static struct platform_driver qcom_camss_driver = {
 	.probe = camss_probe,
-	.remove_new = camss_remove,
+	.remove = camss_remove,
 	.driver = {
 		.name = "qcom-camss",
 		.of_match_table = camss_dt_match,
diff --git a/drivers/media/platform/qcom/camss/camss.h b/drivers/media/platform/qcom/camss/camss.h
index a0c2dcc779f0..63c0afee154a 100644
--- a/drivers/media/platform/qcom/camss/camss.h
+++ b/drivers/media/platform/qcom/camss/camss.h
@@ -22,6 +22,7 @@
 #include "camss-csiphy.h"
 #include "camss-ispif.h"
 #include "camss-vfe.h"
+#include "camss-format.h"
 
 #define to_camss(ptr_module)	\
 	container_of(ptr_module, struct camss, ptr_module)
@@ -48,11 +49,11 @@ struct camss_subdev_resources {
 	u32 clock_rate[CAMSS_RES_MAX][CAMSS_RES_MAX];
 	char *reg[CAMSS_RES_MAX];
 	char *interrupt[CAMSS_RES_MAX];
-	char *pd_name;
-	u8 line_num;
-	bool has_pd;
-	bool is_lite;
-	const void *ops;
+	union {
+		struct csiphy_subdev_resources csiphy;
+		struct csid_subdev_resources csid;
+		struct vfe_subdev_resources vfe;
+	};
 };
 
 struct icc_bw_tbl {
@@ -65,6 +66,10 @@ struct resources_icc {
 	struct icc_bw_tbl icc_bw_tbl;
 };
 
+struct resources_wrapper {
+	char *reg;
+};
+
 enum pm_domain {
 	PM_DOMAIN_VFE0 = 0,
 	PM_DOMAIN_VFE1 = 1,
@@ -72,11 +77,16 @@ enum pm_domain {
 };
 
 enum camss_version {
+	CAMSS_660,
+	CAMSS_7280,
 	CAMSS_8x16,
+	CAMSS_8x53,
 	CAMSS_8x96,
-	CAMSS_660,
-	CAMSS_845,
 	CAMSS_8250,
+	CAMSS_8280XP,
+	CAMSS_845,
+	CAMSS_8550,
+	CAMSS_X1E80100,
 };
 
 enum icc_count {
@@ -91,11 +101,13 @@ struct camss_resources {
 	const struct camss_subdev_resources *csid_res;
 	const struct camss_subdev_resources *ispif_res;
 	const struct camss_subdev_resources *vfe_res;
+	const struct resources_wrapper *csid_wrapper_res;
 	const struct resources_icc *icc_res;
 	const unsigned int icc_path_num;
 	const unsigned int csiphy_num;
 	const unsigned int csid_num;
 	const unsigned int vfe_num;
+	int (*link_entities)(struct camss *camss);
 };
 
 struct camss {
@@ -107,6 +119,7 @@ struct camss {
 	struct csid_device *csid;
 	struct ispif_device *ispif;
 	struct vfe_device *vfe;
+	void __iomem *csid_wrapper_base;
 	atomic_t ref_count;
 	int genpd_num;
 	struct device *genpd;
@@ -132,16 +145,27 @@ struct camss_clock {
 	u32 nfreqs;
 };
 
+struct parent_dev_ops {
+	int (*get)(struct camss *camss, int id);
+	int (*put)(struct camss *camss, int id);
+	void __iomem *(*get_base_address)(struct camss *camss, int id);
+};
+
 void camss_add_clock_margin(u64 *rate);
 int camss_enable_clocks(int nclocks, struct camss_clock *clock,
 			struct device *dev);
 void camss_disable_clocks(int nclocks, struct camss_clock *clock);
-struct media_entity *camss_find_sensor(struct media_entity *entity);
+struct media_pad *camss_find_sensor_pad(struct media_entity *entity);
 s64 camss_get_link_freq(struct media_entity *entity, unsigned int bpp,
 			unsigned int lanes);
 int camss_get_pixel_clock(struct media_entity *entity, u64 *pixel_clock);
 int camss_pm_domain_on(struct camss *camss, int id);
 void camss_pm_domain_off(struct camss *camss, int id);
+int camss_vfe_get(struct camss *camss, int id);
+void camss_vfe_put(struct camss *camss, int id);
 void camss_delete(struct camss *camss);
+void camss_buf_done(struct camss *camss, int hw_id, int port_id);
+void camss_reg_update(struct camss *camss, int hw_id,
+		      int port_id, bool is_clear);
 
 #endif /* QC_MSM_CAMSS_H */
diff --git a/drivers/media/platform/qcom/iris/Kconfig b/drivers/media/platform/qcom/iris/Kconfig
new file mode 100644
index 000000000000..3c803a05305a
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/Kconfig
@@ -0,0 +1,13 @@
+config VIDEO_QCOM_IRIS
+        tristate "Qualcomm iris V4L2 decoder driver"
+        depends on VIDEO_DEV
+        depends on ARCH_QCOM || COMPILE_TEST
+        select V4L2_MEM2MEM_DEV
+        select QCOM_MDT_LOADER if ARCH_QCOM
+        select QCOM_SCM
+        select VIDEOBUF2_DMA_CONTIG
+        help
+          This is a V4L2 driver for Qualcomm iris video accelerator
+          hardware. It accelerates decoding operations on various
+          Qualcomm SoCs.
+          To compile this driver as a module choose m here.
diff --git a/drivers/media/platform/qcom/iris/Makefile b/drivers/media/platform/qcom/iris/Makefile
new file mode 100644
index 000000000000..e86d00ee6f15
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/Makefile
@@ -0,0 +1,31 @@
+qcom-iris-objs += \
+	     iris_buffer.o \
+             iris_core.o \
+             iris_ctrls.o \
+             iris_firmware.o \
+             iris_hfi_common.o \
+             iris_hfi_gen1_command.o \
+             iris_hfi_gen1_response.o \
+             iris_hfi_gen2_command.o \
+             iris_hfi_gen2_packet.o \
+             iris_hfi_gen2_response.o \
+             iris_hfi_queue.o \
+             iris_platform_gen2.o \
+             iris_power.o \
+             iris_probe.o \
+             iris_resources.o \
+             iris_state.o \
+             iris_utils.o \
+             iris_vidc.o \
+             iris_vb2.o \
+             iris_vdec.o \
+             iris_vpu2.o \
+             iris_vpu3x.o \
+             iris_vpu_buffer.o \
+             iris_vpu_common.o \
+
+ifeq ($(CONFIG_VIDEO_QCOM_VENUS),)
+qcom-iris-objs += iris_platform_sm8250.o
+endif
+
+obj-$(CONFIG_VIDEO_QCOM_IRIS) += qcom-iris.o
diff --git a/drivers/media/platform/qcom/iris/iris_buffer.c b/drivers/media/platform/qcom/iris/iris_buffer.c
new file mode 100644
index 000000000000..e5c5a564fcb8
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_buffer.c
@@ -0,0 +1,623 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_buffer.h"
+#include "iris_instance.h"
+#include "iris_power.h"
+#include "iris_vpu_buffer.h"
+
+#define PIXELS_4K 4096
+#define MAX_WIDTH 4096
+#define MAX_HEIGHT 2304
+#define Y_STRIDE_ALIGN 128
+#define UV_STRIDE_ALIGN 128
+#define Y_SCANLINE_ALIGN 32
+#define UV_SCANLINE_ALIGN 16
+#define UV_SCANLINE_ALIGN_QC08C 32
+#define META_STRIDE_ALIGNED 64
+#define META_SCANLINE_ALIGNED 16
+#define NUM_MBS_4K (DIV_ROUND_UP(MAX_WIDTH, 16) * DIV_ROUND_UP(MAX_HEIGHT, 16))
+
+/*
+ * NV12:
+ * YUV 4:2:0 image with a plane of 8 bit Y samples followed
+ * by an interleaved U/V plane containing 8 bit 2x2 subsampled
+ * colour difference samples.
+ *
+ * <-Y/UV_Stride (aligned to 128)->
+ * <------- Width ------->
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  ^           ^
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  Height      |
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |          y_scanlines (aligned to 32)
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+ * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  V           |
+ * . . . . . . . . . . . . . . . .              |
+ * . . . . . . . . . . . . . . . .              |
+ * . . . . . . . . . . . . . . . .              |
+ * . . . . . . . . . . . . . . . .              V
+ * U V U V U V U V U V U V . . . .  ^
+ * U V U V U V U V U V U V . . . .  |
+ * U V U V U V U V U V U V . . . .  |
+ * U V U V U V U V U V U V . . . .  uv_scanlines (aligned to 16)
+ * . . . . . . . . . . . . . . . .  |
+ * . . . . . . . . . . . . . . . .  V
+ * . . . . . . . . . . . . . . . .  --> Buffer size aligned to 4K
+ *
+ * y_stride : Width aligned to 128
+ * uv_stride : Width aligned to 128
+ * y_scanlines: Height aligned to 32
+ * uv_scanlines: Height/2 aligned to 16
+ * Total size = align((y_stride * y_scanlines
+ *          + uv_stride * uv_scanlines , 4096)
+ *
+ * Note: All the alignments are hardware requirements.
+ */
+static u32 iris_yuv_buffer_size_nv12(struct iris_inst *inst)
+{
+	u32 y_plane, uv_plane, y_stride, uv_stride, y_scanlines, uv_scanlines;
+	struct v4l2_format *f = inst->fmt_dst;
+
+	y_stride = ALIGN(f->fmt.pix_mp.width, Y_STRIDE_ALIGN);
+	uv_stride = ALIGN(f->fmt.pix_mp.width, UV_STRIDE_ALIGN);
+	y_scanlines = ALIGN(f->fmt.pix_mp.height, Y_SCANLINE_ALIGN);
+	uv_scanlines = ALIGN((f->fmt.pix_mp.height + 1) >> 1, UV_SCANLINE_ALIGN);
+	y_plane = y_stride * y_scanlines;
+	uv_plane = uv_stride * uv_scanlines;
+
+	return ALIGN(y_plane + uv_plane, PIXELS_4K);
+}
+
+/*
+ * QC08C:
+ * Compressed Macro-tile format for NV12.
+ * Contains 4 planes in the following order -
+ * (A) Y_Meta_Plane
+ * (B) Y_UBWC_Plane
+ * (C) UV_Meta_Plane
+ * (D) UV_UBWC_Plane
+ *
+ * Y_Meta_Plane consists of meta information to decode compressed
+ * tile data in Y_UBWC_Plane.
+ * Y_UBWC_Plane consists of Y data in compressed macro-tile format.
+ * UBWC decoder block will use the Y_Meta_Plane data together with
+ * Y_UBWC_Plane data to produce loss-less uncompressed 8 bit Y samples.
+ *
+ * UV_Meta_Plane consists of meta information to decode compressed
+ * tile data in UV_UBWC_Plane.
+ * UV_UBWC_Plane consists of UV data in compressed macro-tile format.
+ * UBWC decoder block will use UV_Meta_Plane data together with
+ * UV_UBWC_Plane data to produce loss-less uncompressed 8 bit 2x2
+ * subsampled color difference samples.
+ *
+ * Each tile in Y_UBWC_Plane/UV_UBWC_Plane is independently decodable
+ * and randomly accessible. There is no dependency between tiles.
+ *
+ * <----- y_meta_stride ----> (aligned to 64)
+ * <-------- Width ------>
+ * M M M M M M M M M M M M . .      ^           ^
+ * M M M M M M M M M M M M . .      |           |
+ * M M M M M M M M M M M M . .      Height      |
+ * M M M M M M M M M M M M . .      |         y_meta_scanlines  (aligned to 16)
+ * M M M M M M M M M M M M . .      |           |
+ * M M M M M M M M M M M M . .      |           |
+ * M M M M M M M M M M M M . .      |           |
+ * M M M M M M M M M M M M . .      V           |
+ * . . . . . . . . . . . . . .                  |
+ * . . . . . . . . . . . . . .                  |
+ * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+ * . . . . . . . . . . . . . .                  V
+ * <--Compressed tile y_stride---> (aligned to 128)
+ * <------- Width ------->
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  ^           ^
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  Height      |
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |        Macro_tile y_scanlines (aligned to 32)
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+ * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  V           |
+ * . . . . . . . . . . . . . . . .              |
+ * . . . . . . . . . . . . . . . .              |
+ * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+ * . . . . . . . . . . . . . . . .              V
+ * <----- uv_meta_stride ---->  (aligned to 64)
+ * M M M M M M M M M M M M . .      ^
+ * M M M M M M M M M M M M . .      |
+ * M M M M M M M M M M M M . .      |
+ * M M M M M M M M M M M M . .      uv_meta_scanlines (aligned to 16)
+ * . . . . . . . . . . . . . .      |
+ * . . . . . . . . . . . . . .      V
+ * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+ * <--Compressed tile uv_stride---> (aligned to 128)
+ * U* V* U* V* U* V* U* V* . . . .  ^
+ * U* V* U* V* U* V* U* V* . . . .  |
+ * U* V* U* V* U* V* U* V* . . . .  |
+ * U* V* U* V* U* V* U* V* . . . .  uv_scanlines (aligned to 32)
+ * . . . . . . . . . . . . . . . .  |
+ * . . . . . . . . . . . . . . . .  V
+ * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+ *
+ * y_stride: width aligned to 128
+ * uv_stride: width aligned to 128
+ * y_scanlines: height aligned to 32
+ * uv_scanlines: height aligned to 32
+ * y_plane: buffer size aligned to 4096
+ * uv_plane: buffer size aligned to 4096
+ * y_meta_stride: width aligned to 64
+ * y_meta_scanlines: height aligned to 16
+ * y_meta_plane: buffer size aligned to 4096
+ * uv_meta_stride: width aligned to 64
+ * uv_meta_scanlines: height aligned to 16
+ * uv_meta_plane: buffer size aligned to 4096
+ *
+ * Total size = align( y_plane + uv_plane +
+ *           y_meta_plane + uv_meta_plane, 4096)
+ *
+ * Note: All the alignments are hardware requirements.
+ */
+static u32 iris_yuv_buffer_size_qc08c(struct iris_inst *inst)
+{
+	u32 y_plane, uv_plane, y_stride, uv_stride;
+	struct v4l2_format *f = inst->fmt_dst;
+	u32 uv_meta_stride, uv_meta_plane;
+	u32 y_meta_stride, y_meta_plane;
+
+	y_meta_stride = ALIGN(DIV_ROUND_UP(f->fmt.pix_mp.width, META_STRIDE_ALIGNED >> 1),
+			      META_STRIDE_ALIGNED);
+	y_meta_plane = y_meta_stride * ALIGN(DIV_ROUND_UP(f->fmt.pix_mp.height,
+							  META_SCANLINE_ALIGNED >> 1),
+					     META_SCANLINE_ALIGNED);
+	y_meta_plane = ALIGN(y_meta_plane, PIXELS_4K);
+
+	y_stride = ALIGN(f->fmt.pix_mp.width, Y_STRIDE_ALIGN);
+	y_plane = ALIGN(y_stride * ALIGN(f->fmt.pix_mp.height, Y_SCANLINE_ALIGN), PIXELS_4K);
+
+	uv_meta_stride = ALIGN(DIV_ROUND_UP(f->fmt.pix_mp.width / 2, META_STRIDE_ALIGNED >> 2),
+			       META_STRIDE_ALIGNED);
+	uv_meta_plane = uv_meta_stride * ALIGN(DIV_ROUND_UP(f->fmt.pix_mp.height / 2,
+							    META_SCANLINE_ALIGNED >> 1),
+					       META_SCANLINE_ALIGNED);
+	uv_meta_plane = ALIGN(uv_meta_plane, PIXELS_4K);
+
+	uv_stride = ALIGN(f->fmt.pix_mp.width, UV_STRIDE_ALIGN);
+	uv_plane = ALIGN(uv_stride * ALIGN(f->fmt.pix_mp.height / 2, UV_SCANLINE_ALIGN_QC08C),
+			 PIXELS_4K);
+
+	return ALIGN(y_meta_plane + y_plane + uv_meta_plane + uv_plane, PIXELS_4K);
+}
+
+static u32 iris_bitstream_buffer_size(struct iris_inst *inst)
+{
+	struct platform_inst_caps *caps = inst->core->iris_platform_data->inst_caps;
+	u32 base_res_mbs = NUM_MBS_4K;
+	u32 frame_size, num_mbs;
+	u32 div_factor = 2;
+
+	num_mbs = iris_get_mbpf(inst);
+	if (num_mbs > NUM_MBS_4K) {
+		div_factor = 4;
+		base_res_mbs = caps->max_mbpf;
+	}
+
+	/*
+	 * frame_size = YUVsize / div_factor
+	 * where YUVsize = resolution_in_MBs * MBs_in_pixel * 3 / 2
+	 */
+	frame_size = base_res_mbs * (16 * 16) * 3 / 2 / div_factor;
+
+	return ALIGN(frame_size, PIXELS_4K);
+}
+
+int iris_get_buffer_size(struct iris_inst *inst,
+			 enum iris_buffer_type buffer_type)
+{
+	switch (buffer_type) {
+	case BUF_INPUT:
+		return iris_bitstream_buffer_size(inst);
+	case BUF_OUTPUT:
+		return iris_yuv_buffer_size_nv12(inst);
+	case BUF_DPB:
+		return iris_yuv_buffer_size_qc08c(inst);
+	default:
+		return 0;
+	}
+}
+
+static void iris_fill_internal_buf_info(struct iris_inst *inst,
+					enum iris_buffer_type buffer_type)
+{
+	struct iris_buffers *buffers = &inst->buffers[buffer_type];
+
+	buffers->size = iris_vpu_buf_size(inst, buffer_type);
+	buffers->min_count = iris_vpu_buf_count(inst, buffer_type);
+}
+
+void iris_get_internal_buffers(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_platform_data *platform_data = inst->core->iris_platform_data;
+	const u32 *internal_buf_type;
+	u32 internal_buffer_count, i;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		internal_buf_type = platform_data->dec_ip_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_ip_int_buf_tbl_size;
+		for (i = 0; i < internal_buffer_count; i++)
+			iris_fill_internal_buf_info(inst, internal_buf_type[i]);
+	} else {
+		internal_buf_type = platform_data->dec_op_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_op_int_buf_tbl_size;
+		for (i = 0; i < internal_buffer_count; i++)
+			iris_fill_internal_buf_info(inst, internal_buf_type[i]);
+	}
+}
+
+static int iris_create_internal_buffer(struct iris_inst *inst,
+				       enum iris_buffer_type buffer_type, u32 index)
+{
+	struct iris_buffers *buffers = &inst->buffers[buffer_type];
+	struct iris_core *core = inst->core;
+	struct iris_buffer *buffer;
+
+	if (!buffers->size)
+		return 0;
+
+	buffer = kzalloc(sizeof(*buffer), GFP_KERNEL);
+	if (!buffer)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&buffer->list);
+	buffer->type = buffer_type;
+	buffer->index = index;
+	buffer->buffer_size = buffers->size;
+	buffer->dma_attrs = DMA_ATTR_WRITE_COMBINE | DMA_ATTR_NO_KERNEL_MAPPING;
+	list_add_tail(&buffer->list, &buffers->list);
+
+	buffer->kvaddr = dma_alloc_attrs(core->dev, buffer->buffer_size,
+					 &buffer->device_addr, GFP_KERNEL, buffer->dma_attrs);
+	if (!buffer->kvaddr)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int iris_create_internal_buffers(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_platform_data *platform_data = inst->core->iris_platform_data;
+	u32 internal_buffer_count, i, j;
+	struct iris_buffers *buffers;
+	const u32 *internal_buf_type;
+	int ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		internal_buf_type = platform_data->dec_ip_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_ip_int_buf_tbl_size;
+	} else {
+		internal_buf_type = platform_data->dec_op_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_op_int_buf_tbl_size;
+	}
+
+	for (i = 0; i < internal_buffer_count; i++) {
+		buffers = &inst->buffers[internal_buf_type[i]];
+		for (j = 0; j < buffers->min_count; j++) {
+			ret = iris_create_internal_buffer(inst, internal_buf_type[i], j);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int iris_queue_buffer(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	int ret;
+
+	ret = hfi_ops->session_queue_buf(inst, buf);
+	if (ret)
+		return ret;
+
+	buf->attr &= ~BUF_ATTR_DEFERRED;
+	buf->attr |= BUF_ATTR_QUEUED;
+
+	return 0;
+}
+
+int iris_queue_internal_buffers(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_platform_data *platform_data = inst->core->iris_platform_data;
+	struct iris_buffer *buffer, *next;
+	struct iris_buffers *buffers;
+	const u32 *internal_buf_type;
+	u32 internal_buffer_count, i;
+	int ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		internal_buf_type = platform_data->dec_ip_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_ip_int_buf_tbl_size;
+	} else {
+		internal_buf_type = platform_data->dec_op_int_buf_tbl;
+		internal_buffer_count = platform_data->dec_op_int_buf_tbl_size;
+	}
+
+	for (i = 0; i < internal_buffer_count; i++) {
+		buffers = &inst->buffers[internal_buf_type[i]];
+		list_for_each_entry_safe(buffer, next, &buffers->list, list) {
+			if (buffer->attr & BUF_ATTR_PENDING_RELEASE)
+				continue;
+			if (buffer->attr & BUF_ATTR_QUEUED)
+				continue;
+			ret = iris_queue_buffer(inst, buffer);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int iris_destroy_internal_buffer(struct iris_inst *inst, struct iris_buffer *buffer)
+{
+	struct iris_core *core = inst->core;
+
+	list_del(&buffer->list);
+	dma_free_attrs(core->dev, buffer->buffer_size, buffer->kvaddr,
+		       buffer->device_addr, buffer->dma_attrs);
+	kfree(buffer);
+
+	return 0;
+}
+
+int iris_destroy_internal_buffers(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_platform_data *platform_data = inst->core->iris_platform_data;
+	struct iris_buffer *buf, *next;
+	struct iris_buffers *buffers;
+	const u32 *internal_buf_type;
+	u32 i, len;
+	int ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		internal_buf_type = platform_data->dec_ip_int_buf_tbl;
+		len = platform_data->dec_ip_int_buf_tbl_size;
+	} else {
+		internal_buf_type = platform_data->dec_op_int_buf_tbl;
+		len = platform_data->dec_op_int_buf_tbl_size;
+	}
+
+	for (i = 0; i < len; i++) {
+		buffers = &inst->buffers[internal_buf_type[i]];
+		list_for_each_entry_safe(buf, next, &buffers->list, list) {
+			ret = iris_destroy_internal_buffer(inst, buf);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int iris_release_internal_buffers(struct iris_inst *inst,
+					 enum iris_buffer_type buffer_type)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	struct iris_buffers *buffers = &inst->buffers[buffer_type];
+	struct iris_buffer *buffer, *next;
+	int ret;
+
+	list_for_each_entry_safe(buffer, next, &buffers->list, list) {
+		if (buffer->attr & BUF_ATTR_PENDING_RELEASE)
+			continue;
+		if (!(buffer->attr & BUF_ATTR_QUEUED))
+			continue;
+		ret = hfi_ops->session_release_buf(inst, buffer);
+		if (ret)
+			return ret;
+		buffer->attr |= BUF_ATTR_PENDING_RELEASE;
+	}
+
+	return 0;
+}
+
+static int iris_release_input_internal_buffers(struct iris_inst *inst)
+{
+	const struct iris_platform_data *platform_data = inst->core->iris_platform_data;
+	const u32 *internal_buf_type;
+	u32 internal_buffer_count, i;
+	int ret;
+
+	internal_buf_type = platform_data->dec_ip_int_buf_tbl;
+	internal_buffer_count = platform_data->dec_ip_int_buf_tbl_size;
+
+	for (i = 0; i < internal_buffer_count; i++) {
+		ret = iris_release_internal_buffers(inst, internal_buf_type[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int iris_alloc_and_queue_persist_bufs(struct iris_inst *inst)
+{
+	struct iris_buffers *buffers = &inst->buffers[BUF_PERSIST];
+	struct iris_buffer *buffer, *next;
+	int ret;
+	u32 i;
+
+	if (!list_empty(&buffers->list))
+		return 0;
+
+	iris_fill_internal_buf_info(inst, BUF_PERSIST);
+
+	for (i = 0; i < buffers->min_count; i++) {
+		ret = iris_create_internal_buffer(inst, BUF_PERSIST, i);
+		if (ret)
+			return ret;
+	}
+
+	list_for_each_entry_safe(buffer, next, &buffers->list, list) {
+		if (buffer->attr & BUF_ATTR_PENDING_RELEASE)
+			continue;
+		if (buffer->attr & BUF_ATTR_QUEUED)
+			continue;
+		ret = iris_queue_buffer(inst, buffer);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+int iris_alloc_and_queue_input_int_bufs(struct iris_inst *inst)
+{
+	int ret;
+
+	iris_get_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+	ret = iris_release_input_internal_buffers(inst);
+	if (ret)
+		return ret;
+
+	ret = iris_create_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	return iris_queue_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+}
+
+int iris_queue_deferred_buffers(struct iris_inst *inst, enum iris_buffer_type buf_type)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buffer, *n;
+	struct iris_buffer *buf;
+	int ret;
+
+	iris_scale_power(inst);
+
+	if (buf_type == BUF_INPUT) {
+		v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (!(buf->attr & BUF_ATTR_DEFERRED))
+				continue;
+			ret = iris_queue_buffer(inst, buf);
+			if (ret)
+				return ret;
+		}
+	} else {
+		v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (!(buf->attr & BUF_ATTR_DEFERRED))
+				continue;
+			ret = iris_queue_buffer(inst, buf);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+void iris_vb2_queue_error(struct iris_inst *inst)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct vb2_queue *q;
+
+	q = v4l2_m2m_get_src_vq(m2m_ctx);
+	vb2_queue_error(q);
+	q = v4l2_m2m_get_dst_vq(m2m_ctx);
+	vb2_queue_error(q);
+}
+
+static struct vb2_v4l2_buffer *
+iris_helper_find_buf(struct iris_inst *inst, u32 type, u32 idx)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+
+	if (V4L2_TYPE_IS_OUTPUT(type))
+		return v4l2_m2m_src_buf_remove_by_idx(m2m_ctx, idx);
+	else
+		return v4l2_m2m_dst_buf_remove_by_idx(m2m_ctx, idx);
+}
+
+static void iris_get_ts_metadata(struct iris_inst *inst, u64 timestamp_ns,
+				 struct vb2_v4l2_buffer *vbuf)
+{
+	u32 mask = V4L2_BUF_FLAG_TIMECODE | V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(inst->tss); ++i) {
+		if (inst->tss[i].ts_ns != timestamp_ns)
+			continue;
+
+		vbuf->flags &= ~mask;
+		vbuf->flags |= inst->tss[i].flags;
+		vbuf->timecode = inst->tss[i].tc;
+		return;
+	}
+
+	vbuf->flags &= ~mask;
+	vbuf->flags |= inst->tss[inst->metadata_idx].flags;
+	vbuf->timecode = inst->tss[inst->metadata_idx].tc;
+}
+
+int iris_vb2_buffer_done(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct vb2_v4l2_buffer *vbuf;
+	struct vb2_buffer *vb2;
+	u32 type, state;
+
+	switch (buf->type) {
+	case BUF_INPUT:
+		type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+		break;
+	case BUF_OUTPUT:
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		break;
+	default:
+		return 0; /* Internal DPB Buffers */
+	}
+
+	vbuf = iris_helper_find_buf(inst, type, buf->index);
+	if (!vbuf)
+		return -EINVAL;
+
+	vb2 = &vbuf->vb2_buf;
+
+	if (buf->flags & V4L2_BUF_FLAG_ERROR)
+		state = VB2_BUF_STATE_ERROR;
+	else
+		state = VB2_BUF_STATE_DONE;
+
+	vbuf->flags |= buf->flags;
+
+	if (V4L2_TYPE_IS_CAPTURE(type)) {
+		vb2_set_plane_payload(vb2, 0, buf->data_size);
+		vbuf->sequence = inst->sequence_cap++;
+		iris_get_ts_metadata(inst, buf->timestamp, vbuf);
+	} else {
+		vbuf->sequence = inst->sequence_out++;
+	}
+
+	if (vbuf->flags & V4L2_BUF_FLAG_LAST) {
+		if (!v4l2_m2m_has_stopped(m2m_ctx)) {
+			const struct v4l2_event ev = { .type = V4L2_EVENT_EOS };
+
+			v4l2_event_queue_fh(&inst->fh, &ev);
+			v4l2_m2m_mark_stopped(m2m_ctx);
+		}
+	}
+	vb2->timestamp = buf->timestamp;
+	v4l2_m2m_buf_done(vbuf, state);
+
+	return 0;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_buffer.h b/drivers/media/platform/qcom/iris/iris_buffer.h
new file mode 100644
index 000000000000..c36b6347b077
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_buffer.h
@@ -0,0 +1,117 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_BUFFER_H__
+#define __IRIS_BUFFER_H__
+
+#include <media/videobuf2-v4l2.h>
+
+struct iris_inst;
+
+#define to_iris_buffer(ptr)	container_of(ptr, struct iris_buffer, vb2)
+
+/**
+ * enum iris_buffer_type
+ *
+ * @BUF_INPUT: input buffer to the iris hardware
+ * @BUF_OUTPUT: output buffer from the iris hardware
+ * @BUF_BIN: buffer to store intermediate bin data
+ * @BUF_ARP: buffer for auto register programming
+ * @BUF_COMV: buffer to store colocated motion vectors
+ * @BUF_NON_COMV: buffer to hold config data for HW
+ * @BUF_LINE: buffer to store decoding/encoding context data for HW
+ * @BUF_DPB: buffer to store display picture buffers for reference
+ * @BUF_PERSIST: buffer to store session context data
+ * @BUF_SCRATCH_1: buffer to store decoding/encoding context data for HW
+ * @BUF_TYPE_MAX: max buffer types
+ */
+enum iris_buffer_type {
+	BUF_INPUT = 1,
+	BUF_OUTPUT,
+	BUF_BIN,
+	BUF_ARP,
+	BUF_COMV,
+	BUF_NON_COMV,
+	BUF_LINE,
+	BUF_DPB,
+	BUF_PERSIST,
+	BUF_SCRATCH_1,
+	BUF_TYPE_MAX,
+};
+
+/*
+ * enum iris_buffer_attributes
+ *
+ * BUF_ATTR_DEFERRED: buffer queued by client but not submitted to firmware.
+ * BUF_ATTR_PENDING_RELEASE: buffers requested to be released from firmware.
+ * BUF_ATTR_QUEUED: buffers submitted to firmware.
+ * BUF_ATTR_DEQUEUED: buffers received from firmware.
+ * BUF_ATTR_BUFFER_DONE: buffers sent back to vb2.
+ */
+enum iris_buffer_attributes {
+	BUF_ATTR_DEFERRED		= BIT(0),
+	BUF_ATTR_PENDING_RELEASE	= BIT(1),
+	BUF_ATTR_QUEUED			= BIT(2),
+	BUF_ATTR_DEQUEUED		= BIT(3),
+	BUF_ATTR_BUFFER_DONE		= BIT(4),
+};
+
+/**
+ * struct iris_buffer
+ *
+ * @vb2: v4l2 vb2 buffer
+ * @list: list head for the iris_buffers structure
+ * @inst: iris instance structure
+ * @type: enum for type of iris buffer
+ * @index: identifier for the iris buffer
+ * @fd: file descriptor of the buffer
+ * @buffer_size: accessible buffer size in bytes starting from addr_offset
+ * @data_offset: accessible buffer offset from base address
+ * @data_size: data size in bytes
+ * @device_addr: device address of the buffer
+ * @kvaddr: kernel virtual address of the buffer
+ * @dma_attrs: dma attributes
+ * @flags: buffer flags. It is represented as bit masks.
+ * @timestamp: timestamp of the buffer in nano seconds (ns)
+ * @attr: enum for iris buffer attributes
+ */
+struct iris_buffer {
+	struct vb2_v4l2_buffer		vb2;
+	struct list_head		list;
+	struct iris_inst		*inst;
+	enum iris_buffer_type		type;
+	u32				index;
+	int				fd;
+	size_t				buffer_size;
+	u32				data_offset;
+	size_t				data_size;
+	dma_addr_t			device_addr;
+	void				*kvaddr;
+	unsigned long			dma_attrs;
+	u32				flags; /* V4L2_BUF_FLAG_* */
+	u64				timestamp;
+	enum iris_buffer_attributes	attr;
+};
+
+struct iris_buffers {
+	struct list_head	list;
+	u32			min_count;
+	u32			size;
+};
+
+int iris_get_buffer_size(struct iris_inst *inst, enum iris_buffer_type buffer_type);
+void iris_get_internal_buffers(struct iris_inst *inst, u32 plane);
+int iris_create_internal_buffers(struct iris_inst *inst, u32 plane);
+int iris_queue_internal_buffers(struct iris_inst *inst, u32 plane);
+int iris_destroy_internal_buffer(struct iris_inst *inst, struct iris_buffer *buffer);
+int iris_destroy_internal_buffers(struct iris_inst *inst, u32 plane);
+int iris_alloc_and_queue_persist_bufs(struct iris_inst *inst);
+int iris_alloc_and_queue_input_int_bufs(struct iris_inst *inst);
+int iris_queue_buffer(struct iris_inst *inst, struct iris_buffer *buf);
+int iris_queue_deferred_buffers(struct iris_inst *inst, enum iris_buffer_type buf_type);
+int iris_vb2_buffer_done(struct iris_inst *inst, struct iris_buffer *buf);
+void iris_vb2_queue_error(struct iris_inst *inst);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_core.c b/drivers/media/platform/qcom/iris/iris_core.c
new file mode 100644
index 000000000000..0fa0a3b549a2
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_core.c
@@ -0,0 +1,96 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_runtime.h>
+
+#include "iris_core.h"
+#include "iris_firmware.h"
+#include "iris_state.h"
+#include "iris_vpu_common.h"
+
+void iris_core_deinit(struct iris_core *core)
+{
+	pm_runtime_resume_and_get(core->dev);
+
+	mutex_lock(&core->lock);
+	iris_fw_unload(core);
+	iris_vpu_power_off(core);
+	iris_hfi_queues_deinit(core);
+	core->state = IRIS_CORE_DEINIT;
+	mutex_unlock(&core->lock);
+
+	pm_runtime_put_sync(core->dev);
+}
+
+static int iris_wait_for_system_response(struct iris_core *core)
+{
+	u32 hw_response_timeout_val = core->iris_platform_data->hw_response_timeout;
+	int ret;
+
+	if (core->state == IRIS_CORE_ERROR)
+		return -EIO;
+
+	ret = wait_for_completion_timeout(&core->core_init_done,
+					  msecs_to_jiffies(hw_response_timeout_val));
+	if (!ret) {
+		core->state = IRIS_CORE_ERROR;
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+int iris_core_init(struct iris_core *core)
+{
+	int ret;
+
+	mutex_lock(&core->lock);
+	if (core->state == IRIS_CORE_INIT) {
+		ret = 0;
+		goto exit;
+	} else if (core->state == IRIS_CORE_ERROR) {
+		ret = -EINVAL;
+		goto error;
+	}
+
+	core->state = IRIS_CORE_INIT;
+
+	ret = iris_hfi_queues_init(core);
+	if (ret)
+		goto error;
+
+	ret = iris_vpu_power_on(core);
+	if (ret)
+		goto error_queue_deinit;
+
+	ret = iris_fw_load(core);
+	if (ret)
+		goto error_power_off;
+
+	ret = iris_vpu_boot_firmware(core);
+	if (ret)
+		goto error_unload_fw;
+
+	ret = iris_hfi_core_init(core);
+	if (ret)
+		goto error_unload_fw;
+
+	mutex_unlock(&core->lock);
+
+	return iris_wait_for_system_response(core);
+
+error_unload_fw:
+	iris_fw_unload(core);
+error_power_off:
+	iris_vpu_power_off(core);
+error_queue_deinit:
+	iris_hfi_queues_deinit(core);
+error:
+	core->state = IRIS_CORE_DEINIT;
+exit:
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_core.h b/drivers/media/platform/qcom/iris/iris_core.h
new file mode 100644
index 000000000000..aeeac32a1f6d
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_core.h
@@ -0,0 +1,113 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_CORE_H__
+#define __IRIS_CORE_H__
+
+#include <linux/types.h>
+#include <linux/pm_domain.h>
+#include <media/v4l2-device.h>
+
+#include "iris_hfi_common.h"
+#include "iris_hfi_queue.h"
+#include "iris_platform_common.h"
+#include "iris_resources.h"
+#include "iris_state.h"
+
+struct icc_info {
+	const char		*name;
+	u32			bw_min_kbps;
+	u32			bw_max_kbps;
+};
+
+#define IRIS_FW_VERSION_LENGTH		128
+#define IFACEQ_CORE_PKT_SIZE		(1024 * 4)
+
+/**
+ * struct iris_core - holds core parameters valid for all instances
+ *
+ * @dev: reference to device structure
+ * @reg_base: IO memory base address
+ * @irq: iris irq
+ * @v4l2_dev: a holder for v4l2 device structure
+ * @vdev_dec: iris video device structure for decoder
+ * @iris_v4l2_file_ops: iris v4l2 file ops
+ * @iris_v4l2_ioctl_ops: iris v4l2 ioctl ops
+ * @iris_vb2_ops: iris vb2 ops
+ * @icc_tbl: table of iris interconnects
+ * @icc_count: count of iris interconnects
+ * @pmdomain_tbl: table of iris power domains
+ * @opp_pmdomain_tbl: table of opp power domains
+ * @clock_tbl: table of iris clocks
+ * @clk_count: count of iris clocks
+ * @resets: table of iris reset clocks
+ * @controller_resets: table of controller reset clocks
+ * @iris_platform_data: a structure for platform data
+ * @state: current state of core
+ * @iface_q_table_daddr: device address for interface queue table memory
+ * @sfr_daddr: device address for SFR (Sub System Failure Reason) register memory
+ * @iface_q_table_vaddr: virtual address for interface queue table memory
+ * @sfr_vaddr: virtual address for SFR (Sub System Failure Reason) register memory
+ * @command_queue: shared interface queue to send commands to firmware
+ * @message_queue: shared interface queue to receive responses from firmware
+ * @debug_queue: shared interface queue to receive debug info from firmware
+ * @lock: a lock for this strucure
+ * @response_packet: a pointer to response packet from fw to driver
+ * @header_id: id of packet header
+ * @packet_id: id of packet
+ * @power: a structure for clock and bw information
+ * @hfi_ops: iris hfi command ops
+ * @hfi_response_ops: iris hfi response ops
+ * @core_init_done: structure of signal completion for system response
+ * @intr_status: interrupt status
+ * @sys_error_handler: a delayed work for handling system fatal error
+ * @instances: a list_head of all instances
+ * @inst_fw_caps: an array of supported instance capabilities
+ */
+
+struct iris_core {
+	struct device				*dev;
+	void __iomem				*reg_base;
+	int					irq;
+	struct v4l2_device			v4l2_dev;
+	struct video_device			*vdev_dec;
+	const struct v4l2_file_operations	*iris_v4l2_file_ops;
+	const struct v4l2_ioctl_ops		*iris_v4l2_ioctl_ops;
+	const struct vb2_ops			*iris_vb2_ops;
+	struct icc_bulk_data			*icc_tbl;
+	u32					icc_count;
+	struct dev_pm_domain_list		*pmdomain_tbl;
+	struct dev_pm_domain_list		*opp_pmdomain_tbl;
+	struct clk_bulk_data			*clock_tbl;
+	u32					clk_count;
+	struct reset_control_bulk_data		*resets;
+	struct reset_control_bulk_data		*controller_resets;
+	const struct iris_platform_data		*iris_platform_data;
+	enum iris_core_state			state;
+	dma_addr_t				iface_q_table_daddr;
+	dma_addr_t				sfr_daddr;
+	void					*iface_q_table_vaddr;
+	void					*sfr_vaddr;
+	struct iris_iface_q_info		command_queue;
+	struct iris_iface_q_info		message_queue;
+	struct iris_iface_q_info		debug_queue;
+	struct mutex				lock; /* lock for core related operations */
+	u8					*response_packet;
+	u32					header_id;
+	u32					packet_id;
+	struct iris_core_power			power;
+	const struct iris_hfi_command_ops	*hfi_ops;
+	const struct iris_hfi_response_ops	*hfi_response_ops;
+	struct completion			core_init_done;
+	u32					intr_status;
+	struct delayed_work			sys_error_handler;
+	struct list_head			instances;
+	struct platform_inst_fw_cap		inst_fw_caps[INST_FW_CAP_MAX];
+};
+
+int iris_core_init(struct iris_core *core);
+void iris_core_deinit(struct iris_core *core);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_ctrls.c b/drivers/media/platform/qcom/iris/iris_ctrls.c
new file mode 100644
index 000000000000..b690578256d5
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_ctrls.c
@@ -0,0 +1,259 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/types.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_ctrls.h"
+#include "iris_instance.h"
+
+static inline bool iris_valid_cap_id(enum platform_inst_fw_cap_type cap_id)
+{
+	return cap_id >= 1 && cap_id < INST_FW_CAP_MAX;
+}
+
+static enum platform_inst_fw_cap_type iris_get_cap_id(u32 id)
+{
+	switch (id) {
+	case V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER:
+		return DEBLOCK;
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		return PROFILE;
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		return LEVEL;
+	default:
+		return INST_FW_CAP_MAX;
+	}
+}
+
+static u32 iris_get_v4l2_id(enum platform_inst_fw_cap_type cap_id)
+{
+	if (!iris_valid_cap_id(cap_id))
+		return 0;
+
+	switch (cap_id) {
+	case DEBLOCK:
+		return V4L2_CID_MPEG_VIDEO_DECODER_MPEG4_DEBLOCK_FILTER;
+	case PROFILE:
+		return V4L2_CID_MPEG_VIDEO_H264_PROFILE;
+	case LEVEL:
+		return V4L2_CID_MPEG_VIDEO_H264_LEVEL;
+	default:
+		return 0;
+	}
+}
+
+static int iris_vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct iris_inst *inst = container_of(ctrl->handler, struct iris_inst, ctrl_handler);
+	enum platform_inst_fw_cap_type cap_id;
+	struct platform_inst_fw_cap *cap;
+	struct vb2_queue *q;
+
+	cap = &inst->fw_caps[0];
+	cap_id = iris_get_cap_id(ctrl->id);
+	if (!iris_valid_cap_id(cap_id))
+		return -EINVAL;
+
+	q = v4l2_m2m_get_src_vq(inst->m2m_ctx);
+	if (vb2_is_streaming(q) &&
+	    (!(inst->fw_caps[cap_id].flags & CAP_FLAG_DYNAMIC_ALLOWED)))
+		return -EINVAL;
+
+	cap[cap_id].flags |= CAP_FLAG_CLIENT_SET;
+
+	inst->fw_caps[cap_id].value = ctrl->val;
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops iris_ctrl_ops = {
+	.s_ctrl = iris_vdec_op_s_ctrl,
+};
+
+int iris_ctrls_init(struct iris_inst *inst)
+{
+	struct platform_inst_fw_cap *cap = &inst->fw_caps[0];
+	u32 num_ctrls = 0, ctrl_idx = 0, idx = 0;
+	u32 v4l2_id;
+	int ret;
+
+	for (idx = 1; idx < INST_FW_CAP_MAX; idx++) {
+		if (iris_get_v4l2_id(cap[idx].cap_id))
+			num_ctrls++;
+	}
+	if (!num_ctrls)
+		return -EINVAL;
+
+	/* Adding 1 to num_ctrls to include V4L2_CID_MIN_BUFFERS_FOR_CAPTURE */
+
+	ret = v4l2_ctrl_handler_init(&inst->ctrl_handler, num_ctrls + 1);
+	if (ret)
+		return ret;
+
+	for (idx = 1; idx < INST_FW_CAP_MAX; idx++) {
+		struct v4l2_ctrl *ctrl;
+
+		v4l2_id = iris_get_v4l2_id(cap[idx].cap_id);
+		if (!v4l2_id)
+			continue;
+
+		if (ctrl_idx >= num_ctrls) {
+			ret = -EINVAL;
+			goto error;
+		}
+
+		if (cap[idx].flags & CAP_FLAG_MENU) {
+			ctrl = v4l2_ctrl_new_std_menu(&inst->ctrl_handler,
+						      &iris_ctrl_ops,
+						      v4l2_id,
+						      cap[idx].max,
+						      ~(cap[idx].step_or_mask),
+						      cap[idx].value);
+		} else {
+			ctrl = v4l2_ctrl_new_std(&inst->ctrl_handler,
+						 &iris_ctrl_ops,
+						 v4l2_id,
+						 cap[idx].min,
+						 cap[idx].max,
+						 cap[idx].step_or_mask,
+						 cap[idx].value);
+		}
+		if (!ctrl) {
+			ret = -EINVAL;
+			goto error;
+		}
+
+		ctrl_idx++;
+	}
+
+	v4l2_ctrl_new_std(&inst->ctrl_handler, NULL,
+			  V4L2_CID_MIN_BUFFERS_FOR_CAPTURE, 1, 32, 1, 4);
+
+	ret = inst->ctrl_handler.error;
+	if (ret)
+		goto error;
+
+	return 0;
+error:
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+
+	return ret;
+}
+
+void iris_session_init_caps(struct iris_core *core)
+{
+	struct platform_inst_fw_cap *caps;
+	u32 i, num_cap, cap_id;
+
+	caps = core->iris_platform_data->inst_fw_caps;
+	num_cap = core->iris_platform_data->inst_fw_caps_size;
+
+	for (i = 0; i < num_cap; i++) {
+		cap_id = caps[i].cap_id;
+		if (!iris_valid_cap_id(cap_id))
+			continue;
+
+		core->inst_fw_caps[cap_id].cap_id = caps[i].cap_id;
+		core->inst_fw_caps[cap_id].min = caps[i].min;
+		core->inst_fw_caps[cap_id].max = caps[i].max;
+		core->inst_fw_caps[cap_id].step_or_mask = caps[i].step_or_mask;
+		core->inst_fw_caps[cap_id].value = caps[i].value;
+		core->inst_fw_caps[cap_id].flags = caps[i].flags;
+		core->inst_fw_caps[cap_id].hfi_id = caps[i].hfi_id;
+	}
+}
+
+static u32 iris_get_port_info(struct iris_inst *inst,
+			      enum platform_inst_fw_cap_type cap_id)
+{
+	if (inst->fw_caps[cap_id].flags & CAP_FLAG_INPUT_PORT)
+		return HFI_PORT_BITSTREAM;
+	else if (inst->fw_caps[cap_id].flags & CAP_FLAG_OUTPUT_PORT)
+		return HFI_PORT_RAW;
+
+	return HFI_PORT_NONE;
+}
+
+int iris_set_u32_enum(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	u32 hfi_value = inst->fw_caps[cap_id].value;
+	u32 hfi_id = inst->fw_caps[cap_id].hfi_id;
+
+	return hfi_ops->session_set_property(inst, hfi_id,
+					     HFI_HOST_FLAGS_NONE,
+					     iris_get_port_info(inst, cap_id),
+					     HFI_PAYLOAD_U32_ENUM,
+					     &hfi_value, sizeof(u32));
+}
+
+int iris_set_u32(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	u32 hfi_value = inst->fw_caps[cap_id].value;
+	u32 hfi_id = inst->fw_caps[cap_id].hfi_id;
+
+	return hfi_ops->session_set_property(inst, hfi_id,
+					     HFI_HOST_FLAGS_NONE,
+					     iris_get_port_info(inst, cap_id),
+					     HFI_PAYLOAD_U32,
+					     &hfi_value, sizeof(u32));
+}
+
+int iris_set_stage(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	struct v4l2_format *inp_f = inst->fmt_src;
+	u32 hfi_id = inst->fw_caps[cap_id].hfi_id;
+	u32 height = inp_f->fmt.pix_mp.height;
+	u32 width = inp_f->fmt.pix_mp.width;
+	u32 work_mode = STAGE_2;
+
+	if (iris_res_is_less_than(width, height, 1280, 720))
+		work_mode = STAGE_1;
+
+	return hfi_ops->session_set_property(inst, hfi_id,
+					     HFI_HOST_FLAGS_NONE,
+					     iris_get_port_info(inst, cap_id),
+					     HFI_PAYLOAD_U32,
+					     &work_mode, sizeof(u32));
+}
+
+int iris_set_pipe(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	u32 work_route = inst->fw_caps[PIPE].value;
+	u32 hfi_id = inst->fw_caps[cap_id].hfi_id;
+
+	return hfi_ops->session_set_property(inst, hfi_id,
+					     HFI_HOST_FLAGS_NONE,
+					     iris_get_port_info(inst, cap_id),
+					     HFI_PAYLOAD_U32,
+					     &work_route, sizeof(u32));
+}
+
+int iris_set_properties(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	struct platform_inst_fw_cap *cap;
+	int ret;
+	u32 i;
+
+	ret = hfi_ops->session_set_config_params(inst, plane);
+	if (ret)
+		return ret;
+
+	for (i = 1; i < INST_FW_CAP_MAX; i++) {
+		cap = &inst->fw_caps[i];
+		if (!iris_valid_cap_id(cap->cap_id))
+			continue;
+
+		if (cap->cap_id && cap->set)
+			cap->set(inst, i);
+	}
+
+	return 0;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_ctrls.h b/drivers/media/platform/qcom/iris/iris_ctrls.h
new file mode 100644
index 000000000000..9b5741868933
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_ctrls.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_CTRLS_H__
+#define __IRIS_CTRLS_H__
+
+#include "iris_platform_common.h"
+
+struct iris_core;
+struct iris_inst;
+
+int iris_ctrls_init(struct iris_inst *inst);
+void iris_session_init_caps(struct iris_core *core);
+int iris_set_u32_enum(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id);
+int iris_set_stage(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id);
+int iris_set_pipe(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id);
+int iris_set_u32(struct iris_inst *inst, enum platform_inst_fw_cap_type cap_id);
+int iris_set_properties(struct iris_inst *inst, u32 plane);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_firmware.c b/drivers/media/platform/qcom/iris/iris_firmware.c
new file mode 100644
index 000000000000..f1b5cd56db32
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_firmware.c
@@ -0,0 +1,118 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/firmware.h>
+#include <linux/firmware/qcom/qcom_scm.h>
+#include <linux/of_address.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/soc/qcom/mdt_loader.h>
+
+#include "iris_core.h"
+#include "iris_firmware.h"
+
+#define MAX_FIRMWARE_NAME_SIZE	128
+
+static int iris_load_fw_to_memory(struct iris_core *core, const char *fw_name)
+{
+	u32 pas_id = core->iris_platform_data->pas_id;
+	const struct firmware *firmware = NULL;
+	struct device *dev = core->dev;
+	struct reserved_mem *rmem;
+	struct device_node *node;
+	phys_addr_t mem_phys;
+	size_t res_size;
+	ssize_t fw_size;
+	void *mem_virt;
+	int ret;
+
+	if (strlen(fw_name) >= MAX_FIRMWARE_NAME_SIZE - 4)
+		return -EINVAL;
+
+	node = of_parse_phandle(dev->of_node, "memory-region", 0);
+	if (!node)
+		return -EINVAL;
+
+	rmem = of_reserved_mem_lookup(node);
+	of_node_put(node);
+	if (!rmem)
+		return -EINVAL;
+
+	mem_phys = rmem->base;
+	res_size = rmem->size;
+
+	ret = request_firmware(&firmware, fw_name, dev);
+	if (ret)
+		return ret;
+
+	fw_size = qcom_mdt_get_size(firmware);
+	if (fw_size < 0 || res_size < (size_t)fw_size) {
+		ret = -EINVAL;
+		goto err_release_fw;
+	}
+
+	mem_virt = memremap(mem_phys, res_size, MEMREMAP_WC);
+	if (!mem_virt) {
+		ret = -ENOMEM;
+		goto err_release_fw;
+	}
+
+	ret = qcom_mdt_load(dev, firmware, fw_name,
+			    pas_id, mem_virt, mem_phys, res_size, NULL);
+	if (ret)
+		goto err_mem_unmap;
+
+	ret = qcom_scm_pas_auth_and_reset(pas_id);
+	if (ret)
+		goto err_mem_unmap;
+
+	return ret;
+
+err_mem_unmap:
+	memunmap(mem_virt);
+err_release_fw:
+	release_firmware(firmware);
+
+	return ret;
+}
+
+int iris_fw_load(struct iris_core *core)
+{
+	struct tz_cp_config *cp_config = core->iris_platform_data->tz_cp_config_data;
+	const char *fwpath = NULL;
+	int ret;
+
+	ret = of_property_read_string_index(core->dev->of_node, "firmware-name", 0,
+					    &fwpath);
+	if (ret)
+		fwpath = core->iris_platform_data->fwname;
+
+	ret = iris_load_fw_to_memory(core, fwpath);
+	if (ret) {
+		dev_err(core->dev, "firmware download failed\n");
+		return -ENOMEM;
+	}
+
+	ret = qcom_scm_mem_protect_video_var(cp_config->cp_start,
+					     cp_config->cp_size,
+					     cp_config->cp_nonpixel_start,
+					     cp_config->cp_nonpixel_size);
+	if (ret) {
+		dev_err(core->dev, "protect memory failed\n");
+		qcom_scm_pas_shutdown(core->iris_platform_data->pas_id);
+		return ret;
+	}
+
+	return ret;
+}
+
+int iris_fw_unload(struct iris_core *core)
+{
+	return qcom_scm_pas_shutdown(core->iris_platform_data->pas_id);
+}
+
+int iris_set_hw_state(struct iris_core *core, bool resume)
+{
+	return qcom_scm_set_remote_state(resume, 0);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_firmware.h b/drivers/media/platform/qcom/iris/iris_firmware.h
new file mode 100644
index 000000000000..e833ecd34887
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_firmware.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_FIRMWARE_H__
+#define __IRIS_FIRMWARE_H__
+
+struct iris_core;
+
+int iris_fw_load(struct iris_core *core);
+int iris_fw_unload(struct iris_core *core);
+int iris_set_hw_state(struct iris_core *core, bool resume);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_common.c b/drivers/media/platform/qcom/iris/iris_hfi_common.c
new file mode 100644
index 000000000000..92112eb16c11
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_common.c
@@ -0,0 +1,176 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_runtime.h>
+
+#include "iris_firmware.h"
+#include "iris_core.h"
+#include "iris_hfi_common.h"
+#include "iris_vpu_common.h"
+
+u32 iris_hfi_get_v4l2_color_primaries(u32 hfi_primaries)
+{
+	switch (hfi_primaries) {
+	case HFI_PRIMARIES_RESERVED:
+		return V4L2_COLORSPACE_DEFAULT;
+	case HFI_PRIMARIES_BT709:
+		return V4L2_COLORSPACE_REC709;
+	case HFI_PRIMARIES_BT470_SYSTEM_M:
+		return V4L2_COLORSPACE_470_SYSTEM_M;
+	case HFI_PRIMARIES_BT470_SYSTEM_BG:
+		return V4L2_COLORSPACE_470_SYSTEM_BG;
+	case HFI_PRIMARIES_BT601_525:
+		return V4L2_COLORSPACE_SMPTE170M;
+	case HFI_PRIMARIES_SMPTE_ST240M:
+		return V4L2_COLORSPACE_SMPTE240M;
+	case HFI_PRIMARIES_BT2020:
+		return V4L2_COLORSPACE_BT2020;
+	case V4L2_COLORSPACE_DCI_P3:
+		return HFI_PRIMARIES_SMPTE_RP431_2;
+	default:
+		return V4L2_COLORSPACE_DEFAULT;
+	}
+}
+
+u32 iris_hfi_get_v4l2_transfer_char(u32 hfi_characterstics)
+{
+	switch (hfi_characterstics) {
+	case HFI_TRANSFER_RESERVED:
+		return V4L2_XFER_FUNC_DEFAULT;
+	case HFI_TRANSFER_BT709:
+		return V4L2_XFER_FUNC_709;
+	case HFI_TRANSFER_SMPTE_ST240M:
+		return V4L2_XFER_FUNC_SMPTE240M;
+	case HFI_TRANSFER_SRGB_SYCC:
+		return V4L2_XFER_FUNC_SRGB;
+	case HFI_TRANSFER_SMPTE_ST2084_PQ:
+		return V4L2_XFER_FUNC_SMPTE2084;
+	default:
+		return V4L2_XFER_FUNC_DEFAULT;
+	}
+}
+
+u32 iris_hfi_get_v4l2_matrix_coefficients(u32 hfi_coefficients)
+{
+	switch (hfi_coefficients) {
+	case HFI_MATRIX_COEFF_RESERVED:
+		return V4L2_YCBCR_ENC_DEFAULT;
+	case HFI_MATRIX_COEFF_BT709:
+		return V4L2_YCBCR_ENC_709;
+	case HFI_MATRIX_COEFF_BT470_SYS_BG_OR_BT601_625:
+		return V4L2_YCBCR_ENC_XV601;
+	case HFI_MATRIX_COEFF_BT601_525_BT1358_525_OR_625:
+		return V4L2_YCBCR_ENC_601;
+	case HFI_MATRIX_COEFF_SMPTE_ST240:
+		return V4L2_YCBCR_ENC_SMPTE240M;
+	case HFI_MATRIX_COEFF_BT2020_NON_CONSTANT:
+		return V4L2_YCBCR_ENC_BT2020;
+	case HFI_MATRIX_COEFF_BT2020_CONSTANT:
+		return V4L2_YCBCR_ENC_BT2020_CONST_LUM;
+	default:
+		return V4L2_YCBCR_ENC_DEFAULT;
+	}
+}
+
+int iris_hfi_core_init(struct iris_core *core)
+{
+	const struct iris_hfi_command_ops *hfi_ops = core->hfi_ops;
+	int ret;
+
+	ret = hfi_ops->sys_init(core);
+	if (ret)
+		return ret;
+
+	ret = hfi_ops->sys_image_version(core);
+	if (ret)
+		return ret;
+
+	return hfi_ops->sys_interframe_powercollapse(core);
+}
+
+irqreturn_t iris_hfi_isr(int irq, void *data)
+{
+	disable_irq_nosync(irq);
+
+	return IRQ_WAKE_THREAD;
+}
+
+irqreturn_t iris_hfi_isr_handler(int irq, void *data)
+{
+	struct iris_core *core = data;
+
+	if (!core)
+		return IRQ_NONE;
+
+	mutex_lock(&core->lock);
+	pm_runtime_mark_last_busy(core->dev);
+	iris_vpu_clear_interrupt(core);
+	mutex_unlock(&core->lock);
+
+	core->hfi_response_ops->hfi_response_handler(core);
+
+	if (!iris_vpu_watchdog(core, core->intr_status))
+		enable_irq(irq);
+
+	return IRQ_HANDLED;
+}
+
+int iris_hfi_pm_suspend(struct iris_core *core)
+{
+	int ret;
+
+	ret = iris_vpu_prepare_pc(core);
+	if (ret) {
+		pm_runtime_mark_last_busy(core->dev);
+		ret = -EAGAIN;
+		goto error;
+	}
+
+	ret = iris_set_hw_state(core, false);
+	if (ret)
+		goto error;
+
+	iris_vpu_power_off(core);
+
+	return 0;
+
+error:
+	dev_err(core->dev, "failed to suspend\n");
+
+	return ret;
+}
+
+int iris_hfi_pm_resume(struct iris_core *core)
+{
+	const struct iris_hfi_command_ops *ops = core->hfi_ops;
+	int ret;
+
+	ret = iris_vpu_power_on(core);
+	if (ret)
+		goto error;
+
+	ret = iris_set_hw_state(core, true);
+	if (ret)
+		goto err_power_off;
+
+	ret = iris_vpu_boot_firmware(core);
+	if (ret)
+		goto err_suspend_hw;
+
+	ret = ops->sys_interframe_powercollapse(core);
+	if (ret)
+		goto err_suspend_hw;
+
+	return 0;
+
+err_suspend_hw:
+	iris_set_hw_state(core, false);
+err_power_off:
+	iris_vpu_power_off(core);
+error:
+	dev_err(core->dev, "failed to resume\n");
+
+	return -EBUSY;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_common.h b/drivers/media/platform/qcom/iris/iris_hfi_common.h
new file mode 100644
index 000000000000..b2c541367fc6
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_common.h
@@ -0,0 +1,155 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_COMMON_H__
+#define __IRIS_HFI_COMMON_H__
+
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+
+#include "iris_buffer.h"
+
+struct iris_inst;
+struct iris_core;
+
+enum hfi_packet_port_type {
+	HFI_PORT_NONE		= 0x00000000,
+	HFI_PORT_BITSTREAM	= 0x00000001,
+	HFI_PORT_RAW		= 0x00000002,
+};
+
+enum hfi_packet_payload_info {
+	HFI_PAYLOAD_NONE	= 0x00000000,
+	HFI_PAYLOAD_U32		= 0x00000001,
+	HFI_PAYLOAD_S32		= 0x00000002,
+	HFI_PAYLOAD_U64		= 0x00000003,
+	HFI_PAYLOAD_S64		= 0x00000004,
+	HFI_PAYLOAD_STRUCTURE	= 0x00000005,
+	HFI_PAYLOAD_BLOB	= 0x00000006,
+	HFI_PAYLOAD_STRING	= 0x00000007,
+	HFI_PAYLOAD_Q16		= 0x00000008,
+	HFI_PAYLOAD_U32_ENUM	= 0x00000009,
+	HFI_PAYLOAD_32_PACKED	= 0x0000000a,
+	HFI_PAYLOAD_U32_ARRAY	= 0x0000000b,
+	HFI_PAYLOAD_S32_ARRAY	= 0x0000000c,
+	HFI_PAYLOAD_64_PACKED	= 0x0000000d,
+};
+
+enum hfi_packet_host_flags {
+	HFI_HOST_FLAGS_NONE			= 0x00000000,
+	HFI_HOST_FLAGS_INTR_REQUIRED		= 0x00000001,
+	HFI_HOST_FLAGS_RESPONSE_REQUIRED	= 0x00000002,
+	HFI_HOST_FLAGS_NON_DISCARDABLE		= 0x00000004,
+	HFI_HOST_FLAGS_GET_PROPERTY		= 0x00000008,
+};
+
+enum hfi_color_primaries {
+	HFI_PRIMARIES_RESERVED		= 0,
+	HFI_PRIMARIES_BT709		= 1,
+	HFI_PRIMARIES_UNSPECIFIED	= 2,
+	HFI_PRIMARIES_BT470_SYSTEM_M	= 4,
+	HFI_PRIMARIES_BT470_SYSTEM_BG	= 5,
+	HFI_PRIMARIES_BT601_525		= 6,
+	HFI_PRIMARIES_SMPTE_ST240M	= 7,
+	HFI_PRIMARIES_GENERIC_FILM	= 8,
+	HFI_PRIMARIES_BT2020		= 9,
+	HFI_PRIMARIES_SMPTE_ST428_1	= 10,
+	HFI_PRIMARIES_SMPTE_RP431_2	= 11,
+	HFI_PRIMARIES_SMPTE_EG431_1	= 12,
+	HFI_PRIMARIES_SMPTE_EBU_TECH	= 22,
+};
+
+enum hfi_transfer_characteristics {
+	HFI_TRANSFER_RESERVED		= 0,
+	HFI_TRANSFER_BT709		= 1,
+	HFI_TRANSFER_UNSPECIFIED	= 2,
+	HFI_TRANSFER_BT470_SYSTEM_M	= 4,
+	HFI_TRANSFER_BT470_SYSTEM_BG	= 5,
+	HFI_TRANSFER_BT601_525_OR_625	= 6,
+	HFI_TRANSFER_SMPTE_ST240M	= 7,
+	HFI_TRANSFER_LINEAR		= 8,
+	HFI_TRANSFER_LOG_100_1		= 9,
+	HFI_TRANSFER_LOG_SQRT		= 10,
+	HFI_TRANSFER_XVYCC		= 11,
+	HFI_TRANSFER_BT1361_0		= 12,
+	HFI_TRANSFER_SRGB_SYCC		= 13,
+	HFI_TRANSFER_BT2020_14		= 14,
+	HFI_TRANSFER_BT2020_15		= 15,
+	HFI_TRANSFER_SMPTE_ST2084_PQ	= 16,
+	HFI_TRANSFER_SMPTE_ST428_1	= 17,
+	HFI_TRANSFER_BT2100_2_HLG	= 18,
+};
+
+enum hfi_matrix_coefficients {
+	HFI_MATRIX_COEFF_SRGB_SMPTE_ST428_1		= 0,
+	HFI_MATRIX_COEFF_BT709				= 1,
+	HFI_MATRIX_COEFF_UNSPECIFIED			= 2,
+	HFI_MATRIX_COEFF_RESERVED			= 3,
+	HFI_MATRIX_COEFF_FCC_TITLE_47			= 4,
+	HFI_MATRIX_COEFF_BT470_SYS_BG_OR_BT601_625	= 5,
+	HFI_MATRIX_COEFF_BT601_525_BT1358_525_OR_625	= 6,
+	HFI_MATRIX_COEFF_SMPTE_ST240			= 7,
+	HFI_MATRIX_COEFF_YCGCO				= 8,
+	HFI_MATRIX_COEFF_BT2020_NON_CONSTANT		= 9,
+	HFI_MATRIX_COEFF_BT2020_CONSTANT		= 10,
+	HFI_MATRIX_COEFF_SMPTE_ST2085			= 11,
+	HFI_MATRIX_COEFF_SMPTE_CHROM_DERV_NON_CONSTANT	= 12,
+	HFI_MATRIX_COEFF_SMPTE_CHROM_DERV_CONSTANT	= 13,
+	HFI_MATRIX_COEFF_BT2100				= 14,
+};
+
+struct iris_hfi_prop_type_handle {
+	u32 type;
+	int (*handle)(struct iris_inst *inst);
+};
+
+struct iris_hfi_command_ops {
+	int (*sys_init)(struct iris_core *core);
+	int (*sys_image_version)(struct iris_core *core);
+	int (*sys_interframe_powercollapse)(struct iris_core *core);
+	int (*sys_pc_prep)(struct iris_core *core);
+	int (*session_set_config_params)(struct iris_inst *inst, u32 plane);
+	int (*session_set_property)(struct iris_inst *inst,
+				    u32 packet_type, u32 flag, u32 plane, u32 payload_type,
+				    void *payload, u32 payload_size);
+	int (*session_open)(struct iris_inst *inst);
+	int (*session_start)(struct iris_inst *inst, u32 plane);
+	int (*session_queue_buf)(struct iris_inst *inst, struct iris_buffer *buffer);
+	int (*session_release_buf)(struct iris_inst *inst, struct iris_buffer *buffer);
+	int (*session_pause)(struct iris_inst *inst, u32 plane);
+	int (*session_resume_drc)(struct iris_inst *inst, u32 plane);
+	int (*session_stop)(struct iris_inst *inst, u32 plane);
+	int (*session_drain)(struct iris_inst *inst, u32 plane);
+	int (*session_resume_drain)(struct iris_inst *inst, u32 plane);
+	int (*session_close)(struct iris_inst *inst);
+};
+
+struct iris_hfi_response_ops {
+	void (*hfi_response_handler)(struct iris_core *core);
+};
+
+struct hfi_subscription_params {
+	u32	bitstream_resolution;
+	u32	crop_offsets[2];
+	u32	bit_depth;
+	u32	coded_frames;
+	u32	fw_min_count;
+	u32	pic_order_cnt;
+	u32	color_info;
+	u32	profile;
+	u32	level;
+};
+
+u32 iris_hfi_get_v4l2_color_primaries(u32 hfi_primaries);
+u32 iris_hfi_get_v4l2_transfer_char(u32 hfi_characterstics);
+u32 iris_hfi_get_v4l2_matrix_coefficients(u32 hfi_coefficients);
+int iris_hfi_core_init(struct iris_core *core);
+int iris_hfi_pm_suspend(struct iris_core *core);
+int iris_hfi_pm_resume(struct iris_core *core);
+
+irqreturn_t iris_hfi_isr(int irq, void *data);
+irqreturn_t iris_hfi_isr_handler(int irq, void *data);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen1.h b/drivers/media/platform/qcom/iris/iris_hfi_gen1.h
new file mode 100644
index 000000000000..19b8e9054a75
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen1.h
@@ -0,0 +1,16 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_GEN1_H__
+#define __IRIS_HFI_GEN1_H__
+
+struct iris_core;
+struct iris_inst;
+
+void iris_hfi_gen1_command_ops_init(struct iris_core *core);
+void iris_hfi_gen1_response_ops_init(struct iris_core *core);
+struct iris_inst *iris_hfi_gen1_get_instance(void);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen1_command.c b/drivers/media/platform/qcom/iris/iris_hfi_gen1_command.c
new file mode 100644
index 000000000000..64f887d9a17d
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen1_command.c
@@ -0,0 +1,826 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_hfi_gen1.h"
+#include "iris_hfi_gen1_defines.h"
+#include "iris_instance.h"
+#include "iris_vpu_buffer.h"
+
+static u32 iris_hfi_gen1_buf_type_from_driver(enum iris_buffer_type buffer_type)
+{
+	switch (buffer_type) {
+	case BUF_INPUT:
+		return HFI_BUFFER_INPUT;
+	case BUF_OUTPUT:
+		return HFI_BUFFER_OUTPUT;
+	case BUF_PERSIST:
+		return HFI_BUFFER_INTERNAL_PERSIST_1;
+	case BUF_BIN:
+		return HFI_BUFFER_INTERNAL_SCRATCH;
+	case BUF_SCRATCH_1:
+		return HFI_BUFFER_INTERNAL_SCRATCH_1;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int iris_hfi_gen1_sys_init(struct iris_core *core)
+{
+	struct hfi_sys_init_pkt sys_init_pkt;
+
+	sys_init_pkt.hdr.size = sizeof(sys_init_pkt);
+	sys_init_pkt.hdr.pkt_type = HFI_CMD_SYS_INIT;
+	sys_init_pkt.arch_type = HFI_VIDEO_ARCH_OX;
+
+	return iris_hfi_queue_cmd_write_locked(core, &sys_init_pkt, sys_init_pkt.hdr.size);
+}
+
+static int iris_hfi_gen1_sys_image_version(struct iris_core *core)
+{
+	struct hfi_sys_get_property_pkt packet;
+
+	packet.hdr.size = sizeof(packet);
+	packet.hdr.pkt_type = HFI_CMD_SYS_GET_PROPERTY;
+	packet.num_properties = 1;
+	packet.data = HFI_PROPERTY_SYS_IMAGE_VERSION;
+
+	return iris_hfi_queue_cmd_write_locked(core, &packet, packet.hdr.size);
+}
+
+static int iris_hfi_gen1_sys_interframe_powercollapse(struct iris_core *core)
+{
+	struct hfi_sys_set_property_pkt *pkt;
+	struct hfi_enable *hfi;
+	u32 packet_size;
+	int ret;
+
+	packet_size = struct_size(pkt, data, 1) + sizeof(*hfi);
+	pkt = kzalloc(packet_size, GFP_KERNEL);
+	if (!pkt)
+		return -ENOMEM;
+
+	hfi = (struct hfi_enable *)&pkt->data[1];
+
+	pkt->hdr.size = packet_size;
+	pkt->hdr.pkt_type = HFI_CMD_SYS_SET_PROPERTY;
+	pkt->num_properties = 1;
+	pkt->data[0] = HFI_PROPERTY_SYS_CODEC_POWER_PLANE_CTRL;
+	hfi->enable = true;
+
+	ret = iris_hfi_queue_cmd_write_locked(core, pkt, pkt->hdr.size);
+	kfree(pkt);
+
+	return ret;
+}
+
+static int iris_hfi_gen1_sys_pc_prep(struct iris_core *core)
+{
+	struct hfi_sys_pc_prep_pkt pkt;
+
+	pkt.hdr.size = sizeof(struct hfi_sys_pc_prep_pkt);
+	pkt.hdr.pkt_type = HFI_CMD_SYS_PC_PREP;
+
+	return iris_hfi_queue_cmd_write_locked(core, &pkt, pkt.hdr.size);
+}
+
+static int iris_hfi_gen1_session_open(struct iris_inst *inst)
+{
+	struct hfi_session_open_pkt packet;
+	int ret;
+
+	if (inst->state != IRIS_INST_DEINIT)
+		return -EALREADY;
+
+	packet.shdr.hdr.size = sizeof(struct hfi_session_open_pkt);
+	packet.shdr.hdr.pkt_type = HFI_CMD_SYS_SESSION_INIT;
+	packet.shdr.session_id = inst->session_id;
+	packet.session_domain = HFI_SESSION_TYPE_DEC;
+	packet.session_codec = HFI_VIDEO_CODEC_H264;
+
+	reinit_completion(&inst->completion);
+
+	ret = iris_hfi_queue_cmd_write(inst->core, &packet, packet.shdr.hdr.size);
+	if (ret)
+		return ret;
+
+	return iris_wait_for_session_response(inst, false);
+}
+
+static void iris_hfi_gen1_packet_session_cmd(struct iris_inst *inst,
+					     struct hfi_session_pkt *packet,
+					     u32 ptype)
+{
+	packet->shdr.hdr.size = sizeof(*packet);
+	packet->shdr.hdr.pkt_type = ptype;
+	packet->shdr.session_id = inst->session_id;
+}
+
+static int iris_hfi_gen1_session_close(struct iris_inst *inst)
+{
+	struct hfi_session_pkt packet;
+
+	iris_hfi_gen1_packet_session_cmd(inst, &packet, HFI_CMD_SYS_SESSION_END);
+
+	return iris_hfi_queue_cmd_write(inst->core, &packet, packet.shdr.hdr.size);
+}
+
+static int iris_hfi_gen1_session_start(struct iris_inst *inst, u32 plane)
+{
+	struct iris_core *core = inst->core;
+	struct hfi_session_pkt packet;
+	int ret;
+
+	if (!V4L2_TYPE_IS_OUTPUT(plane))
+		return 0;
+
+	if (inst->sub_state & IRIS_INST_SUB_LOAD_RESOURCES)
+		return 0;
+
+	reinit_completion(&inst->completion);
+	iris_hfi_gen1_packet_session_cmd(inst, &packet, HFI_CMD_SESSION_LOAD_RESOURCES);
+
+	ret = iris_hfi_queue_cmd_write(core, &packet, packet.shdr.hdr.size);
+	if (ret)
+		return ret;
+
+	ret = iris_wait_for_session_response(inst, false);
+	if (ret)
+		return ret;
+
+	reinit_completion(&inst->completion);
+	iris_hfi_gen1_packet_session_cmd(inst, &packet, HFI_CMD_SESSION_START);
+
+	ret = iris_hfi_queue_cmd_write(core, &packet, packet.shdr.hdr.size);
+	if (ret)
+		return ret;
+
+	ret = iris_wait_for_session_response(inst, false);
+	if (ret)
+		return ret;
+
+	return iris_inst_change_sub_state(inst, 0, IRIS_INST_SUB_LOAD_RESOURCES);
+}
+
+static int iris_hfi_gen1_session_stop(struct iris_inst *inst, u32 plane)
+{
+	struct hfi_session_flush_pkt flush_pkt;
+	struct iris_core *core = inst->core;
+	struct hfi_session_pkt pkt;
+	u32 flush_type = 0;
+	int ret = 0;
+
+	if ((V4L2_TYPE_IS_OUTPUT(plane) &&
+	     inst->state == IRIS_INST_INPUT_STREAMING) ||
+	    (V4L2_TYPE_IS_CAPTURE(plane) &&
+	     inst->state == IRIS_INST_OUTPUT_STREAMING) ||
+	    inst->state == IRIS_INST_ERROR) {
+		reinit_completion(&inst->completion);
+		iris_hfi_gen1_packet_session_cmd(inst, &pkt, HFI_CMD_SESSION_STOP);
+		ret = iris_hfi_queue_cmd_write(core, &pkt, pkt.shdr.hdr.size);
+		if (!ret)
+			ret = iris_wait_for_session_response(inst, false);
+
+		reinit_completion(&inst->completion);
+		iris_hfi_gen1_packet_session_cmd(inst, &pkt, HFI_CMD_SESSION_RELEASE_RESOURCES);
+		ret = iris_hfi_queue_cmd_write(core, &pkt, pkt.shdr.hdr.size);
+		if (!ret)
+			ret = iris_wait_for_session_response(inst, false);
+
+		iris_inst_change_sub_state(inst, IRIS_INST_SUB_LOAD_RESOURCES, 0);
+
+		iris_helper_buffers_done(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+					 VB2_BUF_STATE_ERROR);
+		iris_helper_buffers_done(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+					 VB2_BUF_STATE_ERROR);
+	} else if (inst->state == IRIS_INST_STREAMING) {
+		if (V4L2_TYPE_IS_OUTPUT(plane))
+			flush_type = HFI_FLUSH_ALL;
+		else if (V4L2_TYPE_IS_CAPTURE(plane))
+			flush_type = HFI_FLUSH_OUTPUT;
+
+		reinit_completion(&inst->flush_completion);
+
+		flush_pkt.shdr.hdr.size = sizeof(struct hfi_session_flush_pkt);
+		flush_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_FLUSH;
+		flush_pkt.shdr.session_id = inst->session_id;
+		flush_pkt.flush_type = flush_type;
+
+		ret = iris_hfi_queue_cmd_write(core, &flush_pkt, flush_pkt.shdr.hdr.size);
+		if (!ret)
+			ret = iris_wait_for_session_response(inst, true);
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen1_session_continue(struct iris_inst *inst, u32 plane)
+{
+	struct hfi_session_pkt packet;
+
+	iris_hfi_gen1_packet_session_cmd(inst, &packet, HFI_CMD_SESSION_CONTINUE);
+
+	return iris_hfi_queue_cmd_write(inst->core, &packet, packet.shdr.hdr.size);
+}
+
+static int iris_hfi_gen1_queue_input_buffer(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	struct hfi_session_empty_buffer_compressed_pkt ip_pkt;
+
+	ip_pkt.shdr.hdr.size = sizeof(struct hfi_session_empty_buffer_compressed_pkt);
+	ip_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_EMPTY_BUFFER;
+	ip_pkt.shdr.session_id = inst->session_id;
+	ip_pkt.time_stamp_hi = upper_32_bits(buf->timestamp);
+	ip_pkt.time_stamp_lo = lower_32_bits(buf->timestamp);
+	ip_pkt.flags = buf->flags;
+	ip_pkt.mark_target = 0;
+	ip_pkt.mark_data = 0;
+	ip_pkt.offset = buf->data_offset;
+	ip_pkt.alloc_len = buf->buffer_size;
+	ip_pkt.filled_len = buf->data_size;
+	ip_pkt.input_tag = buf->index;
+	ip_pkt.packet_buffer = buf->device_addr;
+
+	return iris_hfi_queue_cmd_write(inst->core, &ip_pkt, ip_pkt.shdr.hdr.size);
+}
+
+static int iris_hfi_gen1_queue_output_buffer(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	struct hfi_session_fill_buffer_pkt op_pkt;
+
+	op_pkt.shdr.hdr.size = sizeof(struct hfi_session_fill_buffer_pkt);
+	op_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_FILL_BUFFER;
+	op_pkt.shdr.session_id = inst->session_id;
+	op_pkt.output_tag = buf->index;
+	op_pkt.packet_buffer = buf->device_addr;
+	op_pkt.extradata_buffer = 0;
+	op_pkt.alloc_len = buf->buffer_size;
+	op_pkt.filled_len = buf->data_size;
+	op_pkt.offset = buf->data_offset;
+	op_pkt.data = 0;
+
+	if (buf->type == BUF_OUTPUT && iris_split_mode_enabled(inst))
+		op_pkt.stream_id = 1;
+	else
+		op_pkt.stream_id = 0;
+
+	return iris_hfi_queue_cmd_write(inst->core, &op_pkt, op_pkt.shdr.hdr.size);
+}
+
+static int iris_hfi_gen1_queue_internal_buffer(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	struct hfi_session_set_buffers_pkt *int_pkt;
+	u32 buffer_type, i;
+	u32 packet_size;
+	int ret;
+
+	packet_size = struct_size(int_pkt, buffer_info, 1);
+	int_pkt = kzalloc(packet_size, GFP_KERNEL);
+	if (!int_pkt)
+		return -ENOMEM;
+
+	int_pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_SET_BUFFERS;
+	int_pkt->shdr.session_id = inst->session_id;
+	int_pkt->buffer_size = buf->buffer_size;
+	int_pkt->min_buffer_size = buf->buffer_size;
+	int_pkt->num_buffers = 1;
+	int_pkt->extradata_size = 0;
+	int_pkt->shdr.hdr.size = packet_size;
+	for (i = 0; i < int_pkt->num_buffers; i++)
+		int_pkt->buffer_info[i] = buf->device_addr;
+	buffer_type = iris_hfi_gen1_buf_type_from_driver(buf->type);
+	if (buffer_type == -EINVAL) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	int_pkt->buffer_type = buffer_type;
+	ret = iris_hfi_queue_cmd_write(inst->core, int_pkt, int_pkt->shdr.hdr.size);
+
+exit:
+	kfree(int_pkt);
+
+	return ret;
+}
+
+static int iris_hfi_gen1_session_queue_buffer(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	switch (buf->type) {
+	case BUF_INPUT:
+		return iris_hfi_gen1_queue_input_buffer(inst, buf);
+	case BUF_OUTPUT:
+	case BUF_DPB:
+		return iris_hfi_gen1_queue_output_buffer(inst, buf);
+	case BUF_PERSIST:
+	case BUF_BIN:
+	case BUF_SCRATCH_1:
+		return iris_hfi_gen1_queue_internal_buffer(inst, buf);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int iris_hfi_gen1_session_unset_buffers(struct iris_inst *inst, struct iris_buffer *buf)
+{
+	struct hfi_session_release_buffer_pkt *pkt;
+	u32 packet_size, buffer_type, i;
+	int ret;
+
+	buffer_type = iris_hfi_gen1_buf_type_from_driver(buf->type);
+	if (buffer_type == -EINVAL)
+		return -EINVAL;
+
+	if (buffer_type == HFI_BUFFER_INPUT)
+		return 0;
+
+	packet_size = sizeof(*pkt) + sizeof(struct hfi_buffer_info);
+	pkt = kzalloc(packet_size, GFP_KERNEL);
+	if (!pkt)
+		return -ENOMEM;
+
+	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_RELEASE_BUFFERS;
+	pkt->shdr.session_id = inst->session_id;
+	pkt->buffer_size = buf->buffer_size;
+	pkt->num_buffers = 1;
+
+	if (buffer_type == HFI_BUFFER_OUTPUT ||
+	    buffer_type == HFI_BUFFER_OUTPUT2) {
+		struct hfi_buffer_info *bi;
+
+		bi = (struct hfi_buffer_info *)pkt->buffer_info;
+		for (i = 0; i < pkt->num_buffers; i++) {
+			bi->buffer_addr = buf->device_addr;
+			bi->extradata_addr = 0;
+		}
+		pkt->shdr.hdr.size = packet_size;
+	} else {
+		for (i = 0; i < pkt->num_buffers; i++)
+			pkt->buffer_info[i] = buf->device_addr;
+		pkt->extradata_size = 0;
+		pkt->shdr.hdr.size =
+				sizeof(struct hfi_session_set_buffers_pkt) +
+				((pkt->num_buffers) * sizeof(u32));
+	}
+
+	pkt->response_req = true;
+	pkt->buffer_type = buffer_type;
+
+	ret = iris_hfi_queue_cmd_write(inst->core, pkt, pkt->shdr.hdr.size);
+	if (ret)
+		goto exit;
+
+	ret = iris_wait_for_session_response(inst, false);
+
+exit:
+	kfree(pkt);
+
+	return ret;
+}
+
+static int iris_hfi_gen1_session_drain(struct iris_inst *inst, u32 plane)
+{
+	struct hfi_session_empty_buffer_compressed_pkt ip_pkt = {0};
+
+	ip_pkt.shdr.hdr.size = sizeof(struct hfi_session_empty_buffer_compressed_pkt);
+	ip_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_EMPTY_BUFFER;
+	ip_pkt.shdr.session_id = inst->session_id;
+	ip_pkt.flags = HFI_BUFFERFLAG_EOS;
+
+	return iris_hfi_queue_cmd_write(inst->core, &ip_pkt, ip_pkt.shdr.hdr.size);
+}
+
+static int
+iris_hfi_gen1_packet_session_set_property(struct hfi_session_set_property_pkt *packet,
+					  struct iris_inst *inst, u32 ptype, void *pdata)
+{
+	void *prop_data = &packet->data[1];
+
+	packet->shdr.hdr.size = sizeof(*packet);
+	packet->shdr.hdr.pkt_type = HFI_CMD_SESSION_SET_PROPERTY;
+	packet->shdr.session_id = inst->session_id;
+	packet->num_properties = 1;
+	packet->data[0] = ptype;
+
+	switch (ptype) {
+	case HFI_PROPERTY_PARAM_FRAME_SIZE: {
+		struct hfi_framesize *in = pdata, *fsize = prop_data;
+
+		fsize->buffer_type = in->buffer_type;
+		fsize->height = in->height;
+		fsize->width = in->width;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*fsize);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE: {
+		struct hfi_videocores_usage_type *in = pdata, *cu = prop_data;
+
+		cu->video_core_enable_mask = in->video_core_enable_mask;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*cu);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT: {
+		struct hfi_uncompressed_format_select *in = pdata;
+		struct hfi_uncompressed_format_select *hfi = prop_data;
+
+		hfi->buffer_type = in->buffer_type;
+		hfi->format = in->format;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*hfi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO: {
+		struct hfi_uncompressed_plane_actual_constraints_info *info = prop_data;
+
+		info->buffer_type = HFI_BUFFER_OUTPUT2;
+		info->num_planes = 2;
+		info->plane_format[0].stride_multiples = 128;
+		info->plane_format[0].max_stride = 8192;
+		info->plane_format[0].min_plane_buffer_height_multiple = 32;
+		info->plane_format[0].buffer_alignment = 256;
+		if (info->num_planes > 1) {
+			info->plane_format[1].stride_multiples = 128;
+			info->plane_format[1].max_stride = 8192;
+			info->plane_format[1].min_plane_buffer_height_multiple = 16;
+			info->plane_format[1].buffer_alignment = 256;
+		}
+
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*info);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL: {
+		struct hfi_buffer_count_actual *in = pdata;
+		struct hfi_buffer_count_actual *count = prop_data;
+
+		count->type = in->type;
+		count->count_actual = in->count_actual;
+		count->count_min_host = in->count_min_host;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*count);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM: {
+		struct hfi_multi_stream *in = pdata;
+		struct hfi_multi_stream *multi = prop_data;
+
+		multi->buffer_type = in->buffer_type;
+		multi->enable = in->enable;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*multi);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL: {
+		struct hfi_buffer_size_actual *in = pdata, *sz = prop_data;
+
+		sz->size = in->size;
+		sz->type = in->type;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*sz);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_WORK_ROUTE: {
+		struct hfi_video_work_route *wr = prop_data;
+		u32 *in = pdata;
+
+		wr->video_work_route = *in;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*wr);
+		break;
+	}
+	case HFI_PROPERTY_PARAM_WORK_MODE: {
+		struct hfi_video_work_mode *wm = prop_data;
+		u32 *in = pdata;
+
+		wm->video_work_mode = *in;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*wm);
+		break;
+	}
+	case HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER: {
+		struct hfi_enable *en = prop_data;
+		u32 *in = pdata;
+
+		en->enable = *in;
+		packet->shdr.hdr.size += sizeof(u32) + sizeof(*en);
+		break;
+	}
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int hfi_gen1_set_property(struct iris_inst *inst, u32 packet_type,
+				 void *payload, u32 payload_size)
+{
+	struct hfi_session_set_property_pkt *pkt;
+	u32 packet_size;
+	int ret;
+
+	packet_size = sizeof(*pkt) + sizeof(u32) + payload_size;
+	pkt = kzalloc(packet_size, GFP_KERNEL);
+	if (!pkt)
+		return -ENOMEM;
+
+	ret = iris_hfi_gen1_packet_session_set_property(pkt, inst, packet_type, payload);
+	if (ret == -EOPNOTSUPP) {
+		ret = 0;
+		goto exit;
+	}
+	if (ret)
+		goto exit;
+
+	ret = iris_hfi_queue_cmd_write(inst->core, pkt, pkt->shdr.hdr.size);
+
+exit:
+	kfree(pkt);
+
+	return ret;
+}
+
+static int iris_hfi_gen1_session_set_property(struct iris_inst *inst, u32 packet_type,
+					      u32 flag, u32 plane, u32 payload_type,
+					      void *payload, u32 payload_size)
+{
+	return hfi_gen1_set_property(inst, packet_type, payload, payload_size);
+}
+
+static int iris_hfi_gen1_set_resolution(struct iris_inst *inst)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_FRAME_SIZE;
+	struct hfi_framesize fs;
+	int ret;
+
+	fs.buffer_type = HFI_BUFFER_INPUT;
+	fs.width = inst->fmt_src->fmt.pix_mp.width;
+	fs.height = inst->fmt_src->fmt.pix_mp.height;
+
+	ret = hfi_gen1_set_property(inst, ptype, &fs, sizeof(fs));
+	if (ret)
+		return ret;
+
+	fs.buffer_type = HFI_BUFFER_OUTPUT2;
+	fs.width = inst->fmt_dst->fmt.pix_mp.width;
+	fs.height = inst->fmt_dst->fmt.pix_mp.height;
+
+	return hfi_gen1_set_property(inst, ptype, &fs, sizeof(fs));
+}
+
+static int iris_hfi_gen1_decide_core(struct iris_inst *inst)
+{
+	const u32 ptype = HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE;
+	struct hfi_videocores_usage_type cu;
+
+	cu.video_core_enable_mask = HFI_CORE_ID_1;
+
+	return hfi_gen1_set_property(inst, ptype, &cu, sizeof(cu));
+}
+
+static int iris_hfi_gen1_set_raw_format(struct iris_inst *inst)
+{
+	const u32 ptype = HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT;
+	u32 pixelformat = inst->fmt_dst->fmt.pix_mp.pixelformat;
+	struct hfi_uncompressed_format_select fmt;
+	int ret;
+
+	if (iris_split_mode_enabled(inst)) {
+		fmt.buffer_type = HFI_BUFFER_OUTPUT;
+		fmt.format = pixelformat == V4L2_PIX_FMT_NV12 ? HFI_COLOR_FORMAT_NV12_UBWC : 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &fmt, sizeof(fmt));
+		if (ret)
+			return ret;
+
+		fmt.buffer_type = HFI_BUFFER_OUTPUT2;
+		fmt.format = pixelformat == V4L2_PIX_FMT_NV12 ? HFI_COLOR_FORMAT_NV12 : 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &fmt, sizeof(fmt));
+	} else {
+		fmt.buffer_type = HFI_BUFFER_OUTPUT;
+		fmt.format = pixelformat == V4L2_PIX_FMT_NV12 ? HFI_COLOR_FORMAT_NV12 : 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &fmt, sizeof(fmt));
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen1_set_format_constraints(struct iris_inst *inst)
+{
+	const u32 ptype = HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO;
+	struct hfi_uncompressed_plane_actual_constraints_info pconstraint;
+
+	pconstraint.buffer_type = HFI_BUFFER_OUTPUT2;
+	pconstraint.num_planes = 2;
+	pconstraint.plane_format[0].stride_multiples = 128;
+	pconstraint.plane_format[0].max_stride = 8192;
+	pconstraint.plane_format[0].min_plane_buffer_height_multiple = 32;
+	pconstraint.plane_format[0].buffer_alignment = 256;
+
+	pconstraint.plane_format[1].stride_multiples = 128;
+	pconstraint.plane_format[1].max_stride = 8192;
+	pconstraint.plane_format[1].min_plane_buffer_height_multiple = 16;
+	pconstraint.plane_format[1].buffer_alignment = 256;
+
+	return hfi_gen1_set_property(inst, ptype, &pconstraint, sizeof(pconstraint));
+}
+
+static int iris_hfi_gen1_set_num_bufs(struct iris_inst *inst)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL;
+	struct hfi_buffer_count_actual buf_count;
+	int ret;
+
+	buf_count.type = HFI_BUFFER_INPUT;
+	buf_count.count_actual = VIDEO_MAX_FRAME;
+	buf_count.count_min_host = VIDEO_MAX_FRAME;
+
+	ret = hfi_gen1_set_property(inst, ptype, &buf_count, sizeof(buf_count));
+	if (ret)
+		return ret;
+
+	if (iris_split_mode_enabled(inst)) {
+		buf_count.type = HFI_BUFFER_OUTPUT;
+		buf_count.count_actual = VIDEO_MAX_FRAME;
+		buf_count.count_min_host = VIDEO_MAX_FRAME;
+
+		ret = hfi_gen1_set_property(inst, ptype, &buf_count, sizeof(buf_count));
+		if (ret)
+			return ret;
+
+		buf_count.type = HFI_BUFFER_OUTPUT2;
+		buf_count.count_actual = iris_vpu_buf_count(inst, BUF_DPB);
+		buf_count.count_min_host = iris_vpu_buf_count(inst, BUF_DPB);
+
+		ret = hfi_gen1_set_property(inst, ptype, &buf_count, sizeof(buf_count));
+	} else {
+		buf_count.type = HFI_BUFFER_OUTPUT;
+		buf_count.count_actual = VIDEO_MAX_FRAME;
+		buf_count.count_min_host = VIDEO_MAX_FRAME;
+
+		ret = hfi_gen1_set_property(inst, ptype, &buf_count, sizeof(buf_count));
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen1_set_multistream(struct iris_inst *inst)
+{
+	u32 ptype = HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM;
+	struct hfi_multi_stream multi = {0};
+	int ret;
+
+	if (iris_split_mode_enabled(inst)) {
+		multi.buffer_type = HFI_BUFFER_OUTPUT;
+		multi.enable = 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &multi, sizeof(multi));
+		if (ret)
+			return ret;
+
+		multi.buffer_type = HFI_BUFFER_OUTPUT2;
+		multi.enable = 1;
+
+		ret = hfi_gen1_set_property(inst, ptype, &multi, sizeof(multi));
+	} else {
+		multi.buffer_type = HFI_BUFFER_OUTPUT;
+		multi.enable = 1;
+
+		ret = hfi_gen1_set_property(inst, ptype, &multi, sizeof(multi));
+		if (ret)
+			return ret;
+
+		multi.buffer_type = HFI_BUFFER_OUTPUT2;
+		multi.enable = 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &multi, sizeof(multi));
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen1_set_bufsize(struct iris_inst *inst)
+{
+	const u32 ptype = HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL;
+	struct hfi_buffer_size_actual bufsz;
+	int ret;
+
+	if (iris_split_mode_enabled(inst)) {
+		bufsz.type = HFI_BUFFER_OUTPUT;
+		bufsz.size = iris_vpu_buf_size(inst, BUF_DPB);
+
+		ret = hfi_gen1_set_property(inst, ptype, &bufsz, sizeof(bufsz));
+		if (ret)
+			return ret;
+
+		bufsz.type = HFI_BUFFER_OUTPUT2;
+		bufsz.size = inst->buffers[BUF_OUTPUT].size;
+
+		ret = hfi_gen1_set_property(inst, ptype, &bufsz, sizeof(bufsz));
+	} else {
+		bufsz.type = HFI_BUFFER_OUTPUT;
+		bufsz.size = inst->buffers[BUF_OUTPUT].size;
+
+		ret = hfi_gen1_set_property(inst, ptype, &bufsz, sizeof(bufsz));
+		if (ret)
+			return ret;
+
+		bufsz.type = HFI_BUFFER_OUTPUT2;
+		bufsz.size = 0;
+
+		ret = hfi_gen1_set_property(inst, ptype, &bufsz, sizeof(bufsz));
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen1_session_set_config_params(struct iris_inst *inst, u32 plane)
+{
+	struct iris_core *core = inst->core;
+	u32 config_params_size, i, j;
+	const u32 *config_params;
+	int ret;
+
+	static const struct iris_hfi_prop_type_handle prop_type_handle_inp_arr[] = {
+		{HFI_PROPERTY_PARAM_FRAME_SIZE,
+			iris_hfi_gen1_set_resolution},
+		{HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE,
+			iris_hfi_gen1_decide_core},
+		{HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT,
+			iris_hfi_gen1_set_raw_format},
+		{HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO,
+			iris_hfi_gen1_set_format_constraints},
+		{HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL,
+			iris_hfi_gen1_set_num_bufs},
+		{HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM,
+			iris_hfi_gen1_set_multistream},
+		{HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL,
+			iris_hfi_gen1_set_bufsize},
+	};
+
+	static const struct iris_hfi_prop_type_handle prop_type_handle_out_arr[] = {
+		{HFI_PROPERTY_PARAM_FRAME_SIZE,
+			iris_hfi_gen1_set_resolution},
+		{HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT,
+			iris_hfi_gen1_set_raw_format},
+		{HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO,
+			iris_hfi_gen1_set_format_constraints},
+		{HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL,
+			iris_hfi_gen1_set_num_bufs},
+		{HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM,
+			iris_hfi_gen1_set_multistream},
+		{HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL,
+			iris_hfi_gen1_set_bufsize},
+	};
+
+	config_params = core->iris_platform_data->input_config_params;
+	config_params_size = core->iris_platform_data->input_config_params_size;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		for (i = 0; i < config_params_size; i++) {
+			for (j = 0; j < ARRAY_SIZE(prop_type_handle_inp_arr); j++) {
+				if (prop_type_handle_inp_arr[j].type == config_params[i]) {
+					ret = prop_type_handle_inp_arr[j].handle(inst);
+					if (ret)
+						return ret;
+					break;
+				}
+			}
+		}
+	} else if (V4L2_TYPE_IS_CAPTURE(plane)) {
+		for (i = 0; i < config_params_size; i++) {
+			for (j = 0; j < ARRAY_SIZE(prop_type_handle_out_arr); j++) {
+				if (prop_type_handle_out_arr[j].type == config_params[i]) {
+					ret = prop_type_handle_out_arr[j].handle(inst);
+					if (ret)
+						return ret;
+					break;
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+static const struct iris_hfi_command_ops iris_hfi_gen1_command_ops = {
+	.sys_init = iris_hfi_gen1_sys_init,
+	.sys_image_version = iris_hfi_gen1_sys_image_version,
+	.sys_interframe_powercollapse = iris_hfi_gen1_sys_interframe_powercollapse,
+	.sys_pc_prep = iris_hfi_gen1_sys_pc_prep,
+	.session_open = iris_hfi_gen1_session_open,
+	.session_set_config_params = iris_hfi_gen1_session_set_config_params,
+	.session_set_property = iris_hfi_gen1_session_set_property,
+	.session_start = iris_hfi_gen1_session_start,
+	.session_queue_buf = iris_hfi_gen1_session_queue_buffer,
+	.session_release_buf = iris_hfi_gen1_session_unset_buffers,
+	.session_resume_drc = iris_hfi_gen1_session_continue,
+	.session_stop = iris_hfi_gen1_session_stop,
+	.session_drain = iris_hfi_gen1_session_drain,
+	.session_close = iris_hfi_gen1_session_close,
+};
+
+void iris_hfi_gen1_command_ops_init(struct iris_core *core)
+{
+	core->hfi_ops = &iris_hfi_gen1_command_ops;
+}
+
+struct iris_inst *iris_hfi_gen1_get_instance(void)
+{
+	return kzalloc(sizeof(struct iris_inst), GFP_KERNEL);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen1_defines.h b/drivers/media/platform/qcom/iris/iris_hfi_gen1_defines.h
new file mode 100644
index 000000000000..9f246816a286
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen1_defines.h
@@ -0,0 +1,448 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_GEN1_DEFINES_H__
+#define __IRIS_HFI_GEN1_DEFINES_H__
+
+#include <linux/types.h>
+
+#define HFI_VIDEO_ARCH_OX				0x1
+
+#define HFI_SESSION_TYPE_DEC				2
+
+#define HFI_VIDEO_CODEC_H264				0x00000002
+
+#define HFI_ERR_NONE					0x0
+
+#define HFI_CMD_SYS_INIT				0x10001
+#define HFI_CMD_SYS_PC_PREP				0x10002
+#define HFI_CMD_SYS_SET_PROPERTY			0x10005
+#define HFI_CMD_SYS_GET_PROPERTY			0x10006
+#define HFI_CMD_SYS_SESSION_INIT			0x10007
+#define HFI_CMD_SYS_SESSION_END				0x10008
+
+#define HFI_CMD_SESSION_SET_PROPERTY			0x11001
+#define HFI_CMD_SESSION_SET_BUFFERS			0x11002
+
+#define HFI_CMD_SESSION_LOAD_RESOURCES			0x211001
+#define HFI_CMD_SESSION_START				0x211002
+#define HFI_CMD_SESSION_STOP				0x211003
+#define HFI_CMD_SESSION_EMPTY_BUFFER			0x211004
+#define HFI_CMD_SESSION_FILL_BUFFER			0x211005
+#define HFI_CMD_SESSION_FLUSH				0x211008
+#define HFI_CMD_SESSION_RELEASE_BUFFERS			0x21100b
+#define HFI_CMD_SESSION_RELEASE_RESOURCES		0x21100c
+#define HFI_CMD_SESSION_CONTINUE			0x21100d
+
+#define HFI_ERR_SESSION_UNSUPPORTED_SETTING		0x1008
+#define HFI_ERR_SESSION_UNSUPPORTED_STREAM		0x100d
+#define HFI_ERR_SESSION_UNSUPPORT_BUFFERTYPE		0x1010
+#define HFI_ERR_SESSION_INVALID_SCALE_FACTOR		0x1012
+#define HFI_ERR_SESSION_UPSCALE_NOT_SUPPORTED		0x1013
+
+#define HFI_EVENT_SYS_ERROR				0x1
+#define HFI_EVENT_SESSION_ERROR				0x2
+
+#define HFI_EVENT_DATA_SEQUENCE_CHANGED_SUFFICIENT_BUF_RESOURCES   0x1000001
+#define HFI_EVENT_DATA_SEQUENCE_CHANGED_INSUFFICIENT_BUF_RESOURCES 0x1000002
+#define HFI_EVENT_SESSION_SEQUENCE_CHANGED			   0x1000003
+
+#define HFI_BUFFERFLAG_EOS				0x00000001
+#define HFI_BUFFERFLAG_TIMESTAMPINVALID			0x00000100
+
+#define HFI_FLUSH_OUTPUT				0x1000002
+#define HFI_FLUSH_OUTPUT2				0x1000003
+#define HFI_FLUSH_ALL					0x1000004
+
+#define HFI_INDEX_EXTRADATA_INPUT_CROP			0x0700000e
+
+#define HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL				0x201001
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO	0x201002
+#define HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE				0x201008
+#define HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL				0x20100c
+
+#define HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS		0x202001
+
+#define HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER	0x1200001
+#define HFI_PROPERTY_PARAM_VDEC_DPB_COUNTS		0x120300e
+#define HFI_PROPERTY_CONFIG_VDEC_ENTROPY		0x1204004
+
+#define HFI_BUFFER_INPUT				0x1
+#define HFI_BUFFER_OUTPUT				0x2
+#define HFI_BUFFER_OUTPUT2				0x3
+#define HFI_BUFFER_INTERNAL_PERSIST_1			0x5
+#define HFI_BUFFER_INTERNAL_SCRATCH			0x6
+#define HFI_BUFFER_INTERNAL_SCRATCH_1			0x7
+
+#define HFI_PROPERTY_SYS_CODEC_POWER_PLANE_CTRL		0x5
+#define HFI_PROPERTY_SYS_IMAGE_VERSION			0x6
+
+#define HFI_PROPERTY_PARAM_FRAME_SIZE			0x1001
+#define HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT	0x1003
+#define HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT	0x1005
+#define HFI_PROPERTY_PARAM_WORK_MODE			0x1015
+#define HFI_PROPERTY_PARAM_WORK_ROUTE			0x1017
+#define HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE		0x2002
+
+#define HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM		0x1003001
+#define HFI_PROPERTY_PARAM_VDEC_PIXEL_BITDEPTH		0x1003007
+#define HFI_PROPERTY_PARAM_VDEC_PIC_STRUCT		0x1003009
+#define HFI_PROPERTY_PARAM_VDEC_COLOUR_SPACE		0x100300a
+#define HFI_CORE_ID_1					1
+#define HFI_COLOR_FORMAT_NV12				0x02
+#define HFI_COLOR_FORMAT_NV12_UBWC			0x8002
+
+#define HFI_MSG_SYS_INIT				0x20001
+#define HFI_MSG_SYS_SESSION_INIT			0x20006
+#define HFI_MSG_SYS_SESSION_END				0x20007
+#define HFI_MSG_SYS_COV					0x20009
+#define HFI_MSG_SYS_PROPERTY_INFO			0x2000a
+
+#define HFI_MSG_EVENT_NOTIFY				0x21001
+#define HFI_MSG_SESSION_LOAD_RESOURCES			0x221001
+#define HFI_MSG_SESSION_START				0x221002
+#define HFI_MSG_SESSION_STOP				0x221003
+#define HFI_MSG_SESSION_FLUSH				0x221006
+#define HFI_MSG_SESSION_EMPTY_BUFFER			0x221007
+#define HFI_MSG_SESSION_FILL_BUFFER			0x221008
+#define HFI_MSG_SESSION_RELEASE_RESOURCES		0x22100a
+#define HFI_MSG_SESSION_RELEASE_BUFFERS			0x22100c
+
+#define HFI_PICTURE_I					0x00000001
+#define HFI_PICTURE_P					0x00000002
+#define HFI_PICTURE_B					0x00000004
+#define HFI_PICTURE_IDR					0x00000008
+#define HFI_FRAME_NOTCODED				0x7f002000
+#define HFI_FRAME_YUV					0x7f004000
+#define HFI_UNUSED_PICT					0x10000000
+
+struct hfi_pkt_hdr {
+	u32 size;
+	u32 pkt_type;
+};
+
+struct hfi_session_hdr_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 session_id;
+};
+
+struct hfi_session_open_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 session_domain;
+	u32 session_codec;
+};
+
+struct hfi_session_pkt {
+	struct hfi_session_hdr_pkt shdr;
+};
+
+struct hfi_sys_init_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 arch_type;
+};
+
+struct hfi_sys_set_property_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 data[];
+};
+
+struct hfi_sys_get_property_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 data;
+};
+
+struct hfi_session_set_property_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 num_properties;
+	u32 data[];
+};
+
+struct hfi_sys_pc_prep_pkt {
+	struct hfi_pkt_hdr hdr;
+};
+
+struct hfi_session_set_buffers_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 extradata_size;
+	u32 min_buffer_size;
+	u32 num_buffers;
+	u32 buffer_info[];
+};
+
+struct hfi_session_empty_buffer_compressed_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 input_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data;
+};
+
+struct hfi_session_fill_buffer_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 stream_id;
+	u32 offset;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 output_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data;
+};
+
+struct hfi_session_flush_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 flush_type;
+};
+
+struct hfi_session_release_buffer_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 buffer_type;
+	u32 buffer_size;
+	u32 extradata_size;
+	u32 response_req;
+	u32 num_buffers;
+	u32 buffer_info[];
+};
+
+struct hfi_buffer_info {
+	u32 buffer_addr;
+	u32 extradata_addr;
+};
+
+struct hfi_msg_event_notify_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 event_id;
+	u32 event_data1;
+	u32 event_data2;
+	u32 ext_event_data[];
+};
+
+struct hfi_msg_sys_init_done_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 error_type;
+	u32 num_properties;
+	u32 data[];
+};
+
+struct hfi_msg_session_hdr_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 error_type;
+};
+
+struct hfi_msg_session_init_done_pkt {
+	struct hfi_msg_session_hdr_pkt shdr;
+	u32 num_properties;
+	u32 data[];
+};
+
+struct hfi_msg_sys_property_info_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 num_properties;
+	u32 property;
+	u8 data[];
+};
+
+struct hfi_msg_session_flush_done_pkt {
+	struct hfi_msg_session_hdr_pkt shdr;
+	u32 flush_type;
+};
+
+struct hfi_enable {
+	u32 enable;
+};
+
+struct hfi_profile_level {
+	u32 profile;
+	u32 level;
+};
+
+struct hfi_framesize {
+	u32 buffer_type;
+	u32 width;
+	u32 height;
+};
+
+struct hfi_videocores_usage_type {
+	u32 video_core_enable_mask;
+};
+
+struct hfi_video_work_mode {
+	u32 video_work_mode;
+};
+
+struct hfi_video_work_route {
+	u32 video_work_route;
+};
+
+struct hfi_bit_depth {
+	u32 buffer_type;
+	u32 bit_depth;
+};
+
+struct hfi_pic_struct {
+	u32 progressive_only;
+};
+
+struct hfi_colour_space {
+	u32 colour_space;
+};
+
+struct hfi_extradata_input_crop {
+	u32 size;
+	u32 version;
+	u32 port_index;
+	u32 left;
+	u32 top;
+	u32 width;
+	u32 height;
+};
+
+struct hfi_dpb_counts {
+	u32 max_dpb_count;
+	u32 max_ref_frames;
+	u32 max_dec_buffering;
+	u32 max_reorder_frames;
+	u32 fw_min_count;
+};
+
+struct hfi_uncompressed_format_select {
+	u32 buffer_type;
+	u32 format;
+};
+
+struct hfi_uncompressed_plane_constraints {
+	u32 stride_multiples;
+	u32 max_stride;
+	u32 min_plane_buffer_height_multiple;
+	u32 buffer_alignment;
+};
+
+struct hfi_uncompressed_plane_actual_constraints_info {
+	u32 buffer_type;
+	u32 num_planes;
+	struct hfi_uncompressed_plane_constraints plane_format[2];
+};
+
+struct hfi_buffer_count_actual {
+	u32 type;
+	u32 count_actual;
+	u32 count_min_host;
+};
+
+struct hfi_buffer_size_actual {
+	u32 type;
+	u32 size;
+};
+
+struct hfi_multi_stream {
+	u32 buffer_type;
+	u32 enable;
+};
+
+struct hfi_buffer_requirements {
+	u32 type;
+	u32 size;
+	u32 region_size;
+	u32 hold_count;
+	u32 count_min;
+	u32 count_actual;
+	u32 contiguous;
+	u32 alignment;
+};
+
+struct hfi_event_data {
+	u32 error;
+	u32 height;
+	u32 width;
+	u32 event_type;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 tag;
+	u32 profile;
+	u32 level;
+	u32 bit_depth;
+	u32 pic_struct;
+	u32 colour_space;
+	u32 entropy_mode;
+	u32 buf_count;
+	struct {
+		u32 left, top;
+		u32 width, height;
+	} input_crop;
+};
+
+struct hfi_msg_session_empty_buffer_done_pkt {
+	struct hfi_msg_session_hdr_pkt shdr;
+	u32 offset;
+	u32 filled_len;
+	u32 input_tag;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[];
+};
+
+struct hfi_msg_session_fbd_uncompressed_plane0_pkt {
+	struct hfi_session_hdr_pkt shdr;
+	u32 stream_id;
+	u32 view_id;
+	u32 error_type;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u32 flags;
+	u32 mark_target;
+	u32 mark_data;
+	u32 stats;
+	u32 alloc_len;
+	u32 filled_len;
+	u32 offset;
+	u32 frame_width;
+	u32 frame_height;
+	u32 start_x_coord;
+	u32 start_y_coord;
+	u32 input_tag;
+	u32 input_tag2;
+	u32 output_tag;
+	u32 picture_type;
+	u32 packet_buffer;
+	u32 extradata_buffer;
+	u32 data[];
+};
+
+struct hfi_msg_session_release_buffers_done_pkt {
+	struct hfi_msg_session_hdr_pkt shdr;
+	u32 num_buffers;
+	u32 buffer_info[];
+};
+
+struct hfi_msg_sys_debug_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 msg_type;
+	u32 msg_size;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u8 msg_data[];
+};
+
+struct hfi_msg_sys_coverage_pkt {
+	struct hfi_pkt_hdr hdr;
+	u32 msg_size;
+	u32 time_stamp_hi;
+	u32 time_stamp_lo;
+	u8 msg_data[];
+};
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen1_response.c b/drivers/media/platform/qcom/iris/iris_hfi_gen1_response.c
new file mode 100644
index 000000000000..b72d503dd740
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen1_response.c
@@ -0,0 +1,666 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/bitfield.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_hfi_gen1.h"
+#include "iris_hfi_gen1_defines.h"
+#include "iris_instance.h"
+#include "iris_vdec.h"
+#include "iris_vpu_buffer.h"
+
+static void iris_hfi_gen1_read_changed_params(struct iris_inst *inst,
+					      struct hfi_msg_event_notify_pkt *pkt)
+{
+	struct v4l2_pix_format_mplane *pixmp_ip = &inst->fmt_src->fmt.pix_mp;
+	struct v4l2_pix_format_mplane *pixmp_op = &inst->fmt_dst->fmt.pix_mp;
+	u32 num_properties_changed = pkt->event_data2;
+	u8 *data_ptr = (u8 *)&pkt->ext_event_data[0];
+	u32 primaries, matrix_coeff, transfer_char;
+	struct hfi_dpb_counts *iris_vpu_dpb_count;
+	struct hfi_profile_level *profile_level;
+	struct hfi_buffer_requirements *bufreq;
+	struct hfi_extradata_input_crop *crop;
+	struct hfi_colour_space *colour_info;
+	struct iris_core *core = inst->core;
+	u32 colour_description_present_flag;
+	u32 video_signal_type_present_flag;
+	struct hfi_event_data event = {0};
+	struct hfi_bit_depth *pixel_depth;
+	struct hfi_pic_struct *pic_struct;
+	struct hfi_framesize *frame_sz;
+	struct vb2_queue *dst_q;
+	struct v4l2_ctrl *ctrl;
+	u32 full_range, ptype;
+
+	do {
+		ptype = *((u32 *)data_ptr);
+		switch (ptype) {
+		case HFI_PROPERTY_PARAM_FRAME_SIZE:
+			data_ptr += sizeof(u32);
+			frame_sz = (struct hfi_framesize *)data_ptr;
+			event.width = frame_sz->width;
+			event.height = frame_sz->height;
+			data_ptr += sizeof(*frame_sz);
+			break;
+		case HFI_PROPERTY_PARAM_PROFILE_LEVEL_CURRENT:
+			data_ptr += sizeof(u32);
+			profile_level = (struct hfi_profile_level *)data_ptr;
+			event.profile = profile_level->profile;
+			event.level = profile_level->level;
+			data_ptr += sizeof(*profile_level);
+			break;
+		case HFI_PROPERTY_PARAM_VDEC_PIXEL_BITDEPTH:
+			data_ptr += sizeof(u32);
+			pixel_depth = (struct hfi_bit_depth *)data_ptr;
+			event.bit_depth = pixel_depth->bit_depth;
+			data_ptr += sizeof(*pixel_depth);
+			break;
+		case HFI_PROPERTY_PARAM_VDEC_PIC_STRUCT:
+			data_ptr += sizeof(u32);
+			pic_struct = (struct hfi_pic_struct *)data_ptr;
+			event.pic_struct = pic_struct->progressive_only;
+			data_ptr += sizeof(*pic_struct);
+			break;
+		case HFI_PROPERTY_PARAM_VDEC_COLOUR_SPACE:
+			data_ptr += sizeof(u32);
+			colour_info = (struct hfi_colour_space *)data_ptr;
+			event.colour_space = colour_info->colour_space;
+			data_ptr += sizeof(*colour_info);
+			break;
+		case HFI_PROPERTY_CONFIG_VDEC_ENTROPY:
+			data_ptr += sizeof(u32);
+			event.entropy_mode = *(u32 *)data_ptr;
+			data_ptr += sizeof(u32);
+			break;
+		case HFI_PROPERTY_CONFIG_BUFFER_REQUIREMENTS:
+			data_ptr += sizeof(u32);
+			bufreq = (struct hfi_buffer_requirements *)data_ptr;
+			event.buf_count = bufreq->count_min;
+			data_ptr += sizeof(*bufreq);
+			break;
+		case HFI_INDEX_EXTRADATA_INPUT_CROP:
+			data_ptr += sizeof(u32);
+			crop = (struct hfi_extradata_input_crop *)data_ptr;
+			event.input_crop.left = crop->left;
+			event.input_crop.top = crop->top;
+			event.input_crop.width = crop->width;
+			event.input_crop.height = crop->height;
+			data_ptr += sizeof(*crop);
+			break;
+		case HFI_PROPERTY_PARAM_VDEC_DPB_COUNTS:
+			data_ptr += sizeof(u32);
+			iris_vpu_dpb_count = (struct hfi_dpb_counts *)data_ptr;
+			event.buf_count = iris_vpu_dpb_count->fw_min_count;
+			data_ptr += sizeof(*iris_vpu_dpb_count);
+			break;
+		default:
+			break;
+		}
+		num_properties_changed--;
+	} while (num_properties_changed > 0);
+
+	pixmp_ip->width = event.width;
+	pixmp_ip->height = event.height;
+
+	pixmp_op->width = ALIGN(event.width, 128);
+	pixmp_op->height = ALIGN(event.height, 32);
+	pixmp_op->plane_fmt[0].bytesperline = ALIGN(event.width, 128);
+	pixmp_op->plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_OUTPUT);
+
+	matrix_coeff =  FIELD_GET(GENMASK(7, 0), event.colour_space);
+	transfer_char = FIELD_GET(GENMASK(15, 8), event.colour_space);
+	primaries = FIELD_GET(GENMASK(23, 16), event.colour_space);
+	colour_description_present_flag = FIELD_GET(GENMASK(24, 24), event.colour_space);
+	full_range = FIELD_GET(GENMASK(25, 25), event.colour_space);
+	video_signal_type_present_flag = FIELD_GET(GENMASK(29, 29), event.colour_space);
+
+	pixmp_op->colorspace = V4L2_COLORSPACE_DEFAULT;
+	pixmp_op->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	pixmp_op->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	pixmp_op->quantization = V4L2_QUANTIZATION_DEFAULT;
+
+	if (video_signal_type_present_flag) {
+		pixmp_op->quantization =
+			full_range ?
+			V4L2_QUANTIZATION_FULL_RANGE :
+			V4L2_QUANTIZATION_LIM_RANGE;
+		if (colour_description_present_flag) {
+			pixmp_op->colorspace =
+				iris_hfi_get_v4l2_color_primaries(primaries);
+			pixmp_op->xfer_func =
+				iris_hfi_get_v4l2_transfer_char(transfer_char);
+			pixmp_op->ycbcr_enc =
+				iris_hfi_get_v4l2_matrix_coefficients(matrix_coeff);
+		}
+	}
+
+	pixmp_ip->colorspace = pixmp_op->colorspace;
+	pixmp_ip->xfer_func = pixmp_op->xfer_func;
+	pixmp_ip->ycbcr_enc = pixmp_op->ycbcr_enc;
+	pixmp_ip->quantization = pixmp_op->quantization;
+
+	if (event.input_crop.width > 0 && event.input_crop.height > 0) {
+		inst->crop.left = event.input_crop.left;
+		inst->crop.top = event.input_crop.top;
+		inst->crop.width = event.input_crop.width;
+		inst->crop.height = event.input_crop.height;
+	} else {
+		inst->crop.left = 0;
+		inst->crop.top = 0;
+		inst->crop.width = event.width;
+		inst->crop.height = event.height;
+	}
+
+	inst->fw_min_count = event.buf_count;
+	inst->buffers[BUF_OUTPUT].min_count = iris_vpu_buf_count(inst, BUF_OUTPUT);
+	inst->buffers[BUF_OUTPUT].size = pixmp_op->plane_fmt[0].sizeimage;
+	ctrl = v4l2_ctrl_find(&inst->ctrl_handler, V4L2_CID_MIN_BUFFERS_FOR_CAPTURE);
+	if (ctrl)
+		v4l2_ctrl_s_ctrl(ctrl, inst->buffers[BUF_OUTPUT].min_count);
+
+	dst_q = v4l2_m2m_get_dst_vq(inst->m2m_ctx);
+	dst_q->min_reqbufs_allocation = inst->buffers[BUF_OUTPUT].min_count;
+
+	if (event.bit_depth || !event.pic_struct) {
+		dev_err(core->dev, "unsupported content, bit depth: %x, pic_struct = %x\n",
+			event.bit_depth, event.pic_struct);
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+	}
+}
+
+static void iris_hfi_gen1_event_seq_changed(struct iris_inst *inst,
+					    struct hfi_msg_event_notify_pkt *pkt)
+{
+	struct hfi_session_flush_pkt flush_pkt;
+	u32 num_properties_changed;
+	int ret;
+
+	ret = iris_inst_sub_state_change_drc(inst);
+	if (ret)
+		return;
+
+	switch (pkt->event_data1) {
+	case HFI_EVENT_DATA_SEQUENCE_CHANGED_SUFFICIENT_BUF_RESOURCES:
+	case HFI_EVENT_DATA_SEQUENCE_CHANGED_INSUFFICIENT_BUF_RESOURCES:
+		break;
+	default:
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return;
+	}
+
+	num_properties_changed = pkt->event_data2;
+	if (!num_properties_changed) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return;
+	}
+
+	iris_hfi_gen1_read_changed_params(inst, pkt);
+
+	if (inst->state != IRIS_INST_ERROR) {
+		reinit_completion(&inst->flush_completion);
+
+		flush_pkt.shdr.hdr.size = sizeof(struct hfi_session_flush_pkt);
+		flush_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_FLUSH;
+		flush_pkt.shdr.session_id = inst->session_id;
+		flush_pkt.flush_type = HFI_FLUSH_OUTPUT;
+		iris_hfi_queue_cmd_write(inst->core, &flush_pkt, flush_pkt.shdr.hdr.size);
+	}
+
+	iris_vdec_src_change(inst);
+	iris_inst_sub_state_change_drc_last(inst);
+}
+
+static void
+iris_hfi_gen1_sys_event_notify(struct iris_core *core, void *packet)
+{
+	struct hfi_msg_event_notify_pkt *pkt = packet;
+	struct iris_inst *instance;
+
+	if (pkt->event_id == HFI_EVENT_SYS_ERROR)
+		dev_err(core->dev, "sys error (type: %x, session id:%x, data1:%x, data2:%x)\n",
+			pkt->event_id, pkt->shdr.session_id, pkt->event_data1,
+			pkt->event_data2);
+
+	core->state = IRIS_CORE_ERROR;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(instance, &core->instances, list)
+		iris_inst_change_state(instance, IRIS_INST_ERROR);
+	mutex_unlock(&core->lock);
+
+	schedule_delayed_work(&core->sys_error_handler, msecs_to_jiffies(10));
+}
+
+static void
+iris_hfi_gen1_event_session_error(struct iris_inst *inst, struct hfi_msg_event_notify_pkt *pkt)
+{
+	switch (pkt->event_data1) {
+	/* non fatal session errors */
+	case HFI_ERR_SESSION_INVALID_SCALE_FACTOR:
+	case HFI_ERR_SESSION_UNSUPPORT_BUFFERTYPE:
+	case HFI_ERR_SESSION_UNSUPPORTED_SETTING:
+	case HFI_ERR_SESSION_UPSCALE_NOT_SUPPORTED:
+		dev_dbg(inst->core->dev, "session error: event id:%x, session id:%x\n",
+			pkt->event_data1, pkt->shdr.session_id);
+		break;
+	/* fatal session errors */
+	default:
+		/*
+		 * firmware fills event_data2 as an additional information about the
+		 * hfi command for which session error has ouccured.
+		 */
+		dev_err(inst->core->dev,
+			"session error for command: %x, event id:%x, session id:%x\n",
+			pkt->event_data2, pkt->event_data1,
+			pkt->shdr.session_id);
+		iris_vb2_queue_error(inst);
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		break;
+	}
+}
+
+static void iris_hfi_gen1_session_event_notify(struct iris_inst *inst, void *packet)
+{
+	struct hfi_msg_event_notify_pkt *pkt = packet;
+
+	switch (pkt->event_id) {
+	case HFI_EVENT_SESSION_ERROR:
+		iris_hfi_gen1_event_session_error(inst, pkt);
+		break;
+	case HFI_EVENT_SESSION_SEQUENCE_CHANGED:
+		iris_hfi_gen1_event_seq_changed(inst, pkt);
+		break;
+	default:
+		break;
+	}
+}
+
+static void iris_hfi_gen1_sys_init_done(struct iris_core *core, void *packet)
+{
+	struct hfi_msg_sys_init_done_pkt *pkt = packet;
+
+	if (pkt->error_type != HFI_ERR_NONE) {
+		core->state = IRIS_CORE_ERROR;
+		return;
+	}
+
+	complete(&core->core_init_done);
+}
+
+static void
+iris_hfi_gen1_sys_get_prop_image_version(struct iris_core *core,
+					 struct hfi_msg_sys_property_info_pkt *pkt)
+{
+	int req_bytes = pkt->hdr.size - sizeof(*pkt);
+	char fw_version[IRIS_FW_VERSION_LENGTH];
+	u8 *str_image_version;
+	u32 i;
+
+	if (req_bytes < IRIS_FW_VERSION_LENGTH - 1 || !pkt->data[0] || pkt->num_properties > 1) {
+		dev_err(core->dev, "bad packet\n");
+		return;
+	}
+
+	str_image_version = pkt->data;
+	if (!str_image_version) {
+		dev_err(core->dev, "firmware version not available\n");
+		return;
+	}
+
+	for (i = 0; i < IRIS_FW_VERSION_LENGTH - 1; i++) {
+		if (str_image_version[i] != '\0')
+			fw_version[i] = str_image_version[i];
+		else
+			fw_version[i] = ' ';
+	}
+	fw_version[i] = '\0';
+	dev_dbg(core->dev, "firmware version: %s\n", fw_version);
+}
+
+static void iris_hfi_gen1_sys_property_info(struct iris_core *core, void *packet)
+{
+	struct hfi_msg_sys_property_info_pkt *pkt = packet;
+
+	if (!pkt->num_properties) {
+		dev_dbg(core->dev, "no properties\n");
+		return;
+	}
+
+	switch (pkt->property) {
+	case HFI_PROPERTY_SYS_IMAGE_VERSION:
+		iris_hfi_gen1_sys_get_prop_image_version(core, pkt);
+		break;
+	default:
+		dev_dbg(core->dev, "unknown property data\n");
+		break;
+	}
+}
+
+static void iris_hfi_gen1_session_etb_done(struct iris_inst *inst, void *packet)
+{
+	struct hfi_msg_session_empty_buffer_done_pkt *pkt = packet;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *m2m_buffer, *n;
+	struct iris_buffer *buf = NULL;
+	bool found = false;
+
+	v4l2_m2m_for_each_src_buf_safe(m2m_ctx, m2m_buffer, n) {
+		buf = to_iris_buffer(&m2m_buffer->vb);
+		if (buf->index == pkt->input_tag) {
+			found = true;
+			break;
+		}
+	}
+	if (!found)
+		goto error;
+
+	if (pkt->shdr.error_type == HFI_ERR_SESSION_UNSUPPORTED_STREAM) {
+		buf->flags = V4L2_BUF_FLAG_ERROR;
+		iris_vb2_queue_error(inst);
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+	}
+
+	if (!(buf->attr & BUF_ATTR_QUEUED))
+		return;
+
+	buf->attr &= ~BUF_ATTR_QUEUED;
+
+	if (!(buf->attr & BUF_ATTR_BUFFER_DONE)) {
+		buf->attr |= BUF_ATTR_BUFFER_DONE;
+		iris_vb2_buffer_done(inst, buf);
+	}
+
+	return;
+
+error:
+	iris_inst_change_state(inst, IRIS_INST_ERROR);
+	dev_err(inst->core->dev, "error in etb done\n");
+}
+
+static void iris_hfi_gen1_session_ftb_done(struct iris_inst *inst, void *packet)
+{
+	struct hfi_msg_session_fbd_uncompressed_plane0_pkt *pkt = packet;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *m2m_buffer, *n;
+	struct hfi_session_flush_pkt flush_pkt;
+	u32 timestamp_hi = pkt->time_stamp_hi;
+	u32 timestamp_lo = pkt->time_stamp_lo;
+	struct iris_core *core = inst->core;
+	u32 filled_len = pkt->filled_len;
+	u32 pic_type = pkt->picture_type;
+	u32 output_tag = pkt->output_tag;
+	struct iris_buffer *buf, *iter;
+	struct iris_buffers *buffers;
+	u32 hfi_flags = pkt->flags;
+	u32 offset = pkt->offset;
+	u64 timestamp_us = 0;
+	bool found = false;
+	u32 flags = 0;
+
+	if ((hfi_flags & HFI_BUFFERFLAG_EOS) && !filled_len) {
+		reinit_completion(&inst->flush_completion);
+
+		flush_pkt.shdr.hdr.size = sizeof(struct hfi_session_flush_pkt);
+		flush_pkt.shdr.hdr.pkt_type = HFI_CMD_SESSION_FLUSH;
+		flush_pkt.shdr.session_id = inst->session_id;
+		flush_pkt.flush_type = HFI_FLUSH_OUTPUT;
+		iris_hfi_queue_cmd_write(core, &flush_pkt, flush_pkt.shdr.hdr.size);
+		iris_inst_sub_state_change_drain_last(inst);
+
+		return;
+	}
+
+	if (iris_split_mode_enabled(inst) && pkt->stream_id == 0) {
+		buffers = &inst->buffers[BUF_DPB];
+		if (!buffers)
+			goto error;
+
+		found = false;
+		list_for_each_entry(iter, &buffers->list, list) {
+			if (!(iter->attr & BUF_ATTR_QUEUED))
+				continue;
+
+			found = (iter->index == output_tag &&
+				iter->data_offset == offset);
+
+			if (found) {
+				buf = iter;
+				break;
+			}
+		}
+	} else {
+		v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, m2m_buffer, n) {
+			buf = to_iris_buffer(&m2m_buffer->vb);
+			if (!(buf->attr & BUF_ATTR_QUEUED))
+				continue;
+
+			found = (buf->index == output_tag &&
+				 buf->data_offset == offset);
+
+			if (found)
+				break;
+		}
+	}
+	if (!found)
+		goto error;
+
+	buf->data_offset = offset;
+	buf->data_size = filled_len;
+
+	if (filled_len) {
+		timestamp_us = timestamp_hi;
+		timestamp_us = (timestamp_us << 32) | timestamp_lo;
+	} else {
+		flags |= V4L2_BUF_FLAG_LAST;
+	}
+	buf->timestamp = timestamp_us;
+
+	switch (pic_type) {
+	case HFI_PICTURE_IDR:
+	case HFI_PICTURE_I:
+		flags |= V4L2_BUF_FLAG_KEYFRAME;
+		break;
+	case HFI_PICTURE_P:
+		flags |= V4L2_BUF_FLAG_PFRAME;
+		break;
+	case HFI_PICTURE_B:
+		flags |= V4L2_BUF_FLAG_BFRAME;
+		break;
+	case HFI_FRAME_NOTCODED:
+	case HFI_UNUSED_PICT:
+	case HFI_FRAME_YUV:
+	default:
+		break;
+	}
+
+	buf->attr &= ~BUF_ATTR_QUEUED;
+	buf->attr |= BUF_ATTR_DEQUEUED;
+	buf->attr |= BUF_ATTR_BUFFER_DONE;
+
+	buf->flags |= flags;
+
+	iris_vb2_buffer_done(inst, buf);
+
+	return;
+
+error:
+	iris_inst_change_state(inst, IRIS_INST_ERROR);
+	dev_err(core->dev, "error in ftb done\n");
+}
+
+struct iris_hfi_gen1_response_pkt_info {
+	u32 pkt;
+	u32 pkt_sz;
+};
+
+static const struct iris_hfi_gen1_response_pkt_info pkt_infos[] = {
+	{
+	 .pkt = HFI_MSG_EVENT_NOTIFY,
+	 .pkt_sz = sizeof(struct hfi_msg_event_notify_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SYS_INIT,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_init_done_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SYS_PROPERTY_INFO,
+	 .pkt_sz = sizeof(struct hfi_msg_sys_property_info_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SYS_SESSION_INIT,
+	 .pkt_sz = sizeof(struct hfi_msg_session_init_done_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SYS_SESSION_END,
+	 .pkt_sz = sizeof(struct hfi_msg_session_hdr_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_LOAD_RESOURCES,
+	 .pkt_sz = sizeof(struct hfi_msg_session_hdr_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_START,
+	 .pkt_sz = sizeof(struct hfi_msg_session_hdr_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_STOP,
+	 .pkt_sz = sizeof(struct hfi_msg_session_hdr_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_EMPTY_BUFFER,
+	 .pkt_sz = sizeof(struct hfi_msg_session_empty_buffer_done_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_FILL_BUFFER,
+	 .pkt_sz = sizeof(struct hfi_msg_session_fbd_uncompressed_plane0_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_FLUSH,
+	 .pkt_sz = sizeof(struct hfi_msg_session_flush_done_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_RELEASE_RESOURCES,
+	 .pkt_sz = sizeof(struct hfi_msg_session_hdr_pkt),
+	},
+	{
+	 .pkt = HFI_MSG_SESSION_RELEASE_BUFFERS,
+	 .pkt_sz = sizeof(struct hfi_msg_session_release_buffers_done_pkt),
+	},
+};
+
+static void iris_hfi_gen1_handle_response(struct iris_core *core, void *response)
+{
+	struct hfi_pkt_hdr *hdr = (struct hfi_pkt_hdr *)response;
+	const struct iris_hfi_gen1_response_pkt_info *pkt_info;
+	struct device *dev = core->dev;
+	struct hfi_session_pkt *pkt;
+	struct completion *done;
+	struct iris_inst *inst;
+	bool found = false;
+	u32 i;
+
+	for (i = 0; i < ARRAY_SIZE(pkt_infos); i++) {
+		pkt_info = &pkt_infos[i];
+		if (pkt_info->pkt != hdr->pkt_type)
+			continue;
+		found = true;
+		break;
+	}
+
+	if (!found || hdr->size < pkt_info->pkt_sz) {
+		dev_err(dev, "bad packet size (%d should be %d, pkt type:%x, found %d)\n",
+			hdr->size, pkt_info->pkt_sz, hdr->pkt_type, found);
+
+		return;
+	}
+
+	switch (hdr->pkt_type) {
+	case HFI_MSG_SYS_INIT:
+		iris_hfi_gen1_sys_init_done(core, hdr);
+		break;
+	case HFI_MSG_SYS_PROPERTY_INFO:
+		iris_hfi_gen1_sys_property_info(core, hdr);
+		break;
+	case HFI_MSG_EVENT_NOTIFY:
+		pkt = (struct hfi_session_pkt *)hdr;
+		inst = iris_get_instance(core, pkt->shdr.session_id);
+		if (inst) {
+			mutex_lock(&inst->lock);
+			iris_hfi_gen1_session_event_notify(inst, hdr);
+			mutex_unlock(&inst->lock);
+		} else {
+			iris_hfi_gen1_sys_event_notify(core, hdr);
+		}
+
+		break;
+	default:
+		pkt = (struct hfi_session_pkt *)hdr;
+		inst = iris_get_instance(core, pkt->shdr.session_id);
+		if (!inst) {
+			dev_warn(dev, "no valid instance(pkt session_id:%x, pkt:%x)\n",
+				 pkt->shdr.session_id,
+				 pkt_info ? pkt_info->pkt : 0);
+			return;
+		}
+
+		mutex_lock(&inst->lock);
+		if (hdr->pkt_type == HFI_MSG_SESSION_EMPTY_BUFFER) {
+			iris_hfi_gen1_session_etb_done(inst, hdr);
+		} else if (hdr->pkt_type == HFI_MSG_SESSION_FILL_BUFFER) {
+			iris_hfi_gen1_session_ftb_done(inst, hdr);
+		} else {
+			struct hfi_msg_session_hdr_pkt *shdr;
+
+			shdr = (struct hfi_msg_session_hdr_pkt *)hdr;
+			if (shdr->error_type != HFI_ERR_NONE)
+				iris_inst_change_state(inst, IRIS_INST_ERROR);
+
+			done = pkt_info->pkt == HFI_MSG_SESSION_FLUSH ?
+				&inst->flush_completion : &inst->completion;
+			complete(done);
+		}
+		mutex_unlock(&inst->lock);
+
+		break;
+	}
+}
+
+static void iris_hfi_gen1_flush_debug_queue(struct iris_core *core, u8 *packet)
+{
+	struct hfi_msg_sys_coverage_pkt *pkt;
+
+	while (!iris_hfi_queue_dbg_read(core, packet)) {
+		pkt = (struct hfi_msg_sys_coverage_pkt *)packet;
+
+		if (pkt->hdr.pkt_type != HFI_MSG_SYS_COV) {
+			struct hfi_msg_sys_debug_pkt *pkt =
+				(struct hfi_msg_sys_debug_pkt *)packet;
+
+			dev_dbg(core->dev, "%s", pkt->msg_data);
+		}
+	}
+}
+
+static void iris_hfi_gen1_response_handler(struct iris_core *core)
+{
+	memset(core->response_packet, 0, sizeof(struct hfi_pkt_hdr));
+	while (!iris_hfi_queue_msg_read(core, core->response_packet)) {
+		iris_hfi_gen1_handle_response(core, core->response_packet);
+		memset(core->response_packet, 0, sizeof(struct hfi_pkt_hdr));
+	}
+
+	iris_hfi_gen1_flush_debug_queue(core, core->response_packet);
+}
+
+static const struct iris_hfi_response_ops iris_hfi_gen1_response_ops = {
+	.hfi_response_handler = iris_hfi_gen1_response_handler,
+};
+
+void iris_hfi_gen1_response_ops_init(struct iris_core *core)
+{
+	core->hfi_response_ops = &iris_hfi_gen1_response_ops;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2.h b/drivers/media/platform/qcom/iris/iris_hfi_gen2.h
new file mode 100644
index 000000000000..b9d3749a10ef
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2.h
@@ -0,0 +1,41 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_GEN2_H__
+#define __IRIS_HFI_GEN2_H__
+
+#include "iris_instance.h"
+
+struct iris_core;
+
+#define to_iris_inst_hfi_gen2(ptr) \
+	container_of(ptr, struct iris_inst_hfi_gen2, inst)
+
+/**
+ * struct iris_inst_hfi_gen2 - holds per video instance parameters for hfi_gen2
+ *
+ * @inst: pointer to iris_instance structure
+ * @packet: HFI packet
+ * @ipsc_properties_set: boolean to set ipsc properties to fw
+ * @opsc_properties_set: boolean to set opsc properties to fw
+ * @hfi_frame_info: structure of frame info
+ * @src_subcr_params: subscription params to fw on input port
+ * @dst_subcr_params: subscription params to fw on output port
+ */
+struct iris_inst_hfi_gen2 {
+	struct iris_inst		inst;
+	struct iris_hfi_header		*packet;
+	bool				ipsc_properties_set;
+	bool				opsc_properties_set;
+	struct iris_hfi_frame_info	hfi_frame_info;
+	struct hfi_subscription_params	src_subcr_params;
+	struct hfi_subscription_params	dst_subcr_params;
+};
+
+void iris_hfi_gen2_command_ops_init(struct iris_core *core);
+void iris_hfi_gen2_response_ops_init(struct iris_core *core);
+struct iris_inst *iris_hfi_gen2_get_instance(void);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2_command.c b/drivers/media/platform/qcom/iris/iris_hfi_gen2_command.c
new file mode 100644
index 000000000000..a908b41e2868
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2_command.c
@@ -0,0 +1,957 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/bitfield.h>
+
+#include "iris_hfi_gen2.h"
+#include "iris_hfi_gen2_packet.h"
+
+#define UNSPECIFIED_COLOR_FORMAT 5
+#define NUM_SYS_INIT_PACKETS 8
+
+#define SYS_INIT_PKT_SIZE (sizeof(struct iris_hfi_header) + \
+	NUM_SYS_INIT_PACKETS * (sizeof(struct iris_hfi_packet) + sizeof(u32)))
+
+#define SYS_IFPC_PKT_SIZE (sizeof(struct iris_hfi_header) + \
+	sizeof(struct iris_hfi_packet) + sizeof(u32))
+
+#define SYS_NO_PAYLOAD_PKT_SIZE (sizeof(struct iris_hfi_header) + \
+	sizeof(struct iris_hfi_packet))
+
+static int iris_hfi_gen2_sys_init(struct iris_core *core)
+{
+	struct iris_hfi_header *hdr;
+	int ret;
+
+	hdr = kzalloc(SYS_INIT_PKT_SIZE, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	iris_hfi_gen2_packet_sys_init(core, hdr);
+	ret = iris_hfi_queue_cmd_write_locked(core, hdr, hdr->size);
+
+	kfree(hdr);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_sys_image_version(struct iris_core *core)
+{
+	struct iris_hfi_header *hdr;
+	int ret;
+
+	hdr = kzalloc(SYS_NO_PAYLOAD_PKT_SIZE, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	iris_hfi_gen2_packet_image_version(core, hdr);
+	ret = iris_hfi_queue_cmd_write_locked(core, hdr, hdr->size);
+
+	kfree(hdr);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_sys_interframe_powercollapse(struct iris_core *core)
+{
+	struct iris_hfi_header *hdr;
+	int ret;
+
+	hdr = kzalloc(SYS_IFPC_PKT_SIZE, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	iris_hfi_gen2_packet_sys_interframe_powercollapse(core, hdr);
+	ret = iris_hfi_queue_cmd_write_locked(core, hdr, hdr->size);
+
+	kfree(hdr);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_sys_pc_prep(struct iris_core *core)
+{
+	struct iris_hfi_header *hdr;
+	int ret;
+
+	hdr = kzalloc(SYS_NO_PAYLOAD_PKT_SIZE, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	iris_hfi_gen2_packet_sys_pc_prep(core, hdr);
+	ret = iris_hfi_queue_cmd_write_locked(core, hdr, hdr->size);
+
+	kfree(hdr);
+
+	return ret;
+}
+
+static u32 iris_hfi_gen2_get_port(u32 plane)
+{
+	switch (plane) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		return HFI_PORT_BITSTREAM;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		return HFI_PORT_RAW;
+	default:
+		return HFI_PORT_NONE;
+	}
+}
+
+static u32 iris_hfi_gen2_get_port_from_buf_type(enum iris_buffer_type buffer_type)
+{
+	switch (buffer_type) {
+	case BUF_INPUT:
+	case BUF_BIN:
+	case BUF_COMV:
+	case BUF_NON_COMV:
+	case BUF_LINE:
+		return HFI_PORT_BITSTREAM;
+	case BUF_OUTPUT:
+	case BUF_DPB:
+		return HFI_PORT_RAW;
+	case BUF_PERSIST:
+	default:
+		return HFI_PORT_NONE;
+	}
+}
+
+static int iris_hfi_gen2_session_set_property(struct iris_inst *inst, u32 packet_type, u32 flag,
+					      u32 plane, u32 payload_type, void *payload,
+					      u32 payload_size)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+
+	iris_hfi_gen2_packet_session_property(inst,
+					      packet_type,
+					      flag,
+					      plane,
+					      payload_type,
+					      payload,
+					      payload_size);
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_set_bitstream_resolution(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 resolution = inst->fmt_src->fmt.pix_mp.width << 16 |
+		inst->fmt_src->fmt.pix_mp.height;
+
+	inst_hfi_gen2->src_subcr_params.bitstream_resolution = resolution;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_BITSTREAM_RESOLUTION,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &resolution,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_crop_offsets(struct iris_inst *inst)
+{
+	u32 bottom_offset = (inst->fmt_src->fmt.pix_mp.height - inst->crop.height);
+	u32 right_offset = (inst->fmt_src->fmt.pix_mp.width - inst->crop.width);
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 left_offset = inst->crop.left;
+	u32 top_offset = inst->crop.top;
+	u32 payload[2];
+
+	payload[0] = FIELD_PREP(GENMASK(31, 16), left_offset) | top_offset;
+	payload[1] = FIELD_PREP(GENMASK(31, 16), right_offset) | bottom_offset;
+	inst_hfi_gen2->src_subcr_params.crop_offsets[0] = payload[0];
+	inst_hfi_gen2->src_subcr_params.crop_offsets[1] = payload[1];
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_CROP_OFFSETS,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_64_PACKED,
+						  &payload,
+						  sizeof(u64));
+}
+
+static int iris_hfi_gen2_set_bit_dpeth(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 bitdepth = BIT_DEPTH_8;
+
+	inst_hfi_gen2->src_subcr_params.bit_depth = bitdepth;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_LUMA_CHROMA_BIT_DEPTH,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &bitdepth,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_coded_frames(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 coded_frames = 0;
+
+	if (inst->fw_caps[CODED_FRAMES].value == CODED_FRAMES_PROGRESSIVE)
+		coded_frames = HFI_BITMASK_FRAME_MBS_ONLY_FLAG;
+	inst_hfi_gen2->src_subcr_params.coded_frames = coded_frames;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_CODED_FRAMES,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &coded_frames,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_min_output_count(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 min_output = inst->buffers[BUF_OUTPUT].min_count;
+
+	inst_hfi_gen2->src_subcr_params.fw_min_count = min_output;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &min_output,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_picture_order_count(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 poc = 0;
+
+	inst_hfi_gen2->src_subcr_params.pic_order_cnt = poc;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_PIC_ORDER_CNT_TYPE,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &poc,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_colorspace(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	struct v4l2_pix_format_mplane *pixmp = &inst->fmt_src->fmt.pix_mp;
+	u32 video_signal_type_present_flag = 0, color_info;
+	u32 matrix_coeff = HFI_MATRIX_COEFF_RESERVED;
+	u32 video_format = UNSPECIFIED_COLOR_FORMAT;
+	u32 full_range = V4L2_QUANTIZATION_DEFAULT;
+	u32 transfer_char = HFI_TRANSFER_RESERVED;
+	u32 colour_description_present_flag = 0;
+	u32 primaries = HFI_PRIMARIES_RESERVED;
+
+	if (pixmp->colorspace != V4L2_COLORSPACE_DEFAULT ||
+	    pixmp->ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT ||
+	    pixmp->xfer_func != V4L2_XFER_FUNC_DEFAULT) {
+		colour_description_present_flag = 1;
+		video_signal_type_present_flag = 1;
+		primaries = iris_hfi_gen2_get_color_primaries(pixmp->colorspace);
+		matrix_coeff = iris_hfi_gen2_get_matrix_coefficients(pixmp->ycbcr_enc);
+		transfer_char = iris_hfi_gen2_get_transfer_char(pixmp->xfer_func);
+	}
+
+	if (pixmp->quantization != V4L2_QUANTIZATION_DEFAULT) {
+		video_signal_type_present_flag = 1;
+		full_range = pixmp->quantization == V4L2_QUANTIZATION_FULL_RANGE ? 1 : 0;
+	}
+
+	color_info = iris_hfi_gen2_get_color_info(matrix_coeff, transfer_char, primaries,
+						  colour_description_present_flag, full_range,
+						  video_format, video_signal_type_present_flag);
+
+	inst_hfi_gen2->src_subcr_params.color_info = color_info;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_SIGNAL_COLOR_INFO,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_32_PACKED,
+						  &color_info,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_profile(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 profile = inst->fw_caps[PROFILE].value;
+
+	inst_hfi_gen2->src_subcr_params.profile = profile;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_PROFILE,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32_ENUM,
+						  &profile,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_level(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	u32 level = inst->fw_caps[LEVEL].value;
+
+	inst_hfi_gen2->src_subcr_params.level = level;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_LEVEL,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32_ENUM,
+						  &level,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_colorformat(struct iris_inst *inst)
+{
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	u32 hfi_colorformat, pixelformat;
+
+	pixelformat = inst->fmt_dst->fmt.pix_mp.pixelformat;
+	hfi_colorformat = pixelformat == V4L2_PIX_FMT_NV12 ? HFI_COLOR_FMT_NV12 : 0;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_COLOR_FORMAT,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U32,
+						  &hfi_colorformat,
+						  sizeof(u32));
+}
+
+static int iris_hfi_gen2_set_linear_stride_scanline(struct iris_inst *inst)
+{
+	u32 port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	u32 pixelformat = inst->fmt_dst->fmt.pix_mp.pixelformat;
+	u32 scanline_y = inst->fmt_dst->fmt.pix_mp.height;
+	u32 stride_y = inst->fmt_dst->fmt.pix_mp.width;
+	u32 scanline_uv = scanline_y / 2;
+	u32 stride_uv = stride_y;
+	u32 payload[2];
+
+	if (pixelformat != V4L2_PIX_FMT_NV12)
+		return 0;
+
+	payload[0] = stride_y << 16 | scanline_y;
+	payload[1] = stride_uv << 16 | scanline_uv;
+
+	return iris_hfi_gen2_session_set_property(inst,
+						  HFI_PROP_LINEAR_STRIDE_SCANLINE,
+						  HFI_HOST_FLAGS_NONE,
+						  port,
+						  HFI_PAYLOAD_U64,
+						  &payload,
+						  sizeof(u64));
+}
+
+static int iris_hfi_gen2_session_set_config_params(struct iris_inst *inst, u32 plane)
+{
+	struct iris_core *core = inst->core;
+	u32 config_params_size, i, j;
+	const u32 *config_params;
+	int ret;
+
+	static const struct iris_hfi_prop_type_handle prop_type_handle_arr[] = {
+		{HFI_PROP_BITSTREAM_RESOLUTION,       iris_hfi_gen2_set_bitstream_resolution   },
+		{HFI_PROP_CROP_OFFSETS,               iris_hfi_gen2_set_crop_offsets           },
+		{HFI_PROP_CODED_FRAMES,               iris_hfi_gen2_set_coded_frames           },
+		{HFI_PROP_LUMA_CHROMA_BIT_DEPTH,      iris_hfi_gen2_set_bit_dpeth              },
+		{HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT, iris_hfi_gen2_set_min_output_count       },
+		{HFI_PROP_PIC_ORDER_CNT_TYPE,         iris_hfi_gen2_set_picture_order_count    },
+		{HFI_PROP_SIGNAL_COLOR_INFO,          iris_hfi_gen2_set_colorspace             },
+		{HFI_PROP_PROFILE,                    iris_hfi_gen2_set_profile                },
+		{HFI_PROP_LEVEL,                      iris_hfi_gen2_set_level                  },
+		{HFI_PROP_COLOR_FORMAT,               iris_hfi_gen2_set_colorformat            },
+		{HFI_PROP_LINEAR_STRIDE_SCANLINE,     iris_hfi_gen2_set_linear_stride_scanline },
+	};
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		config_params = core->iris_platform_data->input_config_params;
+		config_params_size = core->iris_platform_data->input_config_params_size;
+	} else {
+		config_params = core->iris_platform_data->output_config_params;
+		config_params_size = core->iris_platform_data->output_config_params_size;
+	}
+
+	if (!config_params || !config_params_size)
+		return -EINVAL;
+
+	for (i = 0; i < config_params_size; i++) {
+		for (j = 0; j < ARRAY_SIZE(prop_type_handle_arr); j++) {
+			if (prop_type_handle_arr[j].type == config_params[i]) {
+				ret = prop_type_handle_arr[j].handle(inst);
+				if (ret)
+					return ret;
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int iris_hfi_gen2_session_set_codec(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 codec = HFI_CODEC_DECODE_AVC;
+
+	iris_hfi_gen2_packet_session_property(inst,
+					      HFI_PROP_CODEC,
+					      HFI_HOST_FLAGS_NONE,
+					      HFI_PORT_NONE,
+					      HFI_PAYLOAD_U32_ENUM,
+					      &codec,
+					      sizeof(u32));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_set_default_header(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 default_header = false;
+
+	iris_hfi_gen2_packet_session_property(inst,
+					      HFI_PROP_DEC_DEFAULT_HEADER,
+					      HFI_HOST_FLAGS_NONE,
+					      HFI_PORT_BITSTREAM,
+					      HFI_PAYLOAD_U32,
+					      &default_header,
+					      sizeof(u32));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_open(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	int ret;
+
+	if (inst->state != IRIS_INST_DEINIT)
+		return -EALREADY;
+
+	inst_hfi_gen2->ipsc_properties_set = false;
+	inst_hfi_gen2->opsc_properties_set = false;
+
+	inst_hfi_gen2->packet = kzalloc(4096, GFP_KERNEL);
+	if (!inst_hfi_gen2->packet)
+		return -ENOMEM;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_OPEN,
+					     HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED,
+					     HFI_PORT_NONE,
+					     0,
+					     HFI_PAYLOAD_U32,
+					     &inst->session_id,
+					     sizeof(u32));
+
+	ret = iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+				       inst_hfi_gen2->packet->size);
+	if (ret)
+		goto fail_free_packet;
+
+	ret = iris_hfi_gen2_session_set_codec(inst);
+	if (ret)
+		goto fail_free_packet;
+
+	ret = iris_hfi_gen2_session_set_default_header(inst);
+	if (ret)
+		goto fail_free_packet;
+
+	return 0;
+
+fail_free_packet:
+	kfree(inst_hfi_gen2->packet);
+	inst_hfi_gen2->packet = NULL;
+
+	return ret;
+}
+
+static int iris_hfi_gen2_session_close(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	int ret;
+
+	if (!inst_hfi_gen2->packet)
+		return -EINVAL;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_CLOSE,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED |
+					     HFI_HOST_FLAGS_NON_DISCARDABLE),
+					     HFI_PORT_NONE,
+					     inst->session_id,
+					     HFI_PAYLOAD_NONE,
+					     NULL,
+					     0);
+
+	ret = iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+				       inst_hfi_gen2->packet->size);
+
+	kfree(inst_hfi_gen2->packet);
+	inst_hfi_gen2->packet = NULL;
+
+	return ret;
+}
+
+static int iris_hfi_gen2_session_subscribe_mode(struct iris_inst *inst,
+						u32 cmd, u32 plane, u32 payload_type,
+						void *payload, u32 payload_size)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     cmd,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     payload_type,
+					     payload,
+					     payload_size);
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_subscribe_change_param(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct hfi_subscription_params subsc_params;
+	u32 prop_type, payload_size, payload_type;
+	struct iris_core *core = inst->core;
+	const u32 *change_param;
+	u32 change_param_size;
+	u32 payload[32] = {0};
+	u32 hfi_port = 0, i;
+	int ret;
+
+	if ((V4L2_TYPE_IS_OUTPUT(plane) && inst_hfi_gen2->ipsc_properties_set) ||
+	    (V4L2_TYPE_IS_CAPTURE(plane) && inst_hfi_gen2->opsc_properties_set)) {
+		dev_err(core->dev, "invalid plane\n");
+		return 0;
+	}
+
+	change_param = core->iris_platform_data->input_config_params;
+	change_param_size = core->iris_platform_data->input_config_params_size;
+
+	payload[0] = HFI_MODE_PORT_SETTINGS_CHANGE;
+
+	for (i = 0; i < change_param_size; i++)
+		payload[i + 1] = change_param[i];
+
+	ret = iris_hfi_gen2_session_subscribe_mode(inst,
+						   HFI_CMD_SUBSCRIBE_MODE,
+						   plane,
+						   HFI_PAYLOAD_U32_ARRAY,
+						   &payload[0],
+						   ((change_param_size + 1) * sizeof(u32)));
+	if (ret)
+		return ret;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		inst_hfi_gen2->ipsc_properties_set = true;
+	} else {
+		hfi_port = iris_hfi_gen2_get_port(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+		memcpy(&inst_hfi_gen2->dst_subcr_params,
+		       &inst_hfi_gen2->src_subcr_params,
+		       sizeof(inst_hfi_gen2->src_subcr_params));
+		subsc_params = inst_hfi_gen2->dst_subcr_params;
+		for (i = 0; i < change_param_size; i++) {
+			payload[0] = 0;
+			payload[1] = 0;
+			payload_size = 0;
+			payload_type = 0;
+			prop_type = change_param[i];
+			switch (prop_type) {
+			case HFI_PROP_BITSTREAM_RESOLUTION:
+				payload[0] = subsc_params.bitstream_resolution;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_CROP_OFFSETS:
+				payload[0] = subsc_params.crop_offsets[0];
+				payload[1] = subsc_params.crop_offsets[1];
+				payload_size = sizeof(u64);
+				payload_type = HFI_PAYLOAD_64_PACKED;
+				break;
+			case HFI_PROP_CODED_FRAMES:
+				payload[0] = subsc_params.coded_frames;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT:
+				payload[0] = subsc_params.fw_min_count;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_PIC_ORDER_CNT_TYPE:
+				payload[0] = subsc_params.pic_order_cnt;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_SIGNAL_COLOR_INFO:
+				payload[0] = subsc_params.color_info;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_PROFILE:
+				payload[0] = subsc_params.profile;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			case HFI_PROP_LEVEL:
+				payload[0] = subsc_params.level;
+				payload_size = sizeof(u32);
+				payload_type = HFI_PAYLOAD_U32;
+				break;
+			default:
+				prop_type = 0;
+				ret = -EINVAL;
+				break;
+			}
+			if (prop_type) {
+				ret = iris_hfi_gen2_session_set_property(inst,
+									 prop_type,
+									 HFI_HOST_FLAGS_NONE,
+									 hfi_port,
+									 payload_type,
+									 &payload,
+									 payload_size);
+				if (ret)
+					return ret;
+			}
+		}
+		inst_hfi_gen2->opsc_properties_set = true;
+	}
+
+	return 0;
+}
+
+static int iris_hfi_gen2_subscribe_property(struct iris_inst *inst, u32 plane)
+{
+	struct iris_core *core = inst->core;
+	u32 subscribe_prop_size, i;
+	const u32 *subcribe_prop;
+	u32 payload[32] = {0};
+
+	payload[0] = HFI_MODE_PROPERTY;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		subscribe_prop_size = core->iris_platform_data->dec_input_prop_size;
+		subcribe_prop = core->iris_platform_data->dec_input_prop;
+	} else {
+		subscribe_prop_size = core->iris_platform_data->dec_output_prop_size;
+		subcribe_prop = core->iris_platform_data->dec_output_prop;
+	}
+
+	for (i = 0; i < subscribe_prop_size; i++)
+		payload[i + 1] = subcribe_prop[i];
+
+	return iris_hfi_gen2_session_subscribe_mode(inst,
+						    HFI_CMD_SUBSCRIBE_MODE,
+						    plane,
+						    HFI_PAYLOAD_U32_ARRAY,
+						    &payload[0],
+						    (subscribe_prop_size + 1) * sizeof(u32));
+}
+
+static int iris_hfi_gen2_session_start(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	int ret = 0;
+
+	ret = iris_hfi_gen2_subscribe_change_param(inst, plane);
+	if (ret)
+		return ret;
+
+	ret = iris_hfi_gen2_subscribe_property(inst, plane);
+	if (ret)
+		return ret;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_START,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_NONE,
+					     NULL,
+					     0);
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_stop(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	int ret = 0;
+
+	reinit_completion(&inst->completion);
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_STOP,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED |
+					     HFI_HOST_FLAGS_NON_DISCARDABLE),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_NONE,
+					     NULL,
+					     0);
+
+	ret = iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+				       inst_hfi_gen2->packet->size);
+	if (ret)
+		return ret;
+
+	return iris_wait_for_session_response(inst, false);
+}
+
+static int iris_hfi_gen2_session_pause(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_PAUSE,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_NONE,
+					     NULL,
+					     0);
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_resume_drc(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 payload = HFI_CMD_SETTINGS_CHANGE;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_RESUME,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_U32,
+					     &payload,
+					     sizeof(u32));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_resume_drain(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 payload = HFI_CMD_DRAIN;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_RESUME,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_U32,
+					     &payload,
+					     sizeof(u32));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_drain(struct iris_inst *inst, u32 plane)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+
+	if (!V4L2_TYPE_IS_OUTPUT(plane))
+		return 0;
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_DRAIN,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED |
+					     HFI_HOST_FLAGS_NON_DISCARDABLE),
+					     iris_hfi_gen2_get_port(plane),
+					     inst->session_id,
+					     HFI_PAYLOAD_NONE,
+					     NULL,
+					     0);
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static u32 iris_hfi_gen2_buf_type_from_driver(enum iris_buffer_type buffer_type)
+{
+	switch (buffer_type) {
+	case BUF_INPUT:
+		return HFI_BUFFER_BITSTREAM;
+	case BUF_OUTPUT:
+		return HFI_BUFFER_RAW;
+	case BUF_BIN:
+		return HFI_BUFFER_BIN;
+	case BUF_COMV:
+		return HFI_BUFFER_COMV;
+	case BUF_NON_COMV:
+		return HFI_BUFFER_NON_COMV;
+	case BUF_LINE:
+		return HFI_BUFFER_LINE;
+	case BUF_DPB:
+		return HFI_BUFFER_DPB;
+	case BUF_PERSIST:
+		return HFI_BUFFER_PERSIST;
+	default:
+		return 0;
+	}
+}
+
+static int iris_set_num_comv(struct iris_inst *inst)
+{
+	struct platform_inst_caps *caps;
+	struct iris_core *core = inst->core;
+	u32 num_comv;
+
+	caps = core->iris_platform_data->inst_caps;
+	num_comv = caps->num_comv;
+
+	return core->hfi_ops->session_set_property(inst,
+						   HFI_PROP_COMV_BUFFER_COUNT,
+						   HFI_HOST_FLAGS_NONE,
+						   HFI_PORT_BITSTREAM,
+						   HFI_PAYLOAD_U32,
+						   &num_comv, sizeof(u32));
+}
+
+static void iris_hfi_gen2_get_buffer(struct iris_buffer *buffer, struct iris_hfi_buffer *buf)
+{
+	memset(buf, 0, sizeof(*buf));
+	buf->type = iris_hfi_gen2_buf_type_from_driver(buffer->type);
+	buf->index = buffer->index;
+	buf->base_address = buffer->device_addr;
+	buf->addr_offset = 0;
+	buf->buffer_size = buffer->buffer_size;
+
+	if (buffer->type == BUF_INPUT)
+		buf->buffer_size = ALIGN(buffer->buffer_size, 256);
+	buf->data_offset = buffer->data_offset;
+	buf->data_size = buffer->data_size;
+	if (buffer->attr & BUF_ATTR_PENDING_RELEASE)
+		buf->flags |= HFI_BUF_HOST_FLAG_RELEASE;
+	buf->flags |= HFI_BUF_HOST_FLAGS_CB_NON_SECURE;
+	buf->timestamp = buffer->timestamp;
+}
+
+static int iris_hfi_gen2_session_queue_buffer(struct iris_inst *inst, struct iris_buffer *buffer)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct iris_hfi_buffer hfi_buffer;
+	u32 port;
+	int ret;
+
+	iris_hfi_gen2_get_buffer(buffer, &hfi_buffer);
+	if (buffer->type == BUF_COMV) {
+		ret = iris_set_num_comv(inst);
+		if (ret)
+			return ret;
+	}
+
+	port = iris_hfi_gen2_get_port_from_buf_type(buffer->type);
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_BUFFER,
+					     HFI_HOST_FLAGS_INTR_REQUIRED,
+					     port,
+					     inst->session_id,
+					     HFI_PAYLOAD_STRUCTURE,
+					     &hfi_buffer,
+					     sizeof(hfi_buffer));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static int iris_hfi_gen2_session_release_buffer(struct iris_inst *inst, struct iris_buffer *buffer)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct iris_hfi_buffer hfi_buffer;
+	u32 port;
+
+	iris_hfi_gen2_get_buffer(buffer, &hfi_buffer);
+	hfi_buffer.flags |= HFI_BUF_HOST_FLAG_RELEASE;
+	port = iris_hfi_gen2_get_port_from_buf_type(buffer->type);
+
+	iris_hfi_gen2_packet_session_command(inst,
+					     HFI_CMD_BUFFER,
+					     (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+					     HFI_HOST_FLAGS_INTR_REQUIRED),
+					     port,
+					     inst->session_id,
+					     HFI_PAYLOAD_STRUCTURE,
+					     &hfi_buffer,
+					     sizeof(hfi_buffer));
+
+	return iris_hfi_queue_cmd_write(inst->core, inst_hfi_gen2->packet,
+					inst_hfi_gen2->packet->size);
+}
+
+static const struct iris_hfi_command_ops iris_hfi_gen2_command_ops = {
+	.sys_init = iris_hfi_gen2_sys_init,
+	.sys_image_version = iris_hfi_gen2_sys_image_version,
+	.sys_interframe_powercollapse = iris_hfi_gen2_sys_interframe_powercollapse,
+	.sys_pc_prep = iris_hfi_gen2_sys_pc_prep,
+	.session_open = iris_hfi_gen2_session_open,
+	.session_set_config_params = iris_hfi_gen2_session_set_config_params,
+	.session_set_property = iris_hfi_gen2_session_set_property,
+	.session_start = iris_hfi_gen2_session_start,
+	.session_queue_buf = iris_hfi_gen2_session_queue_buffer,
+	.session_release_buf = iris_hfi_gen2_session_release_buffer,
+	.session_pause = iris_hfi_gen2_session_pause,
+	.session_resume_drc = iris_hfi_gen2_session_resume_drc,
+	.session_stop = iris_hfi_gen2_session_stop,
+	.session_drain = iris_hfi_gen2_session_drain,
+	.session_resume_drain = iris_hfi_gen2_session_resume_drain,
+	.session_close = iris_hfi_gen2_session_close,
+};
+
+void iris_hfi_gen2_command_ops_init(struct iris_core *core)
+{
+	core->hfi_ops = &iris_hfi_gen2_command_ops;
+}
+
+struct iris_inst *iris_hfi_gen2_get_instance(void)
+{
+	return kzalloc(sizeof(struct iris_inst_hfi_gen2), GFP_KERNEL);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2_defines.h b/drivers/media/platform/qcom/iris/iris_hfi_gen2_defines.h
new file mode 100644
index 000000000000..806f8bb7f505
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2_defines.h
@@ -0,0 +1,161 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_GEN2_DEFINES_H__
+#define __IRIS_HFI_GEN2_DEFINES_H__
+
+#include <linux/types.h>
+
+#define HFI_VIDEO_ARCH_LX			0x1
+
+#define HFI_CMD_BEGIN				0x01000000
+#define HFI_CMD_INIT				0x01000001
+#define HFI_CMD_POWER_COLLAPSE			0x01000002
+#define HFI_CMD_OPEN				0x01000003
+#define HFI_CMD_CLOSE				0x01000004
+#define HFI_CMD_START				0x01000005
+#define HFI_CMD_STOP				0x01000006
+#define HFI_CMD_DRAIN				0x01000007
+#define HFI_CMD_RESUME				0x01000008
+#define HFI_CMD_BUFFER				0x01000009
+#define HFI_CMD_SUBSCRIBE_MODE			0x0100000B
+#define HFI_CMD_SETTINGS_CHANGE			0x0100000C
+#define HFI_CMD_PAUSE				0x01000011
+#define HFI_CMD_END				0x01FFFFFF
+
+#define HFI_BITMASK_BITSTREAM_WIDTH		0xffff0000
+#define HFI_BITMASK_BITSTREAM_HEIGHT		0x0000ffff
+#define HFI_BITMASK_FRAME_MBS_ONLY_FLAG		0x00000001
+
+#define HFI_PROP_BEGIN				0x03000000
+#define HFI_PROP_IMAGE_VERSION			0x03000001
+#define HFI_PROP_INTRA_FRAME_POWER_COLLAPSE	0x03000002
+#define HFI_PROP_UBWC_MAX_CHANNELS		0x03000003
+#define HFI_PROP_UBWC_MAL_LENGTH		0x03000004
+#define HFI_PROP_UBWC_HBB			0x03000005
+#define HFI_PROP_UBWC_BANK_SWZL_LEVEL1		0x03000006
+#define HFI_PROP_UBWC_BANK_SWZL_LEVEL2		0x03000007
+#define HFI_PROP_UBWC_BANK_SWZL_LEVEL3		0x03000008
+#define HFI_PROP_UBWC_BANK_SPREADING		0x03000009
+#define HFI_PROP_CODEC				0x03000100
+#define HFI_PROP_COLOR_FORMAT			0x03000101
+#define HFI_PROP_BITSTREAM_RESOLUTION		0x03000103
+#define HFI_PROP_LINEAR_STRIDE_SCANLINE		0x03000104
+#define HFI_PROP_CROP_OFFSETS			0x03000105
+#define HFI_PROP_PROFILE			0x03000107
+#define HFI_PROP_LEVEL				0x03000108
+#define HFI_PROP_STAGE				0x0300010a
+#define HFI_PROP_PIPE				0x0300010b
+#define HFI_PROP_LUMA_CHROMA_BIT_DEPTH		0x0300010f
+#define HFI_PROP_CODED_FRAMES			0x03000120
+#define HFI_PROP_CABAC_SESSION			0x03000121
+#define HFI_PROP_BUFFER_HOST_MAX_COUNT		0x03000123
+#define HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT	0x03000124
+#define HFI_PROP_PIC_ORDER_CNT_TYPE		0x03000128
+#define HFI_PROP_QUALITY_MODE			0x03000148
+#define HFI_PROP_SIGNAL_COLOR_INFO		0x03000155
+#define HFI_PROP_PICTURE_TYPE			0x03000162
+#define HFI_PROP_DEC_DEFAULT_HEADER		0x03000168
+#define HFI_PROP_DEC_START_FROM_RAP_FRAME	0x03000169
+#define HFI_PROP_NO_OUTPUT			0x0300016a
+#define HFI_PROP_COMV_BUFFER_COUNT		0x03000193
+#define HFI_PROP_END				0x03FFFFFF
+
+#define HFI_SESSION_ERROR_BEGIN			0x04000000
+#define HFI_ERROR_UNKNOWN_SESSION		0x04000001
+#define HFI_ERROR_MAX_SESSIONS			0x04000002
+#define HFI_ERROR_FATAL				0x04000003
+#define HFI_ERROR_INVALID_STATE			0x04000004
+#define HFI_ERROR_INSUFFICIENT_RESOURCES	0x04000005
+#define HFI_ERROR_BUFFER_NOT_SET		0x04000006
+#define HFI_ERROR_STREAM_UNSUPPORTED		0x04000008
+#define HFI_SESSION_ERROR_END			0x04FFFFFF
+
+#define HFI_SYSTEM_ERROR_BEGIN			0x05000000
+#define HFI_SYS_ERROR_WD_TIMEOUT		0x05000001
+#define HFI_SYSTEM_ERROR_END			0x05FFFFFF
+
+#define HFI_INFORMATION_BEGIN			0x06000000
+#define HFI_INFO_UNSUPPORTED			0x06000001
+#define HFI_INFO_DATA_CORRUPT			0x06000002
+#define HFI_INFO_BUFFER_OVERFLOW		0x06000004
+#define HFI_INFO_HFI_FLAG_DRAIN_LAST		0x06000006
+#define HFI_INFO_HFI_FLAG_PSC_LAST		0x06000007
+#define HFI_INFORMATION_END			0x06FFFFFF
+
+enum hfi_property_mode_type {
+	HFI_MODE_PORT_SETTINGS_CHANGE		= 0x00000001,
+	HFI_MODE_PROPERTY			= 0x00000002,
+};
+
+enum hfi_color_format {
+	HFI_COLOR_FMT_OPAQUE			= 0,
+	HFI_COLOR_FMT_NV12			= 1,
+	HFI_COLOR_FMT_NV12_UBWC			= 2,
+	HFI_COLOR_FMT_P010			= 3,
+	HFI_COLOR_FMT_TP10_UBWC			= 4,
+	HFI_COLOR_FMT_RGBA8888			= 5,
+	HFI_COLOR_FMT_RGBA8888_UBWC		= 6,
+	HFI_COLOR_FMT_NV21			= 7,
+};
+
+enum hfi_codec_type {
+	HFI_CODEC_DECODE_AVC			= 1,
+	HFI_CODEC_ENCODE_AVC			= 2,
+};
+
+enum hfi_picture_type {
+	HFI_PICTURE_IDR				= 0x00000001,
+	HFI_PICTURE_P				= 0x00000002,
+	HFI_PICTURE_B				= 0x00000004,
+	HFI_PICTURE_I				= 0x00000008,
+	HFI_PICTURE_CRA				= 0x00000010,
+	HFI_PICTURE_BLA				= 0x00000020,
+};
+
+enum hfi_buffer_type {
+	HFI_BUFFER_BITSTREAM			= 0x00000001,
+	HFI_BUFFER_RAW				= 0x00000002,
+	HFI_BUFFER_METADATA			= 0x00000003,
+	HFI_BUFFER_SUBCACHE			= 0x00000004,
+	HFI_BUFFER_PARTIAL_DATA			= 0x00000005,
+	HFI_BUFFER_DPB				= 0x00000006,
+	HFI_BUFFER_BIN				= 0x00000007,
+	HFI_BUFFER_LINE				= 0x00000008,
+	HFI_BUFFER_ARP				= 0x00000009,
+	HFI_BUFFER_COMV				= 0x0000000A,
+	HFI_BUFFER_NON_COMV			= 0x0000000B,
+	HFI_BUFFER_PERSIST			= 0x0000000C,
+	HFI_BUFFER_VPSS				= 0x0000000D,
+};
+
+enum hfi_buffer_host_flags {
+	HFI_BUF_HOST_FLAG_RELEASE		= 0x00000001,
+	HFI_BUF_HOST_FLAG_READONLY		= 0x00000010,
+	HFI_BUF_HOST_FLAG_CODEC_CONFIG		= 0x00000100,
+	HFI_BUF_HOST_FLAGS_CB_NON_SECURE	= 0x00000200,
+};
+
+enum hfi_buffer_firmware_flags {
+	HFI_BUF_FW_FLAG_RELEASE_DONE		= 0x00000001,
+	HFI_BUF_FW_FLAG_READONLY		= 0x00000010,
+	HFI_BUF_FW_FLAG_LAST			= 0x10000000,
+	HFI_BUF_FW_FLAG_PSC_LAST		= 0x20000000,
+};
+
+enum hfi_packet_firmware_flags {
+	HFI_FW_FLAGS_SUCCESS			= 0x00000001,
+	HFI_FW_FLAGS_INFORMATION		= 0x00000002,
+	HFI_FW_FLAGS_SESSION_ERROR		= 0x00000004,
+	HFI_FW_FLAGS_SYSTEM_ERROR		= 0x00000008,
+};
+
+struct hfi_debug_header {
+	u32 size;
+	u32 debug_level;
+	u32 reserved[2];
+};
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.c b/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.c
new file mode 100644
index 000000000000..d77fa29f44fc
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.c
@@ -0,0 +1,292 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_hfi_common.h"
+#include "iris_hfi_gen2.h"
+#include "iris_hfi_gen2_packet.h"
+
+u32 iris_hfi_gen2_get_color_primaries(u32 primaries)
+{
+	switch (primaries) {
+	case V4L2_COLORSPACE_DEFAULT:
+		return HFI_PRIMARIES_RESERVED;
+	case V4L2_COLORSPACE_REC709:
+		return HFI_PRIMARIES_BT709;
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+		return HFI_PRIMARIES_BT470_SYSTEM_M;
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		return HFI_PRIMARIES_BT470_SYSTEM_BG;
+	case V4L2_COLORSPACE_SMPTE170M:
+		return HFI_PRIMARIES_BT601_525;
+	case V4L2_COLORSPACE_SMPTE240M:
+		return HFI_PRIMARIES_SMPTE_ST240M;
+	case V4L2_COLORSPACE_BT2020:
+		return HFI_PRIMARIES_BT2020;
+	case V4L2_COLORSPACE_DCI_P3:
+		return HFI_PRIMARIES_SMPTE_RP431_2;
+	default:
+		return HFI_PRIMARIES_RESERVED;
+	}
+}
+
+u32 iris_hfi_gen2_get_transfer_char(u32 characterstics)
+{
+	switch (characterstics) {
+	case V4L2_XFER_FUNC_DEFAULT:
+		return HFI_TRANSFER_RESERVED;
+	case V4L2_XFER_FUNC_709:
+		return HFI_TRANSFER_BT709;
+	case V4L2_XFER_FUNC_SMPTE240M:
+		return HFI_TRANSFER_SMPTE_ST240M;
+	case V4L2_XFER_FUNC_SRGB:
+		return HFI_TRANSFER_SRGB_SYCC;
+	case V4L2_XFER_FUNC_SMPTE2084:
+		return HFI_TRANSFER_SMPTE_ST2084_PQ;
+	default:
+		return HFI_TRANSFER_RESERVED;
+	}
+}
+
+u32 iris_hfi_gen2_get_matrix_coefficients(u32 coefficients)
+{
+	switch (coefficients) {
+	case V4L2_YCBCR_ENC_DEFAULT:
+		return HFI_MATRIX_COEFF_RESERVED;
+	case V4L2_YCBCR_ENC_709:
+		return HFI_MATRIX_COEFF_BT709;
+	case V4L2_YCBCR_ENC_XV709:
+		return HFI_MATRIX_COEFF_BT709;
+	case V4L2_YCBCR_ENC_XV601:
+		return HFI_MATRIX_COEFF_BT470_SYS_BG_OR_BT601_625;
+	case V4L2_YCBCR_ENC_601:
+		return HFI_MATRIX_COEFF_BT601_525_BT1358_525_OR_625;
+	case V4L2_YCBCR_ENC_SMPTE240M:
+		return HFI_MATRIX_COEFF_SMPTE_ST240;
+	case V4L2_YCBCR_ENC_BT2020:
+		return HFI_MATRIX_COEFF_BT2020_NON_CONSTANT;
+	case V4L2_YCBCR_ENC_BT2020_CONST_LUM:
+		return HFI_MATRIX_COEFF_BT2020_CONSTANT;
+	default:
+		return HFI_MATRIX_COEFF_RESERVED;
+	}
+}
+
+u32 iris_hfi_gen2_get_color_info(u32 matrix_coeff, u32 transfer_char, u32 primaries,
+				 u32 colour_description_present_flag, u32 full_range,
+				 u32 video_format, u32 video_signal_type_present_flag)
+{
+	return (matrix_coeff & 0xFF) |
+		((transfer_char << 8) & 0xFF00) |
+		((primaries << 16) & 0xFF0000) |
+		((colour_description_present_flag << 24) & 0x1000000) |
+		((full_range << 25) & 0x2000000) |
+		((video_format << 26) & 0x1C000000) |
+		((video_signal_type_present_flag << 29) & 0x20000000);
+}
+
+static void iris_hfi_gen2_create_header(struct iris_hfi_header *hdr,
+					u32 session_id, u32 header_id)
+{
+	memset(hdr, 0, sizeof(*hdr));
+
+	hdr->size = sizeof(*hdr);
+	hdr->session_id = session_id;
+	hdr->header_id = header_id;
+	hdr->num_packets = 0;
+}
+
+static void iris_hfi_gen2_create_packet(struct iris_hfi_header *hdr, u32 pkt_type,
+					u32 pkt_flags, u32 payload_type, u32 port,
+					u32 packet_id, void *payload, u32 payload_size)
+{
+	struct iris_hfi_packet *pkt = (struct iris_hfi_packet *)((u8 *)hdr + hdr->size);
+	u32 pkt_size = sizeof(*pkt) + payload_size;
+
+	memset(pkt, 0, pkt_size);
+	pkt->size = pkt_size;
+	pkt->type = pkt_type;
+	pkt->flags = pkt_flags;
+	pkt->payload_info = payload_type;
+	pkt->port = port;
+	pkt->packet_id = packet_id;
+	if (payload_size)
+		memcpy(&pkt->payload[0], payload, payload_size);
+
+	hdr->num_packets++;
+	hdr->size += pkt->size;
+}
+
+void iris_hfi_gen2_packet_sys_init(struct iris_core *core, struct iris_hfi_header *hdr)
+{
+	u32 payload = 0;
+
+	iris_hfi_gen2_create_header(hdr, 0, core->header_id++);
+
+	payload = HFI_VIDEO_ARCH_LX;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_CMD_INIT,
+				    (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+				    HFI_HOST_FLAGS_INTR_REQUIRED |
+				    HFI_HOST_FLAGS_NON_DISCARDABLE),
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->max_channels;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_MAX_CHANNELS,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->mal_length;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_MAL_LENGTH,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->highest_bank_bit;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_HBB,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->bank_swzl_level;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_BANK_SWZL_LEVEL1,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->bank_swz2_level;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_BANK_SWZL_LEVEL2,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->bank_swz3_level;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_BANK_SWZL_LEVEL3,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+
+	payload = core->iris_platform_data->ubwc_config->bank_spreading;
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_UBWC_BANK_SPREADING,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+}
+
+void iris_hfi_gen2_packet_image_version(struct iris_core *core, struct iris_hfi_header *hdr)
+{
+	iris_hfi_gen2_create_header(hdr, 0, core->header_id++);
+
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_IMAGE_VERSION,
+				    (HFI_HOST_FLAGS_RESPONSE_REQUIRED |
+				    HFI_HOST_FLAGS_INTR_REQUIRED |
+				    HFI_HOST_FLAGS_GET_PROPERTY),
+				    HFI_PAYLOAD_NONE,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    NULL, 0);
+}
+
+void iris_hfi_gen2_packet_session_command(struct iris_inst *inst, u32 pkt_type,
+					  u32 flags, u32 port, u32 session_id,
+					  u32 payload_type, void *payload,
+					  u32 payload_size)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct iris_core *core = inst->core;
+
+	iris_hfi_gen2_create_header(inst_hfi_gen2->packet, session_id, core->header_id++);
+
+	iris_hfi_gen2_create_packet(inst_hfi_gen2->packet,
+				    pkt_type,
+				    flags,
+				    payload_type,
+				    port,
+				    core->packet_id++,
+				    payload,
+				    payload_size);
+}
+
+void iris_hfi_gen2_packet_session_property(struct iris_inst *inst,
+					   u32 pkt_type, u32 flags, u32 port,
+					   u32 payload_type, void *payload, u32 payload_size)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct iris_core *core = inst->core;
+
+	iris_hfi_gen2_create_header(inst_hfi_gen2->packet, inst->session_id, core->header_id++);
+
+	iris_hfi_gen2_create_packet(inst_hfi_gen2->packet,
+				    pkt_type,
+				    flags,
+				    payload_type,
+				    port,
+				    core->packet_id++,
+				    payload,
+				    payload_size);
+}
+
+void iris_hfi_gen2_packet_sys_interframe_powercollapse(struct iris_core *core,
+						       struct iris_hfi_header *hdr)
+{
+	u32 payload = 1; /* HFI_TRUE */
+
+	iris_hfi_gen2_create_header(hdr, 0 /*session_id*/, core->header_id++);
+
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_PROP_INTRA_FRAME_POWER_COLLAPSE,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_U32,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    &payload,
+				    sizeof(u32));
+}
+
+void iris_hfi_gen2_packet_sys_pc_prep(struct iris_core *core, struct iris_hfi_header *hdr)
+{
+	iris_hfi_gen2_create_header(hdr, 0 /*session_id*/, core->header_id++);
+
+	iris_hfi_gen2_create_packet(hdr,
+				    HFI_CMD_POWER_COLLAPSE,
+				    HFI_HOST_FLAGS_NONE,
+				    HFI_PAYLOAD_NONE,
+				    HFI_PORT_NONE,
+				    core->packet_id++,
+				    NULL, 0);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.h b/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.h
new file mode 100644
index 000000000000..25b9582349ca
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2_packet.h
@@ -0,0 +1,125 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_GEN2_PACKET_H__
+#define __IRIS_HFI_GEN2_PACKET_H__
+
+#include "iris_hfi_gen2_defines.h"
+
+struct iris_core;
+
+/**
+ * struct iris_hfi_header
+ *
+ * @size: size of the total packet in bytes including hfi_header
+ * @session_id: For session level hfi_header session_id is non-zero.
+ *                For  system level hfi_header session_id is zero.
+ * @header_id: unique header id for each hfi_header
+ * @reserved: reserved for future use
+ * @num_packets: number of hfi_packet that are included with the hfi_header
+ */
+struct iris_hfi_header {
+	u32 size;
+	u32 session_id;
+	u32 header_id;
+	u32 reserved[4];
+	u32 num_packets;
+};
+
+/**
+ * struct iris_hfi_packet
+ *
+ * @size: size of the hfi_packet in bytes including payload
+ * @type: one of the below hfi_packet types:
+ *        HFI_CMD_*,
+ *        HFI_PROP_*,
+ *        HFI_ERROR_*,
+ *        HFI_INFO_*,
+ *        HFI_SYS_ERROR_*
+ * @flags: hfi_packet flags. It is represented as bit masks.
+ *         host packet flags are "enum hfi_packet_host_flags"
+ *         firmware packet flags are "enum hfi_packet_firmware_flags"
+ * @payload_info: payload information indicated by "enum hfi_packet_payload_info"
+ * @port: hfi_packet port type indicated by "enum hfi_packet_port_type"
+ *        This is bitmask and may be applicable to multiple ports.
+ * @packet_id: host hfi_packet contains unique packet id.
+ *             firmware returns host packet id in response packet
+ *             wherever applicable. If not applicable firmware sets it to zero.
+ * @reserved: reserved for future use.
+ * @payload: flexible array of payload having additional packet information.
+ */
+struct iris_hfi_packet {
+	u32 size;
+	u32 type;
+	u32 flags;
+	u32 payload_info;
+	u32 port;
+	u32 packet_id;
+	u32 reserved[2];
+	u32 payload[];
+};
+
+/**
+ * struct iris_hfi_buffer
+ *
+ * @type: buffer type indicated by "enum hfi_buffer_type"
+ *        FW needs to return proper type for any buffer command.
+ * @index: index of the buffer
+ * @base_address: base address of the buffer.
+ *                This buffer address is always 4KBytes aligned.
+ * @addr_offset: accessible buffer offset from base address
+ *               Decoder bitstream buffer: 256 Bytes aligned
+ *               Firmware can uniquely identify a buffer based on
+ *               base_address & addr_offset.
+ *               HW can read memory only from base_address+addr_offset.
+ * @buffer_size: accessible buffer size in bytes starting from addr_offset
+ * @data_offset: data starts from "base_address + addr_offset + data_offset"
+ *               RAW buffer: data_offset is 0. Restriction: 4KBytes aligned
+ *               decoder bitstream buffer: no restriction (can be any value)
+ * @data_size: data size in bytes
+ * @flags: buffer flags. It is represented as bit masks.
+ *         host buffer flags are "enum hfi_buffer_host_flags"
+ *         firmware buffer flags are "enum hfi_buffer_firmware_flags"
+ * @timestamp: timestamp of the buffer in nano seconds (ns)
+ *             It is Presentation timestamp (PTS) for encoder & decoder.
+ *             Decoder: it is pass through from bitstream to raw buffer.
+ *                      firmware does not need to return as part of input buffer done.
+ *             For any internal buffers: there is no timestamp. Host sets as 0.
+ * @reserved: reserved for future use
+ */
+struct iris_hfi_buffer {
+	u32 type;
+	u32 index;
+	u64 base_address;
+	u32 addr_offset;
+	u32 buffer_size;
+	u32 data_offset;
+	u32 data_size;
+	u64 timestamp;
+	u32 flags;
+	u32 reserved[5];
+};
+
+u32 iris_hfi_gen2_get_color_primaries(u32 primaries);
+u32 iris_hfi_gen2_get_transfer_char(u32 characterstics);
+u32 iris_hfi_gen2_get_matrix_coefficients(u32 coefficients);
+u32 iris_hfi_gen2_get_color_info(u32 matrix_coeff, u32 transfer_char, u32 primaries,
+				 u32 colour_description_present_flag, u32 full_range,
+				 u32 video_format, u32 video_signal_type_present_flag);
+
+void iris_hfi_gen2_packet_sys_init(struct iris_core *core, struct iris_hfi_header *hdr);
+void iris_hfi_gen2_packet_image_version(struct iris_core *core, struct iris_hfi_header *hdr);
+void iris_hfi_gen2_packet_session_command(struct iris_inst *inst, u32 pkt_type,
+					  u32 flags, u32 port, u32 session_id,
+					  u32 payload_type, void *payload,
+					  u32 payload_size);
+void iris_hfi_gen2_packet_session_property(struct iris_inst *inst,
+					   u32 pkt_type, u32 flags, u32 port,
+					   u32 payload_type, void *payload, u32 payload_size);
+void iris_hfi_gen2_packet_sys_interframe_powercollapse(struct iris_core *core,
+						       struct iris_hfi_header *hdr);
+void iris_hfi_gen2_packet_sys_pc_prep(struct iris_core *core, struct iris_hfi_header *hdr);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_gen2_response.c b/drivers/media/platform/qcom/iris/iris_hfi_gen2_response.c
new file mode 100644
index 000000000000..b75a01641d5d
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_gen2_response.c
@@ -0,0 +1,934 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_hfi_gen2.h"
+#include "iris_hfi_gen2_defines.h"
+#include "iris_hfi_gen2_packet.h"
+#include "iris_vdec.h"
+#include "iris_vpu_buffer.h"
+#include "iris_vpu_common.h"
+
+struct iris_hfi_gen2_core_hfi_range {
+	u32 begin;
+	u32 end;
+	int (*handle)(struct iris_core *core, struct iris_hfi_packet *pkt);
+};
+
+struct iris_hfi_gen2_inst_hfi_range {
+	u32 begin;
+	u32 end;
+	int (*handle)(struct iris_inst *inst, struct iris_hfi_packet *pkt);
+};
+
+struct iris_hfi_gen2_packet_handle {
+	enum hfi_buffer_type type;
+	int (*handle)(struct iris_inst *inst, struct iris_hfi_packet *pkt);
+};
+
+static u32 iris_hfi_gen2_buf_type_to_driver(enum hfi_buffer_type buf_type)
+{
+	switch (buf_type) {
+	case HFI_BUFFER_BITSTREAM:
+		return BUF_INPUT;
+	case HFI_BUFFER_RAW:
+		return BUF_OUTPUT;
+	case HFI_BUFFER_BIN:
+		return BUF_BIN;
+	case HFI_BUFFER_ARP:
+		return BUF_ARP;
+	case HFI_BUFFER_COMV:
+		return BUF_COMV;
+	case HFI_BUFFER_NON_COMV:
+		return BUF_NON_COMV;
+	case HFI_BUFFER_LINE:
+		return BUF_LINE;
+	case HFI_BUFFER_DPB:
+		return BUF_DPB;
+	case HFI_BUFFER_PERSIST:
+		return BUF_PERSIST;
+	default:
+		return 0;
+	}
+}
+
+static bool iris_hfi_gen2_is_valid_hfi_buffer_type(u32 buffer_type)
+{
+	switch (buffer_type) {
+	case HFI_BUFFER_BITSTREAM:
+	case HFI_BUFFER_RAW:
+	case HFI_BUFFER_BIN:
+	case HFI_BUFFER_ARP:
+	case HFI_BUFFER_COMV:
+	case HFI_BUFFER_NON_COMV:
+	case HFI_BUFFER_LINE:
+	case HFI_BUFFER_DPB:
+	case HFI_BUFFER_PERSIST:
+	case HFI_BUFFER_VPSS:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool iris_hfi_gen2_is_valid_hfi_port(u32 port, u32 buffer_type)
+{
+	if (port == HFI_PORT_NONE && buffer_type != HFI_BUFFER_PERSIST)
+		return false;
+
+	if (port != HFI_PORT_BITSTREAM && port != HFI_PORT_RAW)
+		return false;
+
+	return true;
+}
+
+static int iris_hfi_gen2_get_driver_buffer_flags(struct iris_inst *inst, u32 hfi_flags)
+{
+	u32 keyframe = HFI_PICTURE_IDR | HFI_PICTURE_I | HFI_PICTURE_CRA | HFI_PICTURE_BLA;
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	u32 driver_flags = 0;
+
+	if (inst_hfi_gen2->hfi_frame_info.picture_type & keyframe)
+		driver_flags |= V4L2_BUF_FLAG_KEYFRAME;
+	else if (inst_hfi_gen2->hfi_frame_info.picture_type & HFI_PICTURE_P)
+		driver_flags |= V4L2_BUF_FLAG_PFRAME;
+	else if (inst_hfi_gen2->hfi_frame_info.picture_type & HFI_PICTURE_B)
+		driver_flags |= V4L2_BUF_FLAG_BFRAME;
+
+	if (inst_hfi_gen2->hfi_frame_info.data_corrupt || inst_hfi_gen2->hfi_frame_info.overflow)
+		driver_flags |= V4L2_BUF_FLAG_ERROR;
+
+	if (hfi_flags & HFI_BUF_FW_FLAG_LAST ||
+	    hfi_flags & HFI_BUF_FW_FLAG_PSC_LAST)
+		driver_flags |= V4L2_BUF_FLAG_LAST;
+
+	return driver_flags;
+}
+
+static bool iris_hfi_gen2_validate_packet_payload(struct iris_hfi_packet *pkt)
+{
+	u32 payload_size = 0;
+
+	switch (pkt->payload_info) {
+	case HFI_PAYLOAD_U32:
+	case HFI_PAYLOAD_S32:
+	case HFI_PAYLOAD_Q16:
+	case HFI_PAYLOAD_U32_ENUM:
+	case HFI_PAYLOAD_32_PACKED:
+		payload_size = 4;
+		break;
+	case HFI_PAYLOAD_U64:
+	case HFI_PAYLOAD_S64:
+	case HFI_PAYLOAD_64_PACKED:
+		payload_size = 8;
+		break;
+	case HFI_PAYLOAD_STRUCTURE:
+		if (pkt->type == HFI_CMD_BUFFER)
+			payload_size = sizeof(struct iris_hfi_buffer);
+		break;
+	default:
+		payload_size = 0;
+		break;
+	}
+
+	if (pkt->size < sizeof(struct iris_hfi_packet) + payload_size)
+		return false;
+
+	return true;
+}
+
+static int iris_hfi_gen2_validate_packet(u8 *response_pkt, u8 *core_resp_pkt)
+{
+	u8 *response_limit = core_resp_pkt + IFACEQ_CORE_PKT_SIZE;
+	u32 response_pkt_size = *(u32 *)response_pkt;
+
+	if (!response_pkt_size)
+		return -EINVAL;
+
+	if (response_pkt_size < sizeof(struct iris_hfi_packet))
+		return -EINVAL;
+
+	if (response_pkt + response_pkt_size > response_limit)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int iris_hfi_gen2_validate_hdr_packet(struct iris_core *core, struct iris_hfi_header *hdr)
+{
+	struct iris_hfi_packet *packet;
+	int ret;
+	u8 *pkt;
+	u32 i;
+
+	if (hdr->size < sizeof(*hdr) + sizeof(*packet))
+		return -EINVAL;
+
+	pkt = (u8 *)((u8 *)hdr + sizeof(*hdr));
+
+	for (i = 0; i < hdr->num_packets; i++) {
+		packet = (struct iris_hfi_packet *)pkt;
+		ret = iris_hfi_gen2_validate_packet(pkt, core->response_packet);
+		if (ret)
+			return ret;
+
+		pkt += packet->size;
+	}
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_session_info(struct iris_inst *inst,
+					     struct iris_hfi_packet *pkt)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct iris_core *core = inst->core;
+	int ret = 0;
+	char *info;
+
+	switch (pkt->type) {
+	case HFI_INFO_UNSUPPORTED:
+		info = "unsupported";
+		break;
+	case HFI_INFO_DATA_CORRUPT:
+		info = "data corrupt";
+		inst_hfi_gen2->hfi_frame_info.data_corrupt = 1;
+		break;
+	case HFI_INFO_BUFFER_OVERFLOW:
+		info = "buffer overflow";
+		inst_hfi_gen2->hfi_frame_info.overflow = 1;
+		break;
+	case HFI_INFO_HFI_FLAG_DRAIN_LAST:
+		info = "drain last flag";
+		ret = iris_inst_sub_state_change_drain_last(inst);
+		break;
+	case HFI_INFO_HFI_FLAG_PSC_LAST:
+		info = "drc last flag";
+		ret = iris_inst_sub_state_change_drc_last(inst);
+		break;
+	default:
+		info = "unknown";
+		break;
+	}
+
+	dev_dbg(core->dev, "session info received %#x: %s\n",
+		pkt->type, info);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_handle_session_error(struct iris_inst *inst,
+					      struct iris_hfi_packet *pkt)
+{
+	struct iris_core *core = inst->core;
+	char *error;
+
+	switch (pkt->type) {
+	case HFI_ERROR_MAX_SESSIONS:
+		error = "exceeded max sessions";
+		break;
+	case HFI_ERROR_UNKNOWN_SESSION:
+		error = "unknown session id";
+		break;
+	case HFI_ERROR_INVALID_STATE:
+		error = "invalid operation for current state";
+		break;
+	case HFI_ERROR_INSUFFICIENT_RESOURCES:
+		error = "insufficient resources";
+		break;
+	case HFI_ERROR_BUFFER_NOT_SET:
+		error = "internal buffers not set";
+		break;
+	case HFI_ERROR_FATAL:
+		error = "fatal error";
+		break;
+	case HFI_ERROR_STREAM_UNSUPPORTED:
+		error = "unsupported stream";
+		break;
+	default:
+		error = "unknown";
+		break;
+	}
+
+	dev_err(core->dev, "session error received %#x: %s\n", pkt->type, error);
+	iris_vb2_queue_error(inst);
+	iris_inst_change_state(inst, IRIS_INST_ERROR);
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_system_error(struct iris_core *core,
+					     struct iris_hfi_packet *pkt)
+{
+	struct iris_inst *instance;
+
+	dev_err(core->dev, "received system error of type %#x\n", pkt->type);
+
+	core->state = IRIS_CORE_ERROR;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(instance, &core->instances, list)
+		iris_inst_change_state(instance, IRIS_INST_ERROR);
+	mutex_unlock(&core->lock);
+
+	schedule_delayed_work(&core->sys_error_handler, msecs_to_jiffies(10));
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_system_init(struct iris_core *core,
+					    struct iris_hfi_packet *pkt)
+{
+	if (!(pkt->flags & HFI_FW_FLAGS_SUCCESS)) {
+		core->state = IRIS_CORE_ERROR;
+		return 0;
+	}
+
+	complete(&core->core_init_done);
+
+	return 0;
+}
+
+static void iris_hfi_gen2_handle_session_close(struct iris_inst *inst,
+					       struct iris_hfi_packet *pkt)
+{
+	if (!(pkt->flags & HFI_FW_FLAGS_SUCCESS)) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return;
+	}
+
+	complete(&inst->completion);
+}
+
+static int iris_hfi_gen2_handle_input_buffer(struct iris_inst *inst,
+					     struct iris_hfi_buffer *buffer)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *m2m_buffer, *n;
+	struct iris_buffer *buf;
+	bool found = false;
+
+	v4l2_m2m_for_each_src_buf_safe(m2m_ctx, m2m_buffer, n) {
+		buf = to_iris_buffer(&m2m_buffer->vb);
+		if (buf->index == buffer->index) {
+			found = true;
+			break;
+		}
+	}
+	if (!found)
+		return -EINVAL;
+
+	if (!(buf->attr & BUF_ATTR_QUEUED))
+		return -EINVAL;
+
+	buf->attr &= ~BUF_ATTR_QUEUED;
+	buf->attr |= BUF_ATTR_DEQUEUED;
+
+	buf->flags = iris_hfi_gen2_get_driver_buffer_flags(inst, buffer->flags);
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_output_buffer(struct iris_inst *inst,
+					      struct iris_hfi_buffer *hfi_buffer)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *m2m_buffer, *n;
+	struct iris_buffer *buf;
+	bool found = false;
+	int ret;
+
+	if (hfi_buffer->flags & HFI_BUF_FW_FLAG_LAST) {
+		ret = iris_inst_sub_state_change_drain_last(inst);
+		if (ret)
+			return ret;
+	}
+
+	if (hfi_buffer->flags & HFI_BUF_FW_FLAG_PSC_LAST) {
+		ret = iris_inst_sub_state_change_drc_last(inst);
+		if (ret)
+			return ret;
+	}
+
+	v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, m2m_buffer, n) {
+		buf = to_iris_buffer(&m2m_buffer->vb);
+		if (buf->index == hfi_buffer->index &&
+		    buf->device_addr == hfi_buffer->base_address &&
+		    buf->data_offset == hfi_buffer->data_offset) {
+			found = true;
+			break;
+		}
+	}
+	if (!found)
+		return -EINVAL;
+
+	if (!(buf->attr & BUF_ATTR_QUEUED))
+		return -EINVAL;
+
+	buf->data_offset = hfi_buffer->data_offset;
+	buf->data_size = hfi_buffer->data_size;
+	buf->timestamp = hfi_buffer->timestamp;
+
+	buf->attr &= ~BUF_ATTR_QUEUED;
+	buf->attr |= BUF_ATTR_DEQUEUED;
+
+	buf->flags = iris_hfi_gen2_get_driver_buffer_flags(inst, hfi_buffer->flags);
+
+	return 0;
+}
+
+static void iris_hfi_gen2_handle_dequeue_buffers(struct iris_inst *inst)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buffer, *n;
+	struct iris_buffer *buf = NULL;
+
+	v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buffer, n) {
+		buf = to_iris_buffer(&buffer->vb);
+		if (buf->attr & BUF_ATTR_DEQUEUED) {
+			buf->attr &= ~BUF_ATTR_DEQUEUED;
+			if (!(buf->attr & BUF_ATTR_BUFFER_DONE)) {
+				buf->attr |= BUF_ATTR_BUFFER_DONE;
+				iris_vb2_buffer_done(inst, buf);
+			}
+		}
+	}
+
+	v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, buffer, n) {
+		buf = to_iris_buffer(&buffer->vb);
+		if (buf->attr & BUF_ATTR_DEQUEUED) {
+			buf->attr &= ~BUF_ATTR_DEQUEUED;
+			if (!(buf->attr & BUF_ATTR_BUFFER_DONE)) {
+				buf->attr |= BUF_ATTR_BUFFER_DONE;
+				iris_vb2_buffer_done(inst, buf);
+			}
+		}
+	}
+}
+
+static int iris_hfi_gen2_handle_release_internal_buffer(struct iris_inst *inst,
+							struct iris_hfi_buffer *buffer)
+{
+	u32 buf_type = iris_hfi_gen2_buf_type_to_driver(buffer->type);
+	struct iris_buffers *buffers = &inst->buffers[buf_type];
+	struct iris_buffer *buf, *iter;
+	bool found = false;
+	int ret = 0;
+
+	list_for_each_entry(iter, &buffers->list, list) {
+		if (iter->device_addr == buffer->base_address) {
+			found = true;
+			buf = iter;
+			break;
+		}
+	}
+	if (!found)
+		return -EINVAL;
+
+	buf->attr &= ~BUF_ATTR_QUEUED;
+	if (buf->attr & BUF_ATTR_PENDING_RELEASE)
+		ret = iris_destroy_internal_buffer(inst, buf);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_handle_session_stop(struct iris_inst *inst,
+					     struct iris_hfi_packet *pkt)
+{
+	int ret = 0;
+
+	if (pkt->port == HFI_PORT_RAW)
+		ret = iris_inst_sub_state_change_pause(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	else if (pkt->port == HFI_PORT_BITSTREAM)
+		ret = iris_inst_sub_state_change_pause(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+	complete(&inst->completion);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_handle_session_buffer(struct iris_inst *inst,
+					       struct iris_hfi_packet *pkt)
+{
+	struct iris_hfi_buffer *buffer;
+
+	if (pkt->payload_info == HFI_PAYLOAD_NONE)
+		return 0;
+
+	if (!iris_hfi_gen2_validate_packet_payload(pkt)) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return 0;
+	}
+
+	buffer = (struct iris_hfi_buffer *)((u8 *)pkt + sizeof(*pkt));
+	if (!iris_hfi_gen2_is_valid_hfi_buffer_type(buffer->type))
+		return 0;
+
+	if (!iris_hfi_gen2_is_valid_hfi_port(pkt->port, buffer->type))
+		return 0;
+
+	if (buffer->type == HFI_BUFFER_BITSTREAM)
+		return iris_hfi_gen2_handle_input_buffer(inst, buffer);
+	else if (buffer->type == HFI_BUFFER_RAW)
+		return iris_hfi_gen2_handle_output_buffer(inst, buffer);
+	else
+		return iris_hfi_gen2_handle_release_internal_buffer(inst, buffer);
+}
+
+static int iris_hfi_gen2_handle_session_drain(struct iris_inst *inst,
+					      struct iris_hfi_packet *pkt)
+{
+	int ret = 0;
+
+	if (!(pkt->flags & HFI_FW_FLAGS_SUCCESS)) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return 0;
+	}
+
+	if (inst->sub_state & IRIS_INST_SUB_DRAIN)
+		ret = iris_inst_change_sub_state(inst, 0, IRIS_INST_SUB_INPUT_PAUSE);
+
+	return ret;
+}
+
+static void iris_hfi_gen2_read_input_subcr_params(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct v4l2_pix_format_mplane *pixmp_ip = &inst->fmt_src->fmt.pix_mp;
+	struct v4l2_pix_format_mplane *pixmp_op = &inst->fmt_dst->fmt.pix_mp;
+	u32 primaries, matrix_coeff, transfer_char;
+	struct hfi_subscription_params subsc_params;
+	u32 colour_description_present_flag;
+	u32 video_signal_type_present_flag;
+	struct iris_core *core = inst->core;
+	u32 full_range, width, height;
+	struct vb2_queue *dst_q;
+	struct v4l2_ctrl *ctrl;
+
+	subsc_params = inst_hfi_gen2->src_subcr_params;
+	width = (subsc_params.bitstream_resolution &
+		HFI_BITMASK_BITSTREAM_WIDTH) >> 16;
+	height = subsc_params.bitstream_resolution &
+		HFI_BITMASK_BITSTREAM_HEIGHT;
+
+	pixmp_ip->width = width;
+	pixmp_ip->height = height;
+
+	pixmp_op->width = ALIGN(width, 128);
+	pixmp_op->height = ALIGN(height, 32);
+	pixmp_op->plane_fmt[0].bytesperline = ALIGN(width, 128);
+	pixmp_op->plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_OUTPUT);
+
+	matrix_coeff = subsc_params.color_info & 0xFF;
+	transfer_char = (subsc_params.color_info & 0xFF00) >> 8;
+	primaries = (subsc_params.color_info & 0xFF0000) >> 16;
+	colour_description_present_flag =
+		(subsc_params.color_info & 0x1000000) >> 24;
+	full_range = (subsc_params.color_info & 0x2000000) >> 25;
+	video_signal_type_present_flag =
+		(subsc_params.color_info & 0x20000000) >> 29;
+
+	pixmp_op->colorspace = V4L2_COLORSPACE_DEFAULT;
+	pixmp_op->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	pixmp_op->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	pixmp_op->quantization = V4L2_QUANTIZATION_DEFAULT;
+
+	if (video_signal_type_present_flag) {
+		pixmp_op->quantization =
+			full_range ?
+			V4L2_QUANTIZATION_FULL_RANGE :
+			V4L2_QUANTIZATION_LIM_RANGE;
+		if (colour_description_present_flag) {
+			pixmp_op->colorspace =
+				iris_hfi_get_v4l2_color_primaries(primaries);
+			pixmp_op->xfer_func =
+				iris_hfi_get_v4l2_transfer_char(transfer_char);
+			pixmp_op->ycbcr_enc =
+				iris_hfi_get_v4l2_matrix_coefficients(matrix_coeff);
+		}
+	}
+
+	pixmp_ip->colorspace = pixmp_op->colorspace;
+	pixmp_ip->xfer_func = pixmp_op->xfer_func;
+	pixmp_ip->ycbcr_enc = pixmp_op->ycbcr_enc;
+	pixmp_ip->quantization = pixmp_op->quantization;
+
+	inst->crop.top = subsc_params.crop_offsets[0] & 0xFFFF;
+	inst->crop.left = (subsc_params.crop_offsets[0] >> 16) & 0xFFFF;
+	inst->crop.height = pixmp_ip->height -
+		(subsc_params.crop_offsets[1] & 0xFFFF) - inst->crop.top;
+	inst->crop.width = pixmp_ip->width -
+		((subsc_params.crop_offsets[1] >> 16) & 0xFFFF) - inst->crop.left;
+
+	inst->fw_caps[PROFILE].value = subsc_params.profile;
+	inst->fw_caps[LEVEL].value = subsc_params.level;
+	inst->fw_caps[POC].value = subsc_params.pic_order_cnt;
+
+	if (subsc_params.bit_depth != BIT_DEPTH_8 ||
+	    !(subsc_params.coded_frames & HFI_BITMASK_FRAME_MBS_ONLY_FLAG)) {
+		dev_err(core->dev, "unsupported content, bit depth: %x, pic_struct = %x\n",
+			subsc_params.bit_depth, subsc_params.coded_frames);
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+	}
+
+	inst->fw_min_count = subsc_params.fw_min_count;
+	inst->buffers[BUF_OUTPUT].min_count = iris_vpu_buf_count(inst, BUF_OUTPUT);
+	inst->buffers[BUF_OUTPUT].size = pixmp_op->plane_fmt[0].sizeimage;
+	ctrl = v4l2_ctrl_find(&inst->ctrl_handler, V4L2_CID_MIN_BUFFERS_FOR_CAPTURE);
+	if (ctrl)
+		v4l2_ctrl_s_ctrl(ctrl, inst->buffers[BUF_OUTPUT].min_count);
+
+	dst_q = v4l2_m2m_get_dst_vq(inst->m2m_ctx);
+	dst_q->min_reqbufs_allocation = inst->buffers[BUF_OUTPUT].min_count;
+}
+
+static int iris_hfi_gen2_handle_src_change(struct iris_inst *inst,
+					   struct iris_hfi_packet *pkt)
+{
+	int ret;
+
+	if (pkt->port != HFI_PORT_BITSTREAM)
+		return 0;
+
+	ret = iris_inst_sub_state_change_drc(inst);
+	if (ret)
+		return ret;
+
+	iris_hfi_gen2_read_input_subcr_params(inst);
+	iris_vdec_src_change(inst);
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_session_command(struct iris_inst *inst,
+						struct iris_hfi_packet *pkt)
+{
+	int ret = 0;
+
+	switch (pkt->type) {
+	case HFI_CMD_CLOSE:
+		iris_hfi_gen2_handle_session_close(inst, pkt);
+		break;
+	case HFI_CMD_STOP:
+		iris_hfi_gen2_handle_session_stop(inst, pkt);
+		break;
+	case HFI_CMD_BUFFER:
+		ret = iris_hfi_gen2_handle_session_buffer(inst, pkt);
+		break;
+	case HFI_CMD_SETTINGS_CHANGE:
+		ret = iris_hfi_gen2_handle_src_change(inst, pkt);
+		break;
+	case HFI_CMD_DRAIN:
+		ret = iris_hfi_gen2_handle_session_drain(inst, pkt);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int iris_hfi_gen2_handle_session_property(struct iris_inst *inst,
+						 struct iris_hfi_packet *pkt)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+
+	if (pkt->port != HFI_PORT_BITSTREAM)
+		return 0;
+
+	if (pkt->flags & HFI_FW_FLAGS_INFORMATION)
+		return 0;
+
+	switch (pkt->type) {
+	case HFI_PROP_BITSTREAM_RESOLUTION:
+		inst_hfi_gen2->src_subcr_params.bitstream_resolution = pkt->payload[0];
+		break;
+	case HFI_PROP_CROP_OFFSETS:
+		inst_hfi_gen2->src_subcr_params.crop_offsets[0] = pkt->payload[0];
+		inst_hfi_gen2->src_subcr_params.crop_offsets[1] = pkt->payload[1];
+		break;
+	case HFI_PROP_CODED_FRAMES:
+		inst_hfi_gen2->src_subcr_params.coded_frames = pkt->payload[0];
+		break;
+	case HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT:
+		inst_hfi_gen2->src_subcr_params.fw_min_count = pkt->payload[0];
+		break;
+	case HFI_PROP_PIC_ORDER_CNT_TYPE:
+		inst_hfi_gen2->src_subcr_params.pic_order_cnt = pkt->payload[0];
+		break;
+	case HFI_PROP_SIGNAL_COLOR_INFO:
+		inst_hfi_gen2->src_subcr_params.color_info = pkt->payload[0];
+		break;
+	case HFI_PROP_PROFILE:
+		inst_hfi_gen2->src_subcr_params.profile = pkt->payload[0];
+		break;
+	case HFI_PROP_LEVEL:
+		inst_hfi_gen2->src_subcr_params.level = pkt->payload[0];
+		break;
+	case HFI_PROP_PICTURE_TYPE:
+		inst_hfi_gen2->hfi_frame_info.picture_type = pkt->payload[0];
+		break;
+	case HFI_PROP_NO_OUTPUT:
+		inst_hfi_gen2->hfi_frame_info.no_output = 1;
+		break;
+	case HFI_PROP_QUALITY_MODE:
+	case HFI_PROP_STAGE:
+	case HFI_PROP_PIPE:
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_image_version_property(struct iris_core *core,
+						       struct iris_hfi_packet *pkt)
+{
+	u8 *str_image_version = (u8 *)pkt + sizeof(*pkt);
+	u32 req_bytes = pkt->size - sizeof(*pkt);
+	char fw_version[IRIS_FW_VERSION_LENGTH];
+	u32 i;
+
+	if (req_bytes < IRIS_FW_VERSION_LENGTH - 1)
+		return -EINVAL;
+
+	for (i = 0; i < IRIS_FW_VERSION_LENGTH - 1; i++) {
+		if (str_image_version[i] != '\0')
+			fw_version[i] = str_image_version[i];
+		else
+			fw_version[i] = ' ';
+	}
+	fw_version[i] = '\0';
+	dev_dbg(core->dev, "firmware version: %s\n", fw_version);
+
+	return 0;
+}
+
+static int iris_hfi_gen2_handle_system_property(struct iris_core *core,
+						struct iris_hfi_packet *pkt)
+{
+	switch (pkt->type) {
+	case HFI_PROP_IMAGE_VERSION:
+		return iris_hfi_gen2_handle_image_version_property(core, pkt);
+	default:
+		return 0;
+	}
+}
+
+static int iris_hfi_gen2_handle_system_response(struct iris_core *core,
+						struct iris_hfi_header *hdr)
+{
+	u8 *start_pkt = (u8 *)((u8 *)hdr + sizeof(*hdr));
+	struct iris_hfi_packet *packet;
+	u32 i, j;
+	u8 *pkt;
+	int ret;
+	static const struct iris_hfi_gen2_core_hfi_range range[] = {
+		{HFI_SYSTEM_ERROR_BEGIN, HFI_SYSTEM_ERROR_END, iris_hfi_gen2_handle_system_error },
+		{HFI_PROP_BEGIN,         HFI_PROP_END, iris_hfi_gen2_handle_system_property },
+		{HFI_CMD_BEGIN,          HFI_CMD_END, iris_hfi_gen2_handle_system_init },
+	};
+
+	for (i = 0; i < ARRAY_SIZE(range); i++) {
+		pkt = start_pkt;
+		for (j = 0; j < hdr->num_packets; j++) {
+			packet = (struct iris_hfi_packet *)pkt;
+			if (packet->flags & HFI_FW_FLAGS_SYSTEM_ERROR) {
+				ret = iris_hfi_gen2_handle_system_error(core, packet);
+				return ret;
+			}
+
+			if (packet->type > range[i].begin && packet->type < range[i].end) {
+				ret = range[i].handle(core, packet);
+				if (ret)
+					return ret;
+
+				if (packet->type >  HFI_SYSTEM_ERROR_BEGIN &&
+				    packet->type < HFI_SYSTEM_ERROR_END)
+					return 0;
+			}
+			pkt += packet->size;
+		}
+	}
+
+	return 0;
+}
+
+static void iris_hfi_gen2_init_src_change_param(struct iris_inst *inst)
+{
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	struct v4l2_pix_format_mplane *pixmp_ip = &inst->fmt_src->fmt.pix_mp;
+	struct v4l2_pix_format_mplane *pixmp_op = &inst->fmt_dst->fmt.pix_mp;
+	u32 bottom_offset = (pixmp_ip->height - inst->crop.height);
+	u32 right_offset = (pixmp_ip->width - inst->crop.width);
+	struct hfi_subscription_params *subsc_params;
+	u32 primaries, matrix_coeff, transfer_char;
+	u32 colour_description_present_flag = 0;
+	u32 video_signal_type_present_flag = 0;
+	u32 full_range, video_format = 0;
+	u32 left_offset = inst->crop.left;
+	u32 top_offset = inst->crop.top;
+
+	subsc_params = &inst_hfi_gen2->src_subcr_params;
+	subsc_params->bitstream_resolution =
+		pixmp_ip->width << 16 | pixmp_ip->height;
+	subsc_params->crop_offsets[0] =
+			left_offset << 16 | top_offset;
+	subsc_params->crop_offsets[1] =
+			right_offset << 16 | bottom_offset;
+	subsc_params->fw_min_count = inst->buffers[BUF_OUTPUT].min_count;
+
+	primaries = iris_hfi_gen2_get_color_primaries(pixmp_op->colorspace);
+	matrix_coeff = iris_hfi_gen2_get_matrix_coefficients(pixmp_op->ycbcr_enc);
+	transfer_char = iris_hfi_gen2_get_transfer_char(pixmp_op->xfer_func);
+	full_range = pixmp_op->quantization == V4L2_QUANTIZATION_FULL_RANGE ? 1 : 0;
+	subsc_params->color_info =
+		iris_hfi_gen2_get_color_info(matrix_coeff, transfer_char, primaries,
+					     colour_description_present_flag,
+					     full_range, video_format,
+					     video_signal_type_present_flag);
+
+	subsc_params->profile = inst->fw_caps[PROFILE].value;
+	subsc_params->level = inst->fw_caps[LEVEL].value;
+	subsc_params->pic_order_cnt = inst->fw_caps[POC].value;
+	subsc_params->bit_depth = inst->fw_caps[BIT_DEPTH].value;
+	if (inst->fw_caps[CODED_FRAMES].value ==
+			CODED_FRAMES_PROGRESSIVE)
+		subsc_params->coded_frames = HFI_BITMASK_FRAME_MBS_ONLY_FLAG;
+	else
+		subsc_params->coded_frames = 0;
+}
+
+static int iris_hfi_gen2_handle_session_response(struct iris_core *core,
+						 struct iris_hfi_header *hdr)
+{
+	u8 *pkt = (u8 *)((u8 *)hdr + sizeof(*hdr));
+	struct iris_inst_hfi_gen2 *inst_hfi_gen2;
+	struct iris_hfi_packet *packet;
+	struct iris_inst *inst;
+	bool dequeue = false;
+	int ret = 0;
+	u32 i, j;
+	static const struct iris_hfi_gen2_inst_hfi_range range[] = {
+		{HFI_SESSION_ERROR_BEGIN, HFI_SESSION_ERROR_END,
+		 iris_hfi_gen2_handle_session_error},
+		{HFI_INFORMATION_BEGIN, HFI_INFORMATION_END,
+		 iris_hfi_gen2_handle_session_info},
+		{HFI_PROP_BEGIN, HFI_PROP_END,
+		 iris_hfi_gen2_handle_session_property},
+		{HFI_CMD_BEGIN, HFI_CMD_END,
+		 iris_hfi_gen2_handle_session_command },
+	};
+
+	inst = iris_get_instance(core, hdr->session_id);
+	if (!inst)
+		return -EINVAL;
+
+	mutex_lock(&inst->lock);
+	inst_hfi_gen2 = to_iris_inst_hfi_gen2(inst);
+	memset(&inst_hfi_gen2->hfi_frame_info, 0, sizeof(struct iris_hfi_frame_info));
+
+	for (i = 0; i < hdr->num_packets; i++) {
+		packet = (struct iris_hfi_packet *)pkt;
+		if (packet->type == HFI_CMD_SETTINGS_CHANGE) {
+			if (packet->port == HFI_PORT_BITSTREAM) {
+				iris_hfi_gen2_init_src_change_param(inst);
+				break;
+			}
+		}
+		pkt += packet->size;
+	}
+
+	pkt = (u8 *)((u8 *)hdr + sizeof(*hdr));
+	for (i = 0; i < ARRAY_SIZE(range); i++) {
+		pkt = (u8 *)((u8 *)hdr + sizeof(*hdr));
+		for (j = 0; j < hdr->num_packets; j++) {
+			packet = (struct iris_hfi_packet *)pkt;
+			if (packet->flags & HFI_FW_FLAGS_SESSION_ERROR)
+				iris_hfi_gen2_handle_session_error(inst, packet);
+
+			if (packet->type > range[i].begin && packet->type < range[i].end) {
+				dequeue |= (packet->type == HFI_CMD_BUFFER);
+				ret = range[i].handle(inst, packet);
+				if (ret)
+					iris_inst_change_state(inst, IRIS_INST_ERROR);
+			}
+			pkt += packet->size;
+		}
+	}
+
+	if (dequeue)
+		iris_hfi_gen2_handle_dequeue_buffers(inst);
+
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+static int iris_hfi_gen2_handle_response(struct iris_core *core, void *response)
+{
+	struct iris_hfi_header *hdr = (struct iris_hfi_header *)response;
+	int ret;
+
+	ret = iris_hfi_gen2_validate_hdr_packet(core, hdr);
+	if (ret)
+		return iris_hfi_gen2_handle_system_error(core, NULL);
+
+	if (!hdr->session_id)
+		return iris_hfi_gen2_handle_system_response(core, hdr);
+	else
+		return iris_hfi_gen2_handle_session_response(core, hdr);
+}
+
+static void iris_hfi_gen2_flush_debug_queue(struct iris_core *core, u8 *packet)
+{
+	struct hfi_debug_header *pkt;
+	u8 *log;
+
+	while (!iris_hfi_queue_dbg_read(core, packet)) {
+		pkt = (struct hfi_debug_header *)packet;
+
+		if (pkt->size < sizeof(*pkt))
+			continue;
+
+		if (pkt->size >= IFACEQ_CORE_PKT_SIZE)
+			continue;
+
+		packet[pkt->size] = '\0';
+		log = (u8 *)packet + sizeof(*pkt) + 1;
+		dev_dbg(core->dev, "%s", log);
+	}
+}
+
+static void iris_hfi_gen2_response_handler(struct iris_core *core)
+{
+	if (iris_vpu_watchdog(core, core->intr_status)) {
+		struct iris_hfi_packet pkt = {.type = HFI_SYS_ERROR_WD_TIMEOUT};
+
+		dev_err(core->dev, "cpu watchdog error received\n");
+		core->state = IRIS_CORE_ERROR;
+		iris_hfi_gen2_handle_system_error(core, &pkt);
+
+		return;
+	}
+
+	memset(core->response_packet, 0, sizeof(struct iris_hfi_header));
+	while (!iris_hfi_queue_msg_read(core, core->response_packet)) {
+		iris_hfi_gen2_handle_response(core, core->response_packet);
+		memset(core->response_packet, 0, sizeof(struct iris_hfi_header));
+	}
+
+	iris_hfi_gen2_flush_debug_queue(core, core->response_packet);
+}
+
+static const struct iris_hfi_response_ops iris_hfi_gen2_response_ops = {
+	.hfi_response_handler = iris_hfi_gen2_response_handler,
+};
+
+void iris_hfi_gen2_response_ops_init(struct iris_core *core)
+{
+	core->hfi_response_ops = &iris_hfi_gen2_response_ops;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_queue.c b/drivers/media/platform/qcom/iris/iris_hfi_queue.c
new file mode 100644
index 000000000000..fac7df0c4d1a
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_queue.c
@@ -0,0 +1,318 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_runtime.h>
+
+#include "iris_core.h"
+#include "iris_hfi_queue.h"
+#include "iris_vpu_common.h"
+
+static int iris_hfi_queue_write(struct iris_iface_q_info *qinfo, void *packet, u32 packet_size)
+{
+	struct iris_hfi_queue_header *queue = qinfo->qhdr;
+	u32 write_idx = queue->write_idx * sizeof(u32);
+	u32 read_idx = queue->read_idx * sizeof(u32);
+	u32 empty_space, new_write_idx, residue;
+	u32 *write_ptr;
+
+	if (write_idx < read_idx)
+		empty_space = read_idx - write_idx;
+	else
+		empty_space = IFACEQ_QUEUE_SIZE - (write_idx -  read_idx);
+	if (empty_space < packet_size)
+		return -ENOSPC;
+
+	queue->tx_req =  0;
+
+	new_write_idx = write_idx + packet_size;
+	write_ptr = (u32 *)((u8 *)qinfo->kernel_vaddr + write_idx);
+
+	if (write_ptr < (u32 *)qinfo->kernel_vaddr ||
+	    write_ptr > (u32 *)(qinfo->kernel_vaddr +
+	    IFACEQ_QUEUE_SIZE))
+		return -EINVAL;
+
+	if (new_write_idx < IFACEQ_QUEUE_SIZE) {
+		memcpy(write_ptr, packet, packet_size);
+	} else {
+		residue = new_write_idx - IFACEQ_QUEUE_SIZE;
+		memcpy(write_ptr, packet, (packet_size - residue));
+		memcpy(qinfo->kernel_vaddr,
+		       packet + (packet_size - residue), residue);
+		new_write_idx = residue;
+	}
+
+	/* Make sure packet is written before updating the write index */
+	mb();
+	queue->write_idx = new_write_idx / sizeof(u32);
+
+	/* Make sure write index is updated before an interrupt is raised */
+	mb();
+
+	return 0;
+}
+
+static int iris_hfi_queue_read(struct iris_iface_q_info *qinfo, void *packet)
+{
+	struct iris_hfi_queue_header *queue = qinfo->qhdr;
+	u32 write_idx = queue->write_idx * sizeof(u32);
+	u32 read_idx = queue->read_idx * sizeof(u32);
+	u32 packet_size, receive_request = 0;
+	u32 new_read_idx, residue;
+	u32 *read_ptr;
+	int ret = 0;
+
+	if (queue->queue_type == IFACEQ_MSGQ_ID)
+		receive_request = 1;
+
+	if (read_idx == write_idx) {
+		queue->rx_req = receive_request;
+		/* Ensure qhdr is updated in main memory */
+		mb();
+		return -ENODATA;
+	}
+
+	read_ptr = qinfo->kernel_vaddr + read_idx;
+	if (read_ptr < (u32 *)qinfo->kernel_vaddr ||
+	    read_ptr > (u32 *)(qinfo->kernel_vaddr +
+	    IFACEQ_QUEUE_SIZE - sizeof(*read_ptr)))
+		return -ENODATA;
+
+	packet_size = *read_ptr;
+	if (!packet_size)
+		return -EINVAL;
+
+	new_read_idx = read_idx + packet_size;
+	if (packet_size <= IFACEQ_CORE_PKT_SIZE) {
+		if (new_read_idx < IFACEQ_QUEUE_SIZE) {
+			memcpy(packet, read_ptr, packet_size);
+		} else {
+			residue = new_read_idx - IFACEQ_QUEUE_SIZE;
+			memcpy(packet, read_ptr, (packet_size - residue));
+			memcpy((packet + (packet_size - residue)),
+			       qinfo->kernel_vaddr, residue);
+			new_read_idx = residue;
+		}
+	} else {
+		new_read_idx = write_idx;
+		ret = -EBADMSG;
+	}
+
+	queue->rx_req = receive_request;
+
+	queue->read_idx = new_read_idx / sizeof(u32);
+	/* Ensure qhdr is updated in main memory */
+	mb();
+
+	return ret;
+}
+
+int iris_hfi_queue_cmd_write_locked(struct iris_core *core, void *pkt, u32 pkt_size)
+{
+	struct iris_iface_q_info *q_info = &core->command_queue;
+
+	if (core->state == IRIS_CORE_ERROR)
+		return -EINVAL;
+
+	if (!iris_hfi_queue_write(q_info, pkt, pkt_size)) {
+		iris_vpu_raise_interrupt(core);
+	} else {
+		dev_err(core->dev, "queue full\n");
+		return -ENODATA;
+	}
+
+	return 0;
+}
+
+int iris_hfi_queue_cmd_write(struct iris_core *core, void *pkt, u32 pkt_size)
+{
+	int ret;
+
+	ret = pm_runtime_resume_and_get(core->dev);
+	if (ret < 0)
+		goto exit;
+
+	mutex_lock(&core->lock);
+	ret = iris_hfi_queue_cmd_write_locked(core, pkt, pkt_size);
+	if (ret) {
+		mutex_unlock(&core->lock);
+		goto exit;
+	}
+	mutex_unlock(&core->lock);
+
+	pm_runtime_mark_last_busy(core->dev);
+	pm_runtime_put_autosuspend(core->dev);
+
+	return 0;
+
+exit:
+	pm_runtime_put_sync(core->dev);
+
+	return ret;
+}
+
+int iris_hfi_queue_msg_read(struct iris_core *core, void *pkt)
+{
+	struct iris_iface_q_info *q_info = &core->message_queue;
+	int ret = 0;
+
+	mutex_lock(&core->lock);
+	if (core->state != IRIS_CORE_INIT) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (iris_hfi_queue_read(q_info, pkt)) {
+		ret = -ENODATA;
+		goto unlock;
+	}
+
+unlock:
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+int iris_hfi_queue_dbg_read(struct iris_core *core, void *pkt)
+{
+	struct iris_iface_q_info *q_info = &core->debug_queue;
+	int ret = 0;
+
+	mutex_lock(&core->lock);
+	if (core->state != IRIS_CORE_INIT) {
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (iris_hfi_queue_read(q_info, pkt)) {
+		ret = -ENODATA;
+		goto unlock;
+	}
+
+unlock:
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+static void iris_hfi_queue_set_header(struct iris_core *core, u32 queue_id,
+				      struct iris_iface_q_info *iface_q)
+{
+	iface_q->qhdr->status = 0x1;
+	iface_q->qhdr->start_addr = iface_q->device_addr;
+	iface_q->qhdr->header_type = IFACEQ_DFLT_QHDR;
+	iface_q->qhdr->queue_type = queue_id;
+	iface_q->qhdr->q_size = IFACEQ_QUEUE_SIZE / sizeof(u32);
+	iface_q->qhdr->pkt_size = 0; /* variable packet size */
+	iface_q->qhdr->rx_wm = 0x1;
+	iface_q->qhdr->tx_wm = 0x1;
+	iface_q->qhdr->rx_req = 0x1;
+	iface_q->qhdr->tx_req = 0x0;
+	iface_q->qhdr->rx_irq_status = 0x0;
+	iface_q->qhdr->tx_irq_status = 0x0;
+	iface_q->qhdr->read_idx = 0x0;
+	iface_q->qhdr->write_idx = 0x0;
+
+	/*
+	 * Set receive request to zero on debug queue as there is no
+	 * need of interrupt from video hardware for debug messages
+	 */
+	if (queue_id == IFACEQ_DBGQ_ID)
+		iface_q->qhdr->rx_req = 0;
+}
+
+static void
+iris_hfi_queue_init(struct iris_core *core, u32 queue_id, struct iris_iface_q_info *iface_q)
+{
+	struct iris_hfi_queue_table_header *q_tbl_hdr = core->iface_q_table_vaddr;
+	u32 offset = sizeof(*q_tbl_hdr) + (queue_id * IFACEQ_QUEUE_SIZE);
+
+	iface_q->device_addr = core->iface_q_table_daddr + offset;
+	iface_q->kernel_vaddr =
+			(void *)((char *)core->iface_q_table_vaddr + offset);
+	iface_q->qhdr = &q_tbl_hdr->q_hdr[queue_id];
+
+	iris_hfi_queue_set_header(core, queue_id, iface_q);
+}
+
+static void iris_hfi_queue_deinit(struct iris_iface_q_info *iface_q)
+{
+	iface_q->qhdr = NULL;
+	iface_q->kernel_vaddr = NULL;
+	iface_q->device_addr = 0;
+}
+
+int iris_hfi_queues_init(struct iris_core *core)
+{
+	struct iris_hfi_queue_table_header *q_tbl_hdr;
+	u32 queue_size;
+
+	/* Iris hardware requires 4K queue alignment */
+	queue_size = ALIGN((sizeof(*q_tbl_hdr) + (IFACEQ_QUEUE_SIZE * IFACEQ_NUMQ)), SZ_4K);
+	core->iface_q_table_vaddr = dma_alloc_attrs(core->dev, queue_size,
+						    &core->iface_q_table_daddr,
+						    GFP_KERNEL, DMA_ATTR_WRITE_COMBINE);
+	if (!core->iface_q_table_vaddr) {
+		dev_err(core->dev, "queues alloc and map failed\n");
+		return -ENOMEM;
+	}
+
+	core->sfr_vaddr = dma_alloc_attrs(core->dev, SFR_SIZE,
+					  &core->sfr_daddr,
+					  GFP_KERNEL, DMA_ATTR_WRITE_COMBINE);
+	if (!core->sfr_vaddr) {
+		dev_err(core->dev, "sfr alloc and map failed\n");
+		dma_free_attrs(core->dev, sizeof(*q_tbl_hdr), core->iface_q_table_vaddr,
+			       core->iface_q_table_daddr, DMA_ATTR_WRITE_COMBINE);
+		return -ENOMEM;
+	}
+
+	iris_hfi_queue_init(core, IFACEQ_CMDQ_ID, &core->command_queue);
+	iris_hfi_queue_init(core, IFACEQ_MSGQ_ID, &core->message_queue);
+	iris_hfi_queue_init(core, IFACEQ_DBGQ_ID, &core->debug_queue);
+
+	q_tbl_hdr = (struct iris_hfi_queue_table_header *)core->iface_q_table_vaddr;
+	q_tbl_hdr->version = 0;
+	q_tbl_hdr->device_addr = (void *)core;
+	strscpy(q_tbl_hdr->name, "iris-hfi-queues", sizeof(q_tbl_hdr->name));
+	q_tbl_hdr->size = sizeof(*q_tbl_hdr);
+	q_tbl_hdr->qhdr0_offset = sizeof(*q_tbl_hdr) -
+		(IFACEQ_NUMQ * sizeof(struct iris_hfi_queue_header));
+	q_tbl_hdr->qhdr_size = sizeof(q_tbl_hdr->q_hdr[0]);
+	q_tbl_hdr->num_q = IFACEQ_NUMQ;
+	q_tbl_hdr->num_active_q = IFACEQ_NUMQ;
+
+	 /* Write sfr size in first word to be used by firmware */
+	*((u32 *)core->sfr_vaddr) = SFR_SIZE;
+
+	return 0;
+}
+
+void iris_hfi_queues_deinit(struct iris_core *core)
+{
+	u32 queue_size;
+
+	if (!core->iface_q_table_vaddr)
+		return;
+
+	iris_hfi_queue_deinit(&core->debug_queue);
+	iris_hfi_queue_deinit(&core->message_queue);
+	iris_hfi_queue_deinit(&core->command_queue);
+
+	dma_free_attrs(core->dev, SFR_SIZE, core->sfr_vaddr,
+		       core->sfr_daddr, DMA_ATTR_WRITE_COMBINE);
+
+	core->sfr_vaddr = NULL;
+	core->sfr_daddr = 0;
+
+	queue_size = ALIGN(sizeof(struct iris_hfi_queue_table_header) +
+		(IFACEQ_QUEUE_SIZE * IFACEQ_NUMQ), SZ_4K);
+
+	dma_free_attrs(core->dev, queue_size, core->iface_q_table_vaddr,
+		       core->iface_q_table_daddr, DMA_ATTR_WRITE_COMBINE);
+
+	core->iface_q_table_vaddr = NULL;
+	core->iface_q_table_daddr = 0;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_hfi_queue.h b/drivers/media/platform/qcom/iris/iris_hfi_queue.h
new file mode 100644
index 000000000000..2174fc5ce618
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_hfi_queue.h
@@ -0,0 +1,182 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_HFI_QUEUE_H__
+#define __IRIS_HFI_QUEUE_H__
+
+struct iris_core;
+
+/*
+ * Max 64 Buffers ( 32 input buffers and 32 output buffers)
+ * can be queued by v4l2 framework at any given time.
+ */
+#define IFACEQ_MAX_BUF_COUNT		64
+/*
+ * Max session supported are 16.
+ * this value is used to calcualte the size of
+ * individual shared queue.
+ */
+#define IFACE_MAX_PARALLEL_SESSIONS	16
+#define IFACEQ_DFLT_QHDR		0x0101
+#define IFACEQ_MAX_PKT_SIZE		1024 /* Maximum size of a packet in the queue */
+
+/*
+ * SFR: Subsystem Failure Reason
+ * when hardware goes into bad state/failure, firmware fills this memory
+ * and driver will get to know the actual failure reason from this SFR buffer.
+ */
+#define SFR_SIZE			SZ_4K /* Iris hardware requires 4K queue alignment */
+
+#define IFACEQ_QUEUE_SIZE		(IFACEQ_MAX_PKT_SIZE * \
+					 IFACEQ_MAX_BUF_COUNT * IFACE_MAX_PARALLEL_SESSIONS)
+
+/*
+ * Memory layout of the shared queues:
+ *
+ *   ||=================||  ^        ^         ^
+ *   ||                 ||  |        |         |
+ *   ||    Queue Table  || 288 Bytes |         |
+ *   ||      Header     ||  |        |         |
+ *   ||                 ||  |        |         |
+ *   ||-----------------||  V        |         |
+ *   ||-----------------||  ^        |         |
+ *   ||                 ||  |        |         |
+ *   ||  Command Queue  || 56 Bytes  |         |
+ *   ||     Header      ||  |        |         |
+ *   ||                 ||  |        |         |
+ *   ||-----------------||  V       456 Bytes  |
+ *   ||-----------------||  ^        |         |
+ *   ||                 ||  |        |         |
+ *   ||  Message Queue  || 56 Bytes  |         |
+ *   ||     Header      ||  |        |         |
+ *   ||                 ||  |        |         |
+ *   ||-----------------||  V        |         Buffer size aligned to 4k
+ *   ||-----------------||  ^        |         Overall Queue Size = 2,404 KB
+ *   ||                 ||  |        |         |
+ *   ||   Debug Queue   || 56 Bytes  |         |
+ *   ||     Header      ||  |        |         |
+ *   ||                 ||  |        |         |
+ *   ||=================||  V        V         |
+ *   ||=================||           ^         |
+ *   ||                 ||           |         |
+ *   ||     Command     ||         800 KB      |
+ *   ||      Queue      ||           |         |
+ *   ||                 ||           |         |
+ *   ||=================||           V         |
+ *   ||=================||           ^         |
+ *   ||                 ||           |         |
+ *   ||     Message     ||         800 KB      |
+ *   ||      Queue      ||           |         |
+ *   ||                 ||           |         |
+ *   ||=================||           V         |
+ *   ||=================||           ^         |
+ *   ||                 ||           |         |
+ *   ||      Debug      ||         800 KB      |
+ *   ||      Queue      ||           |         |
+ *   ||                 ||           |         |
+ *   ||=================||           V         |
+ *   ||                 ||                     |
+ *   ||=================||                     V
+ */
+
+/*
+ * Shared queues are used for communication between driver and firmware.
+ * There are 3 types of queues:
+ * Command queue - driver to write any command to firmware.
+ * Message queue - firmware to send any response to driver.
+ * Debug queue - firmware to write debug message.
+ */
+
+/* Host-firmware shared queue ids */
+enum iris_iface_queue {
+	IFACEQ_CMDQ_ID,
+	IFACEQ_MSGQ_ID,
+	IFACEQ_DBGQ_ID,
+	IFACEQ_NUMQ, /* not an index */
+};
+
+/**
+ * struct iris_hfi_queue_header
+ *
+ * @status: Queue status, bits (7:0), 0x1 - active, 0x0 - inactive
+ * @start_addr: Queue start address in non cached memory
+ * @queue_type: Queue ID
+ * @header_type: Default queue header
+ * @q_size: Queue size
+ *		Number of queue packets if pkt_size is non-zero
+ *		Queue size in bytes if pkt_size is zero
+ * @pkt_size: Size of queue packet entries
+ *		0x0: variable queue packet size
+ *		non zero: size of queue packet entry, fixed
+ * @pkt_drop_cnt: Number of packets dropped by sender
+ * @rx_wm: Receiver watermark, applicable in event driven mode
+ * @tx_wm: Sender watermark, applicable in event driven mode
+ * @rx_req: Receiver sets this bit if queue is empty
+ * @tx_req: Sender sets this bit if queue is full
+ * @rx_irq_status: Receiver sets this bit and triggers an interrupt to
+ *		the sender after packets are dequeued. Sender clears this bit
+ * @tx_irq_status: Sender sets this bit and triggers an interrupt to
+ *		the receiver after packets are queued. Receiver clears this bit
+ * @read_idx: Index till where receiver has consumed the packets from the queue.
+ * @write_idx: Index till where sender has written the packets into the queue.
+ */
+struct iris_hfi_queue_header {
+	u32 status;
+	u32 start_addr;
+	u16 queue_type;
+	u16 header_type;
+	u32 q_size;
+	u32 pkt_size;
+	u32 pkt_drop_cnt;
+	u32 rx_wm;
+	u32 tx_wm;
+	u32 rx_req;
+	u32 tx_req;
+	u32 rx_irq_status;
+	u32 tx_irq_status;
+	u32 read_idx;
+	u32 write_idx;
+};
+
+/**
+ * struct iris_hfi_queue_table_header
+ *
+ * @version: Queue table version number
+ * @size: Queue table size from version to last parametr in qhdr entry
+ * @qhdr0_offset: Offset to the start of first qhdr
+ * @qhdr_size: Queue header size in bytes
+ * @num_q: Total number of queues in Queue table
+ * @num_active_q: Total number of active queues
+ * @device_addr: Device address of the queue
+ * @name: Queue name in characters
+ * @q_hdr: Array of queue headers
+ */
+struct iris_hfi_queue_table_header {
+	u32 version;
+	u32 size;
+	u32 qhdr0_offset;
+	u32 qhdr_size;
+	u32 num_q;
+	u32 num_active_q;
+	void *device_addr;
+	char name[256]; /* NUL-terminated array of characters */
+	struct iris_hfi_queue_header q_hdr[IFACEQ_NUMQ];
+};
+
+struct iris_iface_q_info {
+	struct iris_hfi_queue_header *qhdr;
+	dma_addr_t	device_addr;
+	void		*kernel_vaddr;
+};
+
+int iris_hfi_queues_init(struct iris_core *core);
+void iris_hfi_queues_deinit(struct iris_core *core);
+
+int iris_hfi_queue_cmd_write_locked(struct iris_core *core, void *pkt, u32 pkt_size);
+int iris_hfi_queue_cmd_write(struct iris_core *core, void *pkt, u32 pkt_size);
+int iris_hfi_queue_msg_read(struct iris_core *core, void *pkt);
+int iris_hfi_queue_dbg_read(struct iris_core *core, void *pkt);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_instance.h b/drivers/media/platform/qcom/iris/iris_instance.h
new file mode 100644
index 000000000000..caa3c6507006
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_instance.h
@@ -0,0 +1,77 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_INSTANCE_H__
+#define __IRIS_INSTANCE_H__
+
+#include <media/v4l2-ctrls.h>
+
+#include "iris_buffer.h"
+#include "iris_core.h"
+#include "iris_utils.h"
+
+/**
+ * struct iris_inst - holds per video instance parameters
+ *
+ * @list: used for attach an instance to the core
+ * @core: pointer to core structure
+ * @session_id: id of current video session
+ * @ctx_q_lock: lock to serialize queues related ioctls
+ * @lock: lock to seralise forward and reverse threads
+ * @fh: reference of v4l2 file handler
+ * @fmt_src: structure of v4l2_format for source
+ * @fmt_dst: structure of v4l2_format for destination
+ * @ctrl_handler: reference of v4l2 ctrl handler
+ * @crop: structure of crop info
+ * @completion: structure of signal completions
+ * @flush_completion: structure of signal completions for flush cmd
+ * @fw_caps: array of supported instance firmware capabilities
+ * @buffers: array of different iris buffers
+ * @fw_min_count: minimnum count of buffers needed by fw
+ * @state: instance state
+ * @sub_state: instance sub state
+ * @once_per_session_set: boolean to set once per session property
+ * @max_input_data_size: max size of input data
+ * @power: structure of power info
+ * @icc_data: structure of interconnect data
+ * @m2m_dev:	a reference to m2m device structure
+ * @m2m_ctx:	a reference to m2m context structure
+ * @sequence_cap: a sequence counter for capture queue
+ * @sequence_out: a sequence counter for output queue
+ * @tss: timestamp metadata
+ * @metadata_idx: index for metadata buffer
+ */
+
+struct iris_inst {
+	struct list_head		list;
+	struct iris_core		*core;
+	u32				session_id;
+	struct mutex			ctx_q_lock;/* lock to serialize queues related ioctls */
+	struct mutex			lock; /* lock to serialize forward and reverse threads */
+	struct v4l2_fh			fh;
+	struct v4l2_format		*fmt_src;
+	struct v4l2_format		*fmt_dst;
+	struct v4l2_ctrl_handler	ctrl_handler;
+	struct iris_hfi_rect_desc	crop;
+	struct completion		completion;
+	struct completion		flush_completion;
+	struct platform_inst_fw_cap	fw_caps[INST_FW_CAP_MAX];
+	struct iris_buffers		buffers[BUF_TYPE_MAX];
+	u32				fw_min_count;
+	enum iris_inst_state		state;
+	enum iris_inst_sub_state	sub_state;
+	bool				once_per_session_set;
+	size_t				max_input_data_size;
+	struct iris_inst_power		power;
+	struct icc_vote_data		icc_data;
+	struct v4l2_m2m_dev		*m2m_dev;
+	struct v4l2_m2m_ctx		*m2m_ctx;
+	u32				sequence_cap;
+	u32				sequence_out;
+	struct iris_ts_metadata		tss[VIDEO_MAX_FRAME];
+	u32				metadata_idx;
+};
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_platform_common.h b/drivers/media/platform/qcom/iris/iris_platform_common.h
new file mode 100644
index 000000000000..ac76d9e1ef9c
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_platform_common.h
@@ -0,0 +1,190 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_PLATFORM_COMMON_H__
+#define __IRIS_PLATFORM_COMMON_H__
+
+#include <linux/bits.h>
+
+struct iris_core;
+struct iris_inst;
+
+#define IRIS_PAS_ID				9
+#define HW_RESPONSE_TIMEOUT_VALUE               (1000) /* milliseconds */
+#define AUTOSUSPEND_DELAY_VALUE			(HW_RESPONSE_TIMEOUT_VALUE + 500) /* milliseconds */
+
+#define REGISTER_BIT_DEPTH(luma, chroma)	((luma) << 16 | (chroma))
+#define BIT_DEPTH_8				REGISTER_BIT_DEPTH(8, 8)
+#define CODED_FRAMES_PROGRESSIVE		0x0
+#define DEFAULT_MAX_HOST_BUF_COUNT		64
+#define DEFAULT_MAX_HOST_BURST_BUF_COUNT	256
+#define DEFAULT_FPS				30
+
+enum stage_type {
+	STAGE_1 = 1,
+	STAGE_2 = 2,
+};
+
+enum pipe_type {
+	PIPE_1 = 1,
+	PIPE_2 = 2,
+	PIPE_4 = 4,
+};
+
+extern struct iris_platform_data qcs8300_data;
+extern struct iris_platform_data sm8250_data;
+extern struct iris_platform_data sm8550_data;
+extern struct iris_platform_data sm8650_data;
+
+enum platform_clk_type {
+	IRIS_AXI_CLK,
+	IRIS_CTRL_CLK,
+	IRIS_HW_CLK,
+};
+
+struct platform_clk_data {
+	enum platform_clk_type clk_type;
+	const char *clk_name;
+};
+
+struct tz_cp_config {
+	u32 cp_start;
+	u32 cp_size;
+	u32 cp_nonpixel_start;
+	u32 cp_nonpixel_size;
+};
+
+struct ubwc_config_data {
+	u32	max_channels;
+	u32	mal_length;
+	u32	highest_bank_bit;
+	u32	bank_swzl_level;
+	u32	bank_swz2_level;
+	u32	bank_swz3_level;
+	u32	bank_spreading;
+};
+
+struct platform_inst_caps {
+	u32 min_frame_width;
+	u32 max_frame_width;
+	u32 min_frame_height;
+	u32 max_frame_height;
+	u32 max_mbpf;
+	u32 mb_cycles_vsp;
+	u32 mb_cycles_vpp;
+	u32 mb_cycles_fw;
+	u32 mb_cycles_fw_vpp;
+	u32 num_comv;
+};
+
+enum platform_inst_fw_cap_type {
+	PROFILE = 1,
+	LEVEL,
+	INPUT_BUF_HOST_MAX_COUNT,
+	STAGE,
+	PIPE,
+	POC,
+	CODED_FRAMES,
+	BIT_DEPTH,
+	RAP_FRAME,
+	DEBLOCK,
+	INST_FW_CAP_MAX,
+};
+
+enum platform_inst_fw_cap_flags {
+	CAP_FLAG_DYNAMIC_ALLOWED	= BIT(0),
+	CAP_FLAG_MENU			= BIT(1),
+	CAP_FLAG_INPUT_PORT		= BIT(2),
+	CAP_FLAG_OUTPUT_PORT		= BIT(3),
+	CAP_FLAG_CLIENT_SET		= BIT(4),
+	CAP_FLAG_BITMASK		= BIT(5),
+	CAP_FLAG_VOLATILE		= BIT(6),
+};
+
+struct platform_inst_fw_cap {
+	enum platform_inst_fw_cap_type cap_id;
+	s64 min;
+	s64 max;
+	s64 step_or_mask;
+	s64 value;
+	u32 hfi_id;
+	enum platform_inst_fw_cap_flags flags;
+	int (*set)(struct iris_inst *inst,
+		   enum platform_inst_fw_cap_type cap_id);
+};
+
+struct bw_info {
+	u32 mbs_per_sec;
+	u32 bw_ddr;
+};
+
+struct iris_core_power {
+	u64 clk_freq;
+	u64 icc_bw;
+};
+
+struct iris_inst_power {
+	u64 min_freq;
+	u32 icc_bw;
+};
+
+struct icc_vote_data {
+	u32 height, width;
+	u32 fps;
+};
+
+enum platform_pm_domain_type {
+	IRIS_CTRL_POWER_DOMAIN,
+	IRIS_HW_POWER_DOMAIN,
+};
+
+struct iris_platform_data {
+	void (*init_hfi_command_ops)(struct iris_core *core);
+	void (*init_hfi_response_ops)(struct iris_core *core);
+	struct iris_inst *(*get_instance)(void);
+	const struct vpu_ops *vpu_ops;
+	void (*set_preset_registers)(struct iris_core *core);
+	const struct icc_info *icc_tbl;
+	unsigned int icc_tbl_size;
+	const struct bw_info *bw_tbl_dec;
+	unsigned int bw_tbl_dec_size;
+	const char * const *pmdomain_tbl;
+	unsigned int pmdomain_tbl_size;
+	const char * const *opp_pd_tbl;
+	unsigned int opp_pd_tbl_size;
+	const struct platform_clk_data *clk_tbl;
+	unsigned int clk_tbl_size;
+	const char * const *clk_rst_tbl;
+	unsigned int clk_rst_tbl_size;
+	const char * const *controller_rst_tbl;
+	unsigned int controller_rst_tbl_size;
+	u64 dma_mask;
+	const char *fwname;
+	u32 pas_id;
+	struct platform_inst_caps *inst_caps;
+	struct platform_inst_fw_cap *inst_fw_caps;
+	u32 inst_fw_caps_size;
+	struct tz_cp_config *tz_cp_config_data;
+	u32 core_arch;
+	u32 hw_response_timeout;
+	struct ubwc_config_data *ubwc_config;
+	u32 num_vpp_pipe;
+	u32 max_session_count;
+	u32 max_core_mbpf;
+	const u32 *input_config_params;
+	unsigned int input_config_params_size;
+	const u32 *output_config_params;
+	unsigned int output_config_params_size;
+	const u32 *dec_input_prop;
+	unsigned int dec_input_prop_size;
+	const u32 *dec_output_prop;
+	unsigned int dec_output_prop_size;
+	const u32 *dec_ip_int_buf_tbl;
+	unsigned int dec_ip_int_buf_tbl_size;
+	const u32 *dec_op_int_buf_tbl;
+	unsigned int dec_op_int_buf_tbl_size;
+};
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_platform_gen2.c b/drivers/media/platform/qcom/iris/iris_platform_gen2.c
new file mode 100644
index 000000000000..1e69ba15db0f
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_platform_gen2.c
@@ -0,0 +1,385 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_core.h"
+#include "iris_ctrls.h"
+#include "iris_hfi_gen2.h"
+#include "iris_hfi_gen2_defines.h"
+#include "iris_platform_common.h"
+#include "iris_vpu_common.h"
+
+#include "iris_platform_qcs8300.h"
+#include "iris_platform_sm8650.h"
+
+#define VIDEO_ARCH_LX 1
+
+static struct platform_inst_fw_cap inst_fw_cap_sm8550[] = {
+	{
+		.cap_id = PROFILE,
+		.min = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+		.max = V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_HIGH,
+		.step_or_mask = BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH) |
+				BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_HIGH),
+		.value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		.hfi_id = HFI_PROP_PROFILE,
+		.flags = CAP_FLAG_OUTPUT_PORT | CAP_FLAG_MENU,
+		.set = iris_set_u32_enum,
+	},
+	{
+		.cap_id = LEVEL,
+		.min = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+		.max = V4L2_MPEG_VIDEO_H264_LEVEL_6_2,
+		.step_or_mask = BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_2) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_2) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_0) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_1) |
+				BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_2),
+		.value = V4L2_MPEG_VIDEO_H264_LEVEL_6_1,
+		.hfi_id = HFI_PROP_LEVEL,
+		.flags = CAP_FLAG_OUTPUT_PORT | CAP_FLAG_MENU,
+		.set = iris_set_u32_enum,
+	},
+	{
+		.cap_id = INPUT_BUF_HOST_MAX_COUNT,
+		.min = DEFAULT_MAX_HOST_BUF_COUNT,
+		.max = DEFAULT_MAX_HOST_BURST_BUF_COUNT,
+		.step_or_mask = 1,
+		.value = DEFAULT_MAX_HOST_BUF_COUNT,
+		.hfi_id = HFI_PROP_BUFFER_HOST_MAX_COUNT,
+		.flags = CAP_FLAG_INPUT_PORT,
+		.set = iris_set_u32,
+	},
+	{
+		.cap_id = STAGE,
+		.min = STAGE_1,
+		.max = STAGE_2,
+		.step_or_mask = 1,
+		.value = STAGE_2,
+		.hfi_id = HFI_PROP_STAGE,
+		.set = iris_set_stage,
+	},
+	{
+		.cap_id = PIPE,
+		.min = PIPE_1,
+		.max = PIPE_4,
+		.step_or_mask = 1,
+		.value = PIPE_4,
+		.hfi_id = HFI_PROP_PIPE,
+		.set = iris_set_pipe,
+	},
+	{
+		.cap_id = POC,
+		.min = 0,
+		.max = 2,
+		.step_or_mask = 1,
+		.value = 1,
+		.hfi_id = HFI_PROP_PIC_ORDER_CNT_TYPE,
+	},
+	{
+		.cap_id = CODED_FRAMES,
+		.min = CODED_FRAMES_PROGRESSIVE,
+		.max = CODED_FRAMES_PROGRESSIVE,
+		.step_or_mask = 0,
+		.value = CODED_FRAMES_PROGRESSIVE,
+		.hfi_id = HFI_PROP_CODED_FRAMES,
+	},
+	{
+		.cap_id = BIT_DEPTH,
+		.min = BIT_DEPTH_8,
+		.max = BIT_DEPTH_8,
+		.step_or_mask = 1,
+		.value = BIT_DEPTH_8,
+		.hfi_id = HFI_PROP_LUMA_CHROMA_BIT_DEPTH,
+	},
+	{
+		.cap_id = RAP_FRAME,
+		.min = 0,
+		.max = 1,
+		.step_or_mask = 1,
+		.value = 1,
+		.hfi_id = HFI_PROP_DEC_START_FROM_RAP_FRAME,
+		.flags = CAP_FLAG_INPUT_PORT,
+		.set = iris_set_u32,
+	},
+};
+
+static struct platform_inst_caps platform_inst_cap_sm8550 = {
+	.min_frame_width = 96,
+	.max_frame_width = 8192,
+	.min_frame_height = 96,
+	.max_frame_height = 8192,
+	.max_mbpf = (8192 * 4352) / 256,
+	.mb_cycles_vpp = 200,
+	.mb_cycles_fw = 489583,
+	.mb_cycles_fw_vpp = 66234,
+	.num_comv = 0,
+};
+
+static void iris_set_sm8550_preset_registers(struct iris_core *core)
+{
+	writel(0x0, core->reg_base + 0xB0088);
+}
+
+static const struct icc_info sm8550_icc_table[] = {
+	{ "cpu-cfg",    1000, 1000     },
+	{ "video-mem",  1000, 15000000 },
+};
+
+static const char * const sm8550_clk_reset_table[] = { "bus" };
+
+static const struct bw_info sm8550_bw_table_dec[] = {
+	{ ((4096 * 2160) / 256) * 60, 1608000 },
+	{ ((4096 * 2160) / 256) * 30,  826000 },
+	{ ((1920 * 1080) / 256) * 60,  567000 },
+	{ ((1920 * 1080) / 256) * 30,  294000 },
+};
+
+static const char * const sm8550_pmdomain_table[] = { "venus", "vcodec0" };
+
+static const char * const sm8550_opp_pd_table[] = { "mxc", "mmcx" };
+
+static const struct platform_clk_data sm8550_clk_table[] = {
+	{IRIS_AXI_CLK,  "iface"        },
+	{IRIS_CTRL_CLK, "core"         },
+	{IRIS_HW_CLK,   "vcodec0_core" },
+};
+
+static struct ubwc_config_data ubwc_config_sm8550 = {
+	.max_channels = 8,
+	.mal_length = 32,
+	.highest_bank_bit = 16,
+	.bank_swzl_level = 0,
+	.bank_swz2_level = 1,
+	.bank_swz3_level = 1,
+	.bank_spreading = 1,
+};
+
+static struct tz_cp_config tz_cp_config_sm8550 = {
+	.cp_start = 0,
+	.cp_size = 0x25800000,
+	.cp_nonpixel_start = 0x01000000,
+	.cp_nonpixel_size = 0x24800000,
+};
+
+static const u32 sm8550_vdec_input_config_params[] = {
+	HFI_PROP_BITSTREAM_RESOLUTION,
+	HFI_PROP_CROP_OFFSETS,
+	HFI_PROP_CODED_FRAMES,
+	HFI_PROP_BUFFER_FW_MIN_OUTPUT_COUNT,
+	HFI_PROP_PIC_ORDER_CNT_TYPE,
+	HFI_PROP_PROFILE,
+	HFI_PROP_LEVEL,
+	HFI_PROP_SIGNAL_COLOR_INFO,
+};
+
+static const u32 sm8550_vdec_output_config_params[] = {
+	HFI_PROP_COLOR_FORMAT,
+	HFI_PROP_LINEAR_STRIDE_SCANLINE,
+};
+
+static const u32 sm8550_vdec_subscribe_input_properties[] = {
+	HFI_PROP_NO_OUTPUT,
+};
+
+static const u32 sm8550_vdec_subscribe_output_properties[] = {
+	HFI_PROP_PICTURE_TYPE,
+	HFI_PROP_CABAC_SESSION,
+};
+
+static const u32 sm8550_dec_ip_int_buf_tbl[] = {
+	BUF_BIN,
+	BUF_COMV,
+	BUF_NON_COMV,
+	BUF_LINE,
+};
+
+static const u32 sm8550_dec_op_int_buf_tbl[] = {
+	BUF_DPB,
+};
+
+struct iris_platform_data sm8550_data = {
+	.get_instance = iris_hfi_gen2_get_instance,
+	.init_hfi_command_ops = iris_hfi_gen2_command_ops_init,
+	.init_hfi_response_ops = iris_hfi_gen2_response_ops_init,
+	.vpu_ops = &iris_vpu3_ops,
+	.set_preset_registers = iris_set_sm8550_preset_registers,
+	.icc_tbl = sm8550_icc_table,
+	.icc_tbl_size = ARRAY_SIZE(sm8550_icc_table),
+	.clk_rst_tbl = sm8550_clk_reset_table,
+	.clk_rst_tbl_size = ARRAY_SIZE(sm8550_clk_reset_table),
+	.bw_tbl_dec = sm8550_bw_table_dec,
+	.bw_tbl_dec_size = ARRAY_SIZE(sm8550_bw_table_dec),
+	.pmdomain_tbl = sm8550_pmdomain_table,
+	.pmdomain_tbl_size = ARRAY_SIZE(sm8550_pmdomain_table),
+	.opp_pd_tbl = sm8550_opp_pd_table,
+	.opp_pd_tbl_size = ARRAY_SIZE(sm8550_opp_pd_table),
+	.clk_tbl = sm8550_clk_table,
+	.clk_tbl_size = ARRAY_SIZE(sm8550_clk_table),
+	/* Upper bound of DMA address range */
+	.dma_mask = 0xe0000000 - 1,
+	.fwname = "qcom/vpu/vpu30_p4.mbn",
+	.pas_id = IRIS_PAS_ID,
+	.inst_caps = &platform_inst_cap_sm8550,
+	.inst_fw_caps = inst_fw_cap_sm8550,
+	.inst_fw_caps_size = ARRAY_SIZE(inst_fw_cap_sm8550),
+	.tz_cp_config_data = &tz_cp_config_sm8550,
+	.core_arch = VIDEO_ARCH_LX,
+	.hw_response_timeout = HW_RESPONSE_TIMEOUT_VALUE,
+	.ubwc_config = &ubwc_config_sm8550,
+	.num_vpp_pipe = 4,
+	.max_session_count = 16,
+	.max_core_mbpf = ((8192 * 4352) / 256) * 2,
+	.input_config_params =
+		sm8550_vdec_input_config_params,
+	.input_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_input_config_params),
+	.output_config_params =
+		sm8550_vdec_output_config_params,
+	.output_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_output_config_params),
+	.dec_input_prop = sm8550_vdec_subscribe_input_properties,
+	.dec_input_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_input_properties),
+	.dec_output_prop = sm8550_vdec_subscribe_output_properties,
+	.dec_output_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_output_properties),
+
+	.dec_ip_int_buf_tbl = sm8550_dec_ip_int_buf_tbl,
+	.dec_ip_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_ip_int_buf_tbl),
+	.dec_op_int_buf_tbl = sm8550_dec_op_int_buf_tbl,
+	.dec_op_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_op_int_buf_tbl),
+};
+
+/*
+ * Shares most of SM8550 data except:
+ * - vpu_ops to iris_vpu33_ops
+ * - clk_rst_tbl to sm8650_clk_reset_table
+ * - controller_rst_tbl to sm8650_controller_reset_table
+ * - fwname to "qcom/vpu/vpu33_p4.mbn"
+ */
+struct iris_platform_data sm8650_data = {
+	.get_instance = iris_hfi_gen2_get_instance,
+	.init_hfi_command_ops = iris_hfi_gen2_command_ops_init,
+	.init_hfi_response_ops = iris_hfi_gen2_response_ops_init,
+	.vpu_ops = &iris_vpu33_ops,
+	.set_preset_registers = iris_set_sm8550_preset_registers,
+	.icc_tbl = sm8550_icc_table,
+	.icc_tbl_size = ARRAY_SIZE(sm8550_icc_table),
+	.clk_rst_tbl = sm8650_clk_reset_table,
+	.clk_rst_tbl_size = ARRAY_SIZE(sm8650_clk_reset_table),
+	.controller_rst_tbl = sm8650_controller_reset_table,
+	.controller_rst_tbl_size = ARRAY_SIZE(sm8650_controller_reset_table),
+	.bw_tbl_dec = sm8550_bw_table_dec,
+	.bw_tbl_dec_size = ARRAY_SIZE(sm8550_bw_table_dec),
+	.pmdomain_tbl = sm8550_pmdomain_table,
+	.pmdomain_tbl_size = ARRAY_SIZE(sm8550_pmdomain_table),
+	.opp_pd_tbl = sm8550_opp_pd_table,
+	.opp_pd_tbl_size = ARRAY_SIZE(sm8550_opp_pd_table),
+	.clk_tbl = sm8550_clk_table,
+	.clk_tbl_size = ARRAY_SIZE(sm8550_clk_table),
+	/* Upper bound of DMA address range */
+	.dma_mask = 0xe0000000 - 1,
+	.fwname = "qcom/vpu/vpu33_p4.mbn",
+	.pas_id = IRIS_PAS_ID,
+	.inst_caps = &platform_inst_cap_sm8550,
+	.inst_fw_caps = inst_fw_cap_sm8550,
+	.inst_fw_caps_size = ARRAY_SIZE(inst_fw_cap_sm8550),
+	.tz_cp_config_data = &tz_cp_config_sm8550,
+	.core_arch = VIDEO_ARCH_LX,
+	.hw_response_timeout = HW_RESPONSE_TIMEOUT_VALUE,
+	.ubwc_config = &ubwc_config_sm8550,
+	.num_vpp_pipe = 4,
+	.max_session_count = 16,
+	.max_core_mbpf = ((8192 * 4352) / 256) * 2,
+	.input_config_params =
+		sm8550_vdec_input_config_params,
+	.input_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_input_config_params),
+	.output_config_params =
+		sm8550_vdec_output_config_params,
+	.output_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_output_config_params),
+	.dec_input_prop = sm8550_vdec_subscribe_input_properties,
+	.dec_input_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_input_properties),
+	.dec_output_prop = sm8550_vdec_subscribe_output_properties,
+	.dec_output_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_output_properties),
+
+	.dec_ip_int_buf_tbl = sm8550_dec_ip_int_buf_tbl,
+	.dec_ip_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_ip_int_buf_tbl),
+	.dec_op_int_buf_tbl = sm8550_dec_op_int_buf_tbl,
+	.dec_op_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_op_int_buf_tbl),
+};
+
+/*
+ * Shares most of SM8550 data except:
+ * - inst_caps to platform_inst_cap_qcs8300
+ * - inst_fw_caps to inst_fw_cap_qcs8300
+ */
+struct iris_platform_data qcs8300_data = {
+	.get_instance = iris_hfi_gen2_get_instance,
+	.init_hfi_command_ops = iris_hfi_gen2_command_ops_init,
+	.init_hfi_response_ops = iris_hfi_gen2_response_ops_init,
+	.vpu_ops = &iris_vpu3_ops,
+	.set_preset_registers = iris_set_sm8550_preset_registers,
+	.icc_tbl = sm8550_icc_table,
+	.icc_tbl_size = ARRAY_SIZE(sm8550_icc_table),
+	.clk_rst_tbl = sm8550_clk_reset_table,
+	.clk_rst_tbl_size = ARRAY_SIZE(sm8550_clk_reset_table),
+	.bw_tbl_dec = sm8550_bw_table_dec,
+	.bw_tbl_dec_size = ARRAY_SIZE(sm8550_bw_table_dec),
+	.pmdomain_tbl = sm8550_pmdomain_table,
+	.pmdomain_tbl_size = ARRAY_SIZE(sm8550_pmdomain_table),
+	.opp_pd_tbl = sm8550_opp_pd_table,
+	.opp_pd_tbl_size = ARRAY_SIZE(sm8550_opp_pd_table),
+	.clk_tbl = sm8550_clk_table,
+	.clk_tbl_size = ARRAY_SIZE(sm8550_clk_table),
+	/* Upper bound of DMA address range */
+	.dma_mask = 0xe0000000 - 1,
+	.fwname = "qcom/vpu/vpu30_p4_s6.mbn",
+	.pas_id = IRIS_PAS_ID,
+	.inst_caps = &platform_inst_cap_qcs8300,
+	.inst_fw_caps = inst_fw_cap_qcs8300,
+	.inst_fw_caps_size = ARRAY_SIZE(inst_fw_cap_qcs8300),
+	.tz_cp_config_data = &tz_cp_config_sm8550,
+	.core_arch = VIDEO_ARCH_LX,
+	.hw_response_timeout = HW_RESPONSE_TIMEOUT_VALUE,
+	.ubwc_config = &ubwc_config_sm8550,
+	.num_vpp_pipe = 2,
+	.max_session_count = 16,
+	.max_core_mbpf = ((4096 * 2176) / 256) * 4,
+	.input_config_params =
+		sm8550_vdec_input_config_params,
+	.input_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_input_config_params),
+	.output_config_params =
+		sm8550_vdec_output_config_params,
+	.output_config_params_size =
+		ARRAY_SIZE(sm8550_vdec_output_config_params),
+	.dec_input_prop = sm8550_vdec_subscribe_input_properties,
+	.dec_input_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_input_properties),
+	.dec_output_prop = sm8550_vdec_subscribe_output_properties,
+	.dec_output_prop_size = ARRAY_SIZE(sm8550_vdec_subscribe_output_properties),
+
+	.dec_ip_int_buf_tbl = sm8550_dec_ip_int_buf_tbl,
+	.dec_ip_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_ip_int_buf_tbl),
+	.dec_op_int_buf_tbl = sm8550_dec_op_int_buf_tbl,
+	.dec_op_int_buf_tbl_size = ARRAY_SIZE(sm8550_dec_op_int_buf_tbl),
+};
diff --git a/drivers/media/platform/qcom/iris/iris_platform_qcs8300.h b/drivers/media/platform/qcom/iris/iris_platform_qcs8300.h
new file mode 100644
index 000000000000..f82355d72fcf
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_platform_qcs8300.h
@@ -0,0 +1,124 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2025 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+static struct platform_inst_fw_cap inst_fw_cap_qcs8300[] = {
+	{
+		.cap_id = PROFILE,
+		.min = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
+		.max = V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_HIGH,
+		.step_or_mask = BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
+			BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_HIGH) |
+			BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
+			BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
+			BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH),
+		.value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
+		.hfi_id = HFI_PROP_PROFILE,
+		.flags = CAP_FLAG_OUTPUT_PORT | CAP_FLAG_MENU,
+		.set = iris_set_u32_enum,
+	},
+	{
+		.cap_id = LEVEL,
+		.min = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
+		.max = V4L2_MPEG_VIDEO_H264_LEVEL_6_2,
+		.step_or_mask = BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B)  |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_2) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_5_2) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_0) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_1) |
+			BIT(V4L2_MPEG_VIDEO_H264_LEVEL_6_2),
+		.value = V4L2_MPEG_VIDEO_H264_LEVEL_6_1,
+		.hfi_id = HFI_PROP_LEVEL,
+		.flags = CAP_FLAG_OUTPUT_PORT | CAP_FLAG_MENU,
+		.set = iris_set_u32_enum,
+	},
+	{
+		.cap_id = INPUT_BUF_HOST_MAX_COUNT,
+		.min = DEFAULT_MAX_HOST_BUF_COUNT,
+		.max = DEFAULT_MAX_HOST_BURST_BUF_COUNT,
+		.step_or_mask = 1,
+		.value = DEFAULT_MAX_HOST_BUF_COUNT,
+		.hfi_id = HFI_PROP_BUFFER_HOST_MAX_COUNT,
+		.flags = CAP_FLAG_INPUT_PORT,
+		.set = iris_set_u32,
+	},
+	{
+		.cap_id = STAGE,
+		.min = STAGE_1,
+		.max = STAGE_2,
+		.step_or_mask = 1,
+		.value = STAGE_2,
+		.hfi_id = HFI_PROP_STAGE,
+		.set = iris_set_stage,
+	},
+	{
+		.cap_id = PIPE,
+		.min = PIPE_1,
+		.max = PIPE_2,
+		.step_or_mask = 1,
+		.value = PIPE_2,
+		.hfi_id = HFI_PROP_PIPE,
+		.set = iris_set_pipe,
+	},
+	{
+		.cap_id = POC,
+		.min = 0,
+		.max = 2,
+		.step_or_mask = 1,
+		.value = 1,
+		.hfi_id = HFI_PROP_PIC_ORDER_CNT_TYPE,
+	},
+	{
+		.cap_id = CODED_FRAMES,
+		.min = CODED_FRAMES_PROGRESSIVE,
+		.max = CODED_FRAMES_PROGRESSIVE,
+		.step_or_mask = 0,
+		.value = CODED_FRAMES_PROGRESSIVE,
+		.hfi_id = HFI_PROP_CODED_FRAMES,
+	},
+	{
+		.cap_id = BIT_DEPTH,
+		.min = BIT_DEPTH_8,
+		.max = BIT_DEPTH_8,
+		.step_or_mask = 1,
+		.value = BIT_DEPTH_8,
+		.hfi_id = HFI_PROP_LUMA_CHROMA_BIT_DEPTH,
+	},
+	{
+		.cap_id = RAP_FRAME,
+		.min = 0,
+		.max = 1,
+		.step_or_mask = 1,
+		.value = 1,
+		.hfi_id = HFI_PROP_DEC_START_FROM_RAP_FRAME,
+		.flags = CAP_FLAG_INPUT_PORT,
+		.set = iris_set_u32,
+	},
+};
+
+static struct platform_inst_caps platform_inst_cap_qcs8300 = {
+	.min_frame_width = 96,
+	.max_frame_width = 4096,
+	.min_frame_height = 96,
+	.max_frame_height = 4096,
+	.max_mbpf = (4096 * 2176) / 256,
+	.mb_cycles_vpp = 200,
+	.mb_cycles_fw = 326389,
+	.mb_cycles_fw_vpp = 44156,
+	.num_comv = 0,
+};
diff --git a/drivers/media/platform/qcom/iris/iris_platform_sm8250.c b/drivers/media/platform/qcom/iris/iris_platform_sm8250.c
new file mode 100644
index 000000000000..5c86fd7b7b6f
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_platform_sm8250.c
@@ -0,0 +1,149 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_core.h"
+#include "iris_ctrls.h"
+#include "iris_platform_common.h"
+#include "iris_resources.h"
+#include "iris_hfi_gen1.h"
+#include "iris_hfi_gen1_defines.h"
+#include "iris_vpu_common.h"
+
+static struct platform_inst_fw_cap inst_fw_cap_sm8250[] = {
+	{
+		.cap_id = PIPE,
+		.min = PIPE_1,
+		.max = PIPE_4,
+		.step_or_mask = 1,
+		.value = PIPE_4,
+		.hfi_id = HFI_PROPERTY_PARAM_WORK_ROUTE,
+		.set = iris_set_pipe,
+	},
+	{
+		.cap_id = STAGE,
+		.min = STAGE_1,
+		.max = STAGE_2,
+		.step_or_mask = 1,
+		.value = STAGE_2,
+		.hfi_id = HFI_PROPERTY_PARAM_WORK_MODE,
+		.set = iris_set_stage,
+	},
+	{
+		.cap_id = DEBLOCK,
+		.min = 0,
+		.max = 1,
+		.step_or_mask = 1,
+		.value = 0,
+		.hfi_id = HFI_PROPERTY_CONFIG_VDEC_POST_LOOP_DEBLOCKER,
+		.set = iris_set_u32,
+	},
+};
+
+static struct platform_inst_caps platform_inst_cap_sm8250 = {
+	.min_frame_width = 128,
+	.max_frame_width = 8192,
+	.min_frame_height = 128,
+	.max_frame_height = 8192,
+	.max_mbpf = 138240,
+	.mb_cycles_vsp = 25,
+	.mb_cycles_vpp = 200,
+};
+
+static void iris_set_sm8250_preset_registers(struct iris_core *core)
+{
+	writel(0x0, core->reg_base + 0xB0088);
+}
+
+static const struct icc_info sm8250_icc_table[] = {
+	{ "cpu-cfg",    1000, 1000     },
+	{ "video-mem",  1000, 15000000 },
+};
+
+static const char * const sm8250_clk_reset_table[] = { "bus", "core" };
+
+static const struct bw_info sm8250_bw_table_dec[] = {
+	{ ((4096 * 2160) / 256) * 60, 2403000 },
+	{ ((4096 * 2160) / 256) * 30, 1224000 },
+	{ ((1920 * 1080) / 256) * 60,  812000 },
+	{ ((1920 * 1080) / 256) * 30,  416000 },
+};
+
+static const char * const sm8250_pmdomain_table[] = { "venus", "vcodec0" };
+
+static const char * const sm8250_opp_pd_table[] = { "mx" };
+
+static const struct platform_clk_data sm8250_clk_table[] = {
+	{IRIS_AXI_CLK,  "iface"        },
+	{IRIS_CTRL_CLK, "core"         },
+	{IRIS_HW_CLK,   "vcodec0_core" },
+};
+
+static struct tz_cp_config tz_cp_config_sm8250 = {
+	.cp_start = 0,
+	.cp_size = 0x25800000,
+	.cp_nonpixel_start = 0x01000000,
+	.cp_nonpixel_size = 0x24800000,
+};
+
+static const u32 sm8250_vdec_input_config_param_default[] = {
+	HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE,
+	HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SELECT,
+	HFI_PROPERTY_PARAM_UNCOMPRESSED_PLANE_ACTUAL_CONSTRAINTS_INFO,
+	HFI_PROPERTY_PARAM_BUFFER_COUNT_ACTUAL,
+	HFI_PROPERTY_PARAM_VDEC_MULTI_STREAM,
+	HFI_PROPERTY_PARAM_FRAME_SIZE,
+	HFI_PROPERTY_PARAM_BUFFER_SIZE_ACTUAL,
+	HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE,
+};
+
+static const u32 sm8250_dec_ip_int_buf_tbl[] = {
+	BUF_BIN,
+	BUF_SCRATCH_1,
+};
+
+static const u32 sm8250_dec_op_int_buf_tbl[] = {
+	BUF_DPB,
+};
+
+struct iris_platform_data sm8250_data = {
+	.get_instance = iris_hfi_gen1_get_instance,
+	.init_hfi_command_ops = &iris_hfi_gen1_command_ops_init,
+	.init_hfi_response_ops = iris_hfi_gen1_response_ops_init,
+	.vpu_ops = &iris_vpu2_ops,
+	.set_preset_registers = iris_set_sm8250_preset_registers,
+	.icc_tbl = sm8250_icc_table,
+	.icc_tbl_size = ARRAY_SIZE(sm8250_icc_table),
+	.clk_rst_tbl = sm8250_clk_reset_table,
+	.clk_rst_tbl_size = ARRAY_SIZE(sm8250_clk_reset_table),
+	.bw_tbl_dec = sm8250_bw_table_dec,
+	.bw_tbl_dec_size = ARRAY_SIZE(sm8250_bw_table_dec),
+	.pmdomain_tbl = sm8250_pmdomain_table,
+	.pmdomain_tbl_size = ARRAY_SIZE(sm8250_pmdomain_table),
+	.opp_pd_tbl = sm8250_opp_pd_table,
+	.opp_pd_tbl_size = ARRAY_SIZE(sm8250_opp_pd_table),
+	.clk_tbl = sm8250_clk_table,
+	.clk_tbl_size = ARRAY_SIZE(sm8250_clk_table),
+	/* Upper bound of DMA address range */
+	.dma_mask = 0xe0000000 - 1,
+	.fwname = "qcom/vpu-1.0/venus.mbn",
+	.pas_id = IRIS_PAS_ID,
+	.inst_caps = &platform_inst_cap_sm8250,
+	.inst_fw_caps = inst_fw_cap_sm8250,
+	.inst_fw_caps_size = ARRAY_SIZE(inst_fw_cap_sm8250),
+	.tz_cp_config_data = &tz_cp_config_sm8250,
+	.hw_response_timeout = HW_RESPONSE_TIMEOUT_VALUE,
+	.num_vpp_pipe = 4,
+	.max_session_count = 16,
+	.max_core_mbpf = (8192 * 4352) / 256,
+	.input_config_params =
+		sm8250_vdec_input_config_param_default,
+	.input_config_params_size =
+		ARRAY_SIZE(sm8250_vdec_input_config_param_default),
+
+	.dec_ip_int_buf_tbl = sm8250_dec_ip_int_buf_tbl,
+	.dec_ip_int_buf_tbl_size = ARRAY_SIZE(sm8250_dec_ip_int_buf_tbl),
+	.dec_op_int_buf_tbl = sm8250_dec_op_int_buf_tbl,
+	.dec_op_int_buf_tbl_size = ARRAY_SIZE(sm8250_dec_op_int_buf_tbl),
+};
diff --git a/drivers/media/platform/qcom/iris/iris_platform_sm8650.h b/drivers/media/platform/qcom/iris/iris_platform_sm8650.h
new file mode 100644
index 000000000000..75e9d572e788
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_platform_sm8650.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_PLATFORM_SM8650_H__
+#define __IRIS_PLATFORM_SM8650_H__
+
+static const char * const sm8650_clk_reset_table[] = { "bus", "core" };
+
+static const char * const sm8650_controller_reset_table[] = { "xo" };
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_power.c b/drivers/media/platform/qcom/iris/iris_power.c
new file mode 100644
index 000000000000..dbca42df0910
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_power.c
@@ -0,0 +1,140 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_opp.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_buffer.h"
+#include "iris_instance.h"
+#include "iris_power.h"
+#include "iris_resources.h"
+#include "iris_vpu_common.h"
+
+static u32 iris_calc_bw(struct iris_inst *inst, struct icc_vote_data *data)
+{
+	const struct bw_info *bw_tbl = NULL;
+	struct iris_core *core = inst->core;
+	u32 num_rows, i, mbs, mbps;
+	u32 icc_bw = 0;
+
+	mbs = DIV_ROUND_UP(data->height, 16) * DIV_ROUND_UP(data->width, 16);
+	mbps = mbs * data->fps;
+	if (mbps == 0)
+		goto exit;
+
+	bw_tbl = core->iris_platform_data->bw_tbl_dec;
+	num_rows = core->iris_platform_data->bw_tbl_dec_size;
+
+	for (i = 0; i < num_rows; i++) {
+		if (i != 0 && mbps > bw_tbl[i].mbs_per_sec)
+			break;
+
+		icc_bw = bw_tbl[i].bw_ddr;
+	}
+
+exit:
+	return icc_bw;
+}
+
+static int iris_set_interconnects(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *instance;
+	u64 total_bw_ddr = 0;
+	int ret;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(instance, &core->instances, list) {
+		if (!instance->max_input_data_size)
+			continue;
+
+		total_bw_ddr += instance->power.icc_bw;
+	}
+
+	ret = iris_set_icc_bw(core, total_bw_ddr);
+
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+static int iris_vote_interconnects(struct iris_inst *inst)
+{
+	struct icc_vote_data *vote_data = &inst->icc_data;
+	struct v4l2_format *inp_f = inst->fmt_src;
+
+	vote_data->width = inp_f->fmt.pix_mp.width;
+	vote_data->height = inp_f->fmt.pix_mp.height;
+	vote_data->fps = DEFAULT_FPS;
+
+	inst->power.icc_bw = iris_calc_bw(inst, vote_data);
+
+	return iris_set_interconnects(inst);
+}
+
+static int iris_set_clocks(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *instance;
+	u64 freq = 0;
+	int ret;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(instance, &core->instances, list) {
+		if (!instance->max_input_data_size)
+			continue;
+
+		freq += instance->power.min_freq;
+	}
+
+	core->power.clk_freq = freq;
+	ret = dev_pm_opp_set_rate(core->dev, freq);
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+static int iris_scale_clocks(struct iris_inst *inst)
+{
+	const struct vpu_ops *vpu_ops = inst->core->iris_platform_data->vpu_ops;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buffer, *n;
+	struct iris_buffer *buf;
+	size_t data_size = 0;
+
+	v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buffer, n) {
+		buf = to_iris_buffer(&buffer->vb);
+		data_size = max(data_size, buf->data_size);
+	}
+
+	inst->max_input_data_size = data_size;
+	if (!inst->max_input_data_size)
+		return 0;
+
+	inst->power.min_freq = vpu_ops->calc_freq(inst, inst->max_input_data_size);
+
+	return iris_set_clocks(inst);
+}
+
+int iris_scale_power(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	int ret;
+
+	if (pm_runtime_suspended(core->dev)) {
+		ret = pm_runtime_resume_and_get(core->dev);
+		if (ret < 0)
+			return ret;
+
+		pm_runtime_put_autosuspend(core->dev);
+	}
+
+	ret = iris_scale_clocks(inst);
+	if (ret)
+		return ret;
+
+	return iris_vote_interconnects(inst);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_power.h b/drivers/media/platform/qcom/iris/iris_power.h
new file mode 100644
index 000000000000..55212660e72d
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_power.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_POWER_H__
+#define __IRIS_POWER_H__
+
+struct iris_inst;
+
+int iris_scale_power(struct iris_inst *inst);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_probe.c b/drivers/media/platform/qcom/iris/iris_probe.c
new file mode 100644
index 000000000000..9a7ce142f700
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_probe.c
@@ -0,0 +1,372 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/clk.h>
+#include <linux/interconnect.h>
+#include <linux/module.h>
+#include <linux/pm_domain.h>
+#include <linux/pm_opp.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+
+#include "iris_core.h"
+#include "iris_ctrls.h"
+#include "iris_vidc.h"
+
+static int iris_init_icc(struct iris_core *core)
+{
+	const struct icc_info *icc_tbl;
+	u32 i = 0;
+
+	icc_tbl = core->iris_platform_data->icc_tbl;
+
+	core->icc_count = core->iris_platform_data->icc_tbl_size;
+	core->icc_tbl = devm_kzalloc(core->dev,
+				     sizeof(struct icc_bulk_data) * core->icc_count,
+				     GFP_KERNEL);
+	if (!core->icc_tbl)
+		return -ENOMEM;
+
+	for (i = 0; i < core->icc_count; i++) {
+		core->icc_tbl[i].name = icc_tbl[i].name;
+		core->icc_tbl[i].avg_bw = icc_tbl[i].bw_min_kbps;
+		core->icc_tbl[i].peak_bw = 0;
+	}
+
+	return devm_of_icc_bulk_get(core->dev, core->icc_count, core->icc_tbl);
+}
+
+static int iris_init_power_domains(struct iris_core *core)
+{
+	const struct platform_clk_data *clk_tbl;
+	u32 clk_cnt, i;
+	int ret;
+
+	struct dev_pm_domain_attach_data iris_pd_data = {
+		.pd_names = core->iris_platform_data->pmdomain_tbl,
+		.num_pd_names = core->iris_platform_data->pmdomain_tbl_size,
+		.pd_flags = PD_FLAG_NO_DEV_LINK,
+	};
+
+	struct dev_pm_domain_attach_data iris_opp_pd_data = {
+		.pd_names = core->iris_platform_data->opp_pd_tbl,
+		.num_pd_names = core->iris_platform_data->opp_pd_tbl_size,
+		.pd_flags = PD_FLAG_DEV_LINK_ON,
+	};
+
+	ret = devm_pm_domain_attach_list(core->dev, &iris_pd_data, &core->pmdomain_tbl);
+	if (ret < 0)
+		return ret;
+
+	ret =  devm_pm_domain_attach_list(core->dev, &iris_opp_pd_data, &core->opp_pmdomain_tbl);
+	if (ret < 0)
+		return ret;
+
+	clk_tbl = core->iris_platform_data->clk_tbl;
+	clk_cnt = core->iris_platform_data->clk_tbl_size;
+
+	for (i = 0; i < clk_cnt; i++) {
+		if (clk_tbl[i].clk_type == IRIS_HW_CLK) {
+			ret = devm_pm_opp_set_clkname(core->dev, clk_tbl[i].clk_name);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return devm_pm_opp_of_add_table(core->dev);
+}
+
+static int iris_init_clocks(struct iris_core *core)
+{
+	int ret;
+
+	ret = devm_clk_bulk_get_all(core->dev, &core->clock_tbl);
+	if (ret < 0)
+		return ret;
+
+	core->clk_count = ret;
+
+	return 0;
+}
+
+static int iris_init_reset_table(struct iris_core *core,
+				 struct reset_control_bulk_data **resets,
+				 const char * const *rst_tbl, u32 rst_tbl_size)
+{
+	u32 i = 0;
+
+	*resets = devm_kzalloc(core->dev,
+			       sizeof(struct reset_control_bulk_data) * rst_tbl_size,
+			       GFP_KERNEL);
+	if (!*resets)
+		return -ENOMEM;
+
+	for (i = 0; i < rst_tbl_size; i++)
+		(*resets)[i].id = rst_tbl[i];
+
+	return devm_reset_control_bulk_get_exclusive(core->dev, rst_tbl_size, *resets);
+}
+
+static int iris_init_resets(struct iris_core *core)
+{
+	int ret;
+
+	ret = iris_init_reset_table(core, &core->resets,
+				    core->iris_platform_data->clk_rst_tbl,
+				    core->iris_platform_data->clk_rst_tbl_size);
+	if (ret)
+		return ret;
+
+	if (!core->iris_platform_data->controller_rst_tbl_size)
+		return 0;
+
+	return iris_init_reset_table(core, &core->controller_resets,
+				     core->iris_platform_data->controller_rst_tbl,
+				     core->iris_platform_data->controller_rst_tbl_size);
+}
+
+static int iris_init_resources(struct iris_core *core)
+{
+	int ret;
+
+	ret = iris_init_icc(core);
+	if (ret)
+		return ret;
+
+	ret = iris_init_power_domains(core);
+	if (ret)
+		return ret;
+
+	ret = iris_init_clocks(core);
+	if (ret)
+		return ret;
+
+	return iris_init_resets(core);
+}
+
+static int iris_register_video_device(struct iris_core *core)
+{
+	struct video_device *vdev;
+	int ret;
+
+	vdev = video_device_alloc();
+	if (!vdev)
+		return -ENOMEM;
+
+	strscpy(vdev->name, "qcom-iris-decoder", sizeof(vdev->name));
+	vdev->release = video_device_release;
+	vdev->fops = core->iris_v4l2_file_ops;
+	vdev->ioctl_ops = core->iris_v4l2_ioctl_ops;
+	vdev->vfl_dir = VFL_DIR_M2M;
+	vdev->v4l2_dev = &core->v4l2_dev;
+	vdev->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING;
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret)
+		goto err_vdev_release;
+
+	core->vdev_dec = vdev;
+	video_set_drvdata(vdev, core);
+
+	return 0;
+
+err_vdev_release:
+	video_device_release(vdev);
+
+	return ret;
+}
+
+static void iris_remove(struct platform_device *pdev)
+{
+	struct iris_core *core;
+
+	core = platform_get_drvdata(pdev);
+	if (!core)
+		return;
+
+	iris_core_deinit(core);
+
+	video_unregister_device(core->vdev_dec);
+
+	v4l2_device_unregister(&core->v4l2_dev);
+
+	mutex_destroy(&core->lock);
+}
+
+static void iris_sys_error_handler(struct work_struct *work)
+{
+	struct iris_core *core =
+			container_of(work, struct iris_core, sys_error_handler.work);
+
+	iris_core_deinit(core);
+	iris_core_init(core);
+}
+
+static int iris_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct iris_core *core;
+	u64 dma_mask;
+	int ret;
+
+	core = devm_kzalloc(&pdev->dev, sizeof(*core), GFP_KERNEL);
+	if (!core)
+		return -ENOMEM;
+	core->dev = dev;
+
+	core->state = IRIS_CORE_DEINIT;
+	mutex_init(&core->lock);
+	init_completion(&core->core_init_done);
+
+	core->response_packet = devm_kzalloc(core->dev, IFACEQ_CORE_PKT_SIZE, GFP_KERNEL);
+	if (!core->response_packet)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&core->instances);
+	INIT_DELAYED_WORK(&core->sys_error_handler, iris_sys_error_handler);
+
+	core->reg_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(core->reg_base))
+		return PTR_ERR(core->reg_base);
+
+	core->irq = platform_get_irq(pdev, 0);
+	if (core->irq < 0)
+		return core->irq;
+
+	core->iris_platform_data = of_device_get_match_data(core->dev);
+
+	ret = devm_request_threaded_irq(core->dev, core->irq, iris_hfi_isr,
+					iris_hfi_isr_handler, IRQF_TRIGGER_HIGH, "iris", core);
+	if (ret)
+		return ret;
+
+	disable_irq_nosync(core->irq);
+
+	iris_init_ops(core);
+	core->iris_platform_data->init_hfi_command_ops(core);
+	core->iris_platform_data->init_hfi_response_ops(core);
+
+	ret = iris_init_resources(core);
+	if (ret)
+		return ret;
+
+	iris_session_init_caps(core);
+
+	ret = v4l2_device_register(dev, &core->v4l2_dev);
+	if (ret)
+		return ret;
+
+	ret = iris_register_video_device(core);
+	if (ret)
+		goto err_v4l2_unreg;
+
+	platform_set_drvdata(pdev, core);
+
+	dma_mask = core->iris_platform_data->dma_mask;
+
+	ret = dma_set_mask_and_coherent(dev, dma_mask);
+	if (ret)
+		goto err_vdev_unreg;
+
+	dma_set_max_seg_size(&pdev->dev, DMA_BIT_MASK(32));
+	dma_set_seg_boundary(&pdev->dev, DMA_BIT_MASK(32));
+
+	pm_runtime_set_autosuspend_delay(core->dev, AUTOSUSPEND_DELAY_VALUE);
+	pm_runtime_use_autosuspend(core->dev);
+	ret = devm_pm_runtime_enable(core->dev);
+	if (ret)
+		goto err_vdev_unreg;
+
+	return 0;
+
+err_vdev_unreg:
+	video_unregister_device(core->vdev_dec);
+err_v4l2_unreg:
+	v4l2_device_unregister(&core->v4l2_dev);
+
+	return ret;
+}
+
+static int __maybe_unused iris_pm_suspend(struct device *dev)
+{
+	struct iris_core *core;
+	int ret = 0;
+
+	core = dev_get_drvdata(dev);
+
+	mutex_lock(&core->lock);
+	if (core->state != IRIS_CORE_INIT)
+		goto exit;
+
+	ret = iris_hfi_pm_suspend(core);
+
+exit:
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+static int __maybe_unused iris_pm_resume(struct device *dev)
+{
+	struct iris_core *core;
+	int ret = 0;
+
+	core = dev_get_drvdata(dev);
+
+	mutex_lock(&core->lock);
+	if (core->state != IRIS_CORE_INIT)
+		goto exit;
+
+	ret = iris_hfi_pm_resume(core);
+	pm_runtime_mark_last_busy(core->dev);
+
+exit:
+	mutex_unlock(&core->lock);
+
+	return ret;
+}
+
+static const struct dev_pm_ops iris_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(iris_pm_suspend, iris_pm_resume, NULL)
+};
+
+static const struct of_device_id iris_dt_match[] = {
+	{
+		.compatible = "qcom,qcs8300-iris",
+		.data = &qcs8300_data,
+	},
+#if (!IS_ENABLED(CONFIG_VIDEO_QCOM_VENUS))
+	{
+		.compatible = "qcom,sm8250-venus",
+		.data = &sm8250_data,
+	},
+#endif
+	{
+		.compatible = "qcom,sm8550-iris",
+		.data = &sm8550_data,
+	},
+	{
+		.compatible = "qcom,sm8650-iris",
+		.data = &sm8650_data,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, iris_dt_match);
+
+static struct platform_driver qcom_iris_driver = {
+	.probe = iris_probe,
+	.remove = iris_remove,
+	.driver = {
+		.name = "qcom-iris",
+		.of_match_table = iris_dt_match,
+		.pm = &iris_pm_ops,
+	},
+};
+
+module_platform_driver(qcom_iris_driver);
+MODULE_DESCRIPTION("Qualcomm iris video driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/qcom/iris/iris_resources.c b/drivers/media/platform/qcom/iris/iris_resources.c
new file mode 100644
index 000000000000..cf32f268b703
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_resources.c
@@ -0,0 +1,131 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/clk.h>
+#include <linux/interconnect.h>
+#include <linux/pm_domain.h>
+#include <linux/pm_opp.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+
+#include "iris_core.h"
+#include "iris_resources.h"
+
+#define BW_THRESHOLD 50000
+
+int iris_set_icc_bw(struct iris_core *core, unsigned long icc_bw)
+{
+	unsigned long bw_kbps = 0, bw_prev = 0;
+	const struct icc_info *icc_tbl;
+	int ret = 0, i;
+
+	icc_tbl = core->iris_platform_data->icc_tbl;
+
+	for (i = 0; i < core->icc_count; i++) {
+		if (!strcmp(core->icc_tbl[i].name, "video-mem")) {
+			bw_kbps = icc_bw;
+			bw_prev = core->power.icc_bw;
+
+			bw_kbps = clamp_t(typeof(bw_kbps), bw_kbps,
+					  icc_tbl[i].bw_min_kbps, icc_tbl[i].bw_max_kbps);
+
+			if (abs(bw_kbps - bw_prev) < BW_THRESHOLD && bw_prev)
+				return ret;
+
+			core->icc_tbl[i].avg_bw = bw_kbps;
+
+			core->power.icc_bw = bw_kbps;
+			break;
+		}
+	}
+
+	return icc_bulk_set_bw(core->icc_count, core->icc_tbl);
+}
+
+int iris_unset_icc_bw(struct iris_core *core)
+{
+	u32 i;
+
+	core->power.icc_bw = 0;
+
+	for (i = 0; i < core->icc_count; i++) {
+		core->icc_tbl[i].avg_bw = 0;
+		core->icc_tbl[i].peak_bw = 0;
+	}
+
+	return icc_bulk_set_bw(core->icc_count, core->icc_tbl);
+}
+
+int iris_enable_power_domains(struct iris_core *core, struct device *pd_dev)
+{
+	int ret;
+
+	ret = dev_pm_opp_set_rate(core->dev, ULONG_MAX);
+	if (ret)
+		return ret;
+
+	ret = pm_runtime_get_sync(pd_dev);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+int iris_disable_power_domains(struct iris_core *core, struct device *pd_dev)
+{
+	int ret;
+
+	ret = dev_pm_opp_set_rate(core->dev, 0);
+	if (ret)
+		return ret;
+
+	pm_runtime_put_sync(pd_dev);
+
+	return 0;
+}
+
+static struct clk *iris_get_clk_by_type(struct iris_core *core, enum platform_clk_type clk_type)
+{
+	const struct platform_clk_data *clk_tbl;
+	u32 clk_cnt, i, j;
+
+	clk_tbl = core->iris_platform_data->clk_tbl;
+	clk_cnt = core->iris_platform_data->clk_tbl_size;
+
+	for (i = 0; i < clk_cnt; i++) {
+		if (clk_tbl[i].clk_type == clk_type) {
+			for (j = 0; core->clock_tbl && j < core->clk_count; j++) {
+				if (!strcmp(core->clock_tbl[j].id, clk_tbl[i].clk_name))
+					return core->clock_tbl[j].clk;
+			}
+		}
+	}
+
+	return NULL;
+}
+
+int iris_prepare_enable_clock(struct iris_core *core, enum platform_clk_type clk_type)
+{
+	struct clk *clock;
+
+	clock = iris_get_clk_by_type(core, clk_type);
+	if (!clock)
+		return -EINVAL;
+
+	return clk_prepare_enable(clock);
+}
+
+int iris_disable_unprepare_clock(struct iris_core *core, enum platform_clk_type clk_type)
+{
+	struct clk *clock;
+
+	clock = iris_get_clk_by_type(core, clk_type);
+	if (!clock)
+		return -EINVAL;
+
+	clk_disable_unprepare(clock);
+
+	return 0;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_resources.h b/drivers/media/platform/qcom/iris/iris_resources.h
new file mode 100644
index 000000000000..f723dfe5bd81
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_resources.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_RESOURCES_H__
+#define __IRIS_RESOURCES_H__
+
+struct iris_core;
+
+int iris_enable_power_domains(struct iris_core *core, struct device *pd_dev);
+int iris_disable_power_domains(struct iris_core *core, struct device *pd_dev);
+int iris_unset_icc_bw(struct iris_core *core);
+int iris_set_icc_bw(struct iris_core *core, unsigned long icc_bw);
+int iris_disable_unprepare_clock(struct iris_core *core, enum platform_clk_type clk_type);
+int iris_prepare_enable_clock(struct iris_core *core, enum platform_clk_type clk_type);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_state.c b/drivers/media/platform/qcom/iris/iris_state.c
new file mode 100644
index 000000000000..5976e926c83d
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_state.c
@@ -0,0 +1,276 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_instance.h"
+
+static bool iris_allow_inst_state_change(struct iris_inst *inst,
+					 enum iris_inst_state req_state)
+{
+	switch (inst->state) {
+	case IRIS_INST_INIT:
+		if (req_state == IRIS_INST_INPUT_STREAMING ||
+		    req_state == IRIS_INST_OUTPUT_STREAMING ||
+		    req_state == IRIS_INST_DEINIT)
+			return true;
+		return false;
+	case IRIS_INST_INPUT_STREAMING:
+		if (req_state == IRIS_INST_INIT ||
+		    req_state == IRIS_INST_STREAMING ||
+		    req_state == IRIS_INST_DEINIT)
+			return true;
+		return false;
+	case IRIS_INST_OUTPUT_STREAMING:
+		if (req_state == IRIS_INST_INIT ||
+		    req_state == IRIS_INST_STREAMING ||
+		    req_state == IRIS_INST_DEINIT)
+			return true;
+		return false;
+	case IRIS_INST_STREAMING:
+		if (req_state == IRIS_INST_INPUT_STREAMING ||
+		    req_state == IRIS_INST_OUTPUT_STREAMING ||
+		    req_state == IRIS_INST_DEINIT)
+			return true;
+		return false;
+	case IRIS_INST_DEINIT:
+		if (req_state == IRIS_INST_INIT)
+			return true;
+		return false;
+	default:
+		return false;
+	}
+}
+
+int iris_inst_change_state(struct iris_inst *inst,
+			   enum iris_inst_state request_state)
+{
+	if (inst->state == IRIS_INST_ERROR)
+		return 0;
+
+	if (inst->state == request_state)
+		return 0;
+
+	if (request_state == IRIS_INST_ERROR)
+		goto change_state;
+
+	if (!iris_allow_inst_state_change(inst, request_state))
+		return -EINVAL;
+
+change_state:
+	inst->state = request_state;
+	dev_dbg(inst->core->dev, "state changed from %x to %x\n",
+		inst->state, request_state);
+
+	return 0;
+}
+
+int iris_inst_state_change_streamon(struct iris_inst *inst, u32 plane)
+{
+	enum iris_inst_state new_state = IRIS_INST_ERROR;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		if (inst->state == IRIS_INST_INIT)
+			new_state = IRIS_INST_INPUT_STREAMING;
+		else if (inst->state == IRIS_INST_OUTPUT_STREAMING)
+			new_state = IRIS_INST_STREAMING;
+	} else if (V4L2_TYPE_IS_CAPTURE(plane)) {
+		if (inst->state == IRIS_INST_INIT)
+			new_state = IRIS_INST_OUTPUT_STREAMING;
+		else if (inst->state == IRIS_INST_INPUT_STREAMING)
+			new_state = IRIS_INST_STREAMING;
+	}
+
+	return iris_inst_change_state(inst, new_state);
+}
+
+int iris_inst_state_change_streamoff(struct iris_inst *inst, u32 plane)
+{
+	enum iris_inst_state new_state = IRIS_INST_ERROR;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		if (inst->state == IRIS_INST_INPUT_STREAMING)
+			new_state = IRIS_INST_INIT;
+		else if (inst->state == IRIS_INST_STREAMING)
+			new_state = IRIS_INST_OUTPUT_STREAMING;
+	} else if (V4L2_TYPE_IS_CAPTURE(plane)) {
+		if (inst->state == IRIS_INST_OUTPUT_STREAMING)
+			new_state = IRIS_INST_INIT;
+		else if (inst->state == IRIS_INST_STREAMING)
+			new_state = IRIS_INST_INPUT_STREAMING;
+	}
+
+	return iris_inst_change_state(inst, new_state);
+}
+
+static bool iris_inst_allow_sub_state(struct iris_inst *inst, enum iris_inst_sub_state sub_state)
+{
+	if (!sub_state)
+		return true;
+
+	switch (inst->state) {
+	case IRIS_INST_INIT:
+		if (sub_state & IRIS_INST_SUB_LOAD_RESOURCES)
+			return true;
+		return false;
+	case IRIS_INST_INPUT_STREAMING:
+		if (sub_state & (IRIS_INST_SUB_FIRST_IPSC | IRIS_INST_SUB_DRC |
+			IRIS_INST_SUB_DRAIN | IRIS_INST_SUB_INPUT_PAUSE))
+			return true;
+		return false;
+	case IRIS_INST_OUTPUT_STREAMING:
+		if (sub_state & (IRIS_INST_SUB_DRC_LAST |
+			IRIS_INST_SUB_DRAIN_LAST | IRIS_INST_SUB_OUTPUT_PAUSE))
+			return true;
+		return false;
+	case IRIS_INST_STREAMING:
+		if (sub_state & (IRIS_INST_SUB_DRC | IRIS_INST_SUB_DRAIN |
+			IRIS_INST_SUB_DRC_LAST | IRIS_INST_SUB_DRAIN_LAST |
+			IRIS_INST_SUB_INPUT_PAUSE | IRIS_INST_SUB_OUTPUT_PAUSE))
+			return true;
+		return false;
+	case IRIS_INST_DEINIT:
+		if (sub_state & (IRIS_INST_SUB_DRC | IRIS_INST_SUB_DRAIN |
+			IRIS_INST_SUB_DRC_LAST | IRIS_INST_SUB_DRAIN_LAST |
+			IRIS_INST_SUB_INPUT_PAUSE | IRIS_INST_SUB_OUTPUT_PAUSE))
+			return true;
+		return false;
+	default:
+		return false;
+	}
+}
+
+int iris_inst_change_sub_state(struct iris_inst *inst,
+			       enum iris_inst_sub_state clear_sub_state,
+			       enum iris_inst_sub_state set_sub_state)
+{
+	enum iris_inst_sub_state prev_sub_state;
+
+	if (inst->state == IRIS_INST_ERROR)
+		return 0;
+
+	if (!clear_sub_state && !set_sub_state)
+		return 0;
+
+	if ((clear_sub_state & set_sub_state) ||
+	    set_sub_state > IRIS_INST_MAX_SUB_STATE_VALUE ||
+	    clear_sub_state > IRIS_INST_MAX_SUB_STATE_VALUE)
+		return -EINVAL;
+
+	prev_sub_state = inst->sub_state;
+
+	if (!iris_inst_allow_sub_state(inst, set_sub_state))
+		return -EINVAL;
+
+	inst->sub_state |= set_sub_state;
+	inst->sub_state &= ~clear_sub_state;
+
+	if (inst->sub_state != prev_sub_state)
+		dev_dbg(inst->core->dev, "sub_state changed from %x to %x\n",
+			prev_sub_state, inst->sub_state);
+
+	return 0;
+}
+
+int iris_inst_sub_state_change_drc(struct iris_inst *inst)
+{
+	enum iris_inst_sub_state set_sub_state = 0;
+
+	if (inst->sub_state & IRIS_INST_SUB_DRC)
+		return -EINVAL;
+
+	if (inst->state == IRIS_INST_INPUT_STREAMING ||
+	    inst->state == IRIS_INST_INIT)
+		set_sub_state = IRIS_INST_SUB_FIRST_IPSC | IRIS_INST_SUB_INPUT_PAUSE;
+	else
+		set_sub_state = IRIS_INST_SUB_DRC | IRIS_INST_SUB_INPUT_PAUSE;
+
+	return iris_inst_change_sub_state(inst, 0, set_sub_state);
+}
+
+int iris_inst_sub_state_change_drain_last(struct iris_inst *inst)
+{
+	enum iris_inst_sub_state set_sub_state;
+
+	if (inst->sub_state & IRIS_INST_SUB_DRAIN_LAST)
+		return -EINVAL;
+
+	if (!(inst->sub_state & IRIS_INST_SUB_DRAIN))
+		return -EINVAL;
+
+	set_sub_state = IRIS_INST_SUB_DRAIN_LAST | IRIS_INST_SUB_OUTPUT_PAUSE;
+
+	return iris_inst_change_sub_state(inst, 0, set_sub_state);
+}
+
+int iris_inst_sub_state_change_drc_last(struct iris_inst *inst)
+{
+	enum iris_inst_sub_state set_sub_state;
+
+	if (inst->sub_state & IRIS_INST_SUB_DRC_LAST)
+		return -EINVAL;
+
+	if (!(inst->sub_state & IRIS_INST_SUB_DRC) ||
+	    !(inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE))
+		return -EINVAL;
+
+	if (inst->sub_state & IRIS_INST_SUB_FIRST_IPSC)
+		return 0;
+
+	set_sub_state = IRIS_INST_SUB_DRC_LAST | IRIS_INST_SUB_OUTPUT_PAUSE;
+
+	return iris_inst_change_sub_state(inst, 0, set_sub_state);
+}
+
+int iris_inst_sub_state_change_pause(struct iris_inst *inst, u32 plane)
+{
+	enum iris_inst_sub_state set_sub_state;
+
+	if (V4L2_TYPE_IS_OUTPUT(plane)) {
+		if (inst->sub_state & IRIS_INST_SUB_DRC &&
+		    !(inst->sub_state & IRIS_INST_SUB_DRC_LAST))
+			return -EINVAL;
+
+		if (inst->sub_state & IRIS_INST_SUB_DRAIN &&
+		    !(inst->sub_state & IRIS_INST_SUB_DRAIN_LAST))
+			return -EINVAL;
+
+		set_sub_state = IRIS_INST_SUB_INPUT_PAUSE;
+	} else {
+		set_sub_state = IRIS_INST_SUB_OUTPUT_PAUSE;
+	}
+
+	return iris_inst_change_sub_state(inst, 0, set_sub_state);
+}
+
+static inline bool iris_drc_pending(struct iris_inst *inst)
+{
+	return inst->sub_state & IRIS_INST_SUB_DRC &&
+		inst->sub_state & IRIS_INST_SUB_DRC_LAST;
+}
+
+static inline bool iris_drain_pending(struct iris_inst *inst)
+{
+	return inst->sub_state & IRIS_INST_SUB_DRAIN &&
+		inst->sub_state & IRIS_INST_SUB_DRAIN_LAST;
+}
+
+bool iris_allow_cmd(struct iris_inst *inst, u32 cmd)
+{
+	struct vb2_queue *src_q = v4l2_m2m_get_src_vq(inst->m2m_ctx);
+	struct vb2_queue *dst_q = v4l2_m2m_get_dst_vq(inst->m2m_ctx);
+
+	if (cmd == V4L2_DEC_CMD_START) {
+		if (vb2_is_streaming(src_q) || vb2_is_streaming(dst_q))
+			if (iris_drc_pending(inst) || iris_drain_pending(inst))
+				return true;
+	} else if (cmd == V4L2_DEC_CMD_STOP) {
+		if (vb2_is_streaming(src_q))
+			if (inst->sub_state != IRIS_INST_SUB_DRAIN)
+				return true;
+	}
+
+	return false;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_state.h b/drivers/media/platform/qcom/iris/iris_state.h
new file mode 100644
index 000000000000..78c61aac5e7e
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_state.h
@@ -0,0 +1,144 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_STATE_H__
+#define __IRIS_STATE_H__
+
+struct iris_inst;
+
+/**
+ * enum iris_core_state
+ *
+ * @IRIS_CORE_DEINIT: default state.
+ * @IRIS_CORE_INIT:   core state with core initialized. FW loaded and
+ *                   HW brought out of reset, shared queues established
+ *                   between host driver and firmware.
+ * @IRIS_CORE_ERROR:  error state.
+ *
+ *        -----------
+ *             |
+ *             V
+ *        -----------
+ *   +--->| DEINIT  |<---+
+ *   |   -----------    |
+ *   |         |        |
+ *   |         v        |
+ *   |   -----------    |
+ *   |     /     \      |
+ *   |    /       \     |
+ *   |   /         \    |
+ *   |  v           v   v
+ * -----------    -----------
+ * |  INIT  |--->|  ERROR  |
+ * -----------    -----------
+ */
+enum iris_core_state {
+	IRIS_CORE_DEINIT,
+	IRIS_CORE_INIT,
+	IRIS_CORE_ERROR,
+};
+
+/**
+ * enum iris_inst_state
+ *
+ * @IRIS_INST_INIT: video instance is opened.
+ * @IRIS_INST_INPUT_STREAMING: stream on is completed on output plane.
+ * @IRIS_INST_OUTPUT_STREAMING: stream on is completed on capture plane.
+ * @IRIS_INST_STREAMING: stream on is completed on both output and capture planes.
+ * @IRIS_INST_DEINIT: video instance is closed.
+ * @IRIS_INST_ERROR: error state.
+ *                    |
+ *                    V
+ *             -------------
+ *   +--------|     INIT    |----------+
+ *   |         -------------           |
+ *   |            ^   ^                |
+ *   |           /      \              |
+ *   |          /        \             |
+ *   |         v          v            |
+ *   |   -----------    -----------    |
+ *   |   |   INPUT         OUTPUT  |   |
+ *   |---| STREAMING     STREAMING |---|
+ *   |   -----------    -----------    |
+ *   |       ^            ^            |
+ *   |         \          /            |
+ *   |          \        /             |
+ *   |           v      v              |
+ *   |         -------------           |
+ *   |--------|  STREAMING |-----------|
+ *   |        -------------            |
+ *   |               |                 |
+ *   |               |                 |
+ *   |               v                 |
+ *   |          -----------            |
+ *   +-------->|  DEINIT   |<----------+
+ *   |          -----------            |
+ *   |               |                 |
+ *   |               |                 |
+ *   |               v                 |
+ *   |          ----------             |
+ *   +-------->|   ERROR |<------------+
+ *              ----------
+ */
+enum iris_inst_state {
+	IRIS_INST_DEINIT,
+	IRIS_INST_INIT,
+	IRIS_INST_INPUT_STREAMING,
+	IRIS_INST_OUTPUT_STREAMING,
+	IRIS_INST_STREAMING,
+	IRIS_INST_ERROR,
+};
+
+#define IRIS_INST_SUB_STATES		8
+#define IRIS_INST_MAX_SUB_STATE_VALUE	((1 << IRIS_INST_SUB_STATES) - 1)
+
+/**
+ * enum iris_inst_sub_state
+ *
+ * @IRIS_INST_SUB_FIRST_IPSC: indicates source change is received from firmware
+ *			     when output port is not yet streaming.
+ * @IRIS_INST_SUB_DRC: indicates source change is received from firmware
+ *		      when output port is streaming and source change event is
+ *		      sent to client.
+ * @IRIS_INST_SUB_DRC_LAST: indicates last buffer is received from firmware
+ *                         as part of source change.
+ * @IRIS_INST_SUB_DRAIN: indicates drain is in progress.
+ * @IRIS_INST_SUB_DRAIN_LAST: indicates last buffer is received from firmware
+ *                           as part of drain sequence.
+ * @IRIS_INST_SUB_INPUT_PAUSE: source change is received form firmware. This
+ *                            indicates that firmware is paused to process
+ *                            any further input frames.
+ * @IRIS_INST_SUB_OUTPUT_PAUSE: last buffer is received form firmware as part
+ *                             of drc sequence. This indicates that
+ *                             firmware is paused to process any further output frames.
+ * @IRIS_INST_SUB_LOAD_RESOURCES: indicates all the resources have been loaded by the
+ *                               firmware and it is ready for processing.
+ */
+enum iris_inst_sub_state {
+	IRIS_INST_SUB_FIRST_IPSC	= BIT(0),
+	IRIS_INST_SUB_DRC		= BIT(1),
+	IRIS_INST_SUB_DRC_LAST		= BIT(2),
+	IRIS_INST_SUB_DRAIN		= BIT(3),
+	IRIS_INST_SUB_DRAIN_LAST	= BIT(4),
+	IRIS_INST_SUB_INPUT_PAUSE	= BIT(5),
+	IRIS_INST_SUB_OUTPUT_PAUSE	= BIT(6),
+	IRIS_INST_SUB_LOAD_RESOURCES	= BIT(7),
+};
+
+int iris_inst_change_state(struct iris_inst *inst,
+			   enum iris_inst_state request_state);
+int iris_inst_change_sub_state(struct iris_inst *inst,
+			       enum iris_inst_sub_state clear_sub_state,
+			       enum iris_inst_sub_state set_sub_state);
+
+int iris_inst_state_change_streamon(struct iris_inst *inst, u32 plane);
+int iris_inst_state_change_streamoff(struct iris_inst *inst, u32 plane);
+int iris_inst_sub_state_change_drc(struct iris_inst *inst);
+int iris_inst_sub_state_change_drain_last(struct iris_inst *inst);
+int iris_inst_sub_state_change_drc_last(struct iris_inst *inst);
+int iris_inst_sub_state_change_pause(struct iris_inst *inst, u32 plane);
+bool iris_allow_cmd(struct iris_inst *inst, u32 cmd);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_utils.c b/drivers/media/platform/qcom/iris/iris_utils.c
new file mode 100644
index 000000000000..83c70d6a2d90
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_utils.c
@@ -0,0 +1,90 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_runtime.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_instance.h"
+#include "iris_utils.h"
+
+bool iris_res_is_less_than(u32 width, u32 height,
+			   u32 ref_width, u32 ref_height)
+{
+	u32 num_mbs = NUM_MBS_PER_FRAME(height, width);
+	u32 max_side = max(ref_width, ref_height);
+
+	if (num_mbs < NUM_MBS_PER_FRAME(ref_height, ref_width) &&
+	    width < max_side &&
+	    height < max_side)
+		return true;
+
+	return false;
+}
+
+int iris_get_mbpf(struct iris_inst *inst)
+{
+	struct v4l2_format *inp_f = inst->fmt_src;
+	u32 height = max(inp_f->fmt.pix_mp.height, inst->crop.height);
+	u32 width = max(inp_f->fmt.pix_mp.width, inst->crop.width);
+
+	return NUM_MBS_PER_FRAME(height, width);
+}
+
+bool iris_split_mode_enabled(struct iris_inst *inst)
+{
+	return inst->fmt_dst->fmt.pix_mp.pixelformat == V4L2_PIX_FMT_NV12;
+}
+
+void iris_helper_buffers_done(struct iris_inst *inst, unsigned int type,
+			      enum vb2_buffer_state state)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct vb2_v4l2_buffer *buf;
+
+	if (V4L2_TYPE_IS_OUTPUT(type)) {
+		while ((buf = v4l2_m2m_src_buf_remove(m2m_ctx)))
+			v4l2_m2m_buf_done(buf, state);
+	} else if (V4L2_TYPE_IS_CAPTURE(type)) {
+		while ((buf = v4l2_m2m_dst_buf_remove(m2m_ctx)))
+			v4l2_m2m_buf_done(buf, state);
+	}
+}
+
+int iris_wait_for_session_response(struct iris_inst *inst, bool is_flush)
+{
+	struct iris_core *core = inst->core;
+	u32 hw_response_timeout_val;
+	struct completion *done;
+	int ret;
+
+	hw_response_timeout_val = core->iris_platform_data->hw_response_timeout;
+	done = is_flush ? &inst->flush_completion : &inst->completion;
+
+	mutex_unlock(&inst->lock);
+	ret = wait_for_completion_timeout(done, msecs_to_jiffies(hw_response_timeout_val));
+	mutex_lock(&inst->lock);
+	if (!ret) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+struct iris_inst *iris_get_instance(struct iris_core *core, u32 session_id)
+{
+	struct iris_inst *inst;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(inst, &core->instances, list) {
+		if (inst->session_id == session_id) {
+			mutex_unlock(&core->lock);
+			return inst;
+		}
+	}
+
+	mutex_unlock(&core->lock);
+	return NULL;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_utils.h b/drivers/media/platform/qcom/iris/iris_utils.h
new file mode 100644
index 000000000000..49869cf7a376
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_utils.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_UTILS_H__
+#define __IRIS_UTILS_H__
+
+struct iris_core;
+#include "iris_buffer.h"
+
+struct iris_hfi_rect_desc {
+	u32 left;
+	u32 top;
+	u32 width;
+	u32 height;
+};
+
+struct iris_hfi_frame_info {
+	u32 picture_type;
+	u32 no_output;
+	u32 data_corrupt;
+	u32 overflow;
+};
+
+struct iris_ts_metadata {
+	u64 ts_ns;
+	u64 ts_us;
+	u32 flags;
+	struct v4l2_timecode tc;
+};
+
+#define NUM_MBS_PER_FRAME(height, width) \
+	(DIV_ROUND_UP(height, 16) * DIV_ROUND_UP(width, 16))
+
+static inline enum iris_buffer_type iris_v4l2_type_to_driver(u32 type)
+{
+	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
+		return BUF_INPUT;
+	else
+		return BUF_OUTPUT;
+}
+
+bool iris_res_is_less_than(u32 width, u32 height,
+			   u32 ref_width, u32 ref_height);
+int iris_get_mbpf(struct iris_inst *inst);
+bool iris_split_mode_enabled(struct iris_inst *inst);
+struct iris_inst *iris_get_instance(struct iris_core *core, u32 session_id);
+void iris_helper_buffers_done(struct iris_inst *inst, unsigned int type,
+			      enum vb2_buffer_state state);
+int iris_wait_for_session_response(struct iris_inst *inst, bool is_flush);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vb2.c b/drivers/media/platform/qcom/iris/iris_vb2.c
new file mode 100644
index 000000000000..cdf11feb590b
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vb2.c
@@ -0,0 +1,335 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_instance.h"
+#include "iris_vb2.h"
+#include "iris_vdec.h"
+#include "iris_power.h"
+
+static int iris_check_core_mbpf(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *instance;
+	u32 total_mbpf = 0;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry(instance, &core->instances, list)
+		total_mbpf += iris_get_mbpf(instance);
+	mutex_unlock(&core->lock);
+
+	if (total_mbpf > core->iris_platform_data->max_core_mbpf)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int iris_check_inst_mbpf(struct iris_inst *inst)
+{
+	struct platform_inst_caps *caps;
+	u32 mbpf, max_mbpf;
+
+	caps = inst->core->iris_platform_data->inst_caps;
+	max_mbpf = caps->max_mbpf;
+	mbpf = iris_get_mbpf(inst);
+	if (mbpf > max_mbpf)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static int iris_check_resolution_supported(struct iris_inst *inst)
+{
+	u32 width, height, min_width, min_height, max_width, max_height;
+	struct platform_inst_caps *caps;
+
+	caps = inst->core->iris_platform_data->inst_caps;
+	width = inst->fmt_src->fmt.pix_mp.width;
+	height = inst->fmt_src->fmt.pix_mp.height;
+
+	min_width = caps->min_frame_width;
+	max_width = caps->max_frame_width;
+	min_height = caps->min_frame_height;
+	max_height = caps->max_frame_height;
+
+	if (!(min_width <= width && width <= max_width) ||
+	    !(min_height <= height && height <= max_height))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int iris_check_session_supported(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *instance = NULL;
+	bool found = false;
+	int ret;
+
+	list_for_each_entry(instance, &core->instances, list) {
+		if (instance == inst)
+			found = true;
+	}
+
+	if (!found) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ret = iris_check_core_mbpf(inst);
+	if (ret)
+		goto exit;
+
+	ret = iris_check_inst_mbpf(inst);
+	if (ret)
+		goto exit;
+
+	ret = iris_check_resolution_supported(inst);
+	if (ret)
+		goto exit;
+
+	return 0;
+exit:
+	dev_err(inst->core->dev, "current session not supported(%d)\n", ret);
+
+	return ret;
+}
+
+int iris_vb2_buf_init(struct vb2_buffer *vb2)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2);
+	struct iris_buffer *buf = to_iris_buffer(vbuf);
+
+	buf->device_addr = vb2_dma_contig_plane_dma_addr(vb2, 0);
+
+	return 0;
+}
+
+int iris_vb2_queue_setup(struct vb2_queue *q,
+			 unsigned int *num_buffers, unsigned int *num_planes,
+			 unsigned int sizes[], struct device *alloc_devs[])
+{
+	struct iris_inst *inst;
+	struct iris_core *core;
+	struct v4l2_format *f;
+	int ret = 0;
+
+	inst = vb2_get_drv_priv(q);
+
+	mutex_lock(&inst->lock);
+	if (inst->state == IRIS_INST_ERROR) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	core = inst->core;
+	f = V4L2_TYPE_IS_OUTPUT(q->type) ? inst->fmt_src : inst->fmt_dst;
+
+	if (*num_planes) {
+		if (*num_planes != f->fmt.pix_mp.num_planes ||
+		    sizes[0] < f->fmt.pix_mp.plane_fmt[0].sizeimage)
+			ret = -EINVAL;
+		goto unlock;
+	}
+
+	ret = iris_check_session_supported(inst);
+	if (ret)
+		goto unlock;
+
+	if (!inst->once_per_session_set) {
+		inst->once_per_session_set = true;
+
+		ret = core->hfi_ops->session_open(inst);
+		if (ret) {
+			ret = -EINVAL;
+			dev_err(core->dev, "session open failed\n");
+			goto unlock;
+		}
+
+		ret = iris_inst_change_state(inst, IRIS_INST_INIT);
+		if (ret)
+			goto unlock;
+	}
+
+	*num_planes = 1;
+	sizes[0] = f->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+unlock:
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+int iris_vb2_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	enum iris_buffer_type buf_type;
+	struct iris_inst *inst;
+	int ret = 0;
+
+	inst = vb2_get_drv_priv(q);
+
+	if (V4L2_TYPE_IS_CAPTURE(q->type) && inst->state == IRIS_INST_INIT)
+		return 0;
+
+	mutex_lock(&inst->lock);
+	if (inst->state == IRIS_INST_ERROR) {
+		ret = -EBUSY;
+		goto error;
+	}
+
+	if (!V4L2_TYPE_IS_OUTPUT(q->type) &&
+	    !V4L2_TYPE_IS_CAPTURE(q->type)) {
+		ret = -EINVAL;
+		goto error;
+	}
+
+	iris_scale_power(inst);
+
+	ret = iris_check_session_supported(inst);
+	if (ret)
+		goto error;
+
+	if (V4L2_TYPE_IS_OUTPUT(q->type))
+		ret = iris_vdec_streamon_input(inst);
+	else if (V4L2_TYPE_IS_CAPTURE(q->type))
+		ret = iris_vdec_streamon_output(inst);
+	if (ret)
+		goto error;
+
+	buf_type = iris_v4l2_type_to_driver(q->type);
+
+	ret = iris_queue_deferred_buffers(inst, buf_type);
+	if (ret)
+		goto error;
+
+	mutex_unlock(&inst->lock);
+
+	return ret;
+
+error:
+	iris_helper_buffers_done(inst, q->type, VB2_BUF_STATE_QUEUED);
+	iris_inst_change_state(inst, IRIS_INST_ERROR);
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+void iris_vb2_stop_streaming(struct vb2_queue *q)
+{
+	struct iris_inst *inst;
+	int ret = 0;
+
+	inst = vb2_get_drv_priv(q);
+
+	if (V4L2_TYPE_IS_CAPTURE(q->type) && inst->state == IRIS_INST_INIT)
+		return;
+
+	mutex_lock(&inst->lock);
+
+	if (!V4L2_TYPE_IS_OUTPUT(q->type) &&
+	    !V4L2_TYPE_IS_CAPTURE(q->type))
+		goto exit;
+
+	ret = iris_vdec_session_streamoff(inst, q->type);
+	if (ret)
+		goto exit;
+
+exit:
+	iris_helper_buffers_done(inst, q->type, VB2_BUF_STATE_ERROR);
+	if (ret)
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+
+	mutex_unlock(&inst->lock);
+}
+
+int iris_vb2_buf_prepare(struct vb2_buffer *vb)
+{
+	struct iris_inst *inst = vb2_get_drv_priv(vb->vb2_queue);
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE)
+			return -EINVAL;
+	}
+
+	if (vb->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    vb2_plane_size(vb, 0) < iris_get_buffer_size(inst, BUF_OUTPUT))
+		return -EINVAL;
+	if (vb->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
+	    vb2_plane_size(vb, 0) < iris_get_buffer_size(inst, BUF_INPUT))
+		return -EINVAL;
+
+	return 0;
+}
+
+int iris_vb2_buf_out_validate(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *v4l2_buf = to_vb2_v4l2_buffer(vb);
+
+	v4l2_buf->field = V4L2_FIELD_NONE;
+
+	return 0;
+}
+
+void iris_vb2_buf_queue(struct vb2_buffer *vb2)
+{
+	static const struct v4l2_event eos = { .type = V4L2_EVENT_EOS };
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2);
+	struct v4l2_m2m_ctx *m2m_ctx;
+	struct iris_inst *inst;
+	int ret = 0;
+
+	inst = vb2_get_drv_priv(vb2->vb2_queue);
+
+	mutex_lock(&inst->lock);
+	if (inst->state == IRIS_INST_ERROR) {
+		ret = -EBUSY;
+		goto exit;
+	}
+
+	if (vbuf->field == V4L2_FIELD_ANY)
+		vbuf->field = V4L2_FIELD_NONE;
+
+	m2m_ctx = inst->m2m_ctx;
+
+	if (!vb2->planes[0].bytesused && V4L2_TYPE_IS_OUTPUT(vb2->type)) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	if (V4L2_TYPE_IS_CAPTURE(vb2->vb2_queue->type)) {
+		if ((inst->sub_state & IRIS_INST_SUB_DRC &&
+		     inst->sub_state & IRIS_INST_SUB_DRC_LAST) ||
+		    (inst->sub_state & IRIS_INST_SUB_DRAIN &&
+		     inst->sub_state & IRIS_INST_SUB_DRAIN_LAST)) {
+			vbuf->flags |= V4L2_BUF_FLAG_LAST;
+			vbuf->sequence = inst->sequence_cap++;
+			vbuf->field = V4L2_FIELD_NONE;
+			vb2_set_plane_payload(vb2, 0, 0);
+			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
+			if (!v4l2_m2m_has_stopped(m2m_ctx)) {
+				v4l2_event_queue_fh(&inst->fh, &eos);
+				v4l2_m2m_mark_stopped(m2m_ctx);
+			}
+			goto exit;
+		}
+	}
+
+	v4l2_m2m_buf_queue(m2m_ctx, vbuf);
+
+	ret = iris_vdec_qbuf(inst, vbuf);
+
+exit:
+	if (ret) {
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+	}
+	mutex_unlock(&inst->lock);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_vb2.h b/drivers/media/platform/qcom/iris/iris_vb2.h
new file mode 100644
index 000000000000..a88565fdd3e4
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vb2.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VB2_H__
+#define __IRIS_VB2_H__
+
+int iris_vb2_buf_init(struct vb2_buffer *vb2);
+int iris_vb2_queue_setup(struct vb2_queue *q,
+			 unsigned int *num_buffers, unsigned int *num_planes,
+			 unsigned int sizes[], struct device *alloc_devs[]);
+int iris_vb2_start_streaming(struct vb2_queue *q, unsigned int count);
+void iris_vb2_stop_streaming(struct vb2_queue *q);
+int iris_vb2_buf_prepare(struct vb2_buffer *vb);
+int iris_vb2_buf_out_validate(struct vb2_buffer *vb);
+void iris_vb2_buf_queue(struct vb2_buffer *vb2);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vdec.c b/drivers/media/platform/qcom/iris/iris_vdec.c
new file mode 100644
index 000000000000..4143acedfc57
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vdec.c
@@ -0,0 +1,659 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <media/v4l2-event.h>
+#include <media/v4l2-mem2mem.h>
+
+#include "iris_buffer.h"
+#include "iris_ctrls.h"
+#include "iris_instance.h"
+#include "iris_power.h"
+#include "iris_vdec.h"
+#include "iris_vpu_buffer.h"
+
+#define DEFAULT_WIDTH 320
+#define DEFAULT_HEIGHT 240
+#define DEFAULT_CODEC_ALIGNMENT 16
+
+int iris_vdec_inst_init(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct v4l2_format *f;
+
+	inst->fmt_src  = kzalloc(sizeof(*inst->fmt_src), GFP_KERNEL);
+	inst->fmt_dst  = kzalloc(sizeof(*inst->fmt_dst), GFP_KERNEL);
+
+	inst->fw_min_count = MIN_BUFFERS;
+
+	f = inst->fmt_src;
+	f->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	f->fmt.pix_mp.width = DEFAULT_WIDTH;
+	f->fmt.pix_mp.height = DEFAULT_HEIGHT;
+	f->fmt.pix_mp.pixelformat = V4L2_PIX_FMT_H264;
+	f->fmt.pix_mp.num_planes = 1;
+	f->fmt.pix_mp.plane_fmt[0].bytesperline = 0;
+	f->fmt.pix_mp.plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_INPUT);
+	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	inst->buffers[BUF_INPUT].min_count = iris_vpu_buf_count(inst, BUF_INPUT);
+	inst->buffers[BUF_INPUT].size = f->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+	f = inst->fmt_dst;
+	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	f->fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12;
+	f->fmt.pix_mp.width = ALIGN(DEFAULT_WIDTH, 128);
+	f->fmt.pix_mp.height = ALIGN(DEFAULT_HEIGHT, 32);
+	f->fmt.pix_mp.num_planes = 1;
+	f->fmt.pix_mp.plane_fmt[0].bytesperline = ALIGN(DEFAULT_WIDTH, 128);
+	f->fmt.pix_mp.plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_OUTPUT);
+	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	f->fmt.pix_mp.colorspace = V4L2_COLORSPACE_DEFAULT;
+	f->fmt.pix_mp.xfer_func = V4L2_XFER_FUNC_DEFAULT;
+	f->fmt.pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+	f->fmt.pix_mp.quantization = V4L2_QUANTIZATION_DEFAULT;
+	inst->buffers[BUF_OUTPUT].min_count = iris_vpu_buf_count(inst, BUF_OUTPUT);
+	inst->buffers[BUF_OUTPUT].size = f->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+	memcpy(&inst->fw_caps[0], &core->inst_fw_caps[0],
+	       INST_FW_CAP_MAX * sizeof(struct platform_inst_fw_cap));
+
+	return iris_ctrls_init(inst);
+}
+
+void iris_vdec_inst_deinit(struct iris_inst *inst)
+{
+	kfree(inst->fmt_dst);
+	kfree(inst->fmt_src);
+}
+
+int iris_vdec_enum_fmt(struct iris_inst *inst, struct v4l2_fmtdesc *f)
+{
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		f->pixelformat = V4L2_PIX_FMT_H264;
+		f->flags = V4L2_FMT_FLAG_COMPRESSED | V4L2_FMT_FLAG_DYN_RESOLUTION;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		f->pixelformat = V4L2_PIX_FMT_NV12;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int iris_vdec_try_fmt(struct iris_inst *inst, struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *pixmp = &f->fmt.pix_mp;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_format *f_inst;
+	struct vb2_queue *src_q;
+
+	memset(pixmp->reserved, 0, sizeof(pixmp->reserved));
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (f->fmt.pix_mp.pixelformat != V4L2_PIX_FMT_H264) {
+			f_inst = inst->fmt_src;
+			f->fmt.pix_mp.width = f_inst->fmt.pix_mp.width;
+			f->fmt.pix_mp.height = f_inst->fmt.pix_mp.height;
+			f->fmt.pix_mp.pixelformat = f_inst->fmt.pix_mp.pixelformat;
+		}
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		if (f->fmt.pix_mp.pixelformat != V4L2_PIX_FMT_NV12) {
+			f_inst = inst->fmt_dst;
+			f->fmt.pix_mp.pixelformat = f_inst->fmt.pix_mp.pixelformat;
+			f->fmt.pix_mp.width = f_inst->fmt.pix_mp.width;
+			f->fmt.pix_mp.height = f_inst->fmt.pix_mp.height;
+		}
+
+		src_q = v4l2_m2m_get_src_vq(m2m_ctx);
+		if (vb2_is_streaming(src_q)) {
+			f_inst = inst->fmt_src;
+			f->fmt.pix_mp.height = f_inst->fmt.pix_mp.height;
+			f->fmt.pix_mp.width = f_inst->fmt.pix_mp.width;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (pixmp->field == V4L2_FIELD_ANY)
+		pixmp->field = V4L2_FIELD_NONE;
+
+	pixmp->num_planes = 1;
+
+	return 0;
+}
+
+int iris_vdec_s_fmt(struct iris_inst *inst, struct v4l2_format *f)
+{
+	struct v4l2_format *fmt, *output_fmt;
+	struct vb2_queue *q;
+	u32 codec_align;
+
+	q = v4l2_m2m_get_vq(inst->m2m_ctx, f->type);
+	if (!q)
+		return -EINVAL;
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	iris_vdec_try_fmt(inst, f);
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		if (f->fmt.pix_mp.pixelformat != V4L2_PIX_FMT_H264)
+			return -EINVAL;
+
+		fmt = inst->fmt_src;
+		fmt->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+
+		codec_align = DEFAULT_CODEC_ALIGNMENT;
+		fmt->fmt.pix_mp.width = ALIGN(f->fmt.pix_mp.width, codec_align);
+		fmt->fmt.pix_mp.height = ALIGN(f->fmt.pix_mp.height, codec_align);
+		fmt->fmt.pix_mp.num_planes = 1;
+		fmt->fmt.pix_mp.plane_fmt[0].bytesperline = 0;
+		fmt->fmt.pix_mp.plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_INPUT);
+		inst->buffers[BUF_INPUT].min_count = iris_vpu_buf_count(inst, BUF_INPUT);
+		inst->buffers[BUF_INPUT].size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+		fmt->fmt.pix_mp.colorspace = f->fmt.pix_mp.colorspace;
+		fmt->fmt.pix_mp.xfer_func = f->fmt.pix_mp.xfer_func;
+		fmt->fmt.pix_mp.ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+		fmt->fmt.pix_mp.quantization = f->fmt.pix_mp.quantization;
+
+		output_fmt = inst->fmt_dst;
+		output_fmt->fmt.pix_mp.colorspace = f->fmt.pix_mp.colorspace;
+		output_fmt->fmt.pix_mp.xfer_func = f->fmt.pix_mp.xfer_func;
+		output_fmt->fmt.pix_mp.ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+		output_fmt->fmt.pix_mp.quantization = f->fmt.pix_mp.quantization;
+
+		inst->crop.left = 0;
+		inst->crop.top = 0;
+		inst->crop.width = f->fmt.pix_mp.width;
+		inst->crop.height = f->fmt.pix_mp.height;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		fmt = inst->fmt_dst;
+		fmt->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+		if (fmt->fmt.pix_mp.pixelformat != V4L2_PIX_FMT_NV12)
+			return -EINVAL;
+		fmt->fmt.pix_mp.pixelformat = f->fmt.pix_mp.pixelformat;
+		fmt->fmt.pix_mp.width = ALIGN(f->fmt.pix_mp.width, 128);
+		fmt->fmt.pix_mp.height = ALIGN(f->fmt.pix_mp.height, 32);
+		fmt->fmt.pix_mp.num_planes = 1;
+		fmt->fmt.pix_mp.plane_fmt[0].bytesperline = ALIGN(f->fmt.pix_mp.width, 128);
+		fmt->fmt.pix_mp.plane_fmt[0].sizeimage = iris_get_buffer_size(inst, BUF_OUTPUT);
+		inst->buffers[BUF_OUTPUT].min_count = iris_vpu_buf_count(inst, BUF_OUTPUT);
+		inst->buffers[BUF_OUTPUT].size = fmt->fmt.pix_mp.plane_fmt[0].sizeimage;
+
+		inst->crop.top = 0;
+		inst->crop.left = 0;
+		inst->crop.width = f->fmt.pix_mp.width;
+		inst->crop.height = f->fmt.pix_mp.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+	memcpy(f, fmt, sizeof(*fmt));
+
+	return 0;
+}
+
+int iris_vdec_subscribe_event(struct iris_inst *inst, const struct v4l2_event_subscription *sub)
+{
+	int ret = 0;
+
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		ret = v4l2_event_subscribe(&inst->fh, sub, 0, NULL);
+		break;
+	case V4L2_EVENT_SOURCE_CHANGE:
+		ret = v4l2_src_change_event_subscribe(&inst->fh, sub);
+		break;
+	case V4L2_EVENT_CTRL:
+		ret = v4l2_ctrl_subscribe_event(&inst->fh, sub);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret;
+}
+
+void iris_vdec_src_change(struct iris_inst *inst)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_event event = {0};
+	struct vb2_queue *src_q;
+
+	src_q = v4l2_m2m_get_src_vq(m2m_ctx);
+	if (!vb2_is_streaming(src_q))
+		return;
+
+	event.type = V4L2_EVENT_SOURCE_CHANGE;
+	event.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION;
+	v4l2_event_queue_fh(&inst->fh, &event);
+}
+
+static int iris_vdec_get_num_queued_buffers(struct iris_inst *inst,
+					    enum iris_buffer_type type)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buffer, *n;
+	struct iris_buffer *buf;
+	u32 count = 0;
+
+	switch (type) {
+	case BUF_INPUT:
+		v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (!(buf->attr & BUF_ATTR_QUEUED))
+				continue;
+			count++;
+		}
+		return count;
+	case BUF_OUTPUT:
+		v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (!(buf->attr & BUF_ATTR_QUEUED))
+				continue;
+			count++;
+		}
+		return count;
+	default:
+		return count;
+	}
+}
+
+static void iris_vdec_flush_deferred_buffers(struct iris_inst *inst,
+					     enum iris_buffer_type type)
+{
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+	struct v4l2_m2m_buffer *buffer, *n;
+	struct iris_buffer *buf;
+
+	if (type == BUF_INPUT) {
+		v4l2_m2m_for_each_src_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (buf->attr & BUF_ATTR_DEFERRED) {
+				if (!(buf->attr & BUF_ATTR_BUFFER_DONE)) {
+					buf->attr |= BUF_ATTR_BUFFER_DONE;
+					buf->data_size = 0;
+					iris_vb2_buffer_done(inst, buf);
+				}
+			}
+		}
+	} else {
+		v4l2_m2m_for_each_dst_buf_safe(m2m_ctx, buffer, n) {
+			buf = to_iris_buffer(&buffer->vb);
+			if (buf->attr & BUF_ATTR_DEFERRED) {
+				if (!(buf->attr & BUF_ATTR_BUFFER_DONE)) {
+					buf->attr |= BUF_ATTR_BUFFER_DONE;
+					buf->data_size = 0;
+					iris_vb2_buffer_done(inst, buf);
+				}
+			}
+		}
+	}
+}
+
+static void iris_vdec_kill_session(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+
+	if (!inst->session_id)
+		return;
+
+	hfi_ops->session_close(inst);
+	iris_inst_change_state(inst, IRIS_INST_ERROR);
+}
+
+int iris_vdec_session_streamoff(struct iris_inst *inst, u32 plane)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	enum iris_buffer_type buffer_type;
+	u32 count;
+	int ret;
+
+	switch (plane) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		buffer_type = BUF_INPUT;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+		buffer_type = BUF_OUTPUT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = hfi_ops->session_stop(inst, plane);
+	if (ret)
+		goto error;
+
+	count = iris_vdec_get_num_queued_buffers(inst, buffer_type);
+	if (count) {
+		ret = -EINVAL;
+		goto error;
+	}
+
+	ret = iris_inst_state_change_streamoff(inst, plane);
+	if (ret)
+		goto error;
+
+	iris_vdec_flush_deferred_buffers(inst, buffer_type);
+
+	return 0;
+
+error:
+	iris_vdec_kill_session(inst);
+	iris_vdec_flush_deferred_buffers(inst, buffer_type);
+
+	return ret;
+}
+
+static int iris_vdec_process_streamon_input(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	enum iris_inst_sub_state set_sub_state = 0;
+	int ret;
+
+	iris_scale_power(inst);
+
+	ret = hfi_ops->session_start(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	if (inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE) {
+		ret = iris_inst_change_sub_state(inst, IRIS_INST_SUB_INPUT_PAUSE, 0);
+		if (ret)
+			return ret;
+	}
+
+	if (inst->sub_state & IRIS_INST_SUB_DRC ||
+	    inst->sub_state & IRIS_INST_SUB_DRAIN ||
+	    inst->sub_state & IRIS_INST_SUB_FIRST_IPSC) {
+		if (!(inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE)) {
+			if (hfi_ops->session_pause) {
+				ret = hfi_ops->session_pause(inst,
+							     V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+				if (ret)
+					return ret;
+			}
+			set_sub_state = IRIS_INST_SUB_INPUT_PAUSE;
+		}
+	}
+
+	ret = iris_inst_state_change_streamon(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	return iris_inst_change_sub_state(inst, 0, set_sub_state);
+}
+
+int iris_vdec_streamon_input(struct iris_inst *inst)
+{
+	int ret;
+
+	ret = iris_set_properties(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	ret = iris_alloc_and_queue_persist_bufs(inst);
+	if (ret)
+		return ret;
+
+	iris_get_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+
+	ret = iris_destroy_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	ret = iris_create_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	ret = iris_queue_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	return iris_vdec_process_streamon_input(inst);
+}
+
+static int iris_vdec_process_streamon_output(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	bool drain_active = false, drc_active = false;
+	enum iris_inst_sub_state clear_sub_state = 0;
+	int ret = 0;
+
+	iris_scale_power(inst);
+
+	drain_active = inst->sub_state & IRIS_INST_SUB_DRAIN &&
+		inst->sub_state & IRIS_INST_SUB_DRAIN_LAST;
+
+	drc_active = inst->sub_state & IRIS_INST_SUB_DRC &&
+		inst->sub_state & IRIS_INST_SUB_DRC_LAST;
+
+	if (drc_active)
+		clear_sub_state = IRIS_INST_SUB_DRC | IRIS_INST_SUB_DRC_LAST;
+	else if (drain_active)
+		clear_sub_state = IRIS_INST_SUB_DRAIN | IRIS_INST_SUB_DRAIN_LAST;
+
+	if (inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE) {
+		ret = iris_alloc_and_queue_input_int_bufs(inst);
+		if (ret)
+			return ret;
+		ret = iris_set_stage(inst, STAGE);
+		if (ret)
+			return ret;
+		ret = iris_set_pipe(inst, PIPE);
+		if (ret)
+			return ret;
+	}
+
+	if (inst->state == IRIS_INST_INPUT_STREAMING &&
+	    inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE) {
+		if (!drain_active)
+			ret = hfi_ops->session_resume_drc(inst,
+							  V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		else if (hfi_ops->session_resume_drain)
+			ret = hfi_ops->session_resume_drain(inst,
+							    V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+		if (ret)
+			return ret;
+		clear_sub_state |= IRIS_INST_SUB_INPUT_PAUSE;
+	}
+
+	if (inst->sub_state & IRIS_INST_SUB_FIRST_IPSC)
+		clear_sub_state |= IRIS_INST_SUB_FIRST_IPSC;
+
+	ret = hfi_ops->session_start(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		return ret;
+
+	if (inst->sub_state & IRIS_INST_SUB_OUTPUT_PAUSE)
+		clear_sub_state |= IRIS_INST_SUB_OUTPUT_PAUSE;
+
+	ret = iris_inst_state_change_streamon(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		return ret;
+
+	return iris_inst_change_sub_state(inst, clear_sub_state, 0);
+}
+
+int iris_vdec_streamon_output(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	int ret;
+
+	ret = hfi_ops->session_set_config_params(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		return ret;
+
+	iris_get_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	ret = iris_destroy_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		return ret;
+
+	ret = iris_create_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		return ret;
+
+	ret = iris_vdec_process_streamon_output(inst);
+	if (ret)
+		goto error;
+
+	ret = iris_queue_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	if (ret)
+		goto error;
+
+	return ret;
+
+error:
+	iris_vdec_session_streamoff(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+
+	return ret;
+}
+
+static int
+iris_vdec_vb2_buffer_to_driver(struct vb2_buffer *vb2, struct iris_buffer *buf)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2);
+
+	buf->type = iris_v4l2_type_to_driver(vb2->type);
+	buf->index = vb2->index;
+	buf->fd = vb2->planes[0].m.fd;
+	buf->buffer_size = vb2->planes[0].length;
+	buf->data_offset = vb2->planes[0].data_offset;
+	buf->data_size = vb2->planes[0].bytesused - vb2->planes[0].data_offset;
+	buf->flags = vbuf->flags;
+	buf->timestamp = vb2->timestamp;
+	buf->attr = 0;
+
+	return 0;
+}
+
+static void
+iris_set_ts_metadata(struct iris_inst *inst, struct vb2_v4l2_buffer *vbuf)
+{
+	u32 mask = V4L2_BUF_FLAG_TIMECODE | V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	struct vb2_buffer *vb = &vbuf->vb2_buf;
+	u64 ts_us = vb->timestamp;
+
+	if (inst->metadata_idx >= ARRAY_SIZE(inst->tss))
+		inst->metadata_idx = 0;
+
+	do_div(ts_us, NSEC_PER_USEC);
+
+	inst->tss[inst->metadata_idx].flags = vbuf->flags & mask;
+	inst->tss[inst->metadata_idx].tc = vbuf->timecode;
+	inst->tss[inst->metadata_idx].ts_us = ts_us;
+	inst->tss[inst->metadata_idx].ts_ns = vb->timestamp;
+
+	inst->metadata_idx++;
+}
+
+int iris_vdec_qbuf(struct iris_inst *inst, struct vb2_v4l2_buffer *vbuf)
+{
+	struct iris_buffer *buf = to_iris_buffer(vbuf);
+	struct vb2_buffer *vb2 = &vbuf->vb2_buf;
+	struct vb2_queue *q;
+	int ret;
+
+	ret = iris_vdec_vb2_buffer_to_driver(vb2, buf);
+	if (ret)
+		return ret;
+
+	if (buf->type == BUF_INPUT)
+		iris_set_ts_metadata(inst, vbuf);
+
+	q = v4l2_m2m_get_vq(inst->m2m_ctx, vb2->type);
+	if (!vb2_is_streaming(q)) {
+		buf->attr |= BUF_ATTR_DEFERRED;
+		return 0;
+	}
+
+	iris_scale_power(inst);
+
+	return iris_queue_buffer(inst, buf);
+}
+
+int iris_vdec_start_cmd(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	enum iris_inst_sub_state clear_sub_state = 0;
+	struct vb2_queue *dst_vq;
+	int ret;
+
+	dst_vq = v4l2_m2m_get_dst_vq(inst->m2m_ctx);
+
+	if (inst->sub_state & IRIS_INST_SUB_DRC &&
+	    inst->sub_state & IRIS_INST_SUB_DRC_LAST) {
+		vb2_clear_last_buffer_dequeued(dst_vq);
+		clear_sub_state = IRIS_INST_SUB_DRC | IRIS_INST_SUB_DRC_LAST;
+
+		if (inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE) {
+			ret = hfi_ops->session_resume_drc(inst,
+							  V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+			if (ret)
+				return ret;
+			clear_sub_state |= IRIS_INST_SUB_INPUT_PAUSE;
+		}
+		if (inst->sub_state & IRIS_INST_SUB_OUTPUT_PAUSE) {
+			ret = hfi_ops->session_resume_drc(inst,
+							  V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+			if (ret)
+				return ret;
+			clear_sub_state |= IRIS_INST_SUB_OUTPUT_PAUSE;
+		}
+	} else if (inst->sub_state & IRIS_INST_SUB_DRAIN &&
+		   inst->sub_state & IRIS_INST_SUB_DRAIN_LAST) {
+		vb2_clear_last_buffer_dequeued(dst_vq);
+		clear_sub_state = IRIS_INST_SUB_DRAIN | IRIS_INST_SUB_DRAIN_LAST;
+		if (inst->sub_state & IRIS_INST_SUB_INPUT_PAUSE) {
+			if (hfi_ops->session_resume_drain) {
+				ret =
+				hfi_ops->session_resume_drain(inst,
+							      V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+				if (ret)
+					return ret;
+			}
+
+			clear_sub_state |= IRIS_INST_SUB_INPUT_PAUSE;
+		}
+		if (inst->sub_state & IRIS_INST_SUB_OUTPUT_PAUSE) {
+			if (hfi_ops->session_resume_drain) {
+				ret =
+				hfi_ops->session_resume_drain(inst,
+							      V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+				if (ret)
+					return ret;
+			}
+
+			clear_sub_state |= IRIS_INST_SUB_OUTPUT_PAUSE;
+		}
+	} else {
+		dev_err(inst->core->dev, "start called before receiving last_flag\n");
+		iris_inst_change_state(inst, IRIS_INST_ERROR);
+		return -EBUSY;
+	}
+
+	return iris_inst_change_sub_state(inst, clear_sub_state, 0);
+}
+
+int iris_vdec_stop_cmd(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	int ret;
+
+	ret = hfi_ops->session_drain(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	if (ret)
+		return ret;
+
+	return iris_inst_change_sub_state(inst, 0, IRIS_INST_SUB_DRAIN);
+}
diff --git a/drivers/media/platform/qcom/iris/iris_vdec.h b/drivers/media/platform/qcom/iris/iris_vdec.h
new file mode 100644
index 000000000000..b24932dc511a
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vdec.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VDEC_H__
+#define __IRIS_VDEC_H__
+
+struct iris_inst;
+
+int iris_vdec_inst_init(struct iris_inst *inst);
+void iris_vdec_inst_deinit(struct iris_inst *inst);
+int iris_vdec_enum_fmt(struct iris_inst *inst, struct v4l2_fmtdesc *f);
+int iris_vdec_try_fmt(struct iris_inst *inst, struct v4l2_format *f);
+int iris_vdec_s_fmt(struct iris_inst *inst, struct v4l2_format *f);
+int iris_vdec_subscribe_event(struct iris_inst *inst, const struct v4l2_event_subscription *sub);
+void iris_vdec_src_change(struct iris_inst *inst);
+int iris_vdec_streamon_input(struct iris_inst *inst);
+int iris_vdec_streamon_output(struct iris_inst *inst);
+int iris_vdec_qbuf(struct iris_inst *inst, struct vb2_v4l2_buffer *vbuf);
+int iris_vdec_start_cmd(struct iris_inst *inst);
+int iris_vdec_stop_cmd(struct iris_inst *inst);
+int iris_vdec_session_streamoff(struct iris_inst *inst, u32 plane);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vidc.c b/drivers/media/platform/qcom/iris/iris_vidc.c
new file mode 100644
index 000000000000..ca0f4e310f77
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vidc.c
@@ -0,0 +1,453 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/pm_runtime.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "iris_vidc.h"
+#include "iris_instance.h"
+#include "iris_vdec.h"
+#include "iris_vb2.h"
+#include "iris_vpu_buffer.h"
+#include "iris_platform_common.h"
+
+#define IRIS_DRV_NAME "iris_driver"
+#define IRIS_BUS_NAME "platform:iris_icc"
+#define STEP_WIDTH 1
+#define STEP_HEIGHT 1
+
+static void iris_v4l2_fh_init(struct iris_inst *inst)
+{
+	v4l2_fh_init(&inst->fh, inst->core->vdev_dec);
+	inst->fh.ctrl_handler = &inst->ctrl_handler;
+	v4l2_fh_add(&inst->fh);
+}
+
+static void iris_v4l2_fh_deinit(struct iris_inst *inst)
+{
+	v4l2_fh_del(&inst->fh);
+	inst->fh.ctrl_handler = NULL;
+	v4l2_fh_exit(&inst->fh);
+}
+
+static void iris_add_session(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *iter;
+	u32 count = 0;
+
+	mutex_lock(&core->lock);
+
+	list_for_each_entry(iter, &core->instances, list)
+		count++;
+
+	if (count < core->iris_platform_data->max_session_count)
+		list_add_tail(&inst->list, &core->instances);
+
+	mutex_unlock(&core->lock);
+}
+
+static void iris_remove_session(struct iris_inst *inst)
+{
+	struct iris_core *core = inst->core;
+	struct iris_inst *iter, *temp;
+
+	mutex_lock(&core->lock);
+	list_for_each_entry_safe(iter, temp, &core->instances, list) {
+		if (iter->session_id == inst->session_id) {
+			list_del_init(&iter->list);
+			break;
+		}
+	}
+	mutex_unlock(&core->lock);
+}
+
+static inline struct iris_inst *iris_get_inst(struct file *filp, void *fh)
+{
+	return container_of(filp->private_data, struct iris_inst, fh);
+}
+
+static void iris_m2m_device_run(void *priv)
+{
+}
+
+static void iris_m2m_job_abort(void *priv)
+{
+	struct iris_inst *inst = priv;
+	struct v4l2_m2m_ctx *m2m_ctx = inst->m2m_ctx;
+
+	v4l2_m2m_job_finish(inst->m2m_dev, m2m_ctx);
+}
+
+static const struct v4l2_m2m_ops iris_m2m_ops = {
+	.device_run = iris_m2m_device_run,
+	.job_abort = iris_m2m_job_abort,
+};
+
+static int
+iris_m2m_queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
+{
+	struct iris_inst *inst = priv;
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	src_vq->ops = inst->core->iris_vb2_ops;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+	src_vq->drv_priv = inst;
+	src_vq->buf_struct_size = sizeof(struct iris_buffer);
+	src_vq->min_reqbufs_allocation = MIN_BUFFERS;
+	src_vq->dev = inst->core->dev;
+	src_vq->lock = &inst->ctx_q_lock;
+	ret = vb2_queue_init(src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
+	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	dst_vq->ops = inst->core->iris_vb2_ops;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+	dst_vq->drv_priv = inst;
+	dst_vq->buf_struct_size = sizeof(struct iris_buffer);
+	dst_vq->min_reqbufs_allocation = MIN_BUFFERS;
+	dst_vq->dev = inst->core->dev;
+	dst_vq->lock = &inst->ctx_q_lock;
+
+	return vb2_queue_init(dst_vq);
+}
+
+int iris_open(struct file *filp)
+{
+	struct iris_core *core = video_drvdata(filp);
+	struct iris_inst *inst;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(core->dev);
+	if (ret < 0)
+		return ret;
+
+	ret = iris_core_init(core);
+	if (ret) {
+		dev_err(core->dev, "core init failed\n");
+		pm_runtime_put_sync(core->dev);
+		return ret;
+	}
+
+	pm_runtime_put_sync(core->dev);
+
+	inst = core->iris_platform_data->get_instance();
+	if (!inst)
+		return -ENOMEM;
+
+	inst->core = core;
+	inst->session_id = hash32_ptr(inst);
+	inst->state = IRIS_INST_DEINIT;
+
+	mutex_init(&inst->lock);
+	mutex_init(&inst->ctx_q_lock);
+
+	INIT_LIST_HEAD(&inst->buffers[BUF_BIN].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_ARP].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_COMV].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_NON_COMV].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_LINE].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_DPB].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_PERSIST].list);
+	INIT_LIST_HEAD(&inst->buffers[BUF_SCRATCH_1].list);
+	init_completion(&inst->completion);
+	init_completion(&inst->flush_completion);
+
+	iris_v4l2_fh_init(inst);
+
+	inst->m2m_dev = v4l2_m2m_init(&iris_m2m_ops);
+	if (IS_ERR_OR_NULL(inst->m2m_dev)) {
+		ret = -EINVAL;
+		goto fail_v4l2_fh_deinit;
+	}
+
+	inst->m2m_ctx = v4l2_m2m_ctx_init(inst->m2m_dev, inst, iris_m2m_queue_init);
+	if (IS_ERR_OR_NULL(inst->m2m_ctx)) {
+		ret = -EINVAL;
+		goto fail_m2m_release;
+	}
+
+	ret = iris_vdec_inst_init(inst);
+	if (ret)
+		goto fail_m2m_ctx_release;
+
+	iris_add_session(inst);
+
+	inst->fh.m2m_ctx = inst->m2m_ctx;
+	filp->private_data = &inst->fh;
+
+	return 0;
+
+fail_m2m_ctx_release:
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+fail_m2m_release:
+	v4l2_m2m_release(inst->m2m_dev);
+fail_v4l2_fh_deinit:
+	iris_v4l2_fh_deinit(inst);
+	mutex_destroy(&inst->ctx_q_lock);
+	mutex_destroy(&inst->lock);
+	kfree(inst);
+
+	return ret;
+}
+
+static void iris_session_close(struct iris_inst *inst)
+{
+	const struct iris_hfi_command_ops *hfi_ops = inst->core->hfi_ops;
+	bool wait_for_response = true;
+	int ret;
+
+	if (inst->state == IRIS_INST_DEINIT)
+		return;
+
+	reinit_completion(&inst->completion);
+
+	ret = hfi_ops->session_close(inst);
+	if (ret)
+		wait_for_response = false;
+
+	if (wait_for_response)
+		iris_wait_for_session_response(inst, false);
+}
+
+int iris_close(struct file *filp)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+	v4l2_m2m_release(inst->m2m_dev);
+	mutex_lock(&inst->lock);
+	iris_vdec_inst_deinit(inst);
+	iris_session_close(inst);
+	iris_inst_change_state(inst, IRIS_INST_DEINIT);
+	iris_v4l2_fh_deinit(inst);
+	iris_destroy_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
+	iris_destroy_internal_buffers(inst, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	iris_remove_session(inst);
+	mutex_unlock(&inst->lock);
+	mutex_destroy(&inst->ctx_q_lock);
+	mutex_destroy(&inst->lock);
+	kfree(inst);
+	filp->private_data = NULL;
+
+	return 0;
+}
+
+static int iris_enum_fmt(struct file *filp, void *fh, struct v4l2_fmtdesc *f)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+
+	if (f->index)
+		return -EINVAL;
+
+	return iris_vdec_enum_fmt(inst, f);
+}
+
+static int iris_try_fmt_vid_mplane(struct file *filp, void *fh, struct v4l2_format *f)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+	int ret;
+
+	mutex_lock(&inst->lock);
+	ret = iris_vdec_try_fmt(inst, f);
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+static int iris_s_fmt_vid_mplane(struct file *filp, void *fh, struct v4l2_format *f)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+	int ret;
+
+	mutex_lock(&inst->lock);
+	ret = iris_vdec_s_fmt(inst, f);
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+static int iris_g_fmt_vid_mplane(struct file *filp, void *fh, struct v4l2_format *f)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+	int ret = 0;
+
+	mutex_lock(&inst->lock);
+	if (V4L2_TYPE_IS_OUTPUT(f->type))
+		*f = *inst->fmt_src;
+	else if (V4L2_TYPE_IS_CAPTURE(f->type))
+		*f = *inst->fmt_dst;
+	else
+		ret = -EINVAL;
+
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+static int iris_enum_framesizes(struct file *filp, void *fh,
+				struct v4l2_frmsizeenum *fsize)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+	struct platform_inst_caps *caps;
+
+	if (fsize->index)
+		return -EINVAL;
+
+	if (fsize->pixel_format != V4L2_PIX_FMT_H264 &&
+	    fsize->pixel_format != V4L2_PIX_FMT_NV12)
+		return -EINVAL;
+
+	caps = inst->core->iris_platform_data->inst_caps;
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = caps->min_frame_width;
+	fsize->stepwise.max_width = caps->max_frame_width;
+	fsize->stepwise.step_width = STEP_WIDTH;
+	fsize->stepwise.min_height = caps->min_frame_height;
+	fsize->stepwise.max_height = caps->max_frame_height;
+	fsize->stepwise.step_height = STEP_HEIGHT;
+
+	return 0;
+}
+
+static int iris_querycap(struct file *filp, void *fh, struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, IRIS_DRV_NAME, sizeof(cap->driver));
+	strscpy(cap->card, "Iris Decoder", sizeof(cap->card));
+
+	return 0;
+}
+
+static int iris_g_selection(struct file *filp, void *fh, struct v4l2_selection *s)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE:
+		s->r.left = inst->crop.left;
+		s->r.top = inst->crop.top;
+		s->r.width = inst->crop.width;
+		s->r.height = inst->crop.height;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int iris_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subscription *sub)
+{
+	struct iris_inst *inst = container_of(fh, struct iris_inst, fh);
+
+	return iris_vdec_subscribe_event(inst, sub);
+}
+
+static int iris_dec_cmd(struct file *filp, void *fh,
+			struct v4l2_decoder_cmd *dec)
+{
+	struct iris_inst *inst = iris_get_inst(filp, NULL);
+	int ret = 0;
+
+	mutex_lock(&inst->lock);
+
+	ret = v4l2_m2m_ioctl_decoder_cmd(filp, fh, dec);
+	if (ret)
+		goto unlock;
+
+	if (inst->state == IRIS_INST_DEINIT)
+		goto unlock;
+
+	if (!iris_allow_cmd(inst, dec->cmd)) {
+		ret = -EBUSY;
+		goto unlock;
+	}
+
+	if (dec->cmd == V4L2_DEC_CMD_START)
+		ret = iris_vdec_start_cmd(inst);
+	else if (dec->cmd == V4L2_DEC_CMD_STOP)
+		ret = iris_vdec_stop_cmd(inst);
+	else
+		ret = -EINVAL;
+
+unlock:
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
+static struct v4l2_file_operations iris_v4l2_file_ops = {
+	.owner                          = THIS_MODULE,
+	.open                           = iris_open,
+	.release                        = iris_close,
+	.unlocked_ioctl                 = video_ioctl2,
+	.poll                           = v4l2_m2m_fop_poll,
+	.mmap                           = v4l2_m2m_fop_mmap,
+};
+
+static const struct vb2_ops iris_vb2_ops = {
+	.buf_init                       = iris_vb2_buf_init,
+	.queue_setup                    = iris_vb2_queue_setup,
+	.start_streaming                = iris_vb2_start_streaming,
+	.stop_streaming                 = iris_vb2_stop_streaming,
+	.buf_prepare                    = iris_vb2_buf_prepare,
+	.buf_out_validate               = iris_vb2_buf_out_validate,
+	.buf_queue                      = iris_vb2_buf_queue,
+};
+
+static const struct v4l2_ioctl_ops iris_v4l2_ioctl_ops = {
+	.vidioc_enum_fmt_vid_cap        = iris_enum_fmt,
+	.vidioc_enum_fmt_vid_out        = iris_enum_fmt,
+	.vidioc_try_fmt_vid_cap_mplane  = iris_try_fmt_vid_mplane,
+	.vidioc_try_fmt_vid_out_mplane  = iris_try_fmt_vid_mplane,
+	.vidioc_s_fmt_vid_cap_mplane    = iris_s_fmt_vid_mplane,
+	.vidioc_s_fmt_vid_out_mplane    = iris_s_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_cap_mplane    = iris_g_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_out_mplane    = iris_g_fmt_vid_mplane,
+	.vidioc_enum_framesizes         = iris_enum_framesizes,
+	.vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
+	.vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
+	.vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
+	.vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
+	.vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
+	.vidioc_remove_bufs             = v4l2_m2m_ioctl_remove_bufs,
+	.vidioc_querycap                = iris_querycap,
+	.vidioc_g_selection             = iris_g_selection,
+	.vidioc_subscribe_event         = iris_subscribe_event,
+	.vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
+	.vidioc_streamon                = v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
+	.vidioc_try_decoder_cmd         = v4l2_m2m_ioctl_try_decoder_cmd,
+	.vidioc_decoder_cmd             = iris_dec_cmd,
+};
+
+void iris_init_ops(struct iris_core *core)
+{
+	core->iris_v4l2_file_ops = &iris_v4l2_file_ops;
+	core->iris_vb2_ops = &iris_vb2_ops;
+	core->iris_v4l2_ioctl_ops = &iris_v4l2_ioctl_ops;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_vidc.h b/drivers/media/platform/qcom/iris/iris_vidc.h
new file mode 100644
index 000000000000..a26054ff55b5
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vidc.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VIDC_H__
+#define __IRIS_VIDC_H__
+
+struct iris_core;
+
+void iris_init_ops(struct iris_core *core);
+int iris_open(struct file *filp);
+int iris_close(struct file *filp);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vpu2.c b/drivers/media/platform/qcom/iris/iris_vpu2.c
new file mode 100644
index 000000000000..7cf1bfc352d3
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu2.c
@@ -0,0 +1,39 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_instance.h"
+#include "iris_vpu_common.h"
+
+static u64 iris_vpu2_calc_freq(struct iris_inst *inst, size_t data_size)
+{
+	struct platform_inst_caps *caps = inst->core->iris_platform_data->inst_caps;
+	struct v4l2_format *inp_f = inst->fmt_src;
+	u32 mbs_per_second, mbpf, height, width;
+	unsigned long vpp_freq, vsp_freq;
+	u32 fps = DEFAULT_FPS;
+
+	width = max(inp_f->fmt.pix_mp.width, inst->crop.width);
+	height = max(inp_f->fmt.pix_mp.height, inst->crop.height);
+
+	mbpf = NUM_MBS_PER_FRAME(height, width);
+	mbs_per_second = mbpf * fps;
+
+	vpp_freq = mbs_per_second * caps->mb_cycles_vpp;
+
+	/* 21 / 20 is overhead factor */
+	vpp_freq += vpp_freq / 20;
+	vsp_freq = mbs_per_second * caps->mb_cycles_vsp;
+
+	/* 10 / 7 is overhead factor */
+	vsp_freq += ((fps * data_size * 8) * 10) / 7;
+
+	return max(vpp_freq, vsp_freq);
+}
+
+const struct vpu_ops iris_vpu2_ops = {
+	.power_off_hw = iris_vpu_power_off_hw,
+	.power_off_controller = iris_vpu_power_off_controller,
+	.calc_freq = iris_vpu2_calc_freq,
+};
diff --git a/drivers/media/platform/qcom/iris/iris_vpu3x.c b/drivers/media/platform/qcom/iris/iris_vpu3x.c
new file mode 100644
index 000000000000..9b7c9a1495ee
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu3x.c
@@ -0,0 +1,275 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/iopoll.h>
+#include <linux/reset.h>
+
+#include "iris_instance.h"
+#include "iris_vpu_common.h"
+#include "iris_vpu_register_defines.h"
+
+#define WRAPPER_TZ_BASE_OFFS			0x000C0000
+#define AON_BASE_OFFS				0x000E0000
+#define AON_MVP_NOC_RESET			0x0001F000
+
+#define WRAPPER_DEBUG_BRIDGE_LPI_CONTROL	(WRAPPER_BASE_OFFS + 0x54)
+#define WRAPPER_DEBUG_BRIDGE_LPI_STATUS		(WRAPPER_BASE_OFFS + 0x58)
+#define WRAPPER_IRIS_CPU_NOC_LPI_CONTROL	(WRAPPER_BASE_OFFS + 0x5C)
+#define REQ_POWER_DOWN_PREP			BIT(0)
+#define WRAPPER_IRIS_CPU_NOC_LPI_STATUS		(WRAPPER_BASE_OFFS + 0x60)
+#define WRAPPER_CORE_CLOCK_CONFIG		(WRAPPER_BASE_OFFS + 0x88)
+#define CORE_CLK_RUN				0x0
+
+#define WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG		(WRAPPER_TZ_BASE_OFFS + 0x14)
+#define CTL_AXI_CLK_HALT			BIT(0)
+#define CTL_CLK_HALT				BIT(1)
+
+#define WRAPPER_TZ_QNS4PDXFIFO_RESET		(WRAPPER_TZ_BASE_OFFS + 0x18)
+#define RESET_HIGH				BIT(0)
+
+#define CPU_CS_AHB_BRIDGE_SYNC_RESET		(CPU_CS_BASE_OFFS + 0x160)
+#define CORE_BRIDGE_SW_RESET			BIT(0)
+#define CORE_BRIDGE_HW_RESET_DISABLE		BIT(1)
+
+#define CPU_CS_X2RPMH				(CPU_CS_BASE_OFFS + 0x168)
+#define MSK_SIGNAL_FROM_TENSILICA		BIT(0)
+#define MSK_CORE_POWER_ON			BIT(1)
+
+#define AON_WRAPPER_MVP_NOC_RESET_REQ		(AON_MVP_NOC_RESET + 0x000)
+#define VIDEO_NOC_RESET_REQ			(BIT(0) | BIT(1))
+
+#define AON_WRAPPER_MVP_NOC_RESET_ACK		(AON_MVP_NOC_RESET + 0x004)
+
+#define VCODEC_SS_IDLE_STATUSN			(VCODEC_BASE_OFFS + 0x70)
+
+#define AON_WRAPPER_MVP_NOC_LPI_CONTROL		(AON_BASE_OFFS)
+#define AON_WRAPPER_MVP_NOC_LPI_STATUS		(AON_BASE_OFFS + 0x4)
+
+#define AON_WRAPPER_MVP_NOC_CORE_SW_RESET	(AON_BASE_OFFS + 0x18)
+#define SW_RESET				BIT(0)
+#define AON_WRAPPER_MVP_NOC_CORE_CLK_CONTROL	(AON_BASE_OFFS + 0x20)
+#define NOC_HALT				BIT(0)
+#define AON_WRAPPER_SPARE			(AON_BASE_OFFS + 0x28)
+
+static bool iris_vpu3x_hw_power_collapsed(struct iris_core *core)
+{
+	u32 value, pwr_status;
+
+	value = readl(core->reg_base + WRAPPER_CORE_POWER_STATUS);
+	pwr_status = value & BIT(1);
+
+	return pwr_status ? false : true;
+}
+
+static void iris_vpu3_power_off_hardware(struct iris_core *core)
+{
+	u32 reg_val = 0, value, i;
+	int ret;
+
+	if (iris_vpu3x_hw_power_collapsed(core))
+		goto disable_power;
+
+	dev_err(core->dev, "video hw is power on\n");
+
+	value = readl(core->reg_base + WRAPPER_CORE_CLOCK_CONFIG);
+	if (value)
+		writel(CORE_CLK_RUN, core->reg_base + WRAPPER_CORE_CLOCK_CONFIG);
+
+	for (i = 0; i < core->iris_platform_data->num_vpp_pipe; i++) {
+		ret = readl_poll_timeout(core->reg_base + VCODEC_SS_IDLE_STATUSN + 4 * i,
+					 reg_val, reg_val & 0x400000, 2000, 20000);
+		if (ret)
+			goto disable_power;
+	}
+
+	writel(VIDEO_NOC_RESET_REQ, core->reg_base + AON_WRAPPER_MVP_NOC_RESET_REQ);
+
+	ret = readl_poll_timeout(core->reg_base + AON_WRAPPER_MVP_NOC_RESET_ACK,
+				 reg_val, reg_val & 0x3, 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(0x0, core->reg_base + AON_WRAPPER_MVP_NOC_RESET_REQ);
+
+	ret = readl_poll_timeout(core->reg_base + AON_WRAPPER_MVP_NOC_RESET_ACK,
+				 reg_val, !(reg_val & 0x3), 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(CORE_BRIDGE_SW_RESET | CORE_BRIDGE_HW_RESET_DISABLE,
+	       core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+	writel(CORE_BRIDGE_HW_RESET_DISABLE, core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+	writel(0x0, core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+
+disable_power:
+	iris_vpu_power_off_hw(core);
+}
+
+static void iris_vpu33_power_off_hardware(struct iris_core *core)
+{
+	u32 reg_val = 0, value, i;
+	int ret;
+
+	if (iris_vpu3x_hw_power_collapsed(core))
+		goto disable_power;
+
+	dev_err(core->dev, "video hw is power on\n");
+
+	value = readl(core->reg_base + WRAPPER_CORE_CLOCK_CONFIG);
+	if (value)
+		writel(CORE_CLK_RUN, core->reg_base + WRAPPER_CORE_CLOCK_CONFIG);
+
+	for (i = 0; i < core->iris_platform_data->num_vpp_pipe; i++) {
+		ret = readl_poll_timeout(core->reg_base + VCODEC_SS_IDLE_STATUSN + 4 * i,
+					 reg_val, reg_val & 0x400000, 2000, 20000);
+		if (ret)
+			goto disable_power;
+	}
+
+	ret = readl_poll_timeout(core->reg_base + AON_WRAPPER_MVP_NOC_LPI_STATUS,
+				 reg_val, reg_val & BIT(0), 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	/* set MNoC to low power, set PD_NOC_QREQ (bit 0) */
+	writel(BIT(0), core->reg_base + AON_WRAPPER_MVP_NOC_LPI_CONTROL);
+
+	writel(CORE_BRIDGE_SW_RESET | CORE_BRIDGE_HW_RESET_DISABLE,
+	       core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+	writel(CORE_BRIDGE_HW_RESET_DISABLE, core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+	writel(0x0, core->reg_base + CPU_CS_AHB_BRIDGE_SYNC_RESET);
+
+disable_power:
+	iris_vpu_power_off_hw(core);
+}
+
+static int iris_vpu33_power_off_controller(struct iris_core *core)
+{
+	u32 xo_rst_tbl_size = core->iris_platform_data->controller_rst_tbl_size;
+	u32 clk_rst_tbl_size = core->iris_platform_data->clk_rst_tbl_size;
+	u32 val = 0;
+	int ret;
+
+	writel(MSK_SIGNAL_FROM_TENSILICA | MSK_CORE_POWER_ON, core->reg_base + CPU_CS_X2RPMH);
+
+	writel(REQ_POWER_DOWN_PREP, core->reg_base + WRAPPER_IRIS_CPU_NOC_LPI_CONTROL);
+
+	ret = readl_poll_timeout(core->reg_base + WRAPPER_IRIS_CPU_NOC_LPI_STATUS,
+				 val, val & BIT(0), 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(0x0, core->reg_base + WRAPPER_DEBUG_BRIDGE_LPI_CONTROL);
+
+	ret = readl_poll_timeout(core->reg_base + WRAPPER_DEBUG_BRIDGE_LPI_STATUS,
+				 val, val == 0, 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(CTL_AXI_CLK_HALT | CTL_CLK_HALT,
+	       core->reg_base + WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG);
+	writel(RESET_HIGH, core->reg_base + WRAPPER_TZ_QNS4PDXFIFO_RESET);
+	writel(0x0, core->reg_base + WRAPPER_TZ_QNS4PDXFIFO_RESET);
+	writel(0x0, core->reg_base + WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG);
+
+	reset_control_bulk_reset(clk_rst_tbl_size, core->resets);
+
+	/* Disable MVP NoC clock */
+	val = readl(core->reg_base + AON_WRAPPER_MVP_NOC_CORE_CLK_CONTROL);
+	val |= NOC_HALT;
+	writel(val, core->reg_base + AON_WRAPPER_MVP_NOC_CORE_CLK_CONTROL);
+
+	/* enable MVP NoC reset */
+	val = readl(core->reg_base + AON_WRAPPER_MVP_NOC_CORE_SW_RESET);
+	val |= SW_RESET;
+	writel(val, core->reg_base + AON_WRAPPER_MVP_NOC_CORE_SW_RESET);
+
+	/* poll AON spare register bit0 to become zero with 50ms timeout */
+	ret = readl_poll_timeout(core->reg_base + AON_WRAPPER_SPARE,
+				 val, (val & BIT(0)) == 0, 1000, 50000);
+	if (ret)
+		goto disable_power;
+
+	/* enable bit(1) to avoid cvp noc xo reset */
+	val = readl(core->reg_base + AON_WRAPPER_SPARE);
+	val |= BIT(1);
+	writel(val, core->reg_base + AON_WRAPPER_SPARE);
+
+	reset_control_bulk_assert(xo_rst_tbl_size, core->controller_resets);
+
+	/* De-assert MVP NoC reset */
+	val = readl(core->reg_base + AON_WRAPPER_MVP_NOC_CORE_SW_RESET);
+	val &= ~SW_RESET;
+	writel(val, core->reg_base + AON_WRAPPER_MVP_NOC_CORE_SW_RESET);
+
+	usleep_range(80, 100);
+
+	reset_control_bulk_deassert(xo_rst_tbl_size, core->controller_resets);
+
+	/* reset AON spare register */
+	writel(0, core->reg_base + AON_WRAPPER_SPARE);
+
+	/* Enable MVP NoC clock */
+	val = readl(core->reg_base + AON_WRAPPER_MVP_NOC_CORE_CLK_CONTROL);
+	val &= ~NOC_HALT;
+	writel(val, core->reg_base + AON_WRAPPER_MVP_NOC_CORE_CLK_CONTROL);
+
+	iris_disable_unprepare_clock(core, IRIS_CTRL_CLK);
+
+disable_power:
+	iris_disable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_CTRL_POWER_DOMAIN]);
+	iris_disable_unprepare_clock(core, IRIS_AXI_CLK);
+
+	return 0;
+}
+
+static u64 iris_vpu3x_calculate_frequency(struct iris_inst *inst, size_t data_size)
+{
+	struct platform_inst_caps *caps = inst->core->iris_platform_data->inst_caps;
+	struct v4l2_format *inp_f = inst->fmt_src;
+	u32 height, width, mbs_per_second, mbpf;
+	u64 fw_cycles, fw_vpp_cycles;
+	u64 vsp_cycles, vpp_cycles;
+	u32 fps = DEFAULT_FPS;
+
+	width = max(inp_f->fmt.pix_mp.width, inst->crop.width);
+	height = max(inp_f->fmt.pix_mp.height, inst->crop.height);
+
+	mbpf = NUM_MBS_PER_FRAME(height, width);
+	mbs_per_second = mbpf * fps;
+
+	fw_cycles = fps * caps->mb_cycles_fw;
+	fw_vpp_cycles = fps * caps->mb_cycles_fw_vpp;
+
+	vpp_cycles = mult_frac(mbs_per_second, caps->mb_cycles_vpp, (u32)inst->fw_caps[PIPE].value);
+	/* 21 / 20 is minimum overhead factor */
+	vpp_cycles += max(div_u64(vpp_cycles, 20), fw_vpp_cycles);
+
+	/* 1.059 is multi-pipe overhead */
+	if (inst->fw_caps[PIPE].value > 1)
+		vpp_cycles += div_u64(vpp_cycles * 59, 1000);
+
+	vsp_cycles = fps * data_size * 8;
+	vsp_cycles = div_u64(vsp_cycles, 2);
+	/* VSP FW overhead 1.05 */
+	vsp_cycles = div_u64(vsp_cycles * 21, 20);
+
+	if (inst->fw_caps[STAGE].value == STAGE_1)
+		vsp_cycles = vsp_cycles * 3;
+
+	return max3(vpp_cycles, vsp_cycles, fw_cycles);
+}
+
+const struct vpu_ops iris_vpu3_ops = {
+	.power_off_hw = iris_vpu3_power_off_hardware,
+	.power_off_controller = iris_vpu_power_off_controller,
+	.calc_freq = iris_vpu3x_calculate_frequency,
+};
+
+const struct vpu_ops iris_vpu33_ops = {
+	.power_off_hw = iris_vpu33_power_off_hardware,
+	.power_off_controller = iris_vpu33_power_off_controller,
+	.calc_freq = iris_vpu3x_calculate_frequency,
+};
diff --git a/drivers/media/platform/qcom/iris/iris_vpu_buffer.c b/drivers/media/platform/qcom/iris/iris_vpu_buffer.c
new file mode 100644
index 000000000000..dce25e410d80
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu_buffer.c
@@ -0,0 +1,270 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "iris_instance.h"
+#include "iris_vpu_buffer.h"
+
+static u32 size_h264d_hw_bin_buffer(u32 frame_width, u32 frame_height, u32 num_vpp_pipes)
+{
+	u32 size_yuv, size_bin_hdr, size_bin_res;
+
+	size_yuv = ((frame_width * frame_height) <= BIN_BUFFER_THRESHOLD) ?
+			((BIN_BUFFER_THRESHOLD * 3) >> 1) :
+			((frame_width * frame_height * 3) >> 1);
+	size_bin_hdr = size_yuv * H264_CABAC_HDR_RATIO_HD_TOT;
+	size_bin_res = size_yuv * H264_CABAC_RES_RATIO_HD_TOT;
+	size_bin_hdr = ALIGN(size_bin_hdr / num_vpp_pipes,
+			     DMA_ALIGNMENT) * num_vpp_pipes;
+	size_bin_res = ALIGN(size_bin_res / num_vpp_pipes,
+			     DMA_ALIGNMENT) * num_vpp_pipes;
+
+	return size_bin_hdr + size_bin_res;
+}
+
+static u32 hfi_buffer_bin_h264d(u32 frame_width, u32 frame_height, u32 num_vpp_pipes)
+{
+	u32 n_aligned_h = ALIGN(frame_height, 16);
+	u32 n_aligned_w = ALIGN(frame_width, 16);
+
+	return size_h264d_hw_bin_buffer(n_aligned_w, n_aligned_h, num_vpp_pipes);
+}
+
+static u32 hfi_buffer_comv_h264d(u32 frame_width, u32 frame_height, u32 _comv_bufcount)
+{
+	u32 frame_height_in_mbs = DIV_ROUND_UP(frame_height, 16);
+	u32 frame_width_in_mbs = DIV_ROUND_UP(frame_width, 16);
+	u32 col_zero_aligned_width = (frame_width_in_mbs << 2);
+	u32 col_mv_aligned_width = (frame_width_in_mbs << 7);
+	u32 col_zero_size, size_colloc;
+
+	col_mv_aligned_width = ALIGN(col_mv_aligned_width, 16);
+	col_zero_aligned_width = ALIGN(col_zero_aligned_width, 16);
+	col_zero_size = col_zero_aligned_width *
+			((frame_height_in_mbs + 1) >> 1);
+	col_zero_size = ALIGN(col_zero_size, 64);
+	col_zero_size <<= 1;
+	col_zero_size = ALIGN(col_zero_size, 512);
+	size_colloc = col_mv_aligned_width * ((frame_height_in_mbs + 1) >> 1);
+	size_colloc = ALIGN(size_colloc, 64);
+	size_colloc <<= 1;
+	size_colloc = ALIGN(size_colloc, 512);
+	size_colloc += (col_zero_size + SIZE_H264D_BUFTAB_T * 2);
+
+	return (size_colloc * (_comv_bufcount)) + 512;
+}
+
+static u32 size_h264d_bse_cmd_buf(u32 frame_height)
+{
+	u32 height = ALIGN(frame_height, 32);
+
+	return min_t(u32, (DIV_ROUND_UP(height, 16) * 48), H264D_MAX_SLICE) *
+		SIZE_H264D_BSE_CMD_PER_BUF;
+}
+
+static u32 size_h264d_vpp_cmd_buf(u32 frame_height)
+{
+	u32 size, height = ALIGN(frame_height, 32);
+
+	size = min_t(u32, (DIV_ROUND_UP(height, 16) * 48), H264D_MAX_SLICE) *
+			SIZE_H264D_VPP_CMD_PER_BUF;
+
+	return size > VPP_CMD_MAX_SIZE ? VPP_CMD_MAX_SIZE : size;
+}
+
+static u32 hfi_buffer_persist_h264d(void)
+{
+	return ALIGN(SIZE_SLIST_BUF_H264 * NUM_SLIST_BUF_H264 +
+		    H264_DISPLAY_BUF_SIZE * H264_NUM_FRM_INFO +
+		    NUM_HW_PIC_BUF * SIZE_SEI_USERDATA,
+		    DMA_ALIGNMENT);
+}
+
+static u32 hfi_buffer_non_comv_h264d(u32 frame_width, u32 frame_height, u32 num_vpp_pipes)
+{
+	u32 size_bse, size_vpp, size;
+
+	size_bse = size_h264d_bse_cmd_buf(frame_height);
+	size_vpp = size_h264d_vpp_cmd_buf(frame_height);
+	size = ALIGN(size_bse, DMA_ALIGNMENT) +
+		ALIGN(size_vpp, DMA_ALIGNMENT) +
+		ALIGN(SIZE_HW_PIC(SIZE_H264D_HW_PIC_T), DMA_ALIGNMENT);
+
+	return ALIGN(size, DMA_ALIGNMENT);
+}
+
+static u32 size_vpss_lb(u32 frame_width, u32 frame_height)
+{
+	u32 opb_lb_wr_llb_y_buffer_size, opb_lb_wr_llb_uv_buffer_size;
+	u32 opb_wr_top_line_chroma_buffer_size;
+	u32 opb_wr_top_line_luma_buffer_size;
+	u32 macrotiling_size = 32;
+
+	opb_wr_top_line_luma_buffer_size =
+		ALIGN(frame_width, macrotiling_size) / macrotiling_size * 256;
+	opb_wr_top_line_luma_buffer_size =
+		ALIGN(opb_wr_top_line_luma_buffer_size, DMA_ALIGNMENT) +
+		(MAX_TILE_COLUMNS - 1) * 256;
+	opb_wr_top_line_luma_buffer_size =
+		max_t(u32, opb_wr_top_line_luma_buffer_size, (32 * ALIGN(frame_height, 8)));
+	opb_wr_top_line_chroma_buffer_size = opb_wr_top_line_luma_buffer_size;
+	opb_lb_wr_llb_uv_buffer_size =
+		ALIGN((ALIGN(frame_height, 8) / (4 / 2)) * 64, 32);
+	opb_lb_wr_llb_y_buffer_size =
+		ALIGN((ALIGN(frame_height, 8) / (4 / 2)) * 64, 32);
+	return opb_wr_top_line_luma_buffer_size +
+		opb_wr_top_line_chroma_buffer_size +
+		opb_lb_wr_llb_uv_buffer_size +
+		opb_lb_wr_llb_y_buffer_size;
+}
+
+static u32 hfi_buffer_line_h264d(u32 frame_width, u32 frame_height,
+				 bool is_opb, u32 num_vpp_pipes)
+{
+	u32 vpss_lb_size = 0;
+	u32 size;
+
+	size = ALIGN(size_h264d_lb_fe_top_data(frame_width), DMA_ALIGNMENT) +
+		ALIGN(size_h264d_lb_fe_top_ctrl(frame_width), DMA_ALIGNMENT) +
+		ALIGN(size_h264d_lb_fe_left_ctrl(frame_height), DMA_ALIGNMENT) * num_vpp_pipes +
+		ALIGN(size_h264d_lb_se_top_ctrl(frame_width), DMA_ALIGNMENT) +
+		ALIGN(size_h264d_lb_se_left_ctrl(frame_height), DMA_ALIGNMENT) * num_vpp_pipes +
+		ALIGN(size_h264d_lb_pe_top_data(frame_width), DMA_ALIGNMENT) +
+		ALIGN(size_h264d_lb_vsp_top(frame_width), DMA_ALIGNMENT) +
+		ALIGN(size_h264d_lb_recon_dma_metadata_wr(frame_height), DMA_ALIGNMENT) * 2 +
+		ALIGN(size_h264d_qp(frame_width, frame_height), DMA_ALIGNMENT);
+	size = ALIGN(size, DMA_ALIGNMENT);
+	if (is_opb)
+		vpss_lb_size = size_vpss_lb(frame_width, frame_height);
+
+	return ALIGN((size + vpss_lb_size), DMA_ALIGNMENT);
+}
+
+static u32 iris_vpu_dec_bin_size(struct iris_inst *inst)
+{
+	u32 num_vpp_pipes = inst->core->iris_platform_data->num_vpp_pipe;
+	struct v4l2_format *f = inst->fmt_src;
+	u32 height = f->fmt.pix_mp.height;
+	u32 width = f->fmt.pix_mp.width;
+
+	return hfi_buffer_bin_h264d(width, height, num_vpp_pipes);
+}
+
+static u32 iris_vpu_dec_comv_size(struct iris_inst *inst)
+{
+	u32 num_comv = VIDEO_MAX_FRAME;
+	struct v4l2_format *f = inst->fmt_src;
+	u32 height = f->fmt.pix_mp.height;
+	u32 width = f->fmt.pix_mp.width;
+
+	return hfi_buffer_comv_h264d(width, height, num_comv);
+}
+
+static u32 iris_vpu_dec_persist_size(struct iris_inst *inst)
+{
+	return hfi_buffer_persist_h264d();
+}
+
+static u32 iris_vpu_dec_dpb_size(struct iris_inst *inst)
+{
+	if (iris_split_mode_enabled(inst))
+		return iris_get_buffer_size(inst, BUF_DPB);
+	else
+		return 0;
+}
+
+static u32 iris_vpu_dec_non_comv_size(struct iris_inst *inst)
+{
+	u32 num_vpp_pipes = inst->core->iris_platform_data->num_vpp_pipe;
+	struct v4l2_format *f = inst->fmt_src;
+	u32 height = f->fmt.pix_mp.height;
+	u32 width = f->fmt.pix_mp.width;
+
+	return hfi_buffer_non_comv_h264d(width, height, num_vpp_pipes);
+}
+
+static u32 iris_vpu_dec_line_size(struct iris_inst *inst)
+{
+	u32 num_vpp_pipes = inst->core->iris_platform_data->num_vpp_pipe;
+	struct v4l2_format *f = inst->fmt_src;
+	u32 height = f->fmt.pix_mp.height;
+	u32 width = f->fmt.pix_mp.width;
+	bool is_opb = false;
+
+	if (iris_split_mode_enabled(inst))
+		is_opb = true;
+
+	return hfi_buffer_line_h264d(width, height, is_opb, num_vpp_pipes);
+}
+
+static u32 iris_vpu_dec_scratch1_size(struct iris_inst *inst)
+{
+	return iris_vpu_dec_comv_size(inst) +
+		iris_vpu_dec_non_comv_size(inst) +
+		iris_vpu_dec_line_size(inst);
+}
+
+struct iris_vpu_buf_type_handle {
+	enum iris_buffer_type type;
+	u32 (*handle)(struct iris_inst *inst);
+};
+
+int iris_vpu_buf_size(struct iris_inst *inst, enum iris_buffer_type buffer_type)
+{
+	const struct iris_vpu_buf_type_handle *buf_type_handle_arr;
+	u32 size = 0, buf_type_handle_size, i;
+
+	static const struct iris_vpu_buf_type_handle dec_internal_buf_type_handle[] = {
+		{BUF_BIN,         iris_vpu_dec_bin_size             },
+		{BUF_COMV,        iris_vpu_dec_comv_size            },
+		{BUF_NON_COMV,    iris_vpu_dec_non_comv_size        },
+		{BUF_LINE,        iris_vpu_dec_line_size            },
+		{BUF_PERSIST,     iris_vpu_dec_persist_size         },
+		{BUF_DPB,         iris_vpu_dec_dpb_size             },
+		{BUF_SCRATCH_1,   iris_vpu_dec_scratch1_size        },
+	};
+
+	buf_type_handle_size = ARRAY_SIZE(dec_internal_buf_type_handle);
+	buf_type_handle_arr = dec_internal_buf_type_handle;
+
+	for (i = 0; i < buf_type_handle_size; i++) {
+		if (buf_type_handle_arr[i].type == buffer_type) {
+			size = buf_type_handle_arr[i].handle(inst);
+			break;
+		}
+	}
+
+	return size;
+}
+
+static inline int iris_vpu_dpb_count(struct iris_inst *inst)
+{
+	if (iris_split_mode_enabled(inst)) {
+		return inst->fw_min_count ?
+			inst->fw_min_count : inst->buffers[BUF_OUTPUT].min_count;
+	}
+
+	return 0;
+}
+
+int iris_vpu_buf_count(struct iris_inst *inst, enum iris_buffer_type buffer_type)
+{
+	switch (buffer_type) {
+	case BUF_INPUT:
+		return MIN_BUFFERS;
+	case BUF_OUTPUT:
+		return inst->fw_min_count;
+	case BUF_BIN:
+	case BUF_COMV:
+	case BUF_NON_COMV:
+	case BUF_LINE:
+	case BUF_PERSIST:
+	case BUF_SCRATCH_1:
+		return 1; /* internal buffer count needed by firmware is 1 */
+	case BUF_DPB:
+		return iris_vpu_dpb_count(inst);
+	default:
+		return 0;
+	}
+}
diff --git a/drivers/media/platform/qcom/iris/iris_vpu_buffer.h b/drivers/media/platform/qcom/iris/iris_vpu_buffer.h
new file mode 100644
index 000000000000..62af6ea6ba1f
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu_buffer.h
@@ -0,0 +1,91 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VPU_BUFFER_H__
+#define __IRIS_VPU_BUFFER_H__
+
+struct iris_inst;
+
+#define MIN_BUFFERS			4
+
+#define DMA_ALIGNMENT			256
+
+#define NUM_HW_PIC_BUF			32
+#define SIZE_HW_PIC(size_per_buf)	(NUM_HW_PIC_BUF * (size_per_buf))
+
+#define MAX_TILE_COLUMNS		32
+#define BIN_BUFFER_THRESHOLD		(1280 * 736)
+#define VPP_CMD_MAX_SIZE		(BIT(20))
+#define H264D_MAX_SLICE			1800
+
+#define SIZE_H264D_BUFTAB_T		256
+#define SIZE_H264D_BSE_CMD_PER_BUF	(32 * 4)
+#define SIZE_H264D_VPP_CMD_PER_BUF	512
+
+#define NUM_SLIST_BUF_H264		(256 + 32)
+#define SIZE_SLIST_BUF_H264		512
+#define H264_DISPLAY_BUF_SIZE		3328
+#define H264_NUM_FRM_INFO		66
+
+#define SIZE_SEI_USERDATA		4096
+
+#define H264_CABAC_HDR_RATIO_HD_TOT	1
+#define H264_CABAC_RES_RATIO_HD_TOT	3
+#define SIZE_H264D_HW_PIC_T		(BIT(11))
+
+#define MAX_FE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE	64
+#define MAX_SE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE	16
+#define MAX_PE_NBR_DATA_LCU64_LINE_BUFFER_SIZE	384
+#define MAX_FE_NBR_DATA_LUMA_LINE_BUFFER_SIZE	640
+
+static inline u32 size_h264d_lb_fe_top_data(u32 frame_width)
+{
+	return MAX_FE_NBR_DATA_LUMA_LINE_BUFFER_SIZE * ALIGN(frame_width, 16) * 3;
+}
+
+static inline u32 size_h264d_lb_fe_top_ctrl(u32 frame_width)
+{
+	return MAX_FE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE * DIV_ROUND_UP(frame_width, 16);
+}
+
+static inline u32 size_h264d_lb_fe_left_ctrl(u32 frame_height)
+{
+	return MAX_FE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE * DIV_ROUND_UP(frame_height, 16);
+}
+
+static inline u32 size_h264d_lb_se_top_ctrl(u32 frame_width)
+{
+	return MAX_SE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE * DIV_ROUND_UP(frame_width, 16);
+}
+
+static inline u32 size_h264d_lb_se_left_ctrl(u32 frame_height)
+{
+	return MAX_SE_NBR_CTRL_LCU64_LINE_BUFFER_SIZE * DIV_ROUND_UP(frame_height, 16);
+}
+
+static inline u32 size_h264d_lb_pe_top_data(u32 frame_width)
+{
+	return MAX_PE_NBR_DATA_LCU64_LINE_BUFFER_SIZE * DIV_ROUND_UP(frame_width, 16);
+}
+
+static inline u32 size_h264d_lb_vsp_top(u32 frame_width)
+{
+	return (DIV_ROUND_UP(frame_width, 16) << 7);
+}
+
+static inline u32 size_h264d_lb_recon_dma_metadata_wr(u32 frame_height)
+{
+	return ALIGN(frame_height, 16) * 32;
+}
+
+static inline u32 size_h264d_qp(u32 frame_width, u32 frame_height)
+{
+	return DIV_ROUND_UP(frame_width, 64) * DIV_ROUND_UP(frame_height, 64) * 128;
+}
+
+int iris_vpu_buf_size(struct iris_inst *inst, enum iris_buffer_type buffer_type);
+int iris_vpu_buf_count(struct iris_inst *inst, enum iris_buffer_type buffer_type);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vpu_common.c b/drivers/media/platform/qcom/iris/iris_vpu_common.c
new file mode 100644
index 000000000000..268e45acaa7c
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu_common.c
@@ -0,0 +1,369 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include <linux/iopoll.h>
+#include <linux/pm_opp.h>
+#include <linux/reset.h>
+
+#include "iris_core.h"
+#include "iris_vpu_common.h"
+#include "iris_vpu_register_defines.h"
+
+#define WRAPPER_TZ_BASE_OFFS			0x000C0000
+#define AON_BASE_OFFS				0x000E0000
+
+#define CPU_IC_BASE_OFFS			(CPU_BASE_OFFS)
+
+#define CPU_CS_A2HSOFTINTCLR			(CPU_CS_BASE_OFFS + 0x1C)
+#define CLEAR_XTENSA2HOST_INTR			BIT(0)
+
+#define CTRL_INIT				(CPU_CS_BASE_OFFS + 0x48)
+#define CTRL_STATUS				(CPU_CS_BASE_OFFS + 0x4C)
+
+#define CTRL_INIT_IDLE_MSG_BMSK			0x40000000
+#define CTRL_ERROR_STATUS__M			0xfe
+#define CTRL_STATUS_PC_READY			0x100
+
+#define QTBL_INFO				(CPU_CS_BASE_OFFS + 0x50)
+#define QTBL_ENABLE				BIT(0)
+
+#define QTBL_ADDR				(CPU_CS_BASE_OFFS + 0x54)
+#define CPU_CS_SCIACMDARG3			(CPU_CS_BASE_OFFS + 0x58)
+#define SFR_ADDR				(CPU_CS_BASE_OFFS + 0x5C)
+#define UC_REGION_ADDR				(CPU_CS_BASE_OFFS + 0x64)
+#define UC_REGION_SIZE				(CPU_CS_BASE_OFFS + 0x68)
+
+#define CPU_CS_H2XSOFTINTEN			(CPU_CS_BASE_OFFS + 0x148)
+#define HOST2XTENSA_INTR_ENABLE			BIT(0)
+
+#define CPU_CS_X2RPMH				(CPU_CS_BASE_OFFS + 0x168)
+#define MSK_SIGNAL_FROM_TENSILICA		BIT(0)
+#define MSK_CORE_POWER_ON			BIT(1)
+
+#define CPU_IC_SOFTINT				(CPU_IC_BASE_OFFS + 0x150)
+#define CPU_IC_SOFTINT_H2A_SHFT			0x0
+
+#define WRAPPER_INTR_STATUS			(WRAPPER_BASE_OFFS + 0x0C)
+#define WRAPPER_INTR_STATUS_A2HWD_BMSK		BIT(3)
+#define WRAPPER_INTR_STATUS_A2H_BMSK		BIT(2)
+
+#define WRAPPER_INTR_MASK			(WRAPPER_BASE_OFFS + 0x10)
+#define WRAPPER_INTR_MASK_A2HWD_BMSK		BIT(3)
+#define WRAPPER_INTR_MASK_A2HCPU_BMSK		BIT(2)
+
+#define WRAPPER_DEBUG_BRIDGE_LPI_CONTROL	(WRAPPER_BASE_OFFS + 0x54)
+#define WRAPPER_DEBUG_BRIDGE_LPI_STATUS		(WRAPPER_BASE_OFFS + 0x58)
+#define WRAPPER_IRIS_CPU_NOC_LPI_CONTROL	(WRAPPER_BASE_OFFS + 0x5C)
+#define WRAPPER_IRIS_CPU_NOC_LPI_STATUS		(WRAPPER_BASE_OFFS + 0x60)
+
+#define WRAPPER_TZ_CPU_STATUS			(WRAPPER_TZ_BASE_OFFS + 0x10)
+#define WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG		(WRAPPER_TZ_BASE_OFFS + 0x14)
+#define CTL_AXI_CLK_HALT			BIT(0)
+#define CTL_CLK_HALT				BIT(1)
+
+#define WRAPPER_TZ_QNS4PDXFIFO_RESET		(WRAPPER_TZ_BASE_OFFS + 0x18)
+#define RESET_HIGH				BIT(0)
+
+#define AON_WRAPPER_MVP_NOC_LPI_CONTROL		(AON_BASE_OFFS)
+#define REQ_POWER_DOWN_PREP			BIT(0)
+
+#define AON_WRAPPER_MVP_NOC_LPI_STATUS		(AON_BASE_OFFS + 0x4)
+
+static void iris_vpu_interrupt_init(struct iris_core *core)
+{
+	u32 mask_val;
+
+	mask_val = readl(core->reg_base + WRAPPER_INTR_MASK);
+	mask_val &= ~(WRAPPER_INTR_MASK_A2HWD_BMSK |
+		      WRAPPER_INTR_MASK_A2HCPU_BMSK);
+	writel(mask_val, core->reg_base + WRAPPER_INTR_MASK);
+}
+
+static void iris_vpu_setup_ucregion_memory_map(struct iris_core *core)
+{
+	u32 queue_size, value;
+
+	/* Iris hardware requires 4K queue alignment */
+	queue_size = ALIGN(sizeof(struct iris_hfi_queue_table_header) +
+		(IFACEQ_QUEUE_SIZE * IFACEQ_NUMQ), SZ_4K);
+
+	value = (u32)core->iface_q_table_daddr;
+	writel(value, core->reg_base + UC_REGION_ADDR);
+
+	/* Iris hardware requires 1M queue alignment */
+	value = ALIGN(SFR_SIZE + queue_size, SZ_1M);
+	writel(value, core->reg_base + UC_REGION_SIZE);
+
+	value = (u32)core->iface_q_table_daddr;
+	writel(value, core->reg_base + QTBL_ADDR);
+
+	writel(QTBL_ENABLE, core->reg_base + QTBL_INFO);
+
+	if (core->sfr_daddr) {
+		value = (u32)core->sfr_daddr + core->iris_platform_data->core_arch;
+		writel(value, core->reg_base + SFR_ADDR);
+	}
+}
+
+int iris_vpu_boot_firmware(struct iris_core *core)
+{
+	u32 ctrl_init = BIT(0), ctrl_status = 0, count = 0, max_tries = 1000;
+
+	iris_vpu_setup_ucregion_memory_map(core);
+
+	writel(ctrl_init, core->reg_base + CTRL_INIT);
+	writel(0x1, core->reg_base + CPU_CS_SCIACMDARG3);
+
+	while (!ctrl_status && count < max_tries) {
+		ctrl_status = readl(core->reg_base + CTRL_STATUS);
+		if ((ctrl_status & CTRL_ERROR_STATUS__M) == 0x4) {
+			dev_err(core->dev, "invalid setting for uc_region\n");
+			break;
+		}
+
+		usleep_range(50, 100);
+		count++;
+	}
+
+	if (count >= max_tries) {
+		dev_err(core->dev, "error booting up iris firmware\n");
+		return -ETIME;
+	}
+
+	writel(HOST2XTENSA_INTR_ENABLE, core->reg_base + CPU_CS_H2XSOFTINTEN);
+	writel(0x0, core->reg_base + CPU_CS_X2RPMH);
+
+	return 0;
+}
+
+void iris_vpu_raise_interrupt(struct iris_core *core)
+{
+	writel(1 << CPU_IC_SOFTINT_H2A_SHFT, core->reg_base + CPU_IC_SOFTINT);
+}
+
+void iris_vpu_clear_interrupt(struct iris_core *core)
+{
+	u32 intr_status, mask;
+
+	intr_status = readl(core->reg_base + WRAPPER_INTR_STATUS);
+	mask = (WRAPPER_INTR_STATUS_A2H_BMSK |
+		WRAPPER_INTR_STATUS_A2HWD_BMSK |
+		CTRL_INIT_IDLE_MSG_BMSK);
+
+	if (intr_status & mask)
+		core->intr_status |= intr_status;
+
+	writel(CLEAR_XTENSA2HOST_INTR, core->reg_base + CPU_CS_A2HSOFTINTCLR);
+}
+
+int iris_vpu_watchdog(struct iris_core *core, u32 intr_status)
+{
+	if (intr_status & WRAPPER_INTR_STATUS_A2HWD_BMSK) {
+		dev_err(core->dev, "received watchdog interrupt\n");
+		return -ETIME;
+	}
+
+	return 0;
+}
+
+int iris_vpu_prepare_pc(struct iris_core *core)
+{
+	u32 wfi_status, idle_status, pc_ready;
+	u32 ctrl_status, val = 0;
+	int ret;
+
+	ctrl_status = readl(core->reg_base + CTRL_STATUS);
+	pc_ready = ctrl_status & CTRL_STATUS_PC_READY;
+	idle_status = ctrl_status & BIT(30);
+	if (pc_ready)
+		return 0;
+
+	wfi_status = readl(core->reg_base + WRAPPER_TZ_CPU_STATUS);
+	wfi_status &= BIT(0);
+	if (!wfi_status || !idle_status)
+		goto skip_power_off;
+
+	ret = core->hfi_ops->sys_pc_prep(core);
+	if (ret)
+		goto skip_power_off;
+
+	ret = readl_poll_timeout(core->reg_base + CTRL_STATUS, val,
+				 val & CTRL_STATUS_PC_READY, 250, 2500);
+	if (ret)
+		goto skip_power_off;
+
+	ret = readl_poll_timeout(core->reg_base + WRAPPER_TZ_CPU_STATUS,
+				 val, val & BIT(0), 250, 2500);
+	if (ret)
+		goto skip_power_off;
+
+	return 0;
+
+skip_power_off:
+	ctrl_status = readl(core->reg_base + CTRL_STATUS);
+	wfi_status = readl(core->reg_base + WRAPPER_TZ_CPU_STATUS);
+	wfi_status &= BIT(0);
+	dev_err(core->dev, "skip power collapse, wfi=%#x, idle=%#x, pcr=%#x, ctrl=%#x)\n",
+		wfi_status, idle_status, pc_ready, ctrl_status);
+
+	return -EAGAIN;
+}
+
+int iris_vpu_power_off_controller(struct iris_core *core)
+{
+	u32 val = 0;
+	int ret;
+
+	writel(MSK_SIGNAL_FROM_TENSILICA | MSK_CORE_POWER_ON, core->reg_base + CPU_CS_X2RPMH);
+
+	writel(REQ_POWER_DOWN_PREP, core->reg_base + AON_WRAPPER_MVP_NOC_LPI_CONTROL);
+
+	ret = readl_poll_timeout(core->reg_base + AON_WRAPPER_MVP_NOC_LPI_STATUS,
+				 val, val & BIT(0), 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(REQ_POWER_DOWN_PREP, core->reg_base + WRAPPER_IRIS_CPU_NOC_LPI_CONTROL);
+
+	ret = readl_poll_timeout(core->reg_base + WRAPPER_IRIS_CPU_NOC_LPI_STATUS,
+				 val, val & BIT(0), 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(0x0, core->reg_base + WRAPPER_DEBUG_BRIDGE_LPI_CONTROL);
+
+	ret = readl_poll_timeout(core->reg_base + WRAPPER_DEBUG_BRIDGE_LPI_STATUS,
+				 val, val == 0, 200, 2000);
+	if (ret)
+		goto disable_power;
+
+	writel(CTL_AXI_CLK_HALT | CTL_CLK_HALT,
+	       core->reg_base + WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG);
+	writel(RESET_HIGH, core->reg_base + WRAPPER_TZ_QNS4PDXFIFO_RESET);
+	writel(0x0, core->reg_base + WRAPPER_TZ_QNS4PDXFIFO_RESET);
+	writel(0x0, core->reg_base + WRAPPER_TZ_CTL_AXI_CLOCK_CONFIG);
+
+disable_power:
+	iris_disable_unprepare_clock(core, IRIS_CTRL_CLK);
+	iris_disable_unprepare_clock(core, IRIS_AXI_CLK);
+	iris_disable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_CTRL_POWER_DOMAIN]);
+
+	return 0;
+}
+
+void iris_vpu_power_off_hw(struct iris_core *core)
+{
+	dev_pm_genpd_set_hwmode(core->pmdomain_tbl->pd_devs[IRIS_HW_POWER_DOMAIN], false);
+	iris_disable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_HW_POWER_DOMAIN]);
+	iris_disable_unprepare_clock(core, IRIS_HW_CLK);
+}
+
+void iris_vpu_power_off(struct iris_core *core)
+{
+	dev_pm_opp_set_rate(core->dev, 0);
+	core->iris_platform_data->vpu_ops->power_off_hw(core);
+	core->iris_platform_data->vpu_ops->power_off_controller(core);
+	iris_unset_icc_bw(core);
+
+	if (!iris_vpu_watchdog(core, core->intr_status))
+		disable_irq_nosync(core->irq);
+}
+
+static int iris_vpu_power_on_controller(struct iris_core *core)
+{
+	u32 rst_tbl_size = core->iris_platform_data->clk_rst_tbl_size;
+	int ret;
+
+	ret = iris_enable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_CTRL_POWER_DOMAIN]);
+	if (ret)
+		return ret;
+
+	ret = reset_control_bulk_reset(rst_tbl_size, core->resets);
+	if (ret)
+		goto err_disable_power;
+
+	ret = iris_prepare_enable_clock(core, IRIS_AXI_CLK);
+	if (ret)
+		goto err_disable_power;
+
+	ret = iris_prepare_enable_clock(core, IRIS_CTRL_CLK);
+	if (ret)
+		goto err_disable_clock;
+
+	return 0;
+
+err_disable_clock:
+	iris_disable_unprepare_clock(core, IRIS_AXI_CLK);
+err_disable_power:
+	iris_disable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_CTRL_POWER_DOMAIN]);
+
+	return ret;
+}
+
+static int iris_vpu_power_on_hw(struct iris_core *core)
+{
+	int ret;
+
+	ret = iris_enable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_HW_POWER_DOMAIN]);
+	if (ret)
+		return ret;
+
+	ret = iris_prepare_enable_clock(core, IRIS_HW_CLK);
+	if (ret)
+		goto err_disable_power;
+
+	ret = dev_pm_genpd_set_hwmode(core->pmdomain_tbl->pd_devs[IRIS_HW_POWER_DOMAIN], true);
+	if (ret)
+		goto err_disable_clock;
+
+	return 0;
+
+err_disable_clock:
+	iris_disable_unprepare_clock(core, IRIS_HW_CLK);
+err_disable_power:
+	iris_disable_power_domains(core, core->pmdomain_tbl->pd_devs[IRIS_HW_POWER_DOMAIN]);
+
+	return ret;
+}
+
+int iris_vpu_power_on(struct iris_core *core)
+{
+	u32 freq;
+	int ret;
+
+	ret = iris_set_icc_bw(core, INT_MAX);
+	if (ret)
+		goto err;
+
+	ret = iris_vpu_power_on_controller(core);
+	if (ret)
+		goto err_unvote_icc;
+
+	ret = iris_vpu_power_on_hw(core);
+	if (ret)
+		goto err_power_off_ctrl;
+
+	freq = core->power.clk_freq ? core->power.clk_freq :
+				      (u32)ULONG_MAX;
+
+	dev_pm_opp_set_rate(core->dev, freq);
+
+	core->iris_platform_data->set_preset_registers(core);
+
+	iris_vpu_interrupt_init(core);
+	core->intr_status = 0;
+	enable_irq(core->irq);
+
+	return 0;
+
+err_power_off_ctrl:
+	iris_vpu_power_off_controller(core);
+err_unvote_icc:
+	iris_unset_icc_bw(core);
+err:
+	dev_err(core->dev, "power on failed\n");
+
+	return ret;
+}
diff --git a/drivers/media/platform/qcom/iris/iris_vpu_common.h b/drivers/media/platform/qcom/iris/iris_vpu_common.h
new file mode 100644
index 000000000000..93b7fa27be3b
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu_common.h
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VPU_COMMON_H__
+#define __IRIS_VPU_COMMON_H__
+
+struct iris_core;
+
+extern const struct vpu_ops iris_vpu2_ops;
+extern const struct vpu_ops iris_vpu3_ops;
+extern const struct vpu_ops iris_vpu33_ops;
+
+struct vpu_ops {
+	void (*power_off_hw)(struct iris_core *core);
+	int (*power_off_controller)(struct iris_core *core);
+	u64 (*calc_freq)(struct iris_inst *inst, size_t data_size);
+};
+
+int iris_vpu_boot_firmware(struct iris_core *core);
+void iris_vpu_raise_interrupt(struct iris_core *core);
+void iris_vpu_clear_interrupt(struct iris_core *core);
+int iris_vpu_watchdog(struct iris_core *core, u32 intr_status);
+int iris_vpu_prepare_pc(struct iris_core *core);
+int iris_vpu_power_on(struct iris_core *core);
+int iris_vpu_power_off_controller(struct iris_core *core);
+void iris_vpu_power_off_hw(struct iris_core *core);
+void iris_vpu_power_off(struct iris_core *core);
+
+#endif
diff --git a/drivers/media/platform/qcom/iris/iris_vpu_register_defines.h b/drivers/media/platform/qcom/iris/iris_vpu_register_defines.h
new file mode 100644
index 000000000000..fe8a39e5e5a3
--- /dev/null
+++ b/drivers/media/platform/qcom/iris/iris_vpu_register_defines.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2022-2024 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef __IRIS_VPU_REGISTER_DEFINES_H__
+#define __IRIS_VPU_REGISTER_DEFINES_H__
+
+#define VCODEC_BASE_OFFS			0x00000000
+#define CPU_BASE_OFFS				0x000A0000
+#define WRAPPER_BASE_OFFS			0x000B0000
+
+#define CPU_CS_BASE_OFFS			(CPU_BASE_OFFS)
+
+#define WRAPPER_CORE_POWER_STATUS		(WRAPPER_BASE_OFFS + 0x80)
+
+#endif
diff --git a/drivers/media/platform/qcom/venus/Kconfig b/drivers/media/platform/qcom/venus/Kconfig
index bfd50e8f3421..ffb731ecd48c 100644
--- a/drivers/media/platform/qcom/venus/Kconfig
+++ b/drivers/media/platform/qcom/venus/Kconfig
@@ -2,7 +2,8 @@ config VIDEO_QCOM_VENUS
 	tristate "Qualcomm Venus V4L2 encoder/decoder driver"
 	depends on V4L_MEM2MEM_DRIVERS
 	depends on VIDEO_DEV && QCOM_SMEM
-	depends on (ARCH_QCOM && IOMMU_DMA) || COMPILE_TEST
+	depends on (ARCH_QCOM && ARM64 && IOMMU_API) || COMPILE_TEST
+	select OF_DYNAMIC if ARCH_QCOM
 	select QCOM_MDT_LOADER if ARCH_QCOM
 	select QCOM_SCM
 	select VIDEOBUF2_DMA_CONTIG
diff --git a/drivers/media/platform/qcom/venus/core.c b/drivers/media/platform/qcom/venus/core.c
index ce206b709754..d305d74bb152 100644
--- a/drivers/media/platform/qcom/venus/core.c
+++ b/drivers/media/platform/qcom/venus/core.c
@@ -19,6 +19,7 @@
 #include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 #include <media/videobuf2-v4l2.h>
+#include <media/v4l2-ctrls.h>
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-ioctl.h>
 
@@ -68,6 +69,7 @@ static void venus_event_notify(struct venus_core *core, u32 event)
 
 	mutex_lock(&core->lock);
 	set_bit(0, &core->sys_error);
+	set_bit(0, &core->dump_core);
 	list_for_each_entry(inst, &core->instances, list)
 		inst->ops->event_notify(inst, EVT_SESSION_ERROR, NULL);
 	mutex_unlock(&core->lock);
@@ -110,7 +112,10 @@ static void venus_sys_error_handler(struct work_struct *work)
 
 	venus_shutdown(core);
 
-	venus_coredump(core);
+	if (test_bit(0, &core->dump_core)) {
+		venus_coredump(core);
+		clear_bit(0, &core->dump_core);
+	}
 
 	pm_runtime_put_sync(core->dev);
 
@@ -281,6 +286,89 @@ static irqreturn_t venus_isr_thread(int irq, void *dev_id)
 	return ret;
 }
 
+#if defined(CONFIG_OF_DYNAMIC)
+static int venus_add_video_core(struct venus_core *core, const char *node_name,
+				const char *compat)
+{
+	struct of_changeset *ocs = core->ocs;
+	struct device *dev = core->dev;
+	struct device_node *np, *enp;
+	int ret;
+
+	if (!node_name)
+		return 0;
+
+	enp = of_find_node_by_name(dev->of_node, node_name);
+	if (enp) {
+		of_node_put(enp);
+		return 0;
+	}
+
+	np = of_changeset_create_node(ocs, dev->of_node, node_name);
+	if (!np) {
+		dev_err(dev, "Unable to create new node\n");
+		return -ENODEV;
+	}
+
+	ret = of_changeset_add_prop_string(ocs, np, "compatible", compat);
+	if (ret)
+		dev_err(dev, "unable to add %s\n", compat);
+
+	of_node_put(np);
+
+	return ret;
+}
+
+static int venus_add_dynamic_nodes(struct venus_core *core)
+{
+	struct device *dev = core->dev;
+	int ret;
+
+	core->ocs = kmalloc(sizeof(*core->ocs), GFP_KERNEL);
+	if (!core->ocs)
+		return -ENOMEM;
+
+	of_changeset_init(core->ocs);
+
+	ret = venus_add_video_core(core, core->res->dec_nodename, "venus-decoder");
+	if (ret)
+		goto err;
+
+	ret = venus_add_video_core(core, core->res->enc_nodename, "venus-encoder");
+	if (ret)
+		goto err;
+
+	ret = of_changeset_apply(core->ocs);
+	if (ret) {
+		dev_err(dev, "applying changeset fail ret %d\n", ret);
+		goto err;
+	}
+
+	return 0;
+err:
+	of_changeset_destroy(core->ocs);
+	kfree(core->ocs);
+	core->ocs = NULL;
+	return ret;
+}
+
+static void venus_remove_dynamic_nodes(struct venus_core *core)
+{
+	if (core->ocs) {
+		of_changeset_revert(core->ocs);
+		of_changeset_destroy(core->ocs);
+		kfree(core->ocs);
+	}
+}
+#else
+static int venus_add_dynamic_nodes(struct venus_core *core)
+{
+	return 0;
+}
+
+static void venus_remove_dynamic_nodes(struct venus_core *core) {}
+#endif
+
 static int venus_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -350,7 +438,7 @@ static int venus_probe(struct platform_device *pdev)
 
 	ret = v4l2_device_register(dev, &core->v4l2_dev);
 	if (ret)
-		goto err_core_deinit;
+		goto err_hfi_destroy;
 
 	platform_set_drvdata(pdev, core);
 
@@ -360,9 +448,15 @@ static int venus_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto err_runtime_disable;
 
+	if (core->res->dec_nodename || core->res->enc_nodename) {
+		ret = venus_add_dynamic_nodes(core);
+		if (ret)
+			goto err_runtime_disable;
+	}
+
 	ret = of_platform_populate(dev->of_node, NULL, NULL, dev);
 	if (ret)
-		goto err_runtime_disable;
+		goto err_remove_dynamic_nodes;
 
 	ret = venus_firmware_init(core);
 	if (ret)
@@ -382,37 +476,39 @@ static int venus_probe(struct platform_device *pdev)
 
 	ret = venus_enumerate_codecs(core, VIDC_SESSION_TYPE_DEC);
 	if (ret)
-		goto err_venus_shutdown;
+		goto err_core_deinit;
 
 	ret = venus_enumerate_codecs(core, VIDC_SESSION_TYPE_ENC);
 	if (ret)
-		goto err_venus_shutdown;
+		goto err_core_deinit;
 
 	ret = pm_runtime_put_sync(dev);
 	if (ret) {
 		pm_runtime_get_noresume(dev);
-		goto err_dev_unregister;
+		goto err_core_deinit;
 	}
 
 	venus_dbgfs_init(core);
 
 	return 0;
 
-err_dev_unregister:
-	v4l2_device_unregister(&core->v4l2_dev);
+err_core_deinit:
+	hfi_core_deinit(core, false);
 err_venus_shutdown:
 	venus_shutdown(core);
 err_firmware_deinit:
 	venus_firmware_deinit(core);
 err_of_depopulate:
 	of_platform_depopulate(dev);
+err_remove_dynamic_nodes:
+	venus_remove_dynamic_nodes(core);
 err_runtime_disable:
 	pm_runtime_put_noidle(dev);
-	pm_runtime_set_suspended(dev);
 	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	v4l2_device_unregister(&core->v4l2_dev);
+err_hfi_destroy:
 	hfi_destroy(core);
-err_core_deinit:
-	hfi_core_deinit(core, false);
 err_core_put:
 	if (core->pm_ops->core_put)
 		core->pm_ops->core_put(core);
@@ -426,6 +522,7 @@ static void venus_remove(struct platform_device *pdev)
 	struct device *dev = core->dev;
 	int ret;
 
+	cancel_delayed_work_sync(&core->work);
 	ret = pm_runtime_get_sync(dev);
 	WARN_ON(ret < 0);
 
@@ -437,6 +534,8 @@ static void venus_remove(struct platform_device *pdev)
 
 	venus_firmware_deinit(core);
 
+	venus_remove_dynamic_nodes(core);
+
 	pm_runtime_put_sync(dev);
 	pm_runtime_disable(dev);
 
@@ -497,6 +596,26 @@ err_cpucfg_path:
 	return ret;
 }
 
+void venus_close_common(struct venus_inst *inst)
+{
+	/*
+	 * Make sure we don't have IRQ/IRQ-thread currently running
+	 * or pending execution, which would race with the inst destruction.
+	 */
+	synchronize_irq(inst->core->irq);
+
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+	v4l2_m2m_release(inst->m2m_dev);
+	hfi_session_destroy(inst);
+	v4l2_fh_del(&inst->fh);
+	v4l2_fh_exit(&inst->fh);
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
+
+	mutex_destroy(&inst->lock);
+	mutex_destroy(&inst->ctx_q_lock);
+}
+EXPORT_SYMBOL_GPL(venus_close_common);
+
 static __maybe_unused int venus_runtime_resume(struct device *dev)
 {
 	struct venus_core *core = dev_get_drvdata(dev);
@@ -552,6 +671,8 @@ static const struct venus_resources msm8916_res = {
 	.vmem_addr = 0,
 	.dma_mask = 0xddc00000 - 1,
 	.fwname = "qcom/venus-1.8/venus.mbn",
+	.dec_nodename = "video-decoder",
+	.enc_nodename = "video-encoder",
 };
 
 static const struct freq_tbl msm8996_freq_table[] = {
@@ -587,6 +708,44 @@ static const struct venus_resources msm8996_res = {
 	.fwname = "qcom/venus-4.2/venus.mbn",
 };
 
+static const struct freq_tbl msm8998_freq_table[] = {
+	{ 1944000, 465000000 },	/* 4k UHD @ 60 (decode only) */
+	{  972000, 465000000 },	/* 4k UHD @ 30 */
+	{  489600, 360000000 },	/* 1080p @ 60 */
+	{  244800, 186000000 },	/* 1080p @ 30 */
+	{  108000, 100000000 },	/* 720p @ 30 */
+};
+
+static const struct reg_val msm8998_reg_preset[] = {
+	{ 0x80124, 0x00000003 },
+	{ 0x80550, 0x01111111 },
+	{ 0x80560, 0x01111111 },
+	{ 0x80568, 0x01111111 },
+	{ 0x80570, 0x01111111 },
+	{ 0x80580, 0x01111111 },
+	{ 0x80588, 0x01111111 },
+	{ 0xe2010, 0x00000000 },
+};
+
+static const struct venus_resources msm8998_res = {
+	.freq_tbl = msm8998_freq_table,
+	.freq_tbl_size = ARRAY_SIZE(msm8998_freq_table),
+	.reg_tbl = msm8998_reg_preset,
+	.reg_tbl_size = ARRAY_SIZE(msm8998_reg_preset),
+	.clks = { "core", "iface", "bus", "mbus" },
+	.clks_num = 4,
+	.vcodec0_clks = { "core" },
+	.vcodec1_clks = { "core" },
+	.vcodec_clks_num = 1,
+	.max_load = 2563200,
+	.hfi_version = HFI_VERSION_3XX,
+	.vmem_id = VIDC_RESOURCE_NONE,
+	.vmem_size = 0,
+	.vmem_addr = 0,
+	.dma_mask = 0xddc00000 - 1,
+	.fwname = "qcom/venus-4.4/venus.mbn",
+};
+
 static const struct freq_tbl sdm660_freq_table[] = {
 	{ 979200, 518400000 },
 	{ 489600, 441600000 },
@@ -709,7 +868,7 @@ static const struct venus_resources sdm845_res_v2 = {
 	.vcodec_clks_num = 2,
 	.vcodec_pmdomains = (const char *[]) { "venus", "vcodec0", "vcodec1" },
 	.vcodec_pmdomains_num = 3,
-	.opp_pmdomain = (const char *[]) { "cx", NULL },
+	.opp_pmdomain = (const char *[]) { "cx" },
 	.vcodec_num = 2,
 	.max_load = 3110400,	/* 4096x2160@90 */
 	.hfi_version = HFI_VERSION_4XX,
@@ -723,6 +882,8 @@ static const struct venus_resources sdm845_res_v2 = {
 	.cp_nonpixel_start = 0x1000000,
 	.cp_nonpixel_size = 0x24800000,
 	.fwname = "qcom/venus-5.2/venus.mbn",
+	.dec_nodename = "video-core0",
+	.enc_nodename = "video-core1",
 };
 
 static const struct freq_tbl sc7180_freq_table[] = {
@@ -758,7 +919,7 @@ static const struct venus_resources sc7180_res = {
 	.vcodec_clks_num = 2,
 	.vcodec_pmdomains = (const char *[]) { "venus", "vcodec0" },
 	.vcodec_pmdomains_num = 2,
-	.opp_pmdomain = (const char *[]) { "cx", NULL },
+	.opp_pmdomain = (const char *[]) { "cx" },
 	.vcodec_num = 1,
 	.hfi_version = HFI_VERSION_4XX,
 	.vpu_version = VPU_VERSION_AR50,
@@ -771,6 +932,8 @@ static const struct venus_resources sc7180_res = {
 	.cp_nonpixel_start = 0x1000000,
 	.cp_nonpixel_size = 0x24800000,
 	.fwname = "qcom/venus-5.4/venus.mbn",
+	.dec_nodename = "video-decoder",
+	.enc_nodename = "video-encoder",
 };
 
 static const struct freq_tbl sm8250_freq_table[] = {
@@ -815,7 +978,7 @@ static const struct venus_resources sm8250_res = {
 	.vcodec_clks_num = 1,
 	.vcodec_pmdomains = (const char *[]) { "venus", "vcodec0" },
 	.vcodec_pmdomains_num = 2,
-	.opp_pmdomain = (const char *[]) { "mx", NULL },
+	.opp_pmdomain = (const char *[]) { "mx" },
 	.vcodec_num = 1,
 	.max_load = 7833600,
 	.hfi_version = HFI_VERSION_6XX,
@@ -826,6 +989,8 @@ static const struct venus_resources sm8250_res = {
 	.vmem_addr = 0,
 	.dma_mask = 0xe0000000 - 1,
 	.fwname = "qcom/vpu-1.0/venus.mbn",
+	.dec_nodename = "video-decoder",
+	.enc_nodename = "video-encoder",
 };
 
 static const struct freq_tbl sc7280_freq_table[] = {
@@ -874,7 +1039,7 @@ static const struct venus_resources sc7280_res = {
 	.vcodec_clks_num = 2,
 	.vcodec_pmdomains = (const char *[]) { "venus", "vcodec0" },
 	.vcodec_pmdomains_num = 2,
-	.opp_pmdomain = (const char *[]) { "cx", NULL },
+	.opp_pmdomain = (const char *[]) { "cx" },
 	.vcodec_num = 1,
 	.hfi_version = HFI_VERSION_6XX,
 	.vpu_version = VPU_VERSION_IRIS2_1,
@@ -888,11 +1053,14 @@ static const struct venus_resources sc7280_res = {
 	.cp_nonpixel_start = 0x1000000,
 	.cp_nonpixel_size = 0x24800000,
 	.fwname = "qcom/vpu-2.0/venus.mbn",
+	.dec_nodename = "video-decoder",
+	.enc_nodename = "video-encoder",
 };
 
 static const struct of_device_id venus_dt_match[] = {
 	{ .compatible = "qcom,msm8916-venus", .data = &msm8916_res, },
 	{ .compatible = "qcom,msm8996-venus", .data = &msm8996_res, },
+	{ .compatible = "qcom,msm8998-venus", .data = &msm8998_res, },
 	{ .compatible = "qcom,sdm660-venus", .data = &sdm660_res, },
 	{ .compatible = "qcom,sdm845-venus", .data = &sdm845_res, },
 	{ .compatible = "qcom,sdm845-venus-v2", .data = &sdm845_res_v2, },
@@ -905,7 +1073,7 @@ MODULE_DEVICE_TABLE(of, venus_dt_match);
 
 static struct platform_driver qcom_venus_driver = {
 	.probe = venus_probe,
-	.remove_new = venus_remove,
+	.remove = venus_remove,
 	.driver = {
 		.name = "qcom-venus",
 		.of_match_table = venus_dt_match,
diff --git a/drivers/media/platform/qcom/venus/core.h b/drivers/media/platform/qcom/venus/core.h
index 6a77de374454..b412e0c5515a 100644
--- a/drivers/media/platform/qcom/venus/core.h
+++ b/drivers/media/platform/qcom/venus/core.h
@@ -26,6 +26,7 @@
 #define VIDC_CLKS_NUM_MAX		4
 #define VIDC_VCODEC_CLKS_NUM_MAX	2
 #define VIDC_RESETS_NUM_MAX		2
+#define VIDC_MAX_HIER_CODING_LAYER 6
 
 extern int venus_fw_debug;
 
@@ -89,6 +90,8 @@ struct venus_resources {
 	u32 cp_nonpixel_start;
 	u32 cp_nonpixel_size;
 	const char *fwname;
+	const char *enc_nodename;
+	const char *dec_nodename;
 };
 
 enum venus_fmt {
@@ -132,9 +135,7 @@ struct venus_format {
  * @vcodec1_clks: an array of vcodec1 struct clk pointers
  * @video_path: an interconnect handle to video to/from memory path
  * @cpucfg_path: an interconnect handle to cpu configuration path
- * @has_opp_table: does OPP table exist
  * @pmdomains:	a pointer to a list of pmdomains
- * @opp_dl_venus: an device-link for device OPP
  * @opp_pmdomain: an OPP power-domain
  * @resets: an array of reset signals
  * @vdev_dec:	a reference to video device structure for decoder instances
@@ -169,6 +170,9 @@ struct venus_format {
  * @core1_usage_count: usage counter for core1
  * @root:	debugfs root directory
  * @venus_ver:	the venus firmware version
+ * @dump_core:	a flag indicating that a core dump is required
+ * @ocs:	OF changeset pointer
+ * @hwmode_dev:	a flag indicating that HW_CTRL_TRIGGER is used in clock driver
  */
 struct venus_core {
 	void __iomem *base;
@@ -185,10 +189,8 @@ struct venus_core {
 	struct clk *vcodec1_clks[VIDC_VCODEC_CLKS_NUM_MAX];
 	struct icc_path *video_path;
 	struct icc_path *cpucfg_path;
-	bool has_opp_table;
 	struct dev_pm_domain_list *pmdomains;
-	struct device_link *opp_dl_venus;
-	struct device *opp_pmdomain;
+	struct dev_pm_domain_list *opp_pmdomain;
 	struct reset_control *resets[VIDC_RESETS_NUM_MAX];
 	struct video_device *vdev_dec;
 	struct video_device *vdev_enc;
@@ -232,6 +234,9 @@ struct venus_core {
 		u32 minor;
 		u32 rev;
 	} venus_ver;
+	unsigned long dump_core;
+	struct of_changeset *ocs;
+	bool hwmode_dev;
 };
 
 struct vdec_controls {
@@ -253,6 +258,7 @@ struct venc_controls {
 	u32 rc_enable;
 	u32 const_quality;
 	u32 frame_skip_mode;
+	u32 layer_bitrate;
 
 	u32 h264_i_period;
 	u32 h264_entropy_mode;
@@ -271,6 +277,8 @@ struct venc_controls {
 	s32 h264_loop_filter_alpha;
 	s32 h264_loop_filter_beta;
 	u32 h264_8x8_transform;
+	u32 h264_hier_layers;
+	u32 h264_hier_layer_bitrate[VIDC_MAX_HIER_CODING_LAYER];
 
 	u32 hevc_i_qp;
 	u32 hevc_p_qp;
@@ -562,4 +570,6 @@ is_fw_rev_or_older(struct venus_core *core, u32 vmajor, u32 vminor, u32 vrev)
 		(core)->venus_ver.minor == vminor &&
 		(core)->venus_ver.rev <= vrev);
 }
+
+void venus_close_common(struct venus_inst *inst);
 #endif
diff --git a/drivers/media/platform/qcom/venus/firmware.c b/drivers/media/platform/qcom/venus/firmware.c
index fe7da2b30482..66a18830e66d 100644
--- a/drivers/media/platform/qcom/venus/firmware.c
+++ b/drivers/media/platform/qcom/venus/firmware.c
@@ -316,10 +316,10 @@ int venus_firmware_init(struct venus_core *core)
 
 	core->fw.dev = &pdev->dev;
 
-	iommu_dom = iommu_domain_alloc(&platform_bus_type);
-	if (!iommu_dom) {
+	iommu_dom = iommu_paging_domain_alloc(core->fw.dev);
+	if (IS_ERR(iommu_dom)) {
 		dev_err(core->fw.dev, "Failed to allocate iommu domain\n");
-		ret = -ENOMEM;
+		ret = PTR_ERR(iommu_dom);
 		goto err_unregister;
 	}
 
diff --git a/drivers/media/platform/qcom/venus/hfi.c b/drivers/media/platform/qcom/venus/hfi.c
index e00aedb41d16..675e6fd1e9fa 100644
--- a/drivers/media/platform/qcom/venus/hfi.c
+++ b/drivers/media/platform/qcom/venus/hfi.c
@@ -138,29 +138,6 @@ int hfi_core_trigger_ssr(struct venus_core *core, u32 type)
 	return core->ops->core_trigger_ssr(core, type);
 }
 
-int hfi_core_ping(struct venus_core *core)
-{
-	int ret;
-
-	mutex_lock(&core->lock);
-
-	ret = core->ops->core_ping(core, 0xbeef);
-	if (ret)
-		goto unlock;
-
-	ret = wait_for_completion_timeout(&core->done, TIMEOUT);
-	if (!ret) {
-		ret = -ETIMEDOUT;
-		goto unlock;
-	}
-	ret = 0;
-	if (core->error != HFI_ERR_NONE)
-		ret = -ENODEV;
-unlock:
-	mutex_unlock(&core->lock);
-	return ret;
-}
-
 static int wait_session_msg(struct venus_inst *inst)
 {
 	int ret;
diff --git a/drivers/media/platform/qcom/venus/hfi.h b/drivers/media/platform/qcom/venus/hfi.h
index f25d412d6553..0338841d5992 100644
--- a/drivers/media/platform/qcom/venus/hfi.h
+++ b/drivers/media/platform/qcom/venus/hfi.h
@@ -108,7 +108,6 @@ struct hfi_inst_ops {
 struct hfi_ops {
 	int (*core_init)(struct venus_core *core);
 	int (*core_deinit)(struct venus_core *core);
-	int (*core_ping)(struct venus_core *core, u32 cookie);
 	int (*core_trigger_ssr)(struct venus_core *core, u32 trigger_type);
 
 	int (*session_init)(struct venus_inst *inst, u32 session_type,
@@ -152,7 +151,6 @@ int hfi_core_deinit(struct venus_core *core, bool blocking);
 int hfi_core_suspend(struct venus_core *core);
 int hfi_core_resume(struct venus_core *core, bool force);
 int hfi_core_trigger_ssr(struct venus_core *core, u32 type);
-int hfi_core_ping(struct venus_core *core);
 int hfi_session_create(struct venus_inst *inst, const struct hfi_inst_ops *ops);
 void hfi_session_destroy(struct venus_inst *inst);
 int hfi_session_init(struct venus_inst *inst, u32 pixfmt);
diff --git a/drivers/media/platform/qcom/venus/hfi_cmds.c b/drivers/media/platform/qcom/venus/hfi_cmds.c
index 3418d2dd9371..3ae063094e3e 100644
--- a/drivers/media/platform/qcom/venus/hfi_cmds.c
+++ b/drivers/media/platform/qcom/venus/hfi_cmds.c
@@ -156,7 +156,7 @@ void pkt_sys_image_version(struct hfi_sys_get_property_pkt *pkt)
 	pkt->hdr.size = sizeof(*pkt);
 	pkt->hdr.pkt_type = HFI_CMD_SYS_GET_PROPERTY;
 	pkt->num_properties = 1;
-	pkt->data[0] = HFI_PROPERTY_SYS_IMAGE_VERSION;
+	pkt->data = HFI_PROPERTY_SYS_IMAGE_VERSION;
 }
 
 int pkt_session_init(struct hfi_session_init_pkt *pkt, void *cookie,
@@ -331,7 +331,7 @@ int pkt_session_ftb(struct hfi_session_fill_buffer_pkt *pkt, void *cookie,
 	pkt->alloc_len = out_frame->alloc_len;
 	pkt->filled_len = out_frame->filled_len;
 	pkt->offset = out_frame->offset;
-	pkt->data[0] = out_frame->extradata_size;
+	pkt->data = out_frame->extradata_size;
 
 	return 0;
 }
@@ -402,7 +402,7 @@ static int pkt_session_get_property_1x(struct hfi_session_get_property_pkt *pkt,
 	pkt->shdr.hdr.pkt_type = HFI_CMD_SESSION_GET_PROPERTY;
 	pkt->shdr.session_id = hash32_ptr(cookie);
 	pkt->num_properties = 1;
-	pkt->data[0] = ptype;
+	pkt->data = ptype;
 
 	return 0;
 }
@@ -1110,7 +1110,7 @@ pkt_session_get_property_3xx(struct hfi_session_get_property_pkt *pkt,
 
 	switch (ptype) {
 	case HFI_PROPERTY_CONFIG_VDEC_ENTROPY:
-		pkt->data[0] = HFI_PROPERTY_CONFIG_VDEC_ENTROPY;
+		pkt->data = HFI_PROPERTY_CONFIG_VDEC_ENTROPY;
 		break;
 	default:
 		ret = pkt_session_get_property_1x(pkt, cookie, ptype);
diff --git a/drivers/media/platform/qcom/venus/hfi_cmds.h b/drivers/media/platform/qcom/venus/hfi_cmds.h
index 20acd412ee7b..a83125bc17aa 100644
--- a/drivers/media/platform/qcom/venus/hfi_cmds.h
+++ b/drivers/media/platform/qcom/venus/hfi_cmds.h
@@ -74,7 +74,7 @@ struct hfi_sys_set_property_pkt {
 struct hfi_sys_get_property_pkt {
 	struct hfi_pkt_hdr hdr;
 	u32 num_properties;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_sys_set_buffers_pkt {
@@ -82,7 +82,7 @@ struct hfi_sys_set_buffers_pkt {
 	u32 buffer_type;
 	u32 buffer_size;
 	u32 num_buffers;
-	u32 buffer_addr[1];
+	u32 buffer_addr[];
 };
 
 struct hfi_sys_ping_pkt {
@@ -151,7 +151,7 @@ struct hfi_session_empty_buffer_compressed_pkt {
 	u32 input_tag;
 	u32 packet_buffer;
 	u32 extradata_buffer;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_empty_buffer_uncompressed_plane0_pkt {
@@ -168,7 +168,7 @@ struct hfi_session_empty_buffer_uncompressed_plane0_pkt {
 	u32 input_tag;
 	u32 packet_buffer;
 	u32 extradata_buffer;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_empty_buffer_uncompressed_plane1_pkt {
@@ -177,7 +177,7 @@ struct hfi_session_empty_buffer_uncompressed_plane1_pkt {
 	u32 filled_len;
 	u32 offset;
 	u32 packet_buffer2;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_empty_buffer_uncompressed_plane2_pkt {
@@ -186,7 +186,7 @@ struct hfi_session_empty_buffer_uncompressed_plane2_pkt {
 	u32 filled_len;
 	u32 offset;
 	u32 packet_buffer3;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_fill_buffer_pkt {
@@ -198,7 +198,7 @@ struct hfi_session_fill_buffer_pkt {
 	u32 output_tag;
 	u32 packet_buffer;
 	u32 extradata_buffer;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_flush_pkt {
@@ -217,7 +217,7 @@ struct hfi_session_resume_pkt {
 struct hfi_session_get_property_pkt {
 	struct hfi_session_hdr_pkt shdr;
 	u32 num_properties;
-	u32 data[1];
+	u32 data;
 };
 
 struct hfi_session_release_buffer_pkt {
@@ -227,7 +227,7 @@ struct hfi_session_release_buffer_pkt {
 	u32 extradata_size;
 	u32 response_req;
 	u32 num_buffers;
-	u32 buffer_info[1];
+	u32 buffer_info[] __counted_by(num_buffers);
 };
 
 struct hfi_session_release_resources_pkt {
diff --git a/drivers/media/platform/qcom/venus/hfi_helper.h b/drivers/media/platform/qcom/venus/hfi_helper.h
index e4c05d62cfc7..f44059f19505 100644
--- a/drivers/media/platform/qcom/venus/hfi_helper.h
+++ b/drivers/media/platform/qcom/venus/hfi_helper.h
@@ -761,7 +761,7 @@ struct hfi_multi_stream_3x {
 
 struct hfi_multi_view_format {
 	u32 views;
-	u32 view_order[1];
+	u32 view_order[];
 };
 
 #define HFI_MULTI_SLICE_OFF			0x1
@@ -1005,13 +1005,13 @@ struct hfi_uncompressed_plane_constraints {
 struct hfi_uncompressed_plane_info {
 	u32 format;
 	u32 num_planes;
-	struct hfi_uncompressed_plane_constraints plane_constraints[1];
+	struct hfi_uncompressed_plane_constraints plane_constraints;
 };
 
 struct hfi_uncompressed_format_supported {
 	u32 buffer_type;
 	u32 format_entries;
-	struct hfi_uncompressed_plane_info plane_info[1];
+	struct hfi_uncompressed_plane_info plane_info;
 };
 
 struct hfi_uncompressed_plane_actual {
@@ -1038,7 +1038,7 @@ struct hfi_codec_supported {
 
 struct hfi_properties_supported {
 	u32 num_properties;
-	u32 properties[1];
+	u32 properties[];
 };
 
 struct hfi_max_sessions_supported {
@@ -1085,12 +1085,12 @@ struct hfi_resource_ocmem_requirement {
 
 struct hfi_resource_ocmem_requirement_info {
 	u32 num_entries;
-	struct hfi_resource_ocmem_requirement requirements[1];
+	struct hfi_resource_ocmem_requirement requirements[];
 };
 
 struct hfi_property_sys_image_version_info_type {
 	u32 string_size;
-	u8  str_image_version[1];
+	u8  str_image_version[];
 };
 
 struct hfi_codec_mask_supported {
@@ -1141,7 +1141,7 @@ struct hfi_extradata_header {
 	u32 port_index;
 	u32 type;
 	u32 data_size;
-	u8 data[1];
+	u8 data[];
 };
 
 struct hfi_batch_info {
@@ -1236,7 +1236,7 @@ static inline void hfi_bufreq_set_count_min_host(struct hfi_buffer_requirements
 
 struct hfi_data_payload {
 	u32 size;
-	u8 data[1];
+	u8 data[];
 };
 
 struct hfi_enable_picture {
@@ -1264,12 +1264,12 @@ struct hfi_interlace_format_supported {
 struct hfi_buffer_alloc_mode_supported {
 	u32 buffer_type;
 	u32 num_entries;
-	u32 data[1];
+	u32 data[];
 };
 
 struct hfi_mb_error_map {
 	u32 error_map_size;
-	u8 error_map[1];
+	u8 error_map[];
 };
 
 struct hfi_metadata_pass_through {
diff --git a/drivers/media/platform/qcom/venus/hfi_parser.c b/drivers/media/platform/qcom/venus/hfi_parser.c
index c43839539d4d..1b3db2caa99f 100644
--- a/drivers/media/platform/qcom/venus/hfi_parser.c
+++ b/drivers/media/platform/qcom/venus/hfi_parser.c
@@ -19,6 +19,8 @@ static void init_codecs(struct venus_core *core)
 	struct hfi_plat_caps *caps = core->caps, *cap;
 	unsigned long bit;
 
+	core->codecs_count = 0;
+
 	if (hweight_long(core->dec_codecs) + hweight_long(core->enc_codecs) > MAX_CODEC_NUM)
 		return;
 
@@ -62,7 +64,7 @@ fill_buf_mode(struct hfi_plat_caps *cap, const void *data, unsigned int num)
 		cap->cap_bufs_mode_dynamic = true;
 }
 
-static void
+static int
 parse_alloc_mode(struct venus_core *core, u32 codecs, u32 domain, void *data)
 {
 	struct hfi_buffer_alloc_mode_supported *mode = data;
@@ -70,7 +72,7 @@ parse_alloc_mode(struct venus_core *core, u32 codecs, u32 domain, void *data)
 	u32 *type;
 
 	if (num_entries > MAX_ALLOC_MODE_ENTRIES)
-		return;
+		return -EINVAL;
 
 	type = mode->data;
 
@@ -82,6 +84,8 @@ parse_alloc_mode(struct venus_core *core, u32 codecs, u32 domain, void *data)
 
 		type++;
 	}
+
+	return sizeof(*mode);
 }
 
 static void fill_profile_level(struct hfi_plat_caps *cap, const void *data,
@@ -96,7 +100,7 @@ static void fill_profile_level(struct hfi_plat_caps *cap, const void *data,
 	cap->num_pl += num;
 }
 
-static void
+static int
 parse_profile_level(struct venus_core *core, u32 codecs, u32 domain, void *data)
 {
 	struct hfi_profile_level_supported *pl = data;
@@ -104,12 +108,14 @@ parse_profile_level(struct venus_core *core, u32 codecs, u32 domain, void *data)
 	struct hfi_profile_level pl_arr[HFI_MAX_PROFILE_COUNT] = {};
 
 	if (pl->profile_count > HFI_MAX_PROFILE_COUNT)
-		return;
+		return -EINVAL;
 
 	memcpy(pl_arr, proflevel, pl->profile_count * sizeof(*proflevel));
 
 	for_each_codec(core->caps, ARRAY_SIZE(core->caps), codecs, domain,
 		       fill_profile_level, pl_arr, pl->profile_count);
+
+	return pl->profile_count * sizeof(*proflevel) + sizeof(u32);
 }
 
 static void
@@ -124,7 +130,7 @@ fill_caps(struct hfi_plat_caps *cap, const void *data, unsigned int num)
 	cap->num_caps += num;
 }
 
-static void
+static int
 parse_caps(struct venus_core *core, u32 codecs, u32 domain, void *data)
 {
 	struct hfi_capabilities *caps = data;
@@ -133,12 +139,14 @@ parse_caps(struct venus_core *core, u32 codecs, u32 domain, void *data)
 	struct hfi_capability caps_arr[MAX_CAP_ENTRIES] = {};
 
 	if (num_caps > MAX_CAP_ENTRIES)
-		return;
+		return -EINVAL;
 
 	memcpy(caps_arr, cap, num_caps * sizeof(*cap));
 
 	for_each_codec(core->caps, ARRAY_SIZE(core->caps), codecs, domain,
 		       fill_caps, caps_arr, num_caps);
+
+	return sizeof(*caps);
 }
 
 static void fill_raw_fmts(struct hfi_plat_caps *cap, const void *fmts,
@@ -153,16 +161,17 @@ static void fill_raw_fmts(struct hfi_plat_caps *cap, const void *fmts,
 	cap->num_fmts += num_fmts;
 }
 
-static void
+static int
 parse_raw_formats(struct venus_core *core, u32 codecs, u32 domain, void *data)
 {
 	struct hfi_uncompressed_format_supported *fmt = data;
-	struct hfi_uncompressed_plane_info *pinfo = fmt->plane_info;
+	struct hfi_uncompressed_plane_info *pinfo = &fmt->plane_info;
 	struct hfi_uncompressed_plane_constraints *constr;
 	struct raw_formats rawfmts[MAX_FMT_ENTRIES] = {};
 	u32 entries = fmt->format_entries;
 	unsigned int i = 0;
-	u32 num_planes;
+	u32 num_planes = 0;
+	u32 size;
 
 	while (entries) {
 		num_planes = pinfo->num_planes;
@@ -172,7 +181,7 @@ parse_raw_formats(struct venus_core *core, u32 codecs, u32 domain, void *data)
 		i++;
 
 		if (i >= MAX_FMT_ENTRIES)
-			return;
+			return -EINVAL;
 
 		if (pinfo->num_planes > MAX_PLANES)
 			break;
@@ -184,9 +193,13 @@ parse_raw_formats(struct venus_core *core, u32 codecs, u32 domain, void *data)
 
 	for_each_codec(core->caps, ARRAY_SIZE(core->caps), codecs, domain,
 		       fill_raw_fmts, rawfmts, i);
+	size = fmt->format_entries * (sizeof(*constr) * num_planes + 2 * sizeof(u32))
+		+ 2 * sizeof(u32);
+
+	return size;
 }
 
-static void parse_codecs(struct venus_core *core, void *data)
+static int parse_codecs(struct venus_core *core, void *data)
 {
 	struct hfi_codec_supported *codecs = data;
 
@@ -198,21 +211,27 @@ static void parse_codecs(struct venus_core *core, void *data)
 		core->dec_codecs &= ~HFI_VIDEO_CODEC_SPARK;
 		core->enc_codecs &= ~HFI_VIDEO_CODEC_HEVC;
 	}
+
+	return sizeof(*codecs);
 }
 
-static void parse_max_sessions(struct venus_core *core, const void *data)
+static int parse_max_sessions(struct venus_core *core, const void *data)
 {
 	const struct hfi_max_sessions_supported *sessions = data;
 
 	core->max_sessions_supported = sessions->max_sessions;
+
+	return sizeof(*sessions);
 }
 
-static void parse_codecs_mask(u32 *codecs, u32 *domain, void *data)
+static int parse_codecs_mask(u32 *codecs, u32 *domain, void *data)
 {
 	struct hfi_codec_mask_supported *mask = data;
 
 	*codecs = mask->codecs;
 	*domain = mask->video_domains;
+
+	return sizeof(*mask);
 }
 
 static void parser_init(struct venus_inst *inst, u32 *codecs, u32 *domain)
@@ -281,8 +300,9 @@ static int hfi_platform_parser(struct venus_core *core, struct venus_inst *inst)
 u32 hfi_parser(struct venus_core *core, struct venus_inst *inst, void *buf,
 	       u32 size)
 {
-	unsigned int words_count = size >> 2;
-	u32 *word = buf, *data, codecs = 0, domain = 0;
+	u32 *words = buf, *payload, codecs = 0, domain = 0;
+	u32 *frame_size = buf + size;
+	u32 rem_bytes = size;
 	int ret;
 
 	ret = hfi_platform_parser(core, inst);
@@ -299,38 +319,66 @@ u32 hfi_parser(struct venus_core *core, struct venus_inst *inst, void *buf,
 		memset(core->caps, 0, sizeof(core->caps));
 	}
 
-	while (words_count) {
-		data = word + 1;
+	while (words < frame_size) {
+		payload = words + 1;
 
-		switch (*word) {
+		switch (*words) {
 		case HFI_PROPERTY_PARAM_CODEC_SUPPORTED:
-			parse_codecs(core, data);
+			if (rem_bytes <= sizeof(struct hfi_codec_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_codecs(core, payload);
+			if (ret < 0)
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
 			init_codecs(core);
 			break;
 		case HFI_PROPERTY_PARAM_MAX_SESSIONS_SUPPORTED:
-			parse_max_sessions(core, data);
+			if (rem_bytes <= sizeof(struct hfi_max_sessions_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_max_sessions(core, payload);
 			break;
 		case HFI_PROPERTY_PARAM_CODEC_MASK_SUPPORTED:
-			parse_codecs_mask(&codecs, &domain, data);
+			if (rem_bytes <= sizeof(struct hfi_codec_mask_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_codecs_mask(&codecs, &domain, payload);
 			break;
 		case HFI_PROPERTY_PARAM_UNCOMPRESSED_FORMAT_SUPPORTED:
-			parse_raw_formats(core, codecs, domain, data);
+			if (rem_bytes <= sizeof(struct hfi_uncompressed_format_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_raw_formats(core, codecs, domain, payload);
 			break;
 		case HFI_PROPERTY_PARAM_CAPABILITY_SUPPORTED:
-			parse_caps(core, codecs, domain, data);
+			if (rem_bytes <= sizeof(struct hfi_capabilities))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_caps(core, codecs, domain, payload);
 			break;
 		case HFI_PROPERTY_PARAM_PROFILE_LEVEL_SUPPORTED:
-			parse_profile_level(core, codecs, domain, data);
+			if (rem_bytes <= sizeof(struct hfi_profile_level_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_profile_level(core, codecs, domain, payload);
 			break;
 		case HFI_PROPERTY_PARAM_BUFFER_ALLOC_MODE_SUPPORTED:
-			parse_alloc_mode(core, codecs, domain, data);
+			if (rem_bytes <= sizeof(struct hfi_buffer_alloc_mode_supported))
+				return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+			ret = parse_alloc_mode(core, codecs, domain, payload);
 			break;
 		default:
+			ret = sizeof(u32);
 			break;
 		}
 
-		word++;
-		words_count--;
+		if (ret < 0)
+			return HFI_ERR_SYS_INSUFFICIENT_RESOURCES;
+
+		words += ret / sizeof(u32);
+		rem_bytes -= ret;
 	}
 
 	if (!core->max_sessions_supported)
diff --git a/drivers/media/platform/qcom/venus/hfi_plat_bufs_v6.c b/drivers/media/platform/qcom/venus/hfi_plat_bufs_v6.c
index f5a655973c08..6289166786ec 100644
--- a/drivers/media/platform/qcom/venus/hfi_plat_bufs_v6.c
+++ b/drivers/media/platform/qcom/venus/hfi_plat_bufs_v6.c
@@ -1063,51 +1063,51 @@ struct enc_bufsize_ops {
 	u32 (*persist)(void);
 };
 
-static struct dec_bufsize_ops dec_h264_ops = {
+static const struct dec_bufsize_ops dec_h264_ops = {
 	.scratch = h264d_scratch_size,
 	.scratch1 = h264d_scratch1_size,
 	.persist1 = h264d_persist1_size,
 };
 
-static struct dec_bufsize_ops dec_h265_ops = {
+static const struct dec_bufsize_ops dec_h265_ops = {
 	.scratch = h265d_scratch_size,
 	.scratch1 = h265d_scratch1_size,
 	.persist1 = h265d_persist1_size,
 };
 
-static struct dec_bufsize_ops dec_vp8_ops = {
+static const struct dec_bufsize_ops dec_vp8_ops = {
 	.scratch = vpxd_scratch_size,
 	.scratch1 = vp8d_scratch1_size,
 	.persist1 = vp8d_persist1_size,
 };
 
-static struct dec_bufsize_ops dec_vp9_ops = {
+static const struct dec_bufsize_ops dec_vp9_ops = {
 	.scratch = vpxd_scratch_size,
 	.scratch1 = vp9d_scratch1_size,
 	.persist1 = vp9d_persist1_size,
 };
 
-static struct dec_bufsize_ops dec_mpeg2_ops = {
+static const struct dec_bufsize_ops dec_mpeg2_ops = {
 	.scratch = mpeg2d_scratch_size,
 	.scratch1 = mpeg2d_scratch1_size,
 	.persist1 = mpeg2d_persist1_size,
 };
 
-static struct enc_bufsize_ops enc_h264_ops = {
+static const struct enc_bufsize_ops enc_h264_ops = {
 	.scratch = h264e_scratch_size,
 	.scratch1 = h264e_scratch1_size,
 	.scratch2 = enc_scratch2_size,
 	.persist = enc_persist_size,
 };
 
-static struct enc_bufsize_ops enc_h265_ops = {
+static const struct enc_bufsize_ops enc_h265_ops = {
 	.scratch = h265e_scratch_size,
 	.scratch1 = h265e_scratch1_size,
 	.scratch2 = enc_scratch2_size,
 	.persist = enc_persist_size,
 };
 
-static struct enc_bufsize_ops enc_vp8_ops = {
+static const struct enc_bufsize_ops enc_vp8_ops = {
 	.scratch = vp8e_scratch_size,
 	.scratch1 = vp8e_scratch1_size,
 	.scratch2 = enc_scratch2_size,
@@ -1186,7 +1186,7 @@ static int bufreq_dec(struct hfi_plat_buffers_params *params, u32 buftype,
 	u32 codec = params->codec;
 	u32 width = params->width, height = params->height, out_min_count;
 	u32 out_width = params->out_width, out_height = params->out_height;
-	struct dec_bufsize_ops *dec_ops;
+	const struct dec_bufsize_ops *dec_ops;
 	bool is_secondary_output = params->dec.is_secondary_output;
 	bool is_interlaced = params->dec.is_interlaced;
 	u32 max_mbs_per_frame = params->dec.max_mbs_per_frame;
@@ -1260,7 +1260,7 @@ static int bufreq_enc(struct hfi_plat_buffers_params *params, u32 buftype,
 		      struct hfi_buffer_requirements *bufreq)
 {
 	enum hfi_version version = params->version;
-	struct enc_bufsize_ops *enc_ops;
+	const struct enc_bufsize_ops *enc_ops;
 	u32 width = params->width;
 	u32 height = params->height;
 	bool is_tenbit = params->enc.is_tenbit;
diff --git a/drivers/media/platform/qcom/venus/hfi_venus.c b/drivers/media/platform/qcom/venus/hfi_venus.c
index f9437b6412b9..b5f2ea879950 100644
--- a/drivers/media/platform/qcom/venus/hfi_venus.c
+++ b/drivers/media/platform/qcom/venus/hfi_venus.c
@@ -187,6 +187,9 @@ static int venus_write_queue(struct venus_hfi_device *hdev,
 	/* ensure rd/wr indices's are read from memory */
 	rmb();
 
+	if (qsize > IFACEQ_QUEUE_SIZE / 4)
+		return -EINVAL;
+
 	if (wr_idx >= rd_idx)
 		empty_space = qsize - (wr_idx - rd_idx);
 	else
@@ -255,6 +258,9 @@ static int venus_read_queue(struct venus_hfi_device *hdev,
 	wr_idx = qhdr->write_idx;
 	qsize = qhdr->q_size;
 
+	if (qsize > IFACEQ_QUEUE_SIZE / 4)
+		return -EINVAL;
+
 	/* make sure data is valid before using it */
 	rmb();
 
@@ -1035,18 +1041,26 @@ static void venus_sfr_print(struct venus_hfi_device *hdev)
 {
 	struct device *dev = hdev->core->dev;
 	struct hfi_sfr *sfr = hdev->sfr.kva;
+	u32 size;
 	void *p;
 
 	if (!sfr)
 		return;
 
-	p = memchr(sfr->data, '\0', sfr->buf_size);
+	size = sfr->buf_size;
+	if (!size)
+		return;
+
+	if (size > ALIGNED_SFR_SIZE)
+		size = ALIGNED_SFR_SIZE;
+
+	p = memchr(sfr->data, '\0', size);
 	/*
 	 * SFR isn't guaranteed to be NULL terminated since SYS_ERROR indicates
 	 * that Venus is in the process of crashing.
 	 */
 	if (!p)
-		sfr->data[sfr->buf_size - 1] = '\0';
+		sfr->data[size - 1] = '\0';
 
 	dev_err_ratelimited(dev, "SFR message from FW: %s\n", sfr->data);
 }
@@ -1178,16 +1192,6 @@ static int venus_core_deinit(struct venus_core *core)
 	return 0;
 }
 
-static int venus_core_ping(struct venus_core *core, u32 cookie)
-{
-	struct venus_hfi_device *hdev = to_hfi_priv(core);
-	struct hfi_sys_ping_pkt pkt;
-
-	pkt_sys_ping(&pkt, cookie);
-
-	return venus_iface_cmdq_write(hdev, &pkt, false);
-}
-
 static int venus_core_trigger_ssr(struct venus_core *core, u32 trigger_type)
 {
 	struct venus_hfi_device *hdev = to_hfi_priv(core);
@@ -1639,7 +1643,6 @@ static int venus_suspend(struct venus_core *core)
 static const struct hfi_ops venus_hfi_ops = {
 	.core_init			= venus_core_init,
 	.core_deinit			= venus_core_deinit,
-	.core_ping			= venus_core_ping,
 	.core_trigger_ssr		= venus_core_trigger_ssr,
 
 	.session_init			= venus_session_init,
diff --git a/drivers/media/platform/qcom/venus/pm_helpers.c b/drivers/media/platform/qcom/venus/pm_helpers.c
index 502822059498..409aa9bd0b5d 100644
--- a/drivers/media/platform/qcom/venus/pm_helpers.c
+++ b/drivers/media/platform/qcom/venus/pm_helpers.c
@@ -412,10 +412,17 @@ static int vcodec_control_v4(struct venus_core *core, u32 coreid, bool enable)
 	u32 val;
 	int ret;
 
-	if (IS_V6(core)) {
-		ctrl = core->wrapper_base + WRAPPER_CORE_POWER_CONTROL_V6;
-		stat = core->wrapper_base + WRAPPER_CORE_POWER_STATUS_V6;
-	} else if (coreid == VIDC_CORE_ID_1) {
+	ret = dev_pm_genpd_set_hwmode(core->pmdomains->pd_devs[coreid], !enable);
+	if (ret == -EOPNOTSUPP) {
+		core->hwmode_dev = false;
+		goto legacy;
+	}
+
+	core->hwmode_dev = true;
+	return ret;
+
+legacy:
+	if (coreid == VIDC_CORE_ID_1) {
 		ctrl = core->wrapper_base + WRAPPER_VCODEC0_MMCC_POWER_CONTROL;
 		stat = core->wrapper_base + WRAPPER_VCODEC0_MMCC_POWER_STATUS;
 	} else {
@@ -451,9 +458,11 @@ static int poweroff_coreid(struct venus_core *core, unsigned int coreid_mask)
 
 		vcodec_clks_disable(core, core->vcodec0_clks);
 
-		ret = vcodec_control_v4(core, VIDC_CORE_ID_1, false);
-		if (ret)
-			return ret;
+		if (!core->hwmode_dev) {
+			ret = vcodec_control_v4(core, VIDC_CORE_ID_1, false);
+			if (ret)
+				return ret;
+		}
 
 		ret = pm_runtime_put_sync(core->pmdomains->pd_devs[1]);
 		if (ret < 0)
@@ -467,9 +476,11 @@ static int poweroff_coreid(struct venus_core *core, unsigned int coreid_mask)
 
 		vcodec_clks_disable(core, core->vcodec1_clks);
 
-		ret = vcodec_control_v4(core, VIDC_CORE_ID_2, false);
-		if (ret)
-			return ret;
+		if (!core->hwmode_dev) {
+			ret = vcodec_control_v4(core, VIDC_CORE_ID_2, false);
+			if (ret)
+				return ret;
+		}
 
 		ret = pm_runtime_put_sync(core->pmdomains->pd_devs[2]);
 		if (ret < 0)
@@ -804,7 +815,7 @@ static int vdec_power_v4(struct device *dev, int on)
 	else
 		vcodec_clks_disable(core, core->vcodec0_clks);
 
-	vcodec_control_v4(core, VIDC_CORE_ID_1, false);
+	ret = vcodec_control_v4(core, VIDC_CORE_ID_1, false);
 
 	return ret;
 }
@@ -849,7 +860,7 @@ static int venc_power_v4(struct device *dev, int on)
 	else
 		vcodec_clks_disable(core, core->vcodec1_clks);
 
-	vcodec_control_v4(core, VIDC_CORE_ID_2, false);
+	ret = vcodec_control_v4(core, VIDC_CORE_ID_2, false);
 
 	return ret;
 }
@@ -857,7 +868,6 @@ static int venc_power_v4(struct device *dev, int on)
 static int vcodec_domains_get(struct venus_core *core)
 {
 	int ret;
-	struct device **opp_virt_dev;
 	struct device *dev = core->dev;
 	const struct venus_resources *res = core->res;
 	struct dev_pm_domain_attach_data vcodec_data = {
@@ -865,49 +875,29 @@ static int vcodec_domains_get(struct venus_core *core)
 		.num_pd_names = res->vcodec_pmdomains_num,
 		.pd_flags = PD_FLAG_NO_DEV_LINK,
 	};
+	struct dev_pm_domain_attach_data opp_pd_data = {
+		.pd_names = res->opp_pmdomain,
+		.num_pd_names = 1,
+		.pd_flags = PD_FLAG_DEV_LINK_ON | PD_FLAG_REQUIRED_OPP,
+	};
 
 	if (!res->vcodec_pmdomains_num)
 		goto skip_pmdomains;
 
-	ret = dev_pm_domain_attach_list(dev, &vcodec_data, &core->pmdomains);
+	ret = devm_pm_domain_attach_list(dev, &vcodec_data, &core->pmdomains);
 	if (ret < 0)
 		return ret;
 
 skip_pmdomains:
-	if (!core->res->opp_pmdomain)
+	if (!res->opp_pmdomain)
 		return 0;
 
 	/* Attach the power domain for setting performance state */
-	ret = devm_pm_opp_attach_genpd(dev, res->opp_pmdomain, &opp_virt_dev);
-	if (ret)
-		goto opp_attach_err;
-
-	core->opp_pmdomain = *opp_virt_dev;
-	core->opp_dl_venus = device_link_add(dev, core->opp_pmdomain,
-					     DL_FLAG_RPM_ACTIVE |
-					     DL_FLAG_PM_RUNTIME |
-					     DL_FLAG_STATELESS);
-	if (!core->opp_dl_venus) {
-		ret = -ENODEV;
-		goto opp_attach_err;
-	}
+	ret = devm_pm_domain_attach_list(dev, &opp_pd_data, &core->opp_pmdomain);
+	if (ret < 0)
+		return ret;
 
 	return 0;
-
-opp_attach_err:
-	dev_pm_domain_detach_list(core->pmdomains);
-	return ret;
-}
-
-static void vcodec_domains_put(struct venus_core *core)
-{
-	dev_pm_domain_detach_list(core->pmdomains);
-
-	if (!core->has_opp_table)
-		return;
-
-	if (core->opp_dl_venus)
-		device_link_del(core->opp_dl_venus);
 }
 
 static int core_resets_reset(struct venus_core *core)
@@ -996,9 +986,7 @@ static int core_get_v4(struct venus_core *core)
 
 	if (core->res->opp_pmdomain) {
 		ret = devm_pm_opp_of_add_table(dev);
-		if (!ret) {
-			core->has_opp_table = true;
-		} else if (ret != -ENODEV) {
+		if (ret && ret != -ENODEV) {
 			dev_err(dev, "invalid OPP table in device tree\n");
 			return ret;
 		}
@@ -1009,10 +997,6 @@ static int core_get_v4(struct venus_core *core)
 
 static void core_put_v4(struct venus_core *core)
 {
-	if (legacy_binding)
-		return;
-
-	vcodec_domains_put(core);
 }
 
 static int core_power_v4(struct venus_core *core, int on)
diff --git a/drivers/media/platform/qcom/venus/vdec.c b/drivers/media/platform/qcom/venus/vdec.c
index 29130a9441e7..99ce5fd41577 100644
--- a/drivers/media/platform/qcom/venus/vdec.c
+++ b/drivers/media/platform/qcom/venus/vdec.c
@@ -154,14 +154,14 @@ find_format_by_index(struct venus_inst *inst, unsigned int index, u32 type)
 		return NULL;
 
 	for (i = 0; i < size; i++) {
-		bool valid;
+		bool valid = false;
 
 		if (fmt[i].type != type)
 			continue;
 
 		if (V4L2_TYPE_IS_OUTPUT(type)) {
 			valid = venus_helper_check_codec(inst, fmt[i].pixfmt);
-		} else if (V4L2_TYPE_IS_CAPTURE(type)) {
+		} else {
 			valid = venus_helper_check_format(inst, fmt[i].pixfmt);
 
 			if (fmt[i].pixfmt == V4L2_PIX_FMT_QC10C &&
@@ -1110,10 +1110,20 @@ static int vdec_start_output(struct venus_inst *inst)
 
 	if (inst->codec_state == VENUS_DEC_STATE_SEEK) {
 		ret = venus_helper_process_initial_out_bufs(inst);
-		if (inst->next_buf_last)
+		if (ret)
+			return ret;
+
+		if (inst->next_buf_last) {
 			inst->codec_state = VENUS_DEC_STATE_DRC;
-		else
+		} else {
 			inst->codec_state = VENUS_DEC_STATE_DECODING;
+
+			if (inst->streamon_cap) {
+				ret = venus_helper_queue_dpb_bufs(inst);
+				if (ret)
+					return ret;
+			}
+		}
 		goto done;
 	}
 
@@ -1255,7 +1265,7 @@ static int vdec_stop_output(struct venus_inst *inst)
 		break;
 	case VENUS_DEC_STATE_INIT:
 	case VENUS_DEC_STATE_CAPTURE_SETUP:
-		ret = hfi_session_flush(inst, HFI_FLUSH_INPUT, true);
+		ret = hfi_session_flush(inst, HFI_FLUSH_ALL, true);
 		break;
 	default:
 		break;
@@ -1697,10 +1707,6 @@ static int vdec_open(struct file *file)
 	if (ret)
 		goto err_free;
 
-	ret = hfi_session_create(inst, &vdec_hfi_ops);
-	if (ret)
-		goto err_ctrl_deinit;
-
 	vdec_inst_init(inst);
 
 	ida_init(&inst->dpb_ids);
@@ -1712,15 +1718,19 @@ static int vdec_open(struct file *file)
 	inst->m2m_dev = v4l2_m2m_init(&vdec_m2m_ops);
 	if (IS_ERR(inst->m2m_dev)) {
 		ret = PTR_ERR(inst->m2m_dev);
-		goto err_session_destroy;
+		goto err_ctrl_deinit;
 	}
 
 	inst->m2m_ctx = v4l2_m2m_ctx_init(inst->m2m_dev, inst, m2m_queue_init);
 	if (IS_ERR(inst->m2m_ctx)) {
 		ret = PTR_ERR(inst->m2m_ctx);
-		goto err_m2m_release;
+		goto err_m2m_dev_release;
 	}
 
+	ret = hfi_session_create(inst, &vdec_hfi_ops);
+	if (ret)
+		goto err_m2m_ctx_release;
+
 	v4l2_fh_init(&inst->fh, core->vdev_dec);
 
 	inst->fh.ctrl_handler = &inst->ctrl_handler;
@@ -1730,12 +1740,12 @@ static int vdec_open(struct file *file)
 
 	return 0;
 
-err_m2m_release:
+err_m2m_ctx_release:
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+err_m2m_dev_release:
 	v4l2_m2m_release(inst->m2m_dev);
-err_session_destroy:
-	hfi_session_destroy(inst);
 err_ctrl_deinit:
-	vdec_ctrl_deinit(inst);
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
 err_free:
 	kfree(inst);
 	return ret;
@@ -1746,17 +1756,9 @@ static int vdec_close(struct file *file)
 	struct venus_inst *inst = to_inst(file);
 
 	vdec_pm_get(inst);
-
-	v4l2_m2m_ctx_release(inst->m2m_ctx);
-	v4l2_m2m_release(inst->m2m_dev);
-	vdec_ctrl_deinit(inst);
+	cancel_work_sync(&inst->delayed_process_work);
+	venus_close_common(inst);
 	ida_destroy(&inst->dpb_ids);
-	hfi_session_destroy(inst);
-	mutex_destroy(&inst->lock);
-	mutex_destroy(&inst->ctx_q_lock);
-	v4l2_fh_del(&inst->fh);
-	v4l2_fh_exit(&inst->fh);
-
 	vdec_pm_put(inst, false);
 
 	kfree(inst);
@@ -1874,7 +1876,7 @@ MODULE_DEVICE_TABLE(of, vdec_dt_match);
 
 static struct platform_driver qcom_venus_dec_driver = {
 	.probe = vdec_probe,
-	.remove_new = vdec_remove,
+	.remove = vdec_remove,
 	.driver = {
 		.name = "qcom-venus-decoder",
 		.of_match_table = vdec_dt_match,
diff --git a/drivers/media/platform/qcom/venus/vdec.h b/drivers/media/platform/qcom/venus/vdec.h
index 6b262d0bf561..0cf981108ff0 100644
--- a/drivers/media/platform/qcom/venus/vdec.h
+++ b/drivers/media/platform/qcom/venus/vdec.h
@@ -9,6 +9,5 @@
 struct venus_inst;
 
 int vdec_ctrl_init(struct venus_inst *inst);
-void vdec_ctrl_deinit(struct venus_inst *inst);
 
 #endif
diff --git a/drivers/media/platform/qcom/venus/vdec_ctrls.c b/drivers/media/platform/qcom/venus/vdec_ctrls.c
index 7e0f29bf7fae..36ed955b0419 100644
--- a/drivers/media/platform/qcom/venus/vdec_ctrls.c
+++ b/drivers/media/platform/qcom/venus/vdec_ctrls.c
@@ -187,8 +187,3 @@ int vdec_ctrl_init(struct venus_inst *inst)
 
 	return 0;
 }
-
-void vdec_ctrl_deinit(struct venus_inst *inst)
-{
-	v4l2_ctrl_handler_free(&inst->ctrl_handler);
-}
diff --git a/drivers/media/platform/qcom/venus/venc.c b/drivers/media/platform/qcom/venus/venc.c
index 3ec2fb8d9fab..c7f8e37dba9b 100644
--- a/drivers/media/platform/qcom/venus/venc.c
+++ b/drivers/media/platform/qcom/venus/venc.c
@@ -734,6 +734,29 @@ static int venc_set_properties(struct venus_inst *inst)
 		if (ret)
 			return ret;
 
+		if (ctr->layer_bitrate) {
+			unsigned int i;
+
+			ptype = HFI_PROPERTY_PARAM_VENC_HIER_P_MAX_NUM_ENH_LAYER;
+			ret = hfi_session_set_property(inst, ptype, &ctr->h264_hier_layers);
+			if (ret)
+				return ret;
+
+			ptype = HFI_PROPERTY_CONFIG_VENC_HIER_P_ENH_LAYER;
+			ret = hfi_session_set_property(inst, ptype, &ctr->layer_bitrate);
+			if (ret)
+				return ret;
+
+			for (i = 0; i < ctr->h264_hier_layers; ++i) {
+				ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
+				brate.bitrate = ctr->h264_hier_layer_bitrate[i];
+				brate.layer_id = i;
+
+				ret = hfi_session_set_property(inst, ptype, &brate);
+				if (ret)
+					return ret;
+			}
+		}
 	}
 
 	if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_H264 ||
@@ -823,18 +846,33 @@ static int venc_set_properties(struct venus_inst *inst)
 			return ret;
 	}
 
-	if (!ctr->bitrate)
-		bitrate = 64000;
-	else
-		bitrate = ctr->bitrate;
+	if (!ctr->layer_bitrate) {
+		if (!ctr->bitrate)
+			bitrate = 64000;
+		else
+			bitrate = ctr->bitrate;
 
-	ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
-	brate.bitrate = bitrate;
-	brate.layer_id = 0;
+		ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
+		brate.bitrate = bitrate;
+		brate.layer_id = 0;
 
-	ret = hfi_session_set_property(inst, ptype, &brate);
-	if (ret)
-		return ret;
+		ret = hfi_session_set_property(inst, ptype, &brate);
+		if (ret)
+			return ret;
+
+		if (!ctr->bitrate_peak)
+			bitrate *= 2;
+		else
+			bitrate = ctr->bitrate_peak;
+
+		ptype = HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE;
+		brate.bitrate = bitrate;
+		brate.layer_id = 0;
+
+		ret = hfi_session_set_property(inst, ptype, &brate);
+		if (ret)
+			return ret;
+	}
 
 	if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_H264 ||
 	    inst->fmt_cap->pixfmt == V4L2_PIX_FMT_HEVC) {
@@ -849,19 +887,6 @@ static int venc_set_properties(struct venus_inst *inst)
 			return ret;
 	}
 
-	if (!ctr->bitrate_peak)
-		bitrate *= 2;
-	else
-		bitrate = ctr->bitrate_peak;
-
-	ptype = HFI_PROPERTY_CONFIG_VENC_MAX_BITRATE;
-	brate.bitrate = bitrate;
-	brate.layer_id = 0;
-
-	ret = hfi_session_set_property(inst, ptype, &brate);
-	if (ret)
-		return ret;
-
 	ptype = HFI_PROPERTY_PARAM_VENC_SESSION_QP;
 	if (inst->fmt_cap->pixfmt == V4L2_PIX_FMT_HEVC) {
 		quant.qp_i = ctr->hevc_i_qp;
@@ -1467,10 +1492,6 @@ static int venc_open(struct file *file)
 	if (ret)
 		goto err_free;
 
-	ret = hfi_session_create(inst, &venc_hfi_ops);
-	if (ret)
-		goto err_ctrl_deinit;
-
 	venc_inst_init(inst);
 
 	/*
@@ -1480,15 +1501,19 @@ static int venc_open(struct file *file)
 	inst->m2m_dev = v4l2_m2m_init(&venc_m2m_ops);
 	if (IS_ERR(inst->m2m_dev)) {
 		ret = PTR_ERR(inst->m2m_dev);
-		goto err_session_destroy;
+		goto err_ctrl_deinit;
 	}
 
 	inst->m2m_ctx = v4l2_m2m_ctx_init(inst->m2m_dev, inst, m2m_queue_init);
 	if (IS_ERR(inst->m2m_ctx)) {
 		ret = PTR_ERR(inst->m2m_ctx);
-		goto err_m2m_release;
+		goto err_m2m_dev_release;
 	}
 
+	ret = hfi_session_create(inst, &venc_hfi_ops);
+	if (ret)
+		goto err_m2m_ctx_release;
+
 	v4l2_fh_init(&inst->fh, core->vdev_enc);
 
 	inst->fh.ctrl_handler = &inst->ctrl_handler;
@@ -1498,12 +1523,12 @@ static int venc_open(struct file *file)
 
 	return 0;
 
-err_m2m_release:
+err_m2m_ctx_release:
+	v4l2_m2m_ctx_release(inst->m2m_ctx);
+err_m2m_dev_release:
 	v4l2_m2m_release(inst->m2m_dev);
-err_session_destroy:
-	hfi_session_destroy(inst);
 err_ctrl_deinit:
-	venc_ctrl_deinit(inst);
+	v4l2_ctrl_handler_free(&inst->ctrl_handler);
 err_free:
 	kfree(inst);
 	return ret;
@@ -1514,18 +1539,8 @@ static int venc_close(struct file *file)
 	struct venus_inst *inst = to_inst(file);
 
 	venc_pm_get(inst);
-
-	v4l2_m2m_ctx_release(inst->m2m_ctx);
-	v4l2_m2m_release(inst->m2m_dev);
-	venc_ctrl_deinit(inst);
-	hfi_session_destroy(inst);
-	mutex_destroy(&inst->lock);
-	mutex_destroy(&inst->ctx_q_lock);
-	v4l2_fh_del(&inst->fh);
-	v4l2_fh_exit(&inst->fh);
-
+	venus_close_common(inst);
 	inst->enc_state = VENUS_ENC_STATE_DEINIT;
-
 	venc_pm_put(inst, false);
 
 	kfree(inst);
@@ -1643,7 +1658,7 @@ MODULE_DEVICE_TABLE(of, venc_dt_match);
 
 static struct platform_driver qcom_venus_enc_driver = {
 	.probe = venc_probe,
-	.remove_new = venc_remove,
+	.remove = venc_remove,
 	.driver = {
 		.name = "qcom-venus-encoder",
 		.of_match_table = venc_dt_match,
diff --git a/drivers/media/platform/qcom/venus/venc.h b/drivers/media/platform/qcom/venus/venc.h
index 4ea37fdcd9b8..719d0f73b14b 100644
--- a/drivers/media/platform/qcom/venus/venc.h
+++ b/drivers/media/platform/qcom/venus/venc.h
@@ -9,6 +9,5 @@
 struct venus_inst;
 
 int venc_ctrl_init(struct venus_inst *inst);
-void venc_ctrl_deinit(struct venus_inst *inst);
 
 #endif
diff --git a/drivers/media/platform/qcom/venus/venc_ctrls.c b/drivers/media/platform/qcom/venus/venc_ctrls.c
index d9d2a293f3ef..4d36c44f9d44 100644
--- a/drivers/media/platform/qcom/venus/venc_ctrls.c
+++ b/drivers/media/platform/qcom/venus/venc_ctrls.c
@@ -67,12 +67,28 @@ static int venc_calc_bpframes(u32 gop_size, u32 conseq_b, u32 *bf, u32 *pf)
 	return 0;
 }
 
+static int dynamic_bitrate_update(struct venus_inst *inst, u32 bitrate,
+				  u32 layer_id)
+{
+	int ret = 0;
+
+	mutex_lock(&inst->lock);
+	if (inst->streamon_out && inst->streamon_cap) {
+		u32 ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
+		struct hfi_bitrate brate = { .bitrate = bitrate, .layer_id = layer_id };
+
+		ret = hfi_session_set_property(inst, ptype, &brate);
+	}
+	mutex_unlock(&inst->lock);
+
+	return ret;
+}
+
 static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct venus_inst *inst = ctrl_to_inst(ctrl);
 	struct venc_controls *ctr = &inst->controls.enc;
 	struct hfi_enable en = { .enable = 1 };
-	struct hfi_bitrate brate;
 	struct hfi_ltr_use ltr_use;
 	struct hfi_ltr_mark ltr_mark;
 	u32 bframes;
@@ -85,19 +101,9 @@ static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 	case V4L2_CID_MPEG_VIDEO_BITRATE:
 		ctr->bitrate = ctrl->val;
-		mutex_lock(&inst->lock);
-		if (inst->streamon_out && inst->streamon_cap) {
-			ptype = HFI_PROPERTY_CONFIG_VENC_TARGET_BITRATE;
-			brate.bitrate = ctr->bitrate;
-			brate.layer_id = 0;
-
-			ret = hfi_session_set_property(inst, ptype, &brate);
-			if (ret) {
-				mutex_unlock(&inst->lock);
-				return ret;
-			}
-		}
-		mutex_unlock(&inst->lock);
+		ret = dynamic_bitrate_update(inst, ctr->bitrate, 0);
+		if (ret)
+			return ret;
 		break;
 	case V4L2_CID_MPEG_VIDEO_BITRATE_PEAK:
 		ctr->bitrate_peak = ctrl->val;
@@ -340,6 +346,55 @@ static int venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
 
 		ctr->h264_8x8_transform = ctrl->val;
 		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE:
+		if (ctrl->val != V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_P)
+			return -EINVAL;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING:
+		ctr->layer_bitrate = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER:
+		if (ctrl->val > VIDC_MAX_HIER_CODING_LAYER)
+			return -EINVAL;
+		ctr->h264_hier_layers = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L0_BR:
+		ctr->h264_hier_layer_bitrate[0] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[0], 0);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L1_BR:
+		ctr->h264_hier_layer_bitrate[1] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[1], 1);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L2_BR:
+		ctr->h264_hier_layer_bitrate[2] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[2], 2);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L3_BR:
+		ctr->h264_hier_layer_bitrate[3] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[3], 3);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L4_BR:
+		ctr->h264_hier_layer_bitrate[4] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[4], 4);
+		if (ret)
+			return ret;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L5_BR:
+		ctr->h264_hier_layer_bitrate[5] = ctrl->val;
+		ret = dynamic_bitrate_update(inst, ctr->h264_hier_layer_bitrate[5], 5);
+		if (ret)
+			return ret;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -607,11 +662,16 @@ int venc_ctrl_init(struct venus_inst *inst)
 
 	v4l2_ctrl_new_std_compound(&inst->ctrl_handler, &venc_ctrl_ops,
 				   V4L2_CID_COLORIMETRY_HDR10_CLL_INFO,
-				   v4l2_ctrl_ptr_create(&p_hdr10_cll));
+				   v4l2_ctrl_ptr_create(&p_hdr10_cll),
+				   v4l2_ctrl_ptr_create(NULL),
+				   v4l2_ctrl_ptr_create(NULL));
 
 	v4l2_ctrl_new_std_compound(&inst->ctrl_handler, &venc_ctrl_ops,
 				   V4L2_CID_COLORIMETRY_HDR10_MASTERING_DISPLAY,
-				   v4l2_ctrl_ptr_create((void *)&p_hdr10_mastering));
+				   v4l2_ctrl_ptr_create((void *)&p_hdr10_mastering),
+				   v4l2_ctrl_ptr_create(NULL),
+				   v4l2_ctrl_ptr_create(NULL));
+
 
 	v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
 			       V4L2_CID_MPEG_VIDEO_INTRA_REFRESH_PERIOD_TYPE,
@@ -622,6 +682,49 @@ int venc_ctrl_init(struct venus_inst *inst)
 			  V4L2_CID_MPEG_VIDEO_INTRA_REFRESH_PERIOD, 0,
 			  ((4096 * 2304) >> 8), 1, 0);
 
+	if (IS_V4(inst->core) || IS_V6(inst->core)) {
+		v4l2_ctrl_new_std_menu(&inst->ctrl_handler, &venc_ctrl_ops,
+				       V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_TYPE,
+				       V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_P,
+				       1, V4L2_MPEG_VIDEO_H264_HIERARCHICAL_CODING_P);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING, 0, 1, 1, 0);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIERARCHICAL_CODING_LAYER, 0,
+				  VIDC_MAX_HIER_CODING_LAYER, 1, 0);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L0_BR,
+				  BITRATE_MIN, BITRATE_MAX, BITRATE_STEP, BITRATE_DEFAULT);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L1_BR,
+				  BITRATE_MIN, BITRATE_MAX,
+				  BITRATE_STEP, BITRATE_DEFAULT);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L2_BR,
+				  BITRATE_MIN, BITRATE_MAX,
+				  BITRATE_STEP, BITRATE_DEFAULT);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L3_BR,
+				  BITRATE_MIN, BITRATE_MAX,
+				  BITRATE_STEP, BITRATE_DEFAULT);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L4_BR,
+				  BITRATE_MIN, BITRATE_MAX,
+				  BITRATE_STEP, BITRATE_DEFAULT);
+
+		v4l2_ctrl_new_std(&inst->ctrl_handler, &venc_ctrl_ops,
+				  V4L2_CID_MPEG_VIDEO_H264_HIER_CODING_L5_BR,
+				  BITRATE_MIN, BITRATE_MAX,
+				  BITRATE_STEP, BITRATE_DEFAULT);
+	}
+
 	ret = inst->ctrl_handler.error;
 	if (ret)
 		goto err;
@@ -635,8 +738,3 @@ err:
 	v4l2_ctrl_handler_free(&inst->ctrl_handler);
 	return ret;
 }
-
-void venc_ctrl_deinit(struct venus_inst *inst)
-{
-	v4l2_ctrl_handler_free(&inst->ctrl_handler);
-}
diff --git a/drivers/media/platform/raspberrypi/Kconfig b/drivers/media/platform/raspberrypi/Kconfig
new file mode 100644
index 000000000000..bd5101ffefb5
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/Kconfig
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+comment "Raspberry Pi media platform drivers"
+
+source "drivers/media/platform/raspberrypi/pisp_be/Kconfig"
+source "drivers/media/platform/raspberrypi/rp1-cfe/Kconfig"
diff --git a/drivers/media/platform/raspberrypi/Makefile b/drivers/media/platform/raspberrypi/Makefile
new file mode 100644
index 000000000000..af7fde84eefe
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-y += pisp_be/
+obj-y += rp1-cfe/
diff --git a/drivers/media/platform/raspberrypi/pisp_be/Kconfig b/drivers/media/platform/raspberrypi/pisp_be/Kconfig
new file mode 100644
index 000000000000..46765a2e4c4d
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/pisp_be/Kconfig
@@ -0,0 +1,13 @@
+config VIDEO_RASPBERRYPI_PISP_BE
+	tristate "Raspberry Pi PiSP Backend (BE) ISP driver"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV
+	depends on ARCH_BCM2835 || COMPILE_TEST
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_CONTIG
+	help
+	  Say Y here to enable support for the PiSP Backend (BE) ISP driver.
+
+	  To compile this driver as a module, choose M here. The module will be
+	  called pisp-be.
diff --git a/drivers/media/platform/raspberrypi/pisp_be/Makefile b/drivers/media/platform/raspberrypi/pisp_be/Makefile
new file mode 100644
index 000000000000..a70bf5716824
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/pisp_be/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for Raspberry Pi PiSP Backend driver
+#
+pisp-be-objs := pisp_be.o
+obj-$(CONFIG_VIDEO_RASPBERRYPI_PISP_BE) += pisp-be.o
diff --git a/drivers/media/platform/raspberrypi/pisp_be/pisp_be.c b/drivers/media/platform/raspberrypi/pisp_be/pisp_be.c
new file mode 100644
index 000000000000..7596ae1f7de6
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/pisp_be/pisp_be.c
@@ -0,0 +1,1797 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * PiSP Back End driver.
+ * Copyright (c) 2021-2024 Raspberry Pi Limited.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/lockdep.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include <uapi/linux/media/raspberrypi/pisp_be_config.h>
+
+#include "pisp_be_formats.h"
+
+/* Maximum number of config buffers possible */
+#define PISP_BE_NUM_CONFIG_BUFFERS VB2_MAX_FRAME
+
+#define PISPBE_NAME "pispbe"
+
+/* Some ISP-BE registers */
+#define PISP_BE_VERSION_REG		0x0
+#define PISP_BE_CONTROL_REG		0x4
+#define PISP_BE_CONTROL_COPY_CONFIG	BIT(1)
+#define PISP_BE_CONTROL_QUEUE_JOB	BIT(0)
+#define PISP_BE_CONTROL_NUM_TILES(n)	((n) << 16)
+#define PISP_BE_TILE_ADDR_LO_REG	0x8
+#define PISP_BE_TILE_ADDR_HI_REG	0xc
+#define PISP_BE_STATUS_REG		0x10
+#define PISP_BE_STATUS_QUEUED		BIT(0)
+#define PISP_BE_BATCH_STATUS_REG	0x14
+#define PISP_BE_INTERRUPT_EN_REG	0x18
+#define PISP_BE_INTERRUPT_STATUS_REG	0x1c
+#define PISP_BE_AXI_REG			0x20
+#define PISP_BE_CONFIG_BASE_REG		0x40
+#define PISP_BE_IO_ADDR_LOW(n)		(PISP_BE_CONFIG_BASE_REG + 8 * (n))
+#define PISP_BE_IO_ADDR_HIGH(n)		(PISP_BE_IO_ADDR_LOW((n)) + 4)
+#define PISP_BE_GLOBAL_BAYER_ENABLE	0xb0
+#define PISP_BE_GLOBAL_RGB_ENABLE	0xb4
+#define N_HW_ADDRESSES			13
+#define N_HW_ENABLES			2
+
+#define PISP_BE_VERSION_2712		0x02252700
+#define PISP_BE_VERSION_MINOR_BITS	0xf
+
+/*
+ * This maps our nodes onto the inputs/outputs of the actual PiSP Back End.
+ * Be wary of the word "OUTPUT" which is used ambiguously here. In a V4L2
+ * context it means an input to the hardware (source image or metadata).
+ * Elsewhere it means an output from the hardware.
+ */
+enum pispbe_node_ids {
+	MAIN_INPUT_NODE,
+	TDN_INPUT_NODE,
+	STITCH_INPUT_NODE,
+	OUTPUT0_NODE,
+	OUTPUT1_NODE,
+	TDN_OUTPUT_NODE,
+	STITCH_OUTPUT_NODE,
+	CONFIG_NODE,
+	PISPBE_NUM_NODES
+};
+
+struct pispbe_node_description {
+	const char *ent_name;
+	enum v4l2_buf_type buf_type;
+	unsigned int caps;
+};
+
+static const struct pispbe_node_description node_desc[PISPBE_NUM_NODES] = {
+	/* MAIN_INPUT_NODE */
+	{
+		.ent_name = PISPBE_NAME "-input",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE,
+	},
+	/* TDN_INPUT_NODE */
+	{
+		.ent_name = PISPBE_NAME "-tdn_input",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE,
+	},
+	/* STITCH_INPUT_NODE */
+	{
+		.ent_name = PISPBE_NAME "-stitch_input",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+		.caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE,
+	},
+	/* OUTPUT0_NODE */
+	{
+		.ent_name = PISPBE_NAME "-output0",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+	},
+	/* OUTPUT1_NODE */
+	{
+		.ent_name = PISPBE_NAME "-output1",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+	},
+	/* TDN_OUTPUT_NODE */
+	{
+		.ent_name = PISPBE_NAME "-tdn_output",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+	},
+	/* STITCH_OUTPUT_NODE */
+	{
+		.ent_name = PISPBE_NAME "-stitch_output",
+		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+		.caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE,
+	},
+	/* CONFIG_NODE */
+	{
+		.ent_name = PISPBE_NAME "-config",
+		.buf_type = V4L2_BUF_TYPE_META_OUTPUT,
+		.caps = V4L2_CAP_META_OUTPUT,
+	}
+};
+
+#define NODE_DESC_IS_OUTPUT(desc) ( \
+	((desc)->buf_type == V4L2_BUF_TYPE_META_OUTPUT) || \
+	((desc)->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT) || \
+	((desc)->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+
+#define NODE_IS_META(node) ( \
+	((node)->buf_type == V4L2_BUF_TYPE_META_OUTPUT))
+#define NODE_IS_OUTPUT(node) ( \
+	((node)->buf_type == V4L2_BUF_TYPE_META_OUTPUT) || \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT) || \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE))
+#define NODE_IS_CAPTURE(node) ( \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE) || \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE))
+#define NODE_IS_MPLANE(node) ( \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) || \
+	((node)->buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE))
+
+/*
+ * Structure to describe a single node /dev/video<N> which represents a single
+ * input or output queue to the PiSP Back End device.
+ */
+struct pispbe_node {
+	unsigned int id;
+	int vfl_dir;
+	enum v4l2_buf_type buf_type;
+	struct video_device vfd;
+	struct media_pad pad;
+	struct media_intf_devnode *intf_devnode;
+	struct media_link *intf_link;
+	struct pispbe_dev *pispbe;
+	/* Video device lock */
+	struct mutex node_lock;
+	/* vb2_queue lock */
+	struct mutex queue_lock;
+	/* Protect pispbe_node->ready_queue and pispbe_buffer->ready_list */
+	spinlock_t ready_lock;
+	struct list_head ready_queue;
+	struct vb2_queue queue;
+	struct v4l2_format format;
+	const struct pisp_be_format *pisp_format;
+};
+
+/* For logging only, use the entity name with "pispbe" and separator removed */
+#define NODE_NAME(node) \
+		(node_desc[(node)->id].ent_name + sizeof(PISPBE_NAME))
+
+/* Records details of the jobs currently running or queued on the h/w. */
+struct pispbe_job {
+	bool valid;
+	/*
+	 * An array of buffer pointers - remember it's source buffers first,
+	 * then captures, then metadata last.
+	 */
+	struct pispbe_buffer *buf[PISPBE_NUM_NODES];
+};
+
+struct pispbe_hw_enables {
+	u32 bayer_enables;
+	u32 rgb_enables;
+};
+
+/* Records a job configuration and memory addresses. */
+struct pispbe_job_descriptor {
+	dma_addr_t hw_dma_addrs[N_HW_ADDRESSES];
+	struct pisp_be_tiles_config *config;
+	struct pispbe_hw_enables hw_enables;
+	dma_addr_t tiles;
+};
+
+/*
+ * Structure representing the entire PiSP Back End device, comprising several
+ * nodes which share platform resources and a mutex for the actual HW.
+ */
+struct pispbe_dev {
+	struct device *dev;
+	struct pispbe_dev *pispbe;
+	struct pisp_be_tiles_config *config;
+	void __iomem *be_reg_base;
+	struct clk *clk;
+	struct v4l2_device v4l2_dev;
+	struct v4l2_subdev sd;
+	struct media_device mdev;
+	struct media_pad pad[PISPBE_NUM_NODES]; /* output pads first */
+	struct pispbe_node node[PISPBE_NUM_NODES];
+	dma_addr_t config_dma_addr;
+	unsigned int sequence;
+	u32 streaming_map;
+	struct pispbe_job queued_job, running_job;
+	spinlock_t hw_lock; /* protects "hw_busy" flag and streaming_map */
+	bool hw_busy; /* non-zero if a job is queued or is being started */
+	int irq;
+	u32 hw_version;
+	u8 done, started;
+};
+
+static u32 pispbe_rd(struct pispbe_dev *pispbe, unsigned int offset)
+{
+	return readl(pispbe->be_reg_base + offset);
+}
+
+static void pispbe_wr(struct pispbe_dev *pispbe, unsigned int offset, u32 val)
+{
+	writel(val, pispbe->be_reg_base + offset);
+}
+
+/*
+ * Queue a job to the h/w. If the h/w is idle it will begin immediately.
+ * Caller must ensure it is "safe to queue", i.e. we don't already have a
+ * queued, unstarted job.
+ */
+static void pispbe_queue_job(struct pispbe_dev *pispbe,
+			     struct pispbe_job_descriptor *job)
+{
+	unsigned int begin, end;
+
+	if (pispbe_rd(pispbe, PISP_BE_STATUS_REG) & PISP_BE_STATUS_QUEUED)
+		dev_err(pispbe->dev, "ERROR: not safe to queue new job!\n");
+
+	/*
+	 * Write configuration to hardware. DMA addresses and enable flags
+	 * are passed separately, because the driver needs to sanitize them,
+	 * and we don't want to modify (or be vulnerable to modifications of)
+	 * the mmap'd buffer.
+	 */
+	for (unsigned int u = 0; u < N_HW_ADDRESSES; ++u) {
+		pispbe_wr(pispbe, PISP_BE_IO_ADDR_LOW(u),
+			  lower_32_bits(job->hw_dma_addrs[u]));
+		pispbe_wr(pispbe, PISP_BE_IO_ADDR_HIGH(u),
+			  upper_32_bits(job->hw_dma_addrs[u]));
+	}
+	pispbe_wr(pispbe, PISP_BE_GLOBAL_BAYER_ENABLE,
+		  job->hw_enables.bayer_enables);
+	pispbe_wr(pispbe, PISP_BE_GLOBAL_RGB_ENABLE,
+		  job->hw_enables.rgb_enables);
+
+	/* Everything else is as supplied by the user. */
+	begin =	offsetof(struct pisp_be_config, global.bayer_order) /
+		sizeof(u32);
+	end = sizeof(struct pisp_be_config) / sizeof(u32);
+	for (unsigned int u = begin; u < end; u++)
+		pispbe_wr(pispbe, PISP_BE_CONFIG_BASE_REG + sizeof(u32) * u,
+			  ((u32 *)job->config)[u]);
+
+	/* Read back the addresses -- an error here could be fatal */
+	for (unsigned int u = 0; u < N_HW_ADDRESSES; ++u) {
+		unsigned int offset = PISP_BE_IO_ADDR_LOW(u);
+		u64 along = pispbe_rd(pispbe, offset);
+
+		along += ((u64)pispbe_rd(pispbe, offset + 4)) << 32;
+		if (along != (u64)(job->hw_dma_addrs[u])) {
+			dev_dbg(pispbe->dev,
+				"ISP BE config error: check if ISP RAMs enabled?\n");
+			return;
+		}
+	}
+
+	/*
+	 * Write tile pointer to hardware. The IOMMU should prevent
+	 * out-of-bounds offsets reaching non-ISP buffers.
+	 */
+	pispbe_wr(pispbe, PISP_BE_TILE_ADDR_LO_REG, lower_32_bits(job->tiles));
+	pispbe_wr(pispbe, PISP_BE_TILE_ADDR_HI_REG, upper_32_bits(job->tiles));
+
+	/* Enqueue the job */
+	pispbe_wr(pispbe, PISP_BE_CONTROL_REG,
+		  PISP_BE_CONTROL_COPY_CONFIG | PISP_BE_CONTROL_QUEUE_JOB |
+		  PISP_BE_CONTROL_NUM_TILES(job->config->num_tiles));
+}
+
+struct pispbe_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head ready_list;
+	unsigned int config_index;
+};
+
+static int pispbe_get_planes_addr(dma_addr_t addr[3], struct pispbe_buffer *buf,
+				  struct pispbe_node *node)
+{
+	unsigned int num_planes = node->format.fmt.pix_mp.num_planes;
+	unsigned int plane_factor = 0;
+	unsigned int size;
+	unsigned int p;
+
+	if (!buf || !node->pisp_format)
+		return 0;
+
+	/*
+	 * Determine the base plane size. This will not be the same
+	 * as node->format.fmt.pix_mp.plane_fmt[0].sizeimage for a single
+	 * plane buffer in an mplane format.
+	 */
+	size = node->format.fmt.pix_mp.plane_fmt[0].bytesperline *
+	       node->format.fmt.pix_mp.height;
+
+	for (p = 0; p < num_planes && p < PISPBE_MAX_PLANES; p++) {
+		addr[p] = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, p);
+		plane_factor += node->pisp_format->plane_factor[p];
+	}
+
+	for (; p < PISPBE_MAX_PLANES && node->pisp_format->plane_factor[p]; p++) {
+		/*
+		 * Calculate the address offset of this plane as needed
+		 * by the hardware. This is specifically for non-mplane
+		 * buffer formats, where there are 3 image planes, e.g.
+		 * for the V4L2_PIX_FMT_YUV420 format.
+		 */
+		addr[p] = addr[0] + ((size * plane_factor) >> 3);
+		plane_factor += node->pisp_format->plane_factor[p];
+	}
+
+	return num_planes;
+}
+
+static dma_addr_t pispbe_get_addr(struct pispbe_buffer *buf)
+{
+	if (buf)
+		return vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+
+	return 0;
+}
+
+static void pispbe_xlate_addrs(struct pispbe_dev *pispbe,
+			       struct pispbe_job_descriptor *job,
+			       struct pispbe_buffer *buf[PISPBE_NUM_NODES])
+{
+	struct pispbe_hw_enables *hw_en = &job->hw_enables;
+	struct pisp_be_tiles_config *config = job->config;
+	dma_addr_t *addrs = job->hw_dma_addrs;
+	int ret;
+
+	/* Take a copy of the "enable" bitmaps so we can modify them. */
+	hw_en->bayer_enables = config->config.global.bayer_enables;
+	hw_en->rgb_enables = config->config.global.rgb_enables;
+
+	/*
+	 * Main input first. There are 3 address pointers, corresponding to up
+	 * to 3 planes.
+	 */
+	ret = pispbe_get_planes_addr(addrs, buf[MAIN_INPUT_NODE],
+				     &pispbe->node[MAIN_INPUT_NODE]);
+	if (ret <= 0) {
+		/*
+		 * This shouldn't happen; pispbe_schedule_internal should insist
+		 * on an input.
+		 */
+		dev_warn(pispbe->dev, "ISP-BE missing input\n");
+		hw_en->bayer_enables = 0;
+		hw_en->rgb_enables = 0;
+		return;
+	}
+
+	/*
+	 * Now TDN/Stitch inputs and outputs. These are single-plane and only
+	 * used with Bayer input. Input enables must match the requirements
+	 * of the processing stages, otherwise the hardware can lock up!
+	 */
+	if (hw_en->bayer_enables & PISP_BE_BAYER_ENABLE_INPUT) {
+		addrs[3] = pispbe_get_addr(buf[TDN_INPUT_NODE]);
+		if (addrs[3] == 0 ||
+		    !(hw_en->bayer_enables & PISP_BE_BAYER_ENABLE_TDN_INPUT) ||
+		    !(hw_en->bayer_enables & PISP_BE_BAYER_ENABLE_TDN) ||
+		    (config->config.tdn.reset & 1)) {
+			hw_en->bayer_enables &=
+				~(PISP_BE_BAYER_ENABLE_TDN_INPUT |
+				  PISP_BE_BAYER_ENABLE_TDN_DECOMPRESS);
+			if (!(config->config.tdn.reset & 1))
+				hw_en->bayer_enables &=
+					~PISP_BE_BAYER_ENABLE_TDN;
+		}
+
+		addrs[4] = pispbe_get_addr(buf[STITCH_INPUT_NODE]);
+		if (addrs[4] == 0 ||
+		    !(hw_en->bayer_enables & PISP_BE_BAYER_ENABLE_STITCH_INPUT) ||
+		    !(hw_en->bayer_enables & PISP_BE_BAYER_ENABLE_STITCH)) {
+			hw_en->bayer_enables &=
+				~(PISP_BE_BAYER_ENABLE_STITCH_INPUT |
+				  PISP_BE_BAYER_ENABLE_STITCH_DECOMPRESS |
+				  PISP_BE_BAYER_ENABLE_STITCH);
+		}
+
+		addrs[5] = pispbe_get_addr(buf[TDN_OUTPUT_NODE]);
+		if (addrs[5] == 0)
+			hw_en->bayer_enables &=
+				~(PISP_BE_BAYER_ENABLE_TDN_COMPRESS |
+				  PISP_BE_BAYER_ENABLE_TDN_OUTPUT);
+
+		addrs[6] = pispbe_get_addr(buf[STITCH_OUTPUT_NODE]);
+		if (addrs[6] == 0)
+			hw_en->bayer_enables &=
+				~(PISP_BE_BAYER_ENABLE_STITCH_COMPRESS |
+				  PISP_BE_BAYER_ENABLE_STITCH_OUTPUT);
+	} else {
+		/* No Bayer input? Disable entire Bayer pipe (else lockup) */
+		hw_en->bayer_enables = 0;
+	}
+
+	/* Main image output channels. */
+	for (unsigned int i = 0; i < PISP_BACK_END_NUM_OUTPUTS; i++) {
+		ret = pispbe_get_planes_addr(addrs + 7 + 3 * i,
+					     buf[OUTPUT0_NODE + i],
+					     &pispbe->node[OUTPUT0_NODE + i]);
+		if (ret <= 0)
+			hw_en->rgb_enables &= ~(PISP_BE_RGB_ENABLE_OUTPUT0 << i);
+	}
+}
+
+/*
+ * Prepare a job description to be submitted to the HW.
+ *
+ * To schedule a job, we need all streaming nodes (apart from Output0,
+ * Output1, Tdn and Stitch) to have a buffer ready, which must
+ * include at least a config buffer and a main input image.
+ *
+ * For Output0, Output1, Tdn and Stitch, a buffer only needs to be
+ * available if the blocks are enabled in the config.
+ *
+ * Needs to be called with hw_lock held.
+ *
+ * Returns 0 if a job has been successfully prepared, < 0 otherwise.
+ */
+static int pispbe_prepare_job(struct pispbe_dev *pispbe,
+			      struct pispbe_job_descriptor *job)
+{
+	struct pispbe_buffer *buf[PISPBE_NUM_NODES] = {};
+	unsigned int config_index;
+	struct pispbe_node *node;
+	unsigned long flags;
+
+	lockdep_assert_held(&pispbe->hw_lock);
+
+	memset(job, 0, sizeof(struct pispbe_job_descriptor));
+
+	if (((BIT(CONFIG_NODE) | BIT(MAIN_INPUT_NODE)) &
+		pispbe->streaming_map) !=
+			(BIT(CONFIG_NODE) | BIT(MAIN_INPUT_NODE)))
+		return -ENODEV;
+
+	node = &pispbe->node[CONFIG_NODE];
+	spin_lock_irqsave(&node->ready_lock, flags);
+	buf[CONFIG_NODE] = list_first_entry_or_null(&node->ready_queue,
+						    struct pispbe_buffer,
+						    ready_list);
+	if (buf[CONFIG_NODE]) {
+		list_del(&buf[CONFIG_NODE]->ready_list);
+		pispbe->queued_job.buf[CONFIG_NODE] = buf[CONFIG_NODE];
+	}
+	spin_unlock_irqrestore(&node->ready_lock, flags);
+
+	/* Exit early if no config buffer has been queued. */
+	if (!buf[CONFIG_NODE])
+		return -ENODEV;
+
+	config_index = buf[CONFIG_NODE]->vb.vb2_buf.index;
+	job->config = &pispbe->config[config_index];
+	job->tiles = pispbe->config_dma_addr +
+		     config_index * sizeof(struct pisp_be_tiles_config) +
+		     offsetof(struct pisp_be_tiles_config, tiles);
+
+	/* remember: srcimages, captures then metadata */
+	for (unsigned int i = 0; i < PISPBE_NUM_NODES; i++) {
+		unsigned int bayer_en =
+			job->config->config.global.bayer_enables;
+		unsigned int rgb_en =
+			job->config->config.global.rgb_enables;
+		bool ignore_buffers = false;
+
+		/* Config node is handled outside the loop above. */
+		if (i == CONFIG_NODE)
+			continue;
+
+		buf[i] = NULL;
+		if (!(pispbe->streaming_map & BIT(i)))
+			continue;
+
+		if ((!(rgb_en & PISP_BE_RGB_ENABLE_OUTPUT0) &&
+		     i == OUTPUT0_NODE) ||
+		    (!(rgb_en & PISP_BE_RGB_ENABLE_OUTPUT1) &&
+		     i == OUTPUT1_NODE) ||
+		    (!(bayer_en & PISP_BE_BAYER_ENABLE_TDN_INPUT) &&
+		     i == TDN_INPUT_NODE) ||
+		    (!(bayer_en & PISP_BE_BAYER_ENABLE_TDN_OUTPUT) &&
+		     i == TDN_OUTPUT_NODE) ||
+		    (!(bayer_en & PISP_BE_BAYER_ENABLE_STITCH_INPUT) &&
+		     i == STITCH_INPUT_NODE) ||
+		    (!(bayer_en & PISP_BE_BAYER_ENABLE_STITCH_OUTPUT) &&
+		     i == STITCH_OUTPUT_NODE)) {
+			/*
+			 * Ignore Output0/Output1/Tdn/Stitch buffer check if the
+			 * global enables aren't set for these blocks. If a
+			 * buffer has been provided, we dequeue it back to the
+			 * user with the other in-use buffers.
+			 */
+			ignore_buffers = true;
+		}
+
+		node = &pispbe->node[i];
+
+		/* Pull a buffer from each V4L2 queue to form the queued job */
+		spin_lock_irqsave(&node->ready_lock, flags);
+		buf[i] = list_first_entry_or_null(&node->ready_queue,
+						  struct pispbe_buffer,
+						  ready_list);
+		if (buf[i]) {
+			list_del(&buf[i]->ready_list);
+			pispbe->queued_job.buf[i] = buf[i];
+		}
+		spin_unlock_irqrestore(&node->ready_lock, flags);
+
+		if (!buf[i] && !ignore_buffers)
+			goto err_return_buffers;
+	}
+
+	pispbe->queued_job.valid = true;
+
+	/* Convert buffers to DMA addresses for the hardware */
+	pispbe_xlate_addrs(pispbe, job, buf);
+
+	return 0;
+
+err_return_buffers:
+	for (unsigned int i = 0; i < PISPBE_NUM_NODES; i++) {
+		struct pispbe_node *n =  &pispbe->node[i];
+
+		if (!buf[i])
+			continue;
+
+		/* Return the buffer to the ready_list queue */
+		spin_lock_irqsave(&n->ready_lock, flags);
+		list_add(&buf[i]->ready_list, &n->ready_queue);
+		spin_unlock_irqrestore(&n->ready_lock, flags);
+	}
+
+	memset(&pispbe->queued_job, 0, sizeof(pispbe->queued_job));
+
+	return -ENODEV;
+}
+
+static void pispbe_schedule(struct pispbe_dev *pispbe, bool clear_hw_busy)
+{
+	struct pispbe_job_descriptor job;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&pispbe->hw_lock, flags);
+
+	if (clear_hw_busy)
+		pispbe->hw_busy = false;
+
+	if (pispbe->hw_busy)
+		goto unlock_and_return;
+
+	ret = pispbe_prepare_job(pispbe, &job);
+	if (ret)
+		goto unlock_and_return;
+
+	/*
+	 * We can kick the job off without the hw_lock, as this can
+	 * never run again until hw_busy is cleared, which will happen
+	 * only when the following job has been queued and an interrupt
+	 * is rised.
+	 */
+	pispbe->hw_busy = true;
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+
+	if (job.config->num_tiles <= 0 ||
+	    job.config->num_tiles > PISP_BACK_END_NUM_TILES ||
+	    !((job.hw_enables.bayer_enables | job.hw_enables.rgb_enables) &
+	      PISP_BE_BAYER_ENABLE_INPUT)) {
+		/*
+		 * Bad job. We can't let it proceed as it could lock up
+		 * the hardware, or worse!
+		 *
+		 * For now, just force num_tiles to 0, which causes the
+		 * H/W to do something bizarre but survivable. It
+		 * increments (started,done) counters by more than 1,
+		 * but we seem to survive...
+		 */
+		dev_dbg(pispbe->dev, "Bad job: invalid number of tiles: %u\n",
+			job.config->num_tiles);
+		job.config->num_tiles = 0;
+	}
+
+	pispbe_queue_job(pispbe, &job);
+
+	return;
+
+unlock_and_return:
+	/* No job has been queued, just release the lock and return. */
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+}
+
+static void pispbe_isr_jobdone(struct pispbe_dev *pispbe,
+			       struct pispbe_job *job)
+{
+	struct pispbe_buffer **buf = job->buf;
+	u64 ts = ktime_get_ns();
+
+	for (unsigned int i = 0; i < PISPBE_NUM_NODES; i++) {
+		if (buf[i]) {
+			buf[i]->vb.vb2_buf.timestamp = ts;
+			buf[i]->vb.sequence = pispbe->sequence;
+			vb2_buffer_done(&buf[i]->vb.vb2_buf,
+					VB2_BUF_STATE_DONE);
+		}
+	}
+
+	pispbe->sequence++;
+}
+
+static irqreturn_t pispbe_isr(int irq, void *dev)
+{
+	struct pispbe_dev *pispbe = (struct pispbe_dev *)dev;
+	bool can_queue_another = false;
+	u8 started, done;
+	u32 u;
+
+	u = pispbe_rd(pispbe, PISP_BE_INTERRUPT_STATUS_REG);
+	if (u == 0)
+		return IRQ_NONE;
+
+	pispbe_wr(pispbe, PISP_BE_INTERRUPT_STATUS_REG, u);
+	u = pispbe_rd(pispbe, PISP_BE_BATCH_STATUS_REG);
+	done = (uint8_t)u;
+	started = (uint8_t)(u >> 8);
+
+	/*
+	 * Be aware that done can go up by 2 and started by 1 when: a job that
+	 * we previously saw "start" now finishes, and we then queued a new job
+	 * which we see both start and finish "simultaneously".
+	 */
+	if (pispbe->running_job.valid && pispbe->done != done) {
+		pispbe_isr_jobdone(pispbe, &pispbe->running_job);
+		memset(&pispbe->running_job, 0, sizeof(pispbe->running_job));
+		pispbe->done++;
+	}
+
+	if (pispbe->started != started) {
+		pispbe->started++;
+		can_queue_another = 1;
+
+		if (pispbe->done != done && pispbe->queued_job.valid) {
+			pispbe_isr_jobdone(pispbe, &pispbe->queued_job);
+			pispbe->done++;
+		} else {
+			pispbe->running_job = pispbe->queued_job;
+		}
+
+		memset(&pispbe->queued_job, 0, sizeof(pispbe->queued_job));
+	}
+
+	if (pispbe->done != done || pispbe->started != started) {
+		dev_dbg(pispbe->dev,
+			"Job counters not matching: done = %u, expected %u - started = %u, expected %u\n",
+			pispbe->done, done, pispbe->started, started);
+		pispbe->started = started;
+		pispbe->done = done;
+	}
+
+	/* check if there's more to do before going to sleep */
+	pispbe_schedule(pispbe, can_queue_another);
+
+	return IRQ_HANDLED;
+}
+
+static int pisp_be_validate_config(struct pispbe_dev *pispbe,
+				   struct pisp_be_tiles_config *config)
+{
+	u32 bayer_enables = config->config.global.bayer_enables;
+	u32 rgb_enables = config->config.global.rgb_enables;
+	struct device *dev = pispbe->dev;
+	struct v4l2_format *fmt;
+	unsigned int bpl, size;
+
+	if (!(bayer_enables & PISP_BE_BAYER_ENABLE_INPUT) ==
+	    !(rgb_enables & PISP_BE_RGB_ENABLE_INPUT)) {
+		dev_dbg(dev, "%s: Not one input enabled\n", __func__);
+		return -EIO;
+	}
+
+	/* Ensure output config strides and buffer sizes match the V4L2 formats. */
+	fmt = &pispbe->node[TDN_OUTPUT_NODE].format;
+	if (bayer_enables & PISP_BE_BAYER_ENABLE_TDN_OUTPUT) {
+		bpl = config->config.tdn_output_format.stride;
+		size = bpl * config->config.tdn_output_format.height;
+
+		if (fmt->fmt.pix_mp.plane_fmt[0].bytesperline < bpl) {
+			dev_dbg(dev, "%s: bpl mismatch on tdn_output\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		if (fmt->fmt.pix_mp.plane_fmt[0].sizeimage < size) {
+			dev_dbg(dev, "%s: size mismatch on tdn_output\n",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	fmt = &pispbe->node[STITCH_OUTPUT_NODE].format;
+	if (bayer_enables & PISP_BE_BAYER_ENABLE_STITCH_OUTPUT) {
+		bpl = config->config.stitch_output_format.stride;
+		size = bpl * config->config.stitch_output_format.height;
+
+		if (fmt->fmt.pix_mp.plane_fmt[0].bytesperline < bpl) {
+			dev_dbg(dev, "%s: bpl mismatch on stitch_output\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		if (fmt->fmt.pix_mp.plane_fmt[0].sizeimage < size) {
+			dev_dbg(dev, "%s: size mismatch on stitch_output\n",
+				__func__);
+			return -EINVAL;
+		}
+	}
+
+	for (unsigned int j = 0; j < PISP_BACK_END_NUM_OUTPUTS; j++) {
+		if (!(rgb_enables & PISP_BE_RGB_ENABLE_OUTPUT(j)))
+			continue;
+
+		if (config->config.output_format[j].image.format &
+		    PISP_IMAGE_FORMAT_WALLPAPER_ROLL)
+			continue; /* TODO: Size checks for wallpaper formats */
+
+		fmt = &pispbe->node[OUTPUT0_NODE + j].format;
+		for (unsigned int i = 0; i < fmt->fmt.pix_mp.num_planes; i++) {
+			bpl = !i ? config->config.output_format[j].image.stride
+			    : config->config.output_format[j].image.stride2;
+			size = bpl * config->config.output_format[j].image.height;
+
+			if (config->config.output_format[j].image.format &
+						PISP_IMAGE_FORMAT_SAMPLING_420)
+				size >>= 1;
+
+			if (fmt->fmt.pix_mp.plane_fmt[i].bytesperline < bpl) {
+				dev_dbg(dev, "%s: bpl mismatch on output %d\n",
+					__func__, j);
+				return -EINVAL;
+			}
+
+			if (fmt->fmt.pix_mp.plane_fmt[i].sizeimage < size) {
+				dev_dbg(dev, "%s: size mismatch on output\n",
+					__func__);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int pispbe_node_queue_setup(struct vb2_queue *q, unsigned int *nbuffers,
+				   unsigned int *nplanes, unsigned int sizes[],
+				   struct device *alloc_devs[])
+{
+	struct pispbe_node *node = vb2_get_drv_priv(q);
+	struct pispbe_dev *pispbe = node->pispbe;
+	unsigned int num_planes = NODE_IS_MPLANE(node) ?
+				  node->format.fmt.pix_mp.num_planes : 1;
+
+	if (*nplanes) {
+		if (*nplanes != num_planes)
+			return -EINVAL;
+
+		for (unsigned int i = 0; i < *nplanes; i++) {
+			unsigned int size = NODE_IS_MPLANE(node) ?
+				node->format.fmt.pix_mp.plane_fmt[i].sizeimage :
+				node->format.fmt.meta.buffersize;
+
+			if (sizes[i] < size)
+				return -EINVAL;
+		}
+
+		return 0;
+	}
+
+	*nplanes = num_planes;
+	for (unsigned int i = 0; i < *nplanes; i++) {
+		unsigned int size = NODE_IS_MPLANE(node) ?
+				node->format.fmt.pix_mp.plane_fmt[i].sizeimage :
+				node->format.fmt.meta.buffersize;
+		sizes[i] = size;
+	}
+
+	dev_dbg(pispbe->dev,
+		"Image (or metadata) size %u, nbuffers %u for node %s\n",
+		sizes[0], *nbuffers, NODE_NAME(node));
+
+	return 0;
+}
+
+static int pispbe_node_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct pispbe_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct pispbe_dev *pispbe = node->pispbe;
+	unsigned int num_planes = NODE_IS_MPLANE(node) ?
+				  node->format.fmt.pix_mp.num_planes : 1;
+
+	for (unsigned int i = 0; i < num_planes; i++) {
+		unsigned long size = NODE_IS_MPLANE(node) ?
+				node->format.fmt.pix_mp.plane_fmt[i].sizeimage :
+				node->format.fmt.meta.buffersize;
+
+		if (vb2_plane_size(vb, i) < size) {
+			dev_dbg(pispbe->dev,
+				"data will not fit into plane %d (%lu < %lu)\n",
+				i, vb2_plane_size(vb, i), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, i, size);
+	}
+
+	if (node->id == CONFIG_NODE) {
+		void *dst = &node->pispbe->config[vb->index];
+		void *src = vb2_plane_vaddr(vb, 0);
+
+		memcpy(dst, src, sizeof(struct pisp_be_tiles_config));
+
+		return pisp_be_validate_config(pispbe, dst);
+	}
+
+	return 0;
+}
+
+static void pispbe_node_buffer_queue(struct vb2_buffer *buf)
+{
+	struct vb2_v4l2_buffer *vbuf =
+		container_of(buf, struct vb2_v4l2_buffer, vb2_buf);
+	struct pispbe_buffer *buffer =
+		container_of(vbuf, struct pispbe_buffer, vb);
+	struct pispbe_node *node = vb2_get_drv_priv(buf->vb2_queue);
+	struct pispbe_dev *pispbe = node->pispbe;
+	unsigned long flags;
+
+	dev_dbg(pispbe->dev, "%s: for node %s\n", __func__, NODE_NAME(node));
+	spin_lock_irqsave(&node->ready_lock, flags);
+	list_add_tail(&buffer->ready_list, &node->ready_queue);
+	spin_unlock_irqrestore(&node->ready_lock, flags);
+
+	/*
+	 * Every time we add a buffer, check if there's now some work for the hw
+	 * to do.
+	 */
+	pispbe_schedule(pispbe, false);
+}
+
+static int pispbe_node_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct pispbe_node *node = vb2_get_drv_priv(q);
+	struct pispbe_dev *pispbe = node->pispbe;
+	struct pispbe_buffer *buf, *tmp;
+	unsigned long flags;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(pispbe->dev);
+	if (ret < 0)
+		goto err_return_buffers;
+
+	spin_lock_irqsave(&pispbe->hw_lock, flags);
+	node->pispbe->streaming_map |=  BIT(node->id);
+	node->pispbe->sequence = 0;
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+
+	dev_dbg(pispbe->dev, "%s: for node %s (count %u)\n",
+		__func__, NODE_NAME(node), count);
+	dev_dbg(pispbe->dev, "Nodes streaming now 0x%x\n",
+		node->pispbe->streaming_map);
+
+	/* Maybe we're ready to run. */
+	pispbe_schedule(pispbe, false);
+
+	return 0;
+
+err_return_buffers:
+	spin_lock_irqsave(&pispbe->hw_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &node->ready_queue, ready_list) {
+		list_del(&buf->ready_list);
+		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+
+	return ret;
+}
+
+static void pispbe_node_stop_streaming(struct vb2_queue *q)
+{
+	struct pispbe_node *node = vb2_get_drv_priv(q);
+	struct pispbe_dev *pispbe = node->pispbe;
+	struct pispbe_buffer *buf;
+	unsigned long flags;
+
+	/*
+	 * Now this is a bit awkward. In a simple M2M device we could just wait
+	 * for all queued jobs to complete, but here there's a risk that a
+	 * partial set of buffers was queued and cannot be run. For now, just
+	 * cancel all buffers stuck in the "ready queue", then wait for any
+	 * running job.
+	 *
+	 * This may return buffers out of order.
+	 */
+	dev_dbg(pispbe->dev, "%s: for node %s\n", __func__, NODE_NAME(node));
+	spin_lock_irqsave(&pispbe->hw_lock, flags);
+	do {
+		unsigned long flags1;
+
+		spin_lock_irqsave(&node->ready_lock, flags1);
+		buf = list_first_entry_or_null(&node->ready_queue,
+					       struct pispbe_buffer,
+					       ready_list);
+		if (buf) {
+			list_del(&buf->ready_list);
+			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+		}
+		spin_unlock_irqrestore(&node->ready_lock, flags1);
+	} while (buf);
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+
+	vb2_wait_for_all_buffers(&node->queue);
+
+	spin_lock_irqsave(&pispbe->hw_lock, flags);
+	pispbe->streaming_map &= ~BIT(node->id);
+	spin_unlock_irqrestore(&pispbe->hw_lock, flags);
+
+	pm_runtime_mark_last_busy(pispbe->dev);
+	pm_runtime_put_autosuspend(pispbe->dev);
+
+	dev_dbg(pispbe->dev, "Nodes streaming now 0x%x\n",
+		pispbe->streaming_map);
+}
+
+static const struct vb2_ops pispbe_node_queue_ops = {
+	.queue_setup = pispbe_node_queue_setup,
+	.buf_prepare = pispbe_node_buffer_prepare,
+	.buf_queue = pispbe_node_buffer_queue,
+	.start_streaming = pispbe_node_start_streaming,
+	.stop_streaming = pispbe_node_stop_streaming,
+};
+
+static const struct v4l2_file_operations pispbe_fops = {
+	.owner          = THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vb2_fop_release,
+	.poll           = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap
+};
+
+static int pispbe_node_querycap(struct file *file, void *priv,
+				struct v4l2_capability *cap)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	strscpy(cap->driver, PISPBE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, PISPBE_NAME, sizeof(cap->card));
+
+	dev_dbg(pispbe->dev, "Caps for node %s: %x and %x (dev %x)\n",
+		NODE_NAME(node), cap->capabilities, cap->device_caps,
+		node->vfd.device_caps);
+
+	return 0;
+}
+
+static int pispbe_node_g_fmt_vid_cap(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (!NODE_IS_CAPTURE(node) || NODE_IS_META(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot get capture fmt for output node %s\n",
+			NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	*f = node->format;
+	dev_dbg(pispbe->dev, "Get capture format for node %s\n",
+		NODE_NAME(node));
+
+	return 0;
+}
+
+static int pispbe_node_g_fmt_vid_out(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (NODE_IS_CAPTURE(node) || NODE_IS_META(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot get capture fmt for output node %s\n",
+			 NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	*f = node->format;
+	dev_dbg(pispbe->dev, "Get output format for node %s\n",
+		NODE_NAME(node));
+
+	return 0;
+}
+
+static int pispbe_node_g_fmt_meta_out(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (!NODE_IS_META(node) || NODE_IS_CAPTURE(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot get capture fmt for meta output node %s\n",
+			NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	*f = node->format;
+	dev_dbg(pispbe->dev, "Get output format for meta node %s\n",
+		NODE_NAME(node));
+
+	return 0;
+}
+
+static const struct pisp_be_format *pispbe_find_fmt(unsigned int fourcc)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(supported_formats); i++) {
+		if (supported_formats[i].fourcc == fourcc)
+			return &supported_formats[i];
+	}
+
+	return NULL;
+}
+
+static void pispbe_set_plane_params(struct v4l2_format *f,
+				    const struct pisp_be_format *fmt)
+{
+	unsigned int nplanes = f->fmt.pix_mp.num_planes;
+	unsigned int total_plane_factor = 0;
+
+	for (unsigned int i = 0; i < PISPBE_MAX_PLANES; i++)
+		total_plane_factor += fmt->plane_factor[i];
+
+	for (unsigned int i = 0; i < nplanes; i++) {
+		struct v4l2_plane_pix_format *p = &f->fmt.pix_mp.plane_fmt[i];
+		unsigned int bpl, plane_size;
+
+		bpl = (f->fmt.pix_mp.width * fmt->bit_depth) >> 3;
+		bpl = ALIGN(max(p->bytesperline, bpl), fmt->align);
+
+		plane_size = bpl * f->fmt.pix_mp.height *
+		      (nplanes > 1 ? fmt->plane_factor[i] : total_plane_factor);
+		/*
+		 * The shift is to divide out the plane_factor fixed point
+		 * scaling of 8.
+		 */
+		plane_size = max(p->sizeimage, plane_size >> 3);
+
+		p->bytesperline = bpl;
+		p->sizeimage = plane_size;
+	}
+}
+
+static void pispbe_try_format(struct v4l2_format *f, struct pispbe_node *node)
+{
+	struct pispbe_dev *pispbe = node->pispbe;
+	u32 pixfmt = f->fmt.pix_mp.pixelformat;
+	const struct pisp_be_format *fmt;
+	bool is_rgb;
+
+	dev_dbg(pispbe->dev,
+		"%s: [%s] req %ux%u %p4cc, planes %d\n",
+		__func__, NODE_NAME(node), f->fmt.pix_mp.width,
+		f->fmt.pix_mp.height, &pixfmt,
+		f->fmt.pix_mp.num_planes);
+
+	fmt = pispbe_find_fmt(pixfmt);
+	if (!fmt) {
+		dev_dbg(pispbe->dev,
+			"%s: [%s] Format not found, defaulting to YUV420\n",
+			__func__, NODE_NAME(node));
+		fmt = pispbe_find_fmt(V4L2_PIX_FMT_YUV420);
+	}
+
+	f->fmt.pix_mp.pixelformat = fmt->fourcc;
+	f->fmt.pix_mp.num_planes = fmt->num_planes;
+	f->fmt.pix_mp.field = V4L2_FIELD_NONE;
+	f->fmt.pix_mp.width = max(min(f->fmt.pix_mp.width, 65536u),
+				  PISP_BACK_END_MIN_TILE_WIDTH);
+	f->fmt.pix_mp.height = max(min(f->fmt.pix_mp.height, 65536u),
+				   PISP_BACK_END_MIN_TILE_HEIGHT);
+
+	/*
+	 * Fill in the actual colour space when the requested one was
+	 * not supported. This also catches the case when the "default"
+	 * colour space was requested (as that's never in the mask).
+	 */
+	if (!(V4L2_COLORSPACE_MASK(f->fmt.pix_mp.colorspace) &
+	    fmt->colorspace_mask))
+		f->fmt.pix_mp.colorspace = fmt->colorspace_default;
+
+	/* In all cases, we only support the defaults for these: */
+	f->fmt.pix_mp.ycbcr_enc =
+		V4L2_MAP_YCBCR_ENC_DEFAULT(f->fmt.pix_mp.colorspace);
+	f->fmt.pix_mp.xfer_func =
+		V4L2_MAP_XFER_FUNC_DEFAULT(f->fmt.pix_mp.colorspace);
+
+	is_rgb = f->fmt.pix_mp.colorspace == V4L2_COLORSPACE_SRGB;
+	f->fmt.pix_mp.quantization =
+		V4L2_MAP_QUANTIZATION_DEFAULT(is_rgb, f->fmt.pix_mp.colorspace,
+					      f->fmt.pix_mp.ycbcr_enc);
+
+	/* Set plane size and bytes/line for each plane. */
+	pispbe_set_plane_params(f, fmt);
+
+	for (unsigned int i = 0; i < f->fmt.pix_mp.num_planes; i++) {
+		dev_dbg(pispbe->dev,
+			"%s: [%s] calc plane %d, %ux%u, depth %u, bpl %u size %u\n",
+			__func__, NODE_NAME(node), i, f->fmt.pix_mp.width,
+			f->fmt.pix_mp.height, fmt->bit_depth,
+			f->fmt.pix_mp.plane_fmt[i].bytesperline,
+			f->fmt.pix_mp.plane_fmt[i].sizeimage);
+	}
+}
+
+static int pispbe_node_try_fmt_vid_cap(struct file *file, void *priv,
+				       struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (!NODE_IS_CAPTURE(node) || NODE_IS_META(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot set capture fmt for output node %s\n",
+			NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	pispbe_try_format(f, node);
+
+	return 0;
+}
+
+static int pispbe_node_try_fmt_vid_out(struct file *file, void *priv,
+				       struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (!NODE_IS_OUTPUT(node) || NODE_IS_META(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot set capture fmt for output node %s\n",
+			NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	pispbe_try_format(f, node);
+
+	return 0;
+}
+
+static int pispbe_node_try_fmt_meta_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (!NODE_IS_META(node) || NODE_IS_CAPTURE(node)) {
+		dev_dbg(pispbe->dev,
+			"Cannot set capture fmt for meta output node %s\n",
+			NODE_NAME(node));
+		return -EINVAL;
+	}
+
+	f->fmt.meta.dataformat = V4L2_META_FMT_RPI_BE_CFG;
+	f->fmt.meta.buffersize = sizeof(struct pisp_be_tiles_config);
+
+	return 0;
+}
+
+static int pispbe_node_s_fmt_vid_cap(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+	int ret;
+
+	ret = pispbe_node_try_fmt_vid_cap(file, priv, f);
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(&node->queue))
+		return -EBUSY;
+
+	node->format = *f;
+	node->pisp_format = pispbe_find_fmt(f->fmt.pix_mp.pixelformat);
+
+	dev_dbg(pispbe->dev, "Set capture format for node %s to %p4cc\n",
+		NODE_NAME(node), &f->fmt.pix_mp.pixelformat);
+
+	return 0;
+}
+
+static int pispbe_node_s_fmt_vid_out(struct file *file, void *priv,
+				     struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+	int ret;
+
+	ret = pispbe_node_try_fmt_vid_out(file, priv, f);
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(&node->queue))
+		return -EBUSY;
+
+	node->format = *f;
+	node->pisp_format = pispbe_find_fmt(f->fmt.pix_mp.pixelformat);
+
+	dev_dbg(pispbe->dev, "Set output format for node %s to %p4cc\n",
+		NODE_NAME(node), &f->fmt.pix_mp.pixelformat);
+
+	return 0;
+}
+
+static int pispbe_node_s_fmt_meta_out(struct file *file, void *priv,
+				      struct v4l2_format *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+	int ret;
+
+	ret = pispbe_node_try_fmt_meta_out(file, priv, f);
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(&node->queue))
+		return -EBUSY;
+
+	node->format = *f;
+	node->pisp_format = &meta_out_supported_formats[0];
+
+	dev_dbg(pispbe->dev, "Set output format for meta node %s to %p4cc\n",
+		NODE_NAME(node), &f->fmt.meta.dataformat);
+
+	return 0;
+}
+
+static int pispbe_node_enum_fmt(struct file *file, void  *priv,
+				struct v4l2_fmtdesc *f)
+{
+	struct pispbe_node *node = video_drvdata(file);
+
+	if (f->type != node->queue.type)
+		return -EINVAL;
+
+	if (NODE_IS_META(node)) {
+		if (f->index)
+			return -EINVAL;
+
+		f->pixelformat = V4L2_META_FMT_RPI_BE_CFG;
+		f->flags = 0;
+		return 0;
+	}
+
+	if (f->index >= ARRAY_SIZE(supported_formats))
+		return -EINVAL;
+
+	f->pixelformat = supported_formats[f->index].fourcc;
+	f->flags = 0;
+
+	return 0;
+}
+
+static int pispbe_enum_framesizes(struct file *file, void *priv,
+				  struct v4l2_frmsizeenum *fsize)
+{
+	struct pispbe_node *node = video_drvdata(file);
+	struct pispbe_dev *pispbe = node->pispbe;
+
+	if (NODE_IS_META(node) || fsize->index)
+		return -EINVAL;
+
+	if (!pispbe_find_fmt(fsize->pixel_format)) {
+		dev_dbg(pispbe->dev, "Invalid pixel code: %x\n",
+			fsize->pixel_format);
+		return -EINVAL;
+	}
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = 32;
+	fsize->stepwise.max_width = 65535;
+	fsize->stepwise.step_width = 2;
+
+	fsize->stepwise.min_height = 32;
+	fsize->stepwise.max_height = 65535;
+	fsize->stepwise.step_height = 2;
+
+	return 0;
+}
+
+static const struct v4l2_ioctl_ops pispbe_node_ioctl_ops = {
+	.vidioc_querycap = pispbe_node_querycap,
+	.vidioc_g_fmt_vid_cap_mplane = pispbe_node_g_fmt_vid_cap,
+	.vidioc_g_fmt_vid_out_mplane = pispbe_node_g_fmt_vid_out,
+	.vidioc_g_fmt_meta_out = pispbe_node_g_fmt_meta_out,
+	.vidioc_try_fmt_vid_cap_mplane = pispbe_node_try_fmt_vid_cap,
+	.vidioc_try_fmt_vid_out_mplane = pispbe_node_try_fmt_vid_out,
+	.vidioc_try_fmt_meta_out = pispbe_node_try_fmt_meta_out,
+	.vidioc_s_fmt_vid_cap_mplane = pispbe_node_s_fmt_vid_cap,
+	.vidioc_s_fmt_vid_out_mplane = pispbe_node_s_fmt_vid_out,
+	.vidioc_s_fmt_meta_out = pispbe_node_s_fmt_meta_out,
+	.vidioc_enum_fmt_vid_cap = pispbe_node_enum_fmt,
+	.vidioc_enum_fmt_vid_out = pispbe_node_enum_fmt,
+	.vidioc_enum_fmt_meta_out = pispbe_node_enum_fmt,
+	.vidioc_enum_framesizes = pispbe_enum_framesizes,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+};
+
+static const struct video_device pispbe_videodev = {
+	.name = PISPBE_NAME,
+	.vfl_dir = VFL_DIR_M2M, /* gets overwritten */
+	.fops = &pispbe_fops,
+	.ioctl_ops = &pispbe_node_ioctl_ops,
+	.minor = -1,
+	.release = video_device_release_empty,
+};
+
+static void pispbe_node_def_fmt(struct pispbe_node *node)
+{
+	if (NODE_IS_META(node) && NODE_IS_OUTPUT(node)) {
+		/* Config node */
+		struct v4l2_format *f = &node->format;
+
+		f->fmt.meta.dataformat = V4L2_META_FMT_RPI_BE_CFG;
+		f->fmt.meta.buffersize = sizeof(struct pisp_be_tiles_config);
+		f->type = node->buf_type;
+	} else {
+		struct v4l2_format f = {
+			.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUV420,
+			.fmt.pix_mp.width = 1920,
+			.fmt.pix_mp.height = 1080,
+			.type = node->buf_type,
+		};
+		pispbe_try_format(&f, node);
+		node->format = f;
+	}
+
+	node->pisp_format = pispbe_find_fmt(node->format.fmt.pix_mp.pixelformat);
+}
+
+/*
+ * Initialise a struct pispbe_node and register it as /dev/video<N>
+ * to represent one of the PiSP Back End's input or output streams.
+ */
+static int pispbe_init_node(struct pispbe_dev *pispbe, unsigned int id)
+{
+	bool output = NODE_DESC_IS_OUTPUT(&node_desc[id]);
+	struct pispbe_node *node = &pispbe->node[id];
+	struct media_entity *entity = &node->vfd.entity;
+	struct video_device *vdev = &node->vfd;
+	struct vb2_queue *q = &node->queue;
+	int ret;
+
+	node->id = id;
+	node->pispbe = pispbe;
+	node->buf_type = node_desc[id].buf_type;
+
+	mutex_init(&node->node_lock);
+	mutex_init(&node->queue_lock);
+	INIT_LIST_HEAD(&node->ready_queue);
+	spin_lock_init(&node->ready_lock);
+
+	node->format.type = node->buf_type;
+	pispbe_node_def_fmt(node);
+
+	q->type = node->buf_type;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->drv_priv = node;
+	q->ops = &pispbe_node_queue_ops;
+	q->buf_struct_size = sizeof(struct pispbe_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->dev = pispbe->dev;
+	/* get V4L2 to handle node->queue locking */
+	q->lock = &node->queue_lock;
+
+	ret = vb2_queue_init(q);
+	if (ret < 0) {
+		dev_err(pispbe->dev, "vb2_queue_init failed\n");
+		goto err_mutex_destroy;
+	}
+
+	*vdev = pispbe_videodev; /* default initialization */
+	strscpy(vdev->name, node_desc[id].ent_name, sizeof(vdev->name));
+	vdev->v4l2_dev = &pispbe->v4l2_dev;
+	vdev->vfl_dir = output ? VFL_DIR_TX : VFL_DIR_RX;
+	/* get V4L2 to serialise our ioctls */
+	vdev->lock = &node->node_lock;
+	vdev->queue = &node->queue;
+	vdev->device_caps = V4L2_CAP_STREAMING | node_desc[id].caps;
+
+	node->pad.flags = output ? MEDIA_PAD_FL_SOURCE : MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(entity, 1, &node->pad);
+	if (ret) {
+		dev_err(pispbe->dev,
+			"Failed to register media pads for %s device node\n",
+			NODE_NAME(node));
+		goto err_unregister_queue;
+	}
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		dev_err(pispbe->dev,
+			"Failed to register video %s device node\n",
+			NODE_NAME(node));
+		goto err_unregister_queue;
+	}
+	video_set_drvdata(vdev, node);
+
+	if (output)
+		ret = media_create_pad_link(entity, 0, &pispbe->sd.entity,
+					    id, MEDIA_LNK_FL_IMMUTABLE |
+					    MEDIA_LNK_FL_ENABLED);
+	else
+		ret = media_create_pad_link(&pispbe->sd.entity, id, entity,
+					    0, MEDIA_LNK_FL_IMMUTABLE |
+					    MEDIA_LNK_FL_ENABLED);
+	if (ret)
+		goto err_unregister_video_dev;
+
+	dev_dbg(pispbe->dev, "%s device node registered as /dev/video%d\n",
+		NODE_NAME(node), node->vfd.num);
+
+	return 0;
+
+err_unregister_video_dev:
+	video_unregister_device(&node->vfd);
+err_unregister_queue:
+	vb2_queue_release(&node->queue);
+err_mutex_destroy:
+	mutex_destroy(&node->node_lock);
+	mutex_destroy(&node->queue_lock);
+	return ret;
+}
+
+static const struct v4l2_subdev_pad_ops pispbe_pad_ops = {
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+static const struct v4l2_subdev_ops pispbe_sd_ops = {
+	.pad = &pispbe_pad_ops,
+};
+
+static int pispbe_init_subdev(struct pispbe_dev *pispbe)
+{
+	struct v4l2_subdev *sd = &pispbe->sd;
+	int ret;
+
+	v4l2_subdev_init(sd, &pispbe_sd_ops);
+	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
+	sd->owner = THIS_MODULE;
+	sd->dev = pispbe->dev;
+	strscpy(sd->name, PISPBE_NAME, sizeof(sd->name));
+
+	for (unsigned int i = 0; i < PISPBE_NUM_NODES; i++)
+		pispbe->pad[i].flags =
+			NODE_DESC_IS_OUTPUT(&node_desc[i]) ?
+			MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&sd->entity, PISPBE_NUM_NODES,
+				     pispbe->pad);
+	if (ret)
+		goto error;
+
+	ret = v4l2_device_register_subdev(&pispbe->v4l2_dev, sd);
+	if (ret)
+		goto error;
+
+	return 0;
+
+error:
+	media_entity_cleanup(&sd->entity);
+	return ret;
+}
+
+static int pispbe_init_devices(struct pispbe_dev *pispbe)
+{
+	struct v4l2_device *v4l2_dev;
+	struct media_device *mdev;
+	unsigned int num_regist;
+	int ret;
+
+	/* Register v4l2_device and media_device */
+	mdev = &pispbe->mdev;
+	mdev->hw_revision = pispbe->hw_version;
+	mdev->dev = pispbe->dev;
+	strscpy(mdev->model, PISPBE_NAME, sizeof(mdev->model));
+	media_device_init(mdev);
+
+	v4l2_dev = &pispbe->v4l2_dev;
+	v4l2_dev->mdev = &pispbe->mdev;
+	strscpy(v4l2_dev->name, PISPBE_NAME, sizeof(v4l2_dev->name));
+
+	ret = v4l2_device_register(pispbe->dev, v4l2_dev);
+	if (ret)
+		goto err_media_dev_cleanup;
+
+	/* Register the PISPBE subdevice. */
+	ret = pispbe_init_subdev(pispbe);
+	if (ret)
+		goto err_unregister_v4l2;
+
+	/* Create device video nodes */
+	for (num_regist = 0; num_regist < PISPBE_NUM_NODES; num_regist++) {
+		ret = pispbe_init_node(pispbe, num_regist);
+		if (ret)
+			goto err_unregister_nodes;
+	}
+
+	ret = media_device_register(mdev);
+	if (ret)
+		goto err_unregister_nodes;
+
+	pispbe->config =
+		dma_alloc_coherent(pispbe->dev,
+				   sizeof(struct pisp_be_tiles_config) *
+					PISP_BE_NUM_CONFIG_BUFFERS,
+				   &pispbe->config_dma_addr, GFP_KERNEL);
+	if (!pispbe->config) {
+		dev_err(pispbe->dev, "Unable to allocate cached config buffers.\n");
+		ret = -ENOMEM;
+		goto err_unregister_mdev;
+	}
+
+	return 0;
+
+err_unregister_mdev:
+	media_device_unregister(mdev);
+err_unregister_nodes:
+	while (num_regist-- > 0) {
+		video_unregister_device(&pispbe->node[num_regist].vfd);
+		vb2_queue_release(&pispbe->node[num_regist].queue);
+	}
+	v4l2_device_unregister_subdev(&pispbe->sd);
+	media_entity_cleanup(&pispbe->sd.entity);
+err_unregister_v4l2:
+	v4l2_device_unregister(v4l2_dev);
+err_media_dev_cleanup:
+	media_device_cleanup(mdev);
+	return ret;
+}
+
+static void pispbe_destroy_devices(struct pispbe_dev *pispbe)
+{
+	if (pispbe->config) {
+		dma_free_coherent(pispbe->dev,
+				  sizeof(struct pisp_be_tiles_config) *
+					PISP_BE_NUM_CONFIG_BUFFERS,
+				  pispbe->config,
+				  pispbe->config_dma_addr);
+	}
+
+	dev_dbg(pispbe->dev, "Unregister from media controller\n");
+
+	v4l2_device_unregister_subdev(&pispbe->sd);
+	media_entity_cleanup(&pispbe->sd.entity);
+	media_device_unregister(&pispbe->mdev);
+
+	for (int i = PISPBE_NUM_NODES - 1; i >= 0; i--) {
+		video_unregister_device(&pispbe->node[i].vfd);
+		vb2_queue_release(&pispbe->node[i].queue);
+		mutex_destroy(&pispbe->node[i].node_lock);
+		mutex_destroy(&pispbe->node[i].queue_lock);
+	}
+
+	media_device_cleanup(&pispbe->mdev);
+	v4l2_device_unregister(&pispbe->v4l2_dev);
+}
+
+static int pispbe_runtime_suspend(struct device *dev)
+{
+	struct pispbe_dev *pispbe = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(pispbe->clk);
+
+	return 0;
+}
+
+static int pispbe_runtime_resume(struct device *dev)
+{
+	struct pispbe_dev *pispbe = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(pispbe->clk);
+	if (ret) {
+		dev_err(dev, "Unable to enable clock\n");
+		return ret;
+	}
+
+	dev_dbg(dev, "%s: Enabled clock, rate=%lu\n",
+		__func__, clk_get_rate(pispbe->clk));
+
+	return 0;
+}
+
+static int pispbe_hw_init(struct pispbe_dev *pispbe)
+{
+	u32 u;
+
+	/* Check the HW is present and has a known version */
+	u = pispbe_rd(pispbe, PISP_BE_VERSION_REG);
+	dev_dbg(pispbe->dev, "pispbe_probe: HW version:  0x%08x", u);
+	pispbe->hw_version = u;
+	if ((u & ~PISP_BE_VERSION_MINOR_BITS) != PISP_BE_VERSION_2712)
+		return -ENODEV;
+
+	/* Clear leftover interrupts */
+	pispbe_wr(pispbe, PISP_BE_INTERRUPT_STATUS_REG, 0xFFFFFFFFu);
+	u = pispbe_rd(pispbe, PISP_BE_BATCH_STATUS_REG);
+	dev_dbg(pispbe->dev, "pispbe_probe: BatchStatus: 0x%08x", u);
+
+	pispbe->done = (uint8_t)u;
+	pispbe->started = (uint8_t)(u >> 8);
+	u = pispbe_rd(pispbe, PISP_BE_STATUS_REG);
+	dev_dbg(pispbe->dev, "pispbe_probe: Status:      0x%08x", u);
+
+	if (u != 0 || pispbe->done != pispbe->started) {
+		dev_err(pispbe->dev, "pispbe_probe: HW is stuck or busy\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * AXI QOS=0, CACHE=4'b0010, PROT=3'b011
+	 * Also set "chicken bits" 22:20 which enable sub-64-byte bursts
+	 * and AXI AWID/BID variability (on versions which support this).
+	 */
+	pispbe_wr(pispbe, PISP_BE_AXI_REG, 0x32703200u);
+
+	/* Enable both interrupt flags */
+	pispbe_wr(pispbe, PISP_BE_INTERRUPT_EN_REG, 0x00000003u);
+
+	return 0;
+}
+
+/* Probe the ISP-BE hardware block, as a single platform device. */
+static int pispbe_probe(struct platform_device *pdev)
+{
+	struct pispbe_dev *pispbe;
+	int ret;
+
+	pispbe = devm_kzalloc(&pdev->dev, sizeof(*pispbe), GFP_KERNEL);
+	if (!pispbe)
+		return -ENOMEM;
+
+	dev_set_drvdata(&pdev->dev, pispbe);
+	pispbe->dev = &pdev->dev;
+	platform_set_drvdata(pdev, pispbe);
+
+	pispbe->be_reg_base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(pispbe->be_reg_base)) {
+		dev_err(&pdev->dev, "Failed to get ISP-BE registers address\n");
+		return PTR_ERR(pispbe->be_reg_base);
+	}
+
+	pispbe->irq = platform_get_irq(pdev, 0);
+	if (pispbe->irq <= 0)
+		return -EINVAL;
+
+	ret = devm_request_irq(&pdev->dev, pispbe->irq, pispbe_isr, 0,
+			       PISPBE_NAME, pispbe);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request interrupt\n");
+		return ret;
+	}
+
+	ret = dma_set_mask_and_coherent(pispbe->dev, DMA_BIT_MASK(36));
+	if (ret)
+		return ret;
+
+	pispbe->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(pispbe->clk))
+		return dev_err_probe(&pdev->dev, PTR_ERR(pispbe->clk),
+				     "Failed to get clock");
+
+	/* Hardware initialisation */
+	pm_runtime_set_autosuspend_delay(pispbe->dev, 200);
+	pm_runtime_use_autosuspend(pispbe->dev);
+	pm_runtime_enable(pispbe->dev);
+
+	ret = pispbe_runtime_resume(pispbe->dev);
+	if (ret)
+		goto pm_runtime_disable_err;
+
+	pispbe->hw_busy = false;
+	spin_lock_init(&pispbe->hw_lock);
+	ret = pispbe_hw_init(pispbe);
+	if (ret)
+		goto pm_runtime_suspend_err;
+
+	ret = pispbe_init_devices(pispbe);
+	if (ret)
+		goto disable_devs_err;
+
+	pm_runtime_mark_last_busy(pispbe->dev);
+	pm_runtime_put_autosuspend(pispbe->dev);
+
+	return 0;
+
+disable_devs_err:
+	pispbe_destroy_devices(pispbe);
+pm_runtime_suspend_err:
+	pispbe_runtime_suspend(pispbe->dev);
+pm_runtime_disable_err:
+	pm_runtime_dont_use_autosuspend(pispbe->dev);
+	pm_runtime_disable(pispbe->dev);
+
+	return ret;
+}
+
+static void pispbe_remove(struct platform_device *pdev)
+{
+	struct pispbe_dev *pispbe = platform_get_drvdata(pdev);
+
+	pispbe_destroy_devices(pispbe);
+
+	pispbe_runtime_suspend(pispbe->dev);
+	pm_runtime_dont_use_autosuspend(pispbe->dev);
+	pm_runtime_disable(pispbe->dev);
+}
+
+static const struct dev_pm_ops pispbe_pm_ops = {
+	SET_RUNTIME_PM_OPS(pispbe_runtime_suspend, pispbe_runtime_resume, NULL)
+};
+
+static const struct of_device_id pispbe_of_match[] = {
+	{
+		.compatible = "raspberrypi,pispbe",
+	},
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, pispbe_of_match);
+
+static struct platform_driver pispbe_pdrv = {
+	.probe		= pispbe_probe,
+	.remove		= pispbe_remove,
+	.driver		= {
+		.name	= PISPBE_NAME,
+		.of_match_table = pispbe_of_match,
+		.pm = &pispbe_pm_ops,
+	},
+};
+
+module_platform_driver(pispbe_pdrv);
+
+MODULE_DESCRIPTION("PiSP Back End driver");
+MODULE_AUTHOR("David Plowman <david.plowman@raspberrypi.com>");
+MODULE_AUTHOR("Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/raspberrypi/pisp_be/pisp_be_formats.h b/drivers/media/platform/raspberrypi/pisp_be/pisp_be_formats.h
new file mode 100644
index 000000000000..b5cb7b8c7531
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/pisp_be/pisp_be_formats.h
@@ -0,0 +1,519 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * PiSP Back End driver image format definitions.
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd
+ */
+
+#ifndef _PISP_BE_FORMATS_
+#define _PISP_BE_FORMATS_
+
+#include <linux/bits.h>
+#include <linux/videodev2.h>
+
+#define PISPBE_MAX_PLANES	3
+#define P3(x)			((x) * 8)
+
+struct pisp_be_format {
+	unsigned int fourcc;
+	unsigned int align;
+	unsigned int bit_depth;
+	/* 0P3 factor for plane sizing */
+	unsigned int plane_factor[PISPBE_MAX_PLANES];
+	unsigned int num_planes;
+	unsigned int colorspace_mask;
+	enum v4l2_colorspace colorspace_default;
+};
+
+#define V4L2_COLORSPACE_MASK(colorspace) BIT(colorspace)
+
+#define V4L2_COLORSPACE_MASK_JPEG	\
+	V4L2_COLORSPACE_MASK(V4L2_COLORSPACE_JPEG)
+#define V4L2_COLORSPACE_MASK_SMPTE170M	\
+	V4L2_COLORSPACE_MASK(V4L2_COLORSPACE_SMPTE170M)
+#define V4L2_COLORSPACE_MASK_REC709	\
+	V4L2_COLORSPACE_MASK(V4L2_COLORSPACE_REC709)
+#define V4L2_COLORSPACE_MASK_SRGB	\
+	V4L2_COLORSPACE_MASK(V4L2_COLORSPACE_SRGB)
+#define V4L2_COLORSPACE_MASK_RAW	\
+	V4L2_COLORSPACE_MASK(V4L2_COLORSPACE_RAW)
+
+/*
+ * All three colour spaces SRGB, SMPTE170M and REC709 are fundamentally sRGB
+ * underneath (as near as makes no difference to us), just with different YCbCr
+ * encodings. Therefore the ISP can generate sRGB on its main output and any of
+ * the others on its low resolution output. Applications should, when using both
+ * outputs, program the colour spaces on them to be the same, matching whatever
+ * is requested for the low resolution output, even if the main output is
+ * producing an RGB format. In turn this requires us to allow all these colour
+ * spaces for every YUV/RGB output format.
+ */
+#define V4L2_COLORSPACE_MASK_ALL_SRGB (V4L2_COLORSPACE_MASK_JPEG	| \
+				       V4L2_COLORSPACE_MASK_SRGB	| \
+				       V4L2_COLORSPACE_MASK_SMPTE170M	| \
+				       V4L2_COLORSPACE_MASK_REC709)
+
+static const struct pisp_be_format supported_formats[] = {
+	/* Single plane YUV formats */
+	{
+		.fourcc		    = V4L2_PIX_FMT_YUV420,
+		/* 128 alignment to ensure U/V planes are 64 byte aligned. */
+		.align		    = 128,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.25), P3(0.25) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YVU420,
+		/* 128 alignment to ensure U/V planes are 64 byte aligned. */
+		.align		    = 128,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.25), P3(0.25) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_NV12,
+		.align		    = 32,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_NV21,
+		.align		    = 32,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YUYV,
+		.align		    = 64,
+		.bit_depth	    = 16,
+		.plane_factor	    = { P3(1) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_UYVY,
+		.align		    = 64,
+		.bit_depth	    = 16,
+		.plane_factor	    = { P3(1) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YVYU,
+		.align		    = 64,
+		.bit_depth	    = 16,
+		.plane_factor	    = { P3(1) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_VYUY,
+		.align		    = 64,
+		.bit_depth	    = 16,
+		.plane_factor	    = { P3(1) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	/* Multiplane YUV formats */
+	{
+		.fourcc		    = V4L2_PIX_FMT_YUV420M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.25), P3(0.25) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_NV12M,
+		.align		    = 32,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5) },
+		.num_planes	    = 2,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_NV21M,
+		.align		    = 32,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5) },
+		.num_planes	    = 2,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YVU420M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.25), P3(0.25) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YUV422M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5), P3(0.5) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YVU422M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(0.5), P3(0.5) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YUV444M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(1), P3(1) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_YVU444M,
+		.align		    = 64,
+		.bit_depth	    = 8,
+		.plane_factor	    = { P3(1), P3(1), P3(1) },
+		.num_planes	    = 3,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SMPTE170M,
+	},
+	/* RGB formats */
+	{
+		.fourcc		    = V4L2_PIX_FMT_RGB24,
+		.align		    = 32,
+		.bit_depth	    = 24,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_BGR24,
+		.align		    = 32,
+		.bit_depth	    = 24,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_XBGR32,
+		.align		    = 64,
+		.bit_depth	    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_RGBX32,
+		.align		    = 64,
+		.bit_depth	    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_RGB48,
+		.align		    = 64,
+		.bit_depth	    = 48,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_BGR48,
+		.align		    = 64,
+		.bit_depth	    = 48,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_ALL_SRGB,
+		.colorspace_default = V4L2_COLORSPACE_SRGB,
+	},
+	/* Bayer formats - 8-bit */
+	{
+		.fourcc		    = V4L2_PIX_FMT_SRGGB8,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SBGGR8,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGRBG8,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGBRG8,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	/* Bayer formats - 16-bit */
+	{
+		.fourcc		    = V4L2_PIX_FMT_SRGGB16,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SBGGR16,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGRBG16,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGBRG16,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		/* Bayer formats unpacked to 16bpp */
+		/* 10 bit */
+		.fourcc		    = V4L2_PIX_FMT_SRGGB10,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SBGGR10,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGRBG10,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGBRG10,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		/* 12 bit */
+		.fourcc		    = V4L2_PIX_FMT_SRGGB12,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SBGGR12,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGRBG12,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGBRG12,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		/* 14 bit */
+		.fourcc		    = V4L2_PIX_FMT_SRGGB14,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SBGGR14,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGRBG14,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_SGBRG14,
+		.bit_depth	    = 16,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	/* Bayer formats - 16-bit PiSP Compressed */
+	{
+		.fourcc		    = V4L2_PIX_FMT_PISP_COMP1_BGGR,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_PISP_COMP1_RGGB,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_PISP_COMP1_GRBG,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		    = V4L2_PIX_FMT_PISP_COMP1_GBRG,
+		.bit_depth	    = 8,
+		.align		    = 32,
+		.plane_factor	    = { P3(1.0) },
+		.num_planes	    = 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	/* Greyscale Formats */
+	{
+		.fourcc		= V4L2_PIX_FMT_GREY,
+		.bit_depth	= 8,
+		.align		= 32,
+		.num_planes	= 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_Y16,
+		.bit_depth	= 16,
+		.align		= 32,
+		.plane_factor	= { P3(1.0) },
+		.num_planes	= 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+	{
+		.fourcc		= V4L2_PIX_FMT_PISP_COMP1_MONO,
+		.bit_depth	= 8,
+		.align		= 32,
+		.plane_factor	= { P3(1.0) },
+		.num_planes	= 1,
+		.colorspace_mask    = V4L2_COLORSPACE_MASK_RAW,
+		.colorspace_default = V4L2_COLORSPACE_RAW,
+	},
+};
+
+static const struct pisp_be_format meta_out_supported_formats[] = {
+	/* Configuration buffer format. */
+	{
+		.fourcc		    = V4L2_META_FMT_RPI_BE_CFG,
+	},
+};
+
+#endif /* _PISP_BE_FORMATS_ */
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/Kconfig b/drivers/media/platform/raspberrypi/rp1-cfe/Kconfig
new file mode 100644
index 000000000000..327b61f1134b
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/Kconfig
@@ -0,0 +1,15 @@
+# RP1 V4L2 camera support
+
+config VIDEO_RP1_CFE
+	tristate "Raspberry Pi RP1 Camera Front End (CFE) video capture driver"
+	depends on VIDEO_DEV
+	depends on PM
+	select VIDEO_V4L2_SUBDEV_API
+	select MEDIA_CONTROLLER
+	select VIDEOBUF2_DMA_CONTIG
+	select V4L2_FWNODE
+	help
+	  Say Y here to enable support for the Raspberry Pi RP1 Camera Front End.
+
+	  To compile this driver as a module, choose M here. The module will be
+	  called rp1-cfe.
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/Makefile b/drivers/media/platform/raspberrypi/rp1-cfe/Makefile
new file mode 100644
index 000000000000..3f0d0fc8570e
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for RP1 Camera Front End driver
+#
+rp1-cfe-objs := cfe.o csi2.o pisp-fe.o dphy.o
+obj-$(CONFIG_VIDEO_RP1_CFE) += rp1-cfe.o
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/cfe-fmts.h b/drivers/media/platform/raspberrypi/rp1-cfe/cfe-fmts.h
new file mode 100644
index 000000000000..7aecf7f83733
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/cfe-fmts.h
@@ -0,0 +1,332 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 Camera Front End formats definition
+ *
+ * Copyright (C) 2021-2024 - Raspberry Pi Ltd.
+ */
+#ifndef _CFE_FMTS_H_
+#define _CFE_FMTS_H_
+
+#include "cfe.h"
+#include <media/mipi-csi2.h>
+
+static const struct cfe_fmt formats[] = {
+	/* YUV Formats */
+	{
+		.fourcc = V4L2_PIX_FMT_YUYV,
+		.code = MEDIA_BUS_FMT_YUYV8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_UYVY,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_YVYU,
+		.code = MEDIA_BUS_FMT_YVYU8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_VYUY,
+		.code = MEDIA_BUS_FMT_VYUY8_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_YUV422_8B,
+	},
+	{
+		/* RGB Formats */
+		.fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB565,
+	},
+	{	.fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.code = MEDIA_BUS_FMT_RGB565_2X8_BE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB565,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
+		.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB555,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
+		.code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RGB555,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB24, /* rgb */
+		.code = MEDIA_BUS_FMT_RGB888_1X24,
+		.depth = 24,
+		.csi_dt = MIPI_CSI2_DT_RGB888,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_BGR24, /* bgr */
+		.code = MEDIA_BUS_FMT_BGR888_1X24,
+		.depth = 24,
+		.csi_dt = MIPI_CSI2_DT_RGB888,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_RGB32, /* argb */
+		.code = MEDIA_BUS_FMT_ARGB8888_1X32,
+		.depth = 32,
+		.csi_dt = 0x0,
+	},
+
+	/* Bayer Formats */
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR8,
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG8,
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG8,
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB8,
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR10P,
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG10P,
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG10P,
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB10P,
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR12P,
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG12P,
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG12P,
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB12P,
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR14P,
+		.code = MEDIA_BUS_FMT_SBGGR14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG14P,
+		.code = MEDIA_BUS_FMT_SGBRG14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG14P,
+		.code = MEDIA_BUS_FMT_SGRBG14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB14P,
+		.code = MEDIA_BUS_FMT_SRGGB14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SBGGR16,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SBGGR16, V4L2_PIX_FMT_PISP_COMP1_BGGR },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGBRG16,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SGBRG16, V4L2_PIX_FMT_PISP_COMP1_GBRG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SGRBG16,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SGRBG16, V4L2_PIX_FMT_PISP_COMP1_GRBG },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_SRGGB16,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_SRGGB16, V4L2_PIX_FMT_PISP_COMP1_RGGB },
+	},
+	/* PiSP Compressed Mode 1 */
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_RGGB,
+		.code = MEDIA_BUS_FMT_SRGGB16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_BGGR,
+		.code = MEDIA_BUS_FMT_SBGGR16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_GBRG,
+		.code = MEDIA_BUS_FMT_SGBRG16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_GRBG,
+		.code = MEDIA_BUS_FMT_SGRBG16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+	/* Greyscale format */
+	{
+		.fourcc = V4L2_PIX_FMT_GREY,
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_RAW8,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y10P,
+		.code = MEDIA_BUS_FMT_Y10_1X10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_RAW10,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y12P,
+		.code = MEDIA_BUS_FMT_Y12_1X12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_RAW12,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y14P,
+		.code = MEDIA_BUS_FMT_Y14_1X14,
+		.depth = 14,
+		.csi_dt = MIPI_CSI2_DT_RAW14,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_Y16,
+		.code = MEDIA_BUS_FMT_Y16_1X16,
+		.depth = 16,
+		.csi_dt = MIPI_CSI2_DT_RAW16,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+		.remap = { V4L2_PIX_FMT_Y16, V4L2_PIX_FMT_PISP_COMP1_MONO },
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_PISP_COMP1_MONO,
+		.code = MEDIA_BUS_FMT_Y16_1X16,
+		.depth = 8,
+		.flags = CFE_FORMAT_FLAG_FE_OUT,
+	},
+
+	/* Embedded data formats */
+	{
+		.fourcc = V4L2_META_FMT_GENERIC_8,
+		.code = MEDIA_BUS_FMT_META_8,
+		.depth = 8,
+		.csi_dt = MIPI_CSI2_DT_EMBEDDED_8B,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+	{
+		.fourcc = V4L2_META_FMT_GENERIC_CSI2_10,
+		.code = MEDIA_BUS_FMT_META_10,
+		.depth = 10,
+		.csi_dt = MIPI_CSI2_DT_EMBEDDED_8B,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+	{
+		.fourcc = V4L2_META_FMT_GENERIC_CSI2_12,
+		.code = MEDIA_BUS_FMT_META_12,
+		.depth = 12,
+		.csi_dt = MIPI_CSI2_DT_EMBEDDED_8B,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+
+	/* Frontend formats */
+	{
+		.fourcc = V4L2_META_FMT_RPI_FE_CFG,
+		.code = MEDIA_BUS_FMT_FIXED,
+		.flags = CFE_FORMAT_FLAG_META_OUT,
+	},
+	{
+		.fourcc = V4L2_META_FMT_RPI_FE_STATS,
+		.code = MEDIA_BUS_FMT_FIXED,
+		.flags = CFE_FORMAT_FLAG_META_CAP,
+	},
+};
+
+#endif /* _CFE_FMTS_H_ */
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/cfe-trace.h b/drivers/media/platform/raspberrypi/rp1-cfe/cfe-trace.h
new file mode 100644
index 000000000000..1a36259f51b7
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/cfe-trace.h
@@ -0,0 +1,202 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2024 Raspberry Pi Ltd.
+ * Copyright (c) 2024 Ideas on Board Oy
+ */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM cfe
+
+#if !defined(_CFE_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _CFE_TRACE_H
+
+#include <linux/tracepoint.h>
+#include <media/videobuf2-v4l2.h>
+
+TRACE_EVENT(cfe_return_buffer,
+	TP_PROTO(u32 node_id, u32 buf_idx, u32 queue_id),
+	TP_ARGS(node_id, buf_idx, queue_id),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, buf_idx)
+		__field(u32, queue_id)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->buf_idx = buf_idx;
+		__entry->queue_id = queue_id;
+	),
+	TP_printk("node=%u buf=%u, queue=%u", __entry->node_id,
+		  __entry->buf_idx, __entry->queue_id)
+);
+
+DECLARE_EVENT_CLASS(cfe_buffer_template,
+	TP_PROTO(u32 node_id, struct vb2_buffer *buf),
+	TP_ARGS(node_id, buf),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, buf_idx)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->buf_idx = buf->index;
+	),
+	TP_printk("node=%u buf=%u", __entry->node_id, __entry->buf_idx)
+);
+
+DEFINE_EVENT(cfe_buffer_template, cfe_buffer_prepare,
+	TP_PROTO(u32 node_id, struct vb2_buffer *buf),
+	TP_ARGS(node_id, buf));
+
+TRACE_EVENT(cfe_buffer_queue,
+	TP_PROTO(u32 node_id, struct vb2_buffer *buf, bool schedule_now),
+	TP_ARGS(node_id, buf, schedule_now),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, buf_idx)
+		__field(bool, schedule_now)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->buf_idx = buf->index;
+		__entry->schedule_now = schedule_now;
+	),
+	TP_printk("node=%u buf=%u%s", __entry->node_id, __entry->buf_idx,
+		  __entry->schedule_now ? " schedule immediately" : "")
+);
+
+DEFINE_EVENT(cfe_buffer_template, cfe_csi2_schedule,
+	TP_PROTO(u32 node_id, struct vb2_buffer *buf),
+	TP_ARGS(node_id, buf));
+
+DEFINE_EVENT(cfe_buffer_template, cfe_fe_schedule,
+	TP_PROTO(u32 node_id, struct vb2_buffer *buf),
+	TP_ARGS(node_id, buf));
+
+TRACE_EVENT(cfe_buffer_complete,
+	TP_PROTO(u32 node_id, struct vb2_v4l2_buffer *buf),
+	TP_ARGS(node_id, buf),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, buf_idx)
+		__field(u32, seq)
+		__field(u64, ts)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->buf_idx = buf->vb2_buf.index;
+		__entry->seq = buf->sequence;
+		__entry->ts = buf->vb2_buf.timestamp;
+	),
+	TP_printk("node=%u buf=%u seq=%u ts=%llu", __entry->node_id,
+		  __entry->buf_idx, __entry->seq, __entry->ts)
+);
+
+TRACE_EVENT(cfe_frame_start,
+	TP_PROTO(u32 node_id, u32 fs_count),
+	TP_ARGS(node_id, fs_count),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, fs_count)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->fs_count = fs_count;
+	),
+	TP_printk("node=%u fs_count=%u", __entry->node_id, __entry->fs_count)
+);
+
+TRACE_EVENT(cfe_frame_end,
+	TP_PROTO(u32 node_id, u32 fs_count),
+	TP_ARGS(node_id, fs_count),
+	TP_STRUCT__entry(
+		__field(u32, node_id)
+		__field(u32, fs_count)
+	),
+	TP_fast_assign(
+		__entry->node_id = node_id;
+		__entry->fs_count = fs_count;
+	),
+	TP_printk("node=%u fs_count=%u", __entry->node_id, __entry->fs_count)
+);
+
+TRACE_EVENT(cfe_prepare_next_job,
+	TP_PROTO(bool fe_enabled),
+	TP_ARGS(fe_enabled),
+	TP_STRUCT__entry(
+		__field(bool, fe_enabled)
+	),
+	TP_fast_assign(
+		__entry->fe_enabled = fe_enabled;
+	),
+	TP_printk("fe_enabled=%u", __entry->fe_enabled)
+);
+
+/* These are copied from csi2.c */
+#define CSI2_STATUS_IRQ_FS(x)			(BIT(0) << (x))
+#define CSI2_STATUS_IRQ_FE(x)			(BIT(4) << (x))
+#define CSI2_STATUS_IRQ_FE_ACK(x)		(BIT(8) << (x))
+#define CSI2_STATUS_IRQ_LE(x)			(BIT(12) << (x))
+#define CSI2_STATUS_IRQ_LE_ACK(x)		(BIT(16) << (x))
+
+TRACE_EVENT(csi2_irq,
+	TP_PROTO(u32 channel, u32 status, u32 dbg),
+	TP_ARGS(channel, status, dbg),
+	TP_STRUCT__entry(
+		__field(u32, channel)
+		__field(u32, status)
+		__field(u32, dbg)
+	),
+	TP_fast_assign(
+		__entry->channel = channel;
+		__entry->status = status;
+		__entry->dbg = dbg;
+	),
+	TP_printk("ch=%u flags=[ %s%s%s%s%s] frame=%u line=%u\n",
+		  __entry->channel,
+		  (__entry->status & CSI2_STATUS_IRQ_FS(__entry->channel)) ?
+			"FS " : "",
+		  (__entry->status & CSI2_STATUS_IRQ_FE(__entry->channel)) ?
+			"FE " : "",
+		  (__entry->status & CSI2_STATUS_IRQ_FE_ACK(__entry->channel)) ?
+			"FE_ACK " : "",
+		  (__entry->status & CSI2_STATUS_IRQ_LE(__entry->channel)) ?
+			"LE " : "",
+		  (__entry->status & CSI2_STATUS_IRQ_LE_ACK(__entry->channel)) ?
+			"LE_ACK " : "",
+		  __entry->dbg >> 16, __entry->dbg & 0xffff)
+);
+
+TRACE_EVENT(fe_irq,
+	TP_PROTO(u32 status, u32 output_status, u32 frame_status,
+		 u32 error_status, u32 int_status),
+	TP_ARGS(status, output_status, frame_status, error_status, int_status),
+	TP_STRUCT__entry(
+		__field(u32, status)
+		__field(u32, output_status)
+		__field(u32, frame_status)
+		__field(u32, error_status)
+		__field(u32, int_status)
+	),
+	TP_fast_assign(
+		__entry->status = status;
+		__entry->output_status = output_status;
+		__entry->frame_status = frame_status;
+		__entry->error_status = error_status;
+		__entry->int_status = int_status;
+	),
+	TP_printk("status 0x%x out_status 0x%x frame_status 0x%x error_status 0x%x int_status 0x%x",
+		  __entry->status,
+		  __entry->output_status,
+		  __entry->frame_status,
+		  __entry->error_status,
+		  __entry->int_status)
+);
+
+#endif /* _CFE_TRACE_H */
+
+/* This part must be outside protection */
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#define TRACE_INCLUDE_FILE ../../drivers/media/platform/raspberrypi/rp1-cfe/cfe-trace
+#include <trace/define_trace.h>
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/cfe.c b/drivers/media/platform/raspberrypi/rp1-cfe/cfe.c
new file mode 100644
index 000000000000..fcadb2143c88
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/cfe.c
@@ -0,0 +1,2508 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 Camera Front End Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/lcm.h>
+#include <linux/math.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/property.h>
+#include <linux/seq_file.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-async.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mc.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include <linux/media/raspberrypi/pisp_fe_config.h>
+#include <linux/media/raspberrypi/pisp_fe_statistics.h>
+
+#include "cfe-fmts.h"
+#include "cfe.h"
+#include "csi2.h"
+#include "pisp-fe.h"
+
+#define CREATE_TRACE_POINTS
+#include "cfe-trace.h"
+
+#define CFE_MODULE_NAME	"rp1-cfe"
+#define CFE_VERSION	"1.0"
+
+#define cfe_dbg(cfe, fmt, arg...) dev_dbg(&(cfe)->pdev->dev, fmt, ##arg)
+#define cfe_info(cfe, fmt, arg...) dev_info(&(cfe)->pdev->dev, fmt, ##arg)
+#define cfe_err(cfe, fmt, arg...) dev_err(&(cfe)->pdev->dev, fmt, ##arg)
+
+/* MIPICFG registers */
+#define MIPICFG_CFG		0x004
+#define MIPICFG_INTR		0x028
+#define MIPICFG_INTE		0x02c
+#define MIPICFG_INTF		0x030
+#define MIPICFG_INTS		0x034
+
+#define MIPICFG_CFG_SEL_CSI	BIT(0)
+
+#define MIPICFG_INT_CSI_DMA	BIT(0)
+#define MIPICFG_INT_CSI_HOST	BIT(2)
+#define MIPICFG_INT_PISP_FE	BIT(4)
+
+#define BPL_ALIGNMENT 16
+#define MAX_BYTESPERLINE 0xffffff00
+#define MAX_BUFFER_SIZE  0xffffff00
+/*
+ * Max width is therefore determined by the max stride divided by the number of
+ * bits per pixel.
+ *
+ * However, to avoid overflow issues let's use a 16k maximum. This lets us
+ * calculate 16k * 16k * 4 with 32bits. If we need higher maximums, a careful
+ * review and adjustment of the code is needed so that it will deal with
+ * overflows correctly.
+ */
+#define MAX_WIDTH 16384
+#define MAX_HEIGHT MAX_WIDTH
+/* Define a nominal minimum image size */
+#define MIN_WIDTH 16
+#define MIN_HEIGHT 16
+
+#define MIN_META_WIDTH 4
+#define MIN_META_HEIGHT 1
+
+const struct v4l2_mbus_framefmt cfe_default_format = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_RAW,
+	.ycbcr_enc = V4L2_YCBCR_ENC_601,
+	.quantization = V4L2_QUANTIZATION_FULL_RANGE,
+	.xfer_func = V4L2_XFER_FUNC_NONE,
+};
+
+enum node_ids {
+	/* CSI2 HW output nodes first. */
+	CSI2_CH0,
+	CSI2_CH1,
+	CSI2_CH2,
+	CSI2_CH3,
+	/* FE only nodes from here on. */
+	FE_OUT0,
+	FE_OUT1,
+	FE_STATS,
+	FE_CONFIG,
+	NUM_NODES
+};
+
+struct node_description {
+	enum node_ids id;
+	const char *name;
+	unsigned int caps;
+	unsigned int pad_flags;
+	unsigned int link_pad;
+};
+
+/* Must match the ordering of enum ids */
+static const struct node_description node_desc[NUM_NODES] = {
+	[CSI2_CH0] = {
+		.name = "csi2-ch0",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_PAD_FIRST_SOURCE + 0
+	},
+	/*
+	 * At the moment the main userspace component (libcamera) doesn't
+	 * support metadata with video nodes that support both video and
+	 * metadata. So for the time being this node is set to only support
+	 * V4L2_CAP_META_CAPTURE.
+	 */
+	[CSI2_CH1] = {
+		.name = "csi2-ch1",
+		.caps = V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_PAD_FIRST_SOURCE + 1
+	},
+	[CSI2_CH2] = {
+		.name = "csi2-ch2",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_PAD_FIRST_SOURCE + 2
+	},
+	[CSI2_CH3] = {
+		.name = "csi2-ch3",
+		.caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = CSI2_PAD_FIRST_SOURCE + 3
+	},
+	[FE_OUT0] = {
+		.name = "fe-image0",
+		.caps = V4L2_CAP_VIDEO_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_OUTPUT0_PAD
+	},
+	[FE_OUT1] = {
+		.name = "fe-image1",
+		.caps = V4L2_CAP_VIDEO_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_OUTPUT1_PAD
+	},
+	[FE_STATS] = {
+		.name = "fe-stats",
+		.caps = V4L2_CAP_META_CAPTURE,
+		.pad_flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_STATS_PAD
+	},
+	[FE_CONFIG] = {
+		.name = "fe-config",
+		.caps = V4L2_CAP_META_OUTPUT,
+		.pad_flags = MEDIA_PAD_FL_SOURCE | MEDIA_PAD_FL_MUST_CONNECT,
+		.link_pad = FE_CONFIG_PAD
+	},
+};
+
+#define is_fe_node(node) (((node)->id) >= FE_OUT0)
+#define is_csi2_node(node) (!is_fe_node(node))
+
+#define node_supports_image_output(node) \
+	(node_desc[(node)->id].caps & V4L2_CAP_VIDEO_CAPTURE)
+#define node_supports_meta_output(node) \
+	(node_desc[(node)->id].caps & V4L2_CAP_META_CAPTURE)
+#define node_supports_image_input(node) \
+	(node_desc[(node)->id].caps & V4L2_CAP_VIDEO_OUTPUT)
+#define node_supports_meta_input(node) \
+	(node_desc[(node)->id].caps & V4L2_CAP_META_OUTPUT)
+#define node_supports_image(node) \
+	(node_supports_image_output(node) || node_supports_image_input(node))
+#define node_supports_meta(node) \
+	(node_supports_meta_output(node) || node_supports_meta_input(node))
+
+#define is_image_output_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+#define is_image_input_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+#define is_image_node(node) \
+	(is_image_output_node(node) || is_image_input_node(node))
+#define is_meta_output_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_META_CAPTURE)
+#define is_meta_input_node(node) \
+	((node)->buffer_queue.type == V4L2_BUF_TYPE_META_OUTPUT)
+#define is_meta_node(node) \
+	(is_meta_output_node(node) || is_meta_input_node(node))
+
+/* To track state across all nodes. */
+#define NODE_REGISTERED		BIT(0)
+#define NODE_ENABLED		BIT(1)
+#define NODE_STREAMING		BIT(2)
+#define FS_INT			BIT(3)
+#define FE_INT			BIT(4)
+#define NUM_STATES		5
+
+struct cfe_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+};
+
+struct cfe_config_buffer {
+	struct cfe_buffer buf;
+	struct pisp_fe_config config;
+};
+
+static inline struct cfe_buffer *to_cfe_buffer(struct vb2_buffer *vb)
+{
+	return container_of(vb, struct cfe_buffer, vb.vb2_buf);
+}
+
+static inline
+struct cfe_config_buffer *to_cfe_config_buffer(struct cfe_buffer *buf)
+{
+	return container_of(buf, struct cfe_config_buffer, buf);
+}
+
+struct cfe_node {
+	/* Node id */
+	enum node_ids id;
+	/* Pointer pointing to current v4l2_buffer */
+	struct cfe_buffer *cur_frm;
+	/* Pointer pointing to next v4l2_buffer */
+	struct cfe_buffer *next_frm;
+	/* Used to store current pixel format */
+	struct v4l2_format vid_fmt;
+	/* Used to store current meta format */
+	struct v4l2_format meta_fmt;
+	/* Buffer queue used in video-buf */
+	struct vb2_queue buffer_queue;
+	/* Queue of filled frames */
+	struct list_head dma_queue;
+	/* lock used to access this structure */
+	struct mutex lock;
+	/* Identifies video device for this channel */
+	struct video_device video_dev;
+	/* Pointer to the parent handle */
+	struct cfe_device *cfe;
+	/* Media pad for this node */
+	struct media_pad pad;
+	/* Frame-start counter */
+	unsigned int fs_count;
+	/* Timestamp of the current buffer */
+	u64 ts;
+};
+
+struct cfe_device {
+	struct dentry *debugfs;
+	struct kref kref;
+
+	/* peripheral base address */
+	void __iomem *mipi_cfg_base;
+
+	struct clk *clk;
+
+	/* V4l2 device */
+	struct v4l2_device v4l2_dev;
+	struct media_device mdev;
+	struct media_pipeline pipe;
+
+	/* IRQ lock for node state and DMA queues */
+	spinlock_t state_lock;
+	bool job_ready;
+	bool job_queued;
+
+	/* parent device */
+	struct platform_device *pdev;
+	/* subdevice async Notifier */
+	struct v4l2_async_notifier notifier;
+
+	/* Source sub device */
+	struct v4l2_subdev *source_sd;
+	/* Source subdev's pad */
+	u32 source_pad;
+
+	struct cfe_node node[NUM_NODES];
+	DECLARE_BITMAP(node_flags, NUM_STATES * NUM_NODES);
+
+	struct csi2_device csi2;
+	struct pisp_fe_device fe;
+
+	int fe_csi2_channel;
+
+	/* Mask of enabled streams */
+	u64 streams_mask;
+};
+
+static inline bool is_fe_enabled(struct cfe_device *cfe)
+{
+	return cfe->fe_csi2_channel != -1;
+}
+
+static inline struct cfe_device *to_cfe_device(struct v4l2_device *v4l2_dev)
+{
+	return container_of(v4l2_dev, struct cfe_device, v4l2_dev);
+}
+
+static inline u32 cfg_reg_read(struct cfe_device *cfe, u32 offset)
+{
+	return readl(cfe->mipi_cfg_base + offset);
+}
+
+static inline void cfg_reg_write(struct cfe_device *cfe, u32 offset, u32 val)
+{
+	writel(val, cfe->mipi_cfg_base + offset);
+}
+
+static bool check_state(struct cfe_device *cfe, unsigned long state,
+			unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state)) {
+		if (!test_bit(bit + (node_id * NUM_STATES), cfe->node_flags))
+			return false;
+	}
+
+	return true;
+}
+
+static void set_state(struct cfe_device *cfe, unsigned long state,
+		      unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state))
+		set_bit(bit + (node_id * NUM_STATES), cfe->node_flags);
+}
+
+static void clear_state(struct cfe_device *cfe, unsigned long state,
+			unsigned int node_id)
+{
+	unsigned long bit;
+
+	for_each_set_bit(bit, &state, sizeof(state))
+		clear_bit(bit + (node_id * NUM_STATES), cfe->node_flags);
+}
+
+static bool test_any_node(struct cfe_device *cfe, unsigned long cond)
+{
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		if (check_state(cfe, cond, i))
+			return true;
+	}
+
+	return false;
+}
+
+static bool test_all_nodes(struct cfe_device *cfe, unsigned long precond,
+			   unsigned long cond)
+{
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		if (check_state(cfe, precond, i)) {
+			if (!check_state(cfe, cond, i))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+static int mipi_cfg_regs_show(struct seq_file *s, void *data)
+{
+	struct cfe_device *cfe = s->private;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret)
+		return ret;
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", cfg_reg_read(cfe, reg))
+	DUMP(MIPICFG_CFG);
+	DUMP(MIPICFG_INTR);
+	DUMP(MIPICFG_INTE);
+	DUMP(MIPICFG_INTF);
+	DUMP(MIPICFG_INTS);
+#undef DUMP
+
+	pm_runtime_put(&cfe->pdev->dev);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(mipi_cfg_regs);
+
+/* Format setup functions */
+const struct cfe_fmt *find_format_by_code(u32 code)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].code == code)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+const struct cfe_fmt *find_format_by_pix(u32 pixelformat)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].fourcc == pixelformat)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+static const struct cfe_fmt *find_format_by_code_and_fourcc(u32 code,
+							    u32 fourcc)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(formats); i++) {
+		if (formats[i].code == code && formats[i].fourcc == fourcc)
+			return &formats[i];
+	}
+
+	return NULL;
+}
+
+/*
+ * Given the mbus code, find the 16 bit remapped code. Returns 0 if no remap
+ * possible.
+ */
+u32 cfe_find_16bit_code(u32 code)
+{
+	const struct cfe_fmt *cfe_fmt;
+
+	cfe_fmt = find_format_by_code(code);
+
+	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_16BIT])
+		return 0;
+
+	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_16BIT]);
+	if (!cfe_fmt)
+		return 0;
+
+	return cfe_fmt->code;
+}
+
+/*
+ * Given the mbus code, find the 8 bit compressed code. Returns 0 if no remap
+ * possible.
+ */
+u32 cfe_find_compressed_code(u32 code)
+{
+	const struct cfe_fmt *cfe_fmt;
+
+	cfe_fmt = find_format_by_code(code);
+
+	if (!cfe_fmt || !cfe_fmt->remap[CFE_REMAP_COMPRESSED])
+		return 0;
+
+	cfe_fmt = find_format_by_pix(cfe_fmt->remap[CFE_REMAP_COMPRESSED]);
+	if (!cfe_fmt)
+		return 0;
+
+	return cfe_fmt->code;
+}
+
+static void cfe_calc_vid_format_size_bpl(struct cfe_device *cfe,
+					 const struct cfe_fmt *fmt,
+					 struct v4l2_format *f)
+{
+	unsigned int min_bytesperline;
+
+	v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, MAX_WIDTH, 2,
+			      &f->fmt.pix.height, MIN_HEIGHT, MAX_HEIGHT, 0, 0);
+
+	min_bytesperline =
+		ALIGN((f->fmt.pix.width * fmt->depth) >> 3, BPL_ALIGNMENT);
+
+	if (f->fmt.pix.bytesperline > min_bytesperline &&
+	    f->fmt.pix.bytesperline <= MAX_BYTESPERLINE)
+		f->fmt.pix.bytesperline =
+			ALIGN(f->fmt.pix.bytesperline, BPL_ALIGNMENT);
+	else
+		f->fmt.pix.bytesperline = min_bytesperline;
+
+	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+
+	cfe_dbg(cfe, "%s: %p4cc size: %ux%u bpl:%u img_size:%u\n", __func__,
+		&f->fmt.pix.pixelformat, f->fmt.pix.width, f->fmt.pix.height,
+		f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
+}
+
+static void cfe_calc_meta_format_size_bpl(struct cfe_device *cfe,
+					  const struct cfe_fmt *fmt,
+					  struct v4l2_format *f)
+{
+	v4l_bound_align_image(&f->fmt.meta.width, MIN_META_WIDTH, MAX_WIDTH, 2,
+			      &f->fmt.meta.height, MIN_META_HEIGHT, MAX_HEIGHT,
+			      0, 0);
+
+	f->fmt.meta.bytesperline = (f->fmt.meta.width * fmt->depth) >> 3;
+	f->fmt.meta.buffersize = f->fmt.meta.height * f->fmt.pix.bytesperline;
+
+	cfe_dbg(cfe, "%s: %p4cc size: %ux%u bpl:%u buf_size:%u\n", __func__,
+		&f->fmt.meta.dataformat, f->fmt.meta.width, f->fmt.meta.height,
+		f->fmt.meta.bytesperline, f->fmt.meta.buffersize);
+}
+
+static void cfe_schedule_next_csi2_job(struct cfe_device *cfe)
+{
+	struct cfe_buffer *buf;
+	dma_addr_t addr;
+
+	for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		struct cfe_node *node = &cfe->node[i];
+		unsigned int stride, size;
+
+		if (!check_state(cfe, NODE_STREAMING, i))
+			continue;
+
+		buf = list_first_entry(&node->dma_queue, struct cfe_buffer,
+				       list);
+		node->next_frm = buf;
+		list_del(&buf->list);
+
+		trace_cfe_csi2_schedule(node->id, &buf->vb.vb2_buf);
+
+		if (is_meta_node(node)) {
+			size = node->meta_fmt.fmt.meta.buffersize;
+			/* We use CSI2_CH_CTRL_PACK_BYTES, so stride == 0 */
+			stride = 0;
+		} else {
+			size = node->vid_fmt.fmt.pix.sizeimage;
+			stride = node->vid_fmt.fmt.pix.bytesperline;
+		}
+
+		addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
+		csi2_set_buffer(&cfe->csi2, node->id, addr, stride, size);
+	}
+}
+
+static void cfe_schedule_next_pisp_job(struct cfe_device *cfe)
+{
+	struct vb2_buffer *vb2_bufs[FE_NUM_PADS] = { 0 };
+	struct cfe_config_buffer *config_buf;
+	struct cfe_buffer *buf;
+
+	for (unsigned int i = CSI2_NUM_CHANNELS; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_STREAMING, i))
+			continue;
+
+		buf = list_first_entry(&node->dma_queue, struct cfe_buffer,
+				       list);
+
+		trace_cfe_fe_schedule(node->id, &buf->vb.vb2_buf);
+
+		node->next_frm = buf;
+		vb2_bufs[node_desc[i].link_pad] = &buf->vb.vb2_buf;
+		list_del(&buf->list);
+	}
+
+	config_buf = to_cfe_config_buffer(cfe->node[FE_CONFIG].next_frm);
+	pisp_fe_submit_job(&cfe->fe, vb2_bufs, &config_buf->config);
+}
+
+static bool cfe_check_job_ready(struct cfe_device *cfe)
+{
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_ENABLED, i))
+			continue;
+
+		if (list_empty(&node->dma_queue))
+			return false;
+	}
+
+	return true;
+}
+
+static void cfe_prepare_next_job(struct cfe_device *cfe)
+{
+	trace_cfe_prepare_next_job(is_fe_enabled(cfe));
+
+	cfe->job_queued = true;
+	cfe_schedule_next_csi2_job(cfe);
+	if (is_fe_enabled(cfe))
+		cfe_schedule_next_pisp_job(cfe);
+
+	/* Flag if another job is ready after this. */
+	cfe->job_ready = cfe_check_job_ready(cfe);
+}
+
+static void cfe_process_buffer_complete(struct cfe_node *node,
+					enum vb2_buffer_state state)
+{
+	trace_cfe_buffer_complete(node->id, &node->cur_frm->vb);
+
+	node->cur_frm->vb.sequence = node->fs_count - 1;
+	vb2_buffer_done(&node->cur_frm->vb.vb2_buf, state);
+}
+
+static void cfe_queue_event_sof(struct cfe_node *node)
+{
+	struct v4l2_event event = {
+		.type = V4L2_EVENT_FRAME_SYNC,
+		.u.frame_sync.frame_sequence = node->fs_count - 1,
+	};
+
+	v4l2_event_queue(&node->video_dev, &event);
+}
+
+static void cfe_sof_isr(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+	bool matching_fs = true;
+
+	trace_cfe_frame_start(node->id, node->fs_count);
+
+	/*
+	 * If the sensor is producing unexpected frame event ordering over a
+	 * sustained period of time, guard against the possibility of coming
+	 * here and orphaning the cur_frm if it's not been dequeued already.
+	 * Unfortunately, there is not enough hardware state to tell if this
+	 * may have occurred.
+	 */
+	if (WARN(node->cur_frm, "%s: [%s] Orphaned frame at seq %u\n",
+		 __func__, node_desc[node->id].name, node->fs_count))
+		cfe_process_buffer_complete(node, VB2_BUF_STATE_ERROR);
+
+	node->cur_frm = node->next_frm;
+	node->next_frm = NULL;
+	node->fs_count++;
+
+	node->ts = ktime_get_ns();
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		if (!check_state(cfe, NODE_STREAMING, i) || i == node->id)
+			continue;
+		/*
+		 * This checks if any other node has seen a FS. If yes, use the
+		 * same timestamp, eventually across all node buffers.
+		 */
+		if (cfe->node[i].fs_count >= node->fs_count)
+			node->ts = cfe->node[i].ts;
+		/*
+		 * This checks if all other node have seen a matching FS. If
+		 * yes, we can flag another job to be queued.
+		 */
+		if (matching_fs && cfe->node[i].fs_count != node->fs_count)
+			matching_fs = false;
+	}
+
+	if (matching_fs)
+		cfe->job_queued = false;
+
+	if (node->cur_frm)
+		node->cur_frm->vb.vb2_buf.timestamp = node->ts;
+
+	set_state(cfe, FS_INT, node->id);
+	clear_state(cfe, FE_INT, node->id);
+
+	if (is_image_output_node(node))
+		cfe_queue_event_sof(node);
+}
+
+static void cfe_eof_isr(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	trace_cfe_frame_end(node->id, node->fs_count - 1);
+
+	if (node->cur_frm)
+		cfe_process_buffer_complete(node, VB2_BUF_STATE_DONE);
+
+	node->cur_frm = NULL;
+	set_state(cfe, FE_INT, node->id);
+	clear_state(cfe, FS_INT, node->id);
+}
+
+static irqreturn_t cfe_isr(int irq, void *dev)
+{
+	struct cfe_device *cfe = dev;
+	bool sof[NUM_NODES] = { 0 }, eof[NUM_NODES] = { 0 };
+	u32 sts;
+
+	sts = cfg_reg_read(cfe, MIPICFG_INTS);
+
+	if (sts & MIPICFG_INT_CSI_DMA)
+		csi2_isr(&cfe->csi2, sof, eof);
+
+	if (sts & MIPICFG_INT_PISP_FE)
+		pisp_fe_isr(&cfe->fe, sof + CSI2_NUM_CHANNELS,
+			    eof + CSI2_NUM_CHANNELS);
+
+	spin_lock(&cfe->state_lock);
+
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		/*
+		 * The check_state(NODE_STREAMING) is to ensure we do not loop
+		 * over the CSI2_CHx nodes when the FE is active since they
+		 * generate interrupts even though the node is not streaming.
+		 */
+		if (!check_state(cfe, NODE_STREAMING, i) || !(sof[i] || eof[i]))
+			continue;
+
+		/*
+		 * There are 3 cases where we could get FS + FE_ACK at
+		 * the same time:
+		 * 1) FE of the current frame, and FS of the next frame.
+		 * 2) FS + FE of the same frame.
+		 * 3) FE of the current frame, and FS + FE of the next
+		 *    frame. To handle this, see the sof handler below.
+		 *
+		 * (1) is handled implicitly by the ordering of the FE and FS
+		 * handlers below.
+		 */
+		if (eof[i]) {
+			/*
+			 * The condition below tests for (2). Run the FS handler
+			 * first before the FE handler, both for the current
+			 * frame.
+			 */
+			if (sof[i] && !check_state(cfe, FS_INT, i)) {
+				cfe_sof_isr(node);
+				sof[i] = false;
+			}
+
+			cfe_eof_isr(node);
+		}
+
+		if (sof[i]) {
+			/*
+			 * The condition below tests for (3). In such cases, we
+			 * come in here with FS flag set in the node state from
+			 * the previous frame since it only gets cleared in
+			 * cfe_eof_isr(). Handle the FE for the previous
+			 * frame first before the FS handler for the current
+			 * frame.
+			 */
+			if (check_state(cfe, FS_INT, node->id) &&
+			    !check_state(cfe, FE_INT, node->id)) {
+				cfe_dbg(cfe, "%s: [%s] Handling missing previous FE interrupt\n",
+					__func__, node_desc[node->id].name);
+				cfe_eof_isr(node);
+			}
+
+			cfe_sof_isr(node);
+		}
+
+		if (!cfe->job_queued && cfe->job_ready)
+			cfe_prepare_next_job(cfe);
+	}
+
+	spin_unlock(&cfe->state_lock);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Stream helpers
+ */
+
+static int cfe_get_vc_dt_fallback(struct cfe_device *cfe, u8 *vc, u8 *dt)
+{
+	struct v4l2_subdev_state *state;
+	struct v4l2_mbus_framefmt *fmt;
+	const struct cfe_fmt *cfe_fmt;
+
+	state = v4l2_subdev_get_locked_active_state(&cfe->csi2.sd);
+
+	fmt = v4l2_subdev_state_get_format(state, CSI2_PAD_SINK, 0);
+	if (!fmt)
+		return -EINVAL;
+
+	cfe_fmt = find_format_by_code(fmt->code);
+	if (!cfe_fmt)
+		return -EINVAL;
+
+	*vc = 0;
+	*dt = cfe_fmt->csi_dt;
+
+	return 0;
+}
+
+static int cfe_get_vc_dt(struct cfe_device *cfe, unsigned int channel, u8 *vc,
+			 u8 *dt)
+{
+	struct v4l2_mbus_frame_desc remote_desc;
+	struct v4l2_subdev_state *state;
+	u32 sink_stream;
+	unsigned int i;
+	int ret;
+
+	state = v4l2_subdev_get_locked_active_state(&cfe->csi2.sd);
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+		CSI2_PAD_FIRST_SOURCE + channel, 0, NULL, &sink_stream);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_call(cfe->source_sd, pad, get_frame_desc,
+			       cfe->source_pad, &remote_desc);
+	if (ret == -ENOIOCTLCMD) {
+		cfe_dbg(cfe, "source does not support get_frame_desc, use fallback\n");
+		return cfe_get_vc_dt_fallback(cfe, vc, dt);
+	} else if (ret) {
+		cfe_err(cfe, "Failed to get frame descriptor\n");
+		return ret;
+	}
+
+	if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		cfe_err(cfe, "Frame descriptor does not describe CSI-2 link");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < remote_desc.num_entries; i++) {
+		if (remote_desc.entry[i].stream == sink_stream)
+			break;
+	}
+
+	if (i == remote_desc.num_entries) {
+		cfe_err(cfe, "Stream %u not found in remote frame desc\n",
+			sink_stream);
+		return -EINVAL;
+	}
+
+	*vc = remote_desc.entry[i].bus.csi2.vc;
+	*dt = remote_desc.entry[i].bus.csi2.dt;
+
+	return 0;
+}
+
+static int cfe_start_channel(struct cfe_node *node)
+{
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_subdev_state *state;
+	struct v4l2_mbus_framefmt *source_fmt;
+	const struct cfe_fmt *fmt;
+	unsigned long flags;
+	bool start_fe;
+	int ret;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	start_fe = is_fe_enabled(cfe) &&
+		   test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);
+
+	state = v4l2_subdev_get_locked_active_state(&cfe->csi2.sd);
+
+	if (start_fe) {
+		unsigned int width, height;
+		u8 vc, dt;
+
+		cfe_dbg(cfe, "%s: %s using csi2 channel %d\n", __func__,
+			node_desc[FE_OUT0].name, cfe->fe_csi2_channel);
+
+		ret = cfe_get_vc_dt(cfe, cfe->fe_csi2_channel, &vc, &dt);
+		if (ret)
+			return ret;
+
+		source_fmt = v4l2_subdev_state_get_format(state,
+			node_desc[cfe->fe_csi2_channel].link_pad);
+		fmt = find_format_by_code(source_fmt->code);
+
+		width = source_fmt->width;
+		height = source_fmt->height;
+
+		/* Must have a valid CSI2 datatype. */
+		WARN_ON(!fmt->csi_dt);
+
+		/*
+		 * Start the associated CSI2 Channel as well.
+		 *
+		 * Must write to the ADDR register to latch the ctrl values
+		 * even if we are connected to the front end. Once running,
+		 * this is handled by the CSI2 AUTO_ARM mode.
+		 */
+		csi2_start_channel(&cfe->csi2, cfe->fe_csi2_channel,
+				   CSI2_MODE_FE_STREAMING,
+				   true, false, width, height, vc, dt);
+		csi2_set_buffer(&cfe->csi2, cfe->fe_csi2_channel, 0, 0, -1);
+		pisp_fe_start(&cfe->fe);
+	}
+
+	if (is_csi2_node(node)) {
+		unsigned int width = 0, height = 0;
+		u8 vc, dt;
+
+		ret = cfe_get_vc_dt(cfe, node->id, &vc, &dt);
+		if (ret) {
+			if (start_fe) {
+				csi2_stop_channel(&cfe->csi2,
+						  cfe->fe_csi2_channel);
+				pisp_fe_stop(&cfe->fe);
+			}
+
+			return ret;
+		}
+
+		u32 mode = CSI2_MODE_NORMAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state,
+			node_desc[node->id].link_pad);
+		fmt = find_format_by_code(source_fmt->code);
+
+		/* Must have a valid CSI2 datatype. */
+		WARN_ON(!fmt->csi_dt);
+
+		if (is_image_output_node(node)) {
+			u32  pixfmt;
+
+			width = source_fmt->width;
+			height = source_fmt->height;
+
+			pixfmt = node->vid_fmt.fmt.pix.pixelformat;
+
+			if (pixfmt == fmt->remap[CFE_REMAP_16BIT]) {
+				mode = CSI2_MODE_REMAP;
+			} else if (pixfmt == fmt->remap[CFE_REMAP_COMPRESSED]) {
+				mode = CSI2_MODE_COMPRESSED;
+				csi2_set_compression(&cfe->csi2, node->id,
+						     CSI2_COMPRESSION_DELTA, 0,
+						     0);
+			}
+		}
+		/* Unconditionally start this CSI2 channel. */
+		csi2_start_channel(&cfe->csi2, node->id,
+				   mode,
+				   /* Auto arm */
+				   false,
+				   /* Pack bytes */
+				   is_meta_node(node) ? true : false,
+				   width, height, vc, dt);
+	}
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	if (cfe->job_ready && test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING))
+		cfe_prepare_next_job(cfe);
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+
+	return 0;
+}
+
+static void cfe_stop_channel(struct cfe_node *node, bool fe_stop)
+{
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg(cfe, "%s: [%s] fe_stop %u\n", __func__,
+		node_desc[node->id].name, fe_stop);
+
+	if (fe_stop) {
+		csi2_stop_channel(&cfe->csi2, cfe->fe_csi2_channel);
+		pisp_fe_stop(&cfe->fe);
+	}
+
+	if (is_csi2_node(node))
+		csi2_stop_channel(&cfe->csi2, node->id);
+}
+
+static void cfe_return_buffers(struct cfe_node *node,
+			       enum vb2_buffer_state state)
+{
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf, *tmp;
+	unsigned long flags;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	list_for_each_entry_safe(buf, tmp, &node->dma_queue, list) {
+		list_del(&buf->list);
+		trace_cfe_return_buffer(node->id, buf->vb.vb2_buf.index, 2);
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+	}
+
+	if (node->cur_frm) {
+		trace_cfe_return_buffer(node->id,
+					node->cur_frm->vb.vb2_buf.index, 0);
+		vb2_buffer_done(&node->cur_frm->vb.vb2_buf, state);
+	}
+	if (node->next_frm && node->cur_frm != node->next_frm) {
+		trace_cfe_return_buffer(node->id,
+					node->next_frm->vb.vb2_buf.index, 1);
+		vb2_buffer_done(&node->next_frm->vb.vb2_buf, state);
+	}
+
+	node->cur_frm = NULL;
+	node->next_frm = NULL;
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+}
+
+/*
+ * vb2 ops
+ */
+
+static int cfe_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+			   unsigned int *nplanes, unsigned int sizes[],
+			   struct device *alloc_devs[])
+{
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	unsigned int size = is_image_node(node) ?
+				    node->vid_fmt.fmt.pix.sizeimage :
+				    node->meta_fmt.fmt.meta.buffersize;
+
+	cfe_dbg(cfe, "%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		node->buffer_queue.type);
+
+	if (vq->max_num_buffers + *nbuffers < 3)
+		*nbuffers = 3 - vq->max_num_buffers;
+
+	if (*nplanes) {
+		if (sizes[0] < size) {
+			cfe_err(cfe, "sizes[0] %i < size %u\n", sizes[0], size);
+			return -EINVAL;
+		}
+		size = sizes[0];
+	}
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	return 0;
+}
+
+static int cfe_buffer_prepare(struct vb2_buffer *vb)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf = to_cfe_buffer(vb);
+	unsigned long size;
+
+	trace_cfe_buffer_prepare(node->id, vb);
+
+	size = is_image_node(node) ? node->vid_fmt.fmt.pix.sizeimage :
+				     node->meta_fmt.fmt.meta.buffersize;
+	if (vb2_plane_size(vb, 0) < size) {
+		cfe_err(cfe, "data will not fit into plane (%lu < %lu)\n",
+			vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+
+	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
+
+	if (node->id == FE_CONFIG) {
+		struct cfe_config_buffer *b = to_cfe_config_buffer(buf);
+		void *addr = vb2_plane_vaddr(vb, 0);
+
+		memcpy(&b->config, addr, sizeof(struct pisp_fe_config));
+		return pisp_fe_validate_config(&cfe->fe, &b->config,
+					       &cfe->node[FE_OUT0].vid_fmt,
+					       &cfe->node[FE_OUT1].vid_fmt);
+	}
+
+	return 0;
+}
+
+static void cfe_buffer_queue(struct vb2_buffer *vb)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vb->vb2_queue);
+	struct cfe_device *cfe = node->cfe;
+	struct cfe_buffer *buf = to_cfe_buffer(vb);
+	unsigned long flags;
+	bool schedule_now;
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+
+	list_add_tail(&buf->list, &node->dma_queue);
+
+	if (!cfe->job_ready)
+		cfe->job_ready = cfe_check_job_ready(cfe);
+
+	schedule_now = !cfe->job_queued && cfe->job_ready &&
+		       test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);
+
+	trace_cfe_buffer_queue(node->id, vb, schedule_now);
+
+	if (schedule_now)
+		cfe_prepare_next_job(cfe);
+
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+}
+
+static s64 cfe_get_source_link_freq(struct cfe_device *cfe)
+{
+	struct media_pad *src_pad =
+		&cfe->source_sd->entity.pads[cfe->source_pad];
+	struct v4l2_subdev_state *state;
+	s64 link_freq;
+	u32 bpp;
+
+	state = v4l2_subdev_get_locked_active_state(&cfe->csi2.sd);
+
+	/*
+	 * v4l2_get_link_freq() uses V4L2_CID_LINK_FREQ first, and falls back
+	 * to V4L2_CID_PIXEL_RATE if V4L2_CID_LINK_FREQ is not available.
+	 *
+	 * With multistream input there is no single pixel rate, and thus we
+	 * cannot use V4L2_CID_PIXEL_RATE, so we pass 0 as the bpp which
+	 * causes v4l2_get_link_freq() to return an error if it falls back to
+	 * V4L2_CID_PIXEL_RATE.
+	 */
+
+	if (state->routing.num_routes == 1) {
+		struct v4l2_subdev_route *route = &state->routing.routes[0];
+		struct v4l2_mbus_framefmt *source_fmt;
+		const struct cfe_fmt *fmt;
+
+		source_fmt = v4l2_subdev_state_get_format(state,
+							  route->sink_pad,
+							  route->sink_stream);
+
+		fmt = find_format_by_code(source_fmt->code);
+		if (!fmt)
+			return -EINVAL;
+
+		bpp = fmt->depth;
+	} else {
+		bpp = 0;
+	}
+
+	link_freq = v4l2_get_link_freq(src_pad, bpp,
+				       2 * cfe->csi2.dphy.active_lanes);
+	if (link_freq < 0)
+		cfe_err(cfe, "failed to get link freq for subdev '%s'\n",
+			cfe->source_sd->name);
+
+	return link_freq;
+}
+
+static int cfe_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct v4l2_mbus_config mbus_config = { 0 };
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev_route *route;
+	s64 link_freq;
+	int ret;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!check_state(cfe, NODE_ENABLED, node->id)) {
+		cfe_err(cfe, "%s node link is not enabled.\n",
+			node_desc[node->id].name);
+		ret = -EINVAL;
+		goto err_streaming;
+	}
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret < 0) {
+		cfe_err(cfe, "pm_runtime_resume_and_get failed\n");
+		goto err_streaming;
+	}
+
+	/* When using the Frontend, we must enable the FE_CONFIG node. */
+	if (is_fe_enabled(cfe) &&
+	    !check_state(cfe, NODE_ENABLED, cfe->node[FE_CONFIG].id)) {
+		cfe_err(cfe, "FE enabled, but FE_CONFIG node is not\n");
+		ret = -EINVAL;
+		goto err_pm_put;
+	}
+
+	ret = media_pipeline_start(&node->pad, &cfe->pipe);
+	if (ret < 0) {
+		cfe_err(cfe, "Failed to start media pipeline: %d\n", ret);
+		goto err_pm_put;
+	}
+
+	state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
+
+	clear_state(cfe, FS_INT | FE_INT, node->id);
+	set_state(cfe, NODE_STREAMING, node->id);
+	node->fs_count = 0;
+
+	ret = cfe_start_channel(node);
+	if (ret)
+		goto err_unlock_state;
+
+	if (!test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING)) {
+		cfe_dbg(cfe, "Streaming on hold, as all nodes are not set to streaming yet\n");
+		v4l2_subdev_unlock_state(state);
+		return 0;
+	}
+
+	cfg_reg_write(cfe, MIPICFG_CFG, MIPICFG_CFG_SEL_CSI);
+	cfg_reg_write(cfe, MIPICFG_INTE,
+		      MIPICFG_INT_CSI_DMA | MIPICFG_INT_PISP_FE);
+
+	ret = v4l2_subdev_call(cfe->source_sd, pad, get_mbus_config, 0,
+			       &mbus_config);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		cfe_err(cfe, "g_mbus_config failed\n");
+		goto err_clear_inte;
+	}
+
+	cfe->csi2.dphy.active_lanes = mbus_config.bus.mipi_csi2.num_data_lanes;
+	if (!cfe->csi2.dphy.active_lanes)
+		cfe->csi2.dphy.active_lanes = cfe->csi2.dphy.max_lanes;
+	if (cfe->csi2.dphy.active_lanes > cfe->csi2.dphy.max_lanes) {
+		cfe_err(cfe, "Device has requested %u data lanes, which is >%u configured in DT\n",
+			cfe->csi2.dphy.active_lanes, cfe->csi2.dphy.max_lanes);
+		ret = -EINVAL;
+		goto err_clear_inte;
+	}
+
+	link_freq = cfe_get_source_link_freq(cfe);
+	if (link_freq < 0)
+		goto err_clear_inte;
+
+	cfe->csi2.dphy.dphy_rate = div_s64(link_freq * 2, 1000000);
+	csi2_open_rx(&cfe->csi2);
+
+	cfe->streams_mask = 0;
+
+	for_each_active_route(&state->routing, route)
+		cfe->streams_mask |= BIT_ULL(route->sink_stream);
+
+	ret = v4l2_subdev_enable_streams(cfe->source_sd, cfe->source_pad,
+					 cfe->streams_mask);
+	if (ret) {
+		cfe_err(cfe, "stream on failed in subdev\n");
+		goto err_disable_cfe;
+	}
+
+	cfe_dbg(cfe, "Streaming enabled\n");
+
+	v4l2_subdev_unlock_state(state);
+
+	return 0;
+
+err_disable_cfe:
+	csi2_close_rx(&cfe->csi2);
+err_clear_inte:
+	cfg_reg_write(cfe, MIPICFG_INTE, 0);
+
+	cfe_stop_channel(node,
+			 is_fe_enabled(cfe) && test_all_nodes(cfe, NODE_ENABLED,
+							      NODE_STREAMING));
+err_unlock_state:
+	v4l2_subdev_unlock_state(state);
+	media_pipeline_stop(&node->pad);
+err_pm_put:
+	pm_runtime_put(&cfe->pdev->dev);
+err_streaming:
+	cfe_return_buffers(node, VB2_BUF_STATE_QUEUED);
+	clear_state(cfe, NODE_STREAMING, node->id);
+
+	return ret;
+}
+
+static void cfe_stop_streaming(struct vb2_queue *vq)
+{
+	struct cfe_node *node = vb2_get_drv_priv(vq);
+	struct cfe_device *cfe = node->cfe;
+	unsigned long flags;
+	bool fe_stop;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	spin_lock_irqsave(&cfe->state_lock, flags);
+	fe_stop = is_fe_enabled(cfe) &&
+		  test_all_nodes(cfe, NODE_ENABLED, NODE_STREAMING);
+
+	cfe->job_ready = false;
+	clear_state(cfe, NODE_STREAMING, node->id);
+	spin_unlock_irqrestore(&cfe->state_lock, flags);
+
+	cfe_stop_channel(node, fe_stop);
+
+	if (!test_any_node(cfe, NODE_STREAMING)) {
+		struct v4l2_subdev_state *state;
+		int ret;
+
+		state = v4l2_subdev_lock_and_get_active_state(&cfe->csi2.sd);
+
+		ret = v4l2_subdev_disable_streams(cfe->source_sd,
+						  cfe->source_pad,
+						  cfe->streams_mask);
+		if (ret)
+			cfe_err(cfe, "stream disable failed in subdev\n");
+
+		v4l2_subdev_unlock_state(state);
+
+		csi2_close_rx(&cfe->csi2);
+
+		cfg_reg_write(cfe, MIPICFG_INTE, 0);
+
+		cfe_dbg(cfe, "%s: Streaming disabled\n", __func__);
+	}
+
+	media_pipeline_stop(&node->pad);
+
+	/* Clear all queued buffers for the node */
+	cfe_return_buffers(node, VB2_BUF_STATE_ERROR);
+
+	pm_runtime_put(&cfe->pdev->dev);
+}
+
+static const struct vb2_ops cfe_video_qops = {
+	.queue_setup = cfe_queue_setup,
+	.buf_prepare = cfe_buffer_prepare,
+	.buf_queue = cfe_buffer_queue,
+	.start_streaming = cfe_start_streaming,
+	.stop_streaming = cfe_stop_streaming,
+};
+
+/*
+ * v4l2 ioctl ops
+ */
+
+static int cfe_querycap(struct file *file, void *priv,
+			struct v4l2_capability *cap)
+{
+	strscpy(cap->driver, CFE_MODULE_NAME, sizeof(cap->driver));
+	strscpy(cap->card, CFE_MODULE_NAME, sizeof(cap->card));
+
+	cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_META_CAPTURE |
+			     V4L2_CAP_META_OUTPUT;
+
+	return 0;
+}
+
+static int cfe_enum_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_fmtdesc *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	unsigned int i, j;
+
+	if (!node_supports_image_output(node))
+		return -EINVAL;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(formats); i++) {
+		if (f->mbus_code && formats[i].code != f->mbus_code)
+			continue;
+
+		if (formats[i].flags & CFE_FORMAT_FLAG_META_OUT ||
+		    formats[i].flags & CFE_FORMAT_FLAG_META_CAP)
+			continue;
+
+		if (is_fe_node(node) &&
+		    !(formats[i].flags & CFE_FORMAT_FLAG_FE_OUT))
+			continue;
+
+		if (j == f->index) {
+			f->pixelformat = formats[i].fourcc;
+			f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+			return 0;
+		}
+		j++;
+	}
+
+	return -EINVAL;
+}
+
+static int cfe_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+
+	if (!node_supports_image(node))
+		return -EINVAL;
+
+	*f = node->vid_fmt;
+
+	return 0;
+}
+
+static int cfe_validate_fmt_vid_cap(struct cfe_node *node,
+				    struct v4l2_format *f)
+{
+	struct cfe_device *cfe = node->cfe;
+	const struct cfe_fmt *fmt;
+
+	cfe_dbg(cfe, "%s: [%s] %ux%u, V4L2 pix %p4cc\n", __func__,
+		node_desc[node->id].name, f->fmt.pix.width, f->fmt.pix.height,
+		&f->fmt.pix.pixelformat);
+
+	if (!node_supports_image_output(node))
+		return -EINVAL;
+
+	/*
+	 * Default to a format that works for both CSI2 and FE.
+	 */
+	fmt = find_format_by_pix(f->fmt.pix.pixelformat);
+	if (!fmt)
+		fmt = find_format_by_code(MEDIA_BUS_FMT_SBGGR10_1X10);
+
+	f->fmt.pix.pixelformat = fmt->fourcc;
+
+	if (is_fe_node(node) && fmt->remap[CFE_REMAP_16BIT]) {
+		f->fmt.pix.pixelformat = fmt->remap[CFE_REMAP_16BIT];
+		fmt = find_format_by_pix(f->fmt.pix.pixelformat);
+	}
+
+	f->fmt.pix.field = V4L2_FIELD_NONE;
+
+	cfe_calc_vid_format_size_bpl(cfe, fmt, f);
+
+	return 0;
+}
+
+static int cfe_s_fmt_vid_cap(struct file *file, void *priv,
+			     struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	struct vb2_queue *q = &node->buffer_queue;
+	int ret;
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	ret = cfe_validate_fmt_vid_cap(node, f);
+	if (ret)
+		return ret;
+
+	node->vid_fmt = *f;
+
+	cfe_dbg(cfe, "%s: Set %ux%u, V4L2 pix %p4cc\n", __func__,
+		node->vid_fmt.fmt.pix.width, node->vid_fmt.fmt.pix.height,
+		&node->vid_fmt.fmt.pix.pixelformat);
+
+	return 0;
+}
+
+static int cfe_try_fmt_vid_cap(struct file *file, void *priv,
+			       struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	return cfe_validate_fmt_vid_cap(node, f);
+}
+
+static int cfe_enum_fmt_meta(struct file *file, void *priv,
+			     struct v4l2_fmtdesc *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	switch (node->id) {
+	case CSI2_CH0...CSI2_CH3:
+		f->flags = V4L2_FMT_FLAG_META_LINE_BASED;
+
+		switch (f->index) {
+		case 0:
+			f->pixelformat = V4L2_META_FMT_GENERIC_8;
+			return 0;
+		case 1:
+			f->pixelformat = V4L2_META_FMT_GENERIC_CSI2_10;
+			return 0;
+		case 2:
+			f->pixelformat = V4L2_META_FMT_GENERIC_CSI2_12;
+			return 0;
+		default:
+			return -EINVAL;
+		}
+	default:
+		break;
+	}
+
+	if (f->index != 0)
+		return -EINVAL;
+
+	switch (node->id) {
+	case FE_STATS:
+		f->pixelformat = V4L2_META_FMT_RPI_FE_STATS;
+		return 0;
+	case FE_CONFIG:
+		f->pixelformat = V4L2_META_FMT_RPI_FE_CFG;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int cfe_validate_fmt_meta(struct cfe_node *node, struct v4l2_format *f)
+{
+	struct cfe_device *cfe = node->cfe;
+	const struct cfe_fmt *fmt;
+
+	switch (node->id) {
+	case CSI2_CH0...CSI2_CH3:
+		cfe_dbg(cfe, "%s: [%s] %ux%u, V4L2 meta %p4cc\n", __func__,
+			node_desc[node->id].name, f->fmt.meta.width,
+			f->fmt.meta.height, &f->fmt.meta.dataformat);
+		break;
+	case FE_STATS:
+	case FE_CONFIG:
+		cfe_dbg(cfe, "%s: [%s] %u bytes, V4L2 meta %p4cc\n", __func__,
+			node_desc[node->id].name, f->fmt.meta.buffersize,
+			&f->fmt.meta.dataformat);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	switch (node->id) {
+	case CSI2_CH0...CSI2_CH3:
+		fmt = find_format_by_pix(f->fmt.meta.dataformat);
+		if (!fmt || !(fmt->flags & CFE_FORMAT_FLAG_META_CAP))
+			fmt = find_format_by_pix(V4L2_META_FMT_GENERIC_CSI2_10);
+
+		f->fmt.meta.dataformat = fmt->fourcc;
+
+		cfe_calc_meta_format_size_bpl(cfe, fmt, f);
+
+		return 0;
+	case FE_STATS:
+		f->fmt.meta.dataformat = V4L2_META_FMT_RPI_FE_STATS;
+		f->fmt.meta.buffersize = sizeof(struct pisp_statistics);
+		return 0;
+	case FE_CONFIG:
+		f->fmt.meta.dataformat = V4L2_META_FMT_RPI_FE_CFG;
+		f->fmt.meta.buffersize = sizeof(struct pisp_fe_config);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int cfe_g_fmt_meta(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	*f = node->meta_fmt;
+
+	return 0;
+}
+
+static int cfe_s_fmt_meta(struct file *file, void *priv, struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	struct vb2_queue *q = &node->buffer_queue;
+	int ret;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	if (!node_supports_meta(node))
+		return -EINVAL;
+
+	ret = cfe_validate_fmt_meta(node, f);
+	if (ret)
+		return ret;
+
+	node->meta_fmt = *f;
+
+	cfe_dbg(cfe, "%s: Set %p4cc\n", __func__,
+		&node->meta_fmt.fmt.meta.dataformat);
+
+	return 0;
+}
+
+static int cfe_try_fmt_meta(struct file *file, void *priv,
+			    struct v4l2_format *f)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+
+	cfe_dbg(cfe, "%s: [%s]\n", __func__, node_desc[node->id].name);
+	return cfe_validate_fmt_meta(node, f);
+}
+
+static int cfe_enum_framesizes(struct file *file, void *priv,
+			       struct v4l2_frmsizeenum *fsize)
+{
+	struct cfe_node *node = video_drvdata(file);
+	struct cfe_device *cfe = node->cfe;
+	const struct cfe_fmt *fmt;
+
+	cfe_dbg(cfe, "%s [%s]\n", __func__, node_desc[node->id].name);
+
+	if (fsize->index > 0)
+		return -EINVAL;
+
+	/* check for valid format */
+	fmt = find_format_by_pix(fsize->pixel_format);
+	if (!fmt) {
+		cfe_dbg(cfe, "Invalid pixel code: %x\n", fsize->pixel_format);
+		return -EINVAL;
+	}
+
+	/* TODO: Do we have limits on the step_width? */
+
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = MIN_WIDTH;
+	fsize->stepwise.max_width = MAX_WIDTH;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.min_height = MIN_HEIGHT;
+	fsize->stepwise.max_height = MAX_HEIGHT;
+	fsize->stepwise.step_height = 1;
+
+	return 0;
+}
+
+static int cfe_vb2_ioctl_reqbufs(struct file *file, void *priv,
+				 struct v4l2_requestbuffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct cfe_node *node = video_get_drvdata(vdev);
+	struct cfe_device *cfe = node->cfe;
+	int ret;
+
+	cfe_dbg(cfe, "%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		p->type);
+
+	if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    p->type != V4L2_BUF_TYPE_META_CAPTURE &&
+	    p->type != V4L2_BUF_TYPE_META_OUTPUT)
+		return -EINVAL;
+
+	ret = vb2_queue_change_type(vdev->queue, p->type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_reqbufs(file, priv, p);
+}
+
+static int cfe_vb2_ioctl_create_bufs(struct file *file, void *priv,
+				     struct v4l2_create_buffers *p)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct cfe_node *node = video_get_drvdata(vdev);
+	struct cfe_device *cfe = node->cfe;
+	int ret;
+
+	cfe_dbg(cfe, "%s: [%s] type:%u\n", __func__, node_desc[node->id].name,
+		p->format.type);
+
+	if (p->format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    p->format.type != V4L2_BUF_TYPE_META_CAPTURE &&
+	    p->format.type != V4L2_BUF_TYPE_META_OUTPUT)
+		return -EINVAL;
+
+	ret = vb2_queue_change_type(vdev->queue, p->format.type);
+	if (ret)
+		return ret;
+
+	return vb2_ioctl_create_bufs(file, priv, p);
+}
+
+static int cfe_subscribe_event(struct v4l2_fh *fh,
+			       const struct v4l2_event_subscription *sub)
+{
+	struct cfe_node *node = video_get_drvdata(fh->vdev);
+
+	switch (sub->type) {
+	case V4L2_EVENT_FRAME_SYNC:
+		if (!node_supports_image_output(node))
+			break;
+
+		return v4l2_event_subscribe(fh, sub, 2, NULL);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		if (!node_supports_image_output(node) &&
+		    !node_supports_meta_output(node))
+			break;
+
+		return v4l2_event_subscribe(fh, sub, 4, NULL);
+	}
+
+	return v4l2_ctrl_subscribe_event(fh, sub);
+}
+
+static const struct v4l2_ioctl_ops cfe_ioctl_ops = {
+	.vidioc_querycap = cfe_querycap,
+	.vidioc_enum_fmt_vid_cap = cfe_enum_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap = cfe_g_fmt,
+	.vidioc_s_fmt_vid_cap = cfe_s_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap = cfe_try_fmt_vid_cap,
+
+	.vidioc_enum_fmt_meta_cap = cfe_enum_fmt_meta,
+	.vidioc_g_fmt_meta_cap = cfe_g_fmt_meta,
+	.vidioc_s_fmt_meta_cap = cfe_s_fmt_meta,
+	.vidioc_try_fmt_meta_cap = cfe_try_fmt_meta,
+
+	.vidioc_enum_fmt_meta_out = cfe_enum_fmt_meta,
+	.vidioc_g_fmt_meta_out = cfe_g_fmt_meta,
+	.vidioc_s_fmt_meta_out = cfe_s_fmt_meta,
+	.vidioc_try_fmt_meta_out = cfe_try_fmt_meta,
+
+	.vidioc_enum_framesizes = cfe_enum_framesizes,
+
+	.vidioc_reqbufs = cfe_vb2_ioctl_reqbufs,
+	.vidioc_create_bufs = cfe_vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+
+	.vidioc_subscribe_event = cfe_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static void cfe_notify(struct v4l2_subdev *sd, unsigned int notification,
+		       void *arg)
+{
+	struct cfe_device *cfe = to_cfe_device(sd->v4l2_dev);
+
+	switch (notification) {
+	case V4L2_DEVICE_NOTIFY_EVENT:
+		for (unsigned int i = 0; i < NUM_NODES; i++) {
+			struct cfe_node *node = &cfe->node[i];
+
+			if (check_state(cfe, NODE_REGISTERED, i))
+				continue;
+
+			v4l2_event_queue(&node->video_dev, arg);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/* cfe capture driver file operations */
+static const struct v4l2_file_operations cfe_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.poll = vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap = vb2_fop_mmap,
+};
+
+static int cfe_video_link_validate(struct media_link *link)
+{
+	struct video_device *vd = container_of(link->sink->entity,
+					       struct video_device, entity);
+	struct cfe_node *node = container_of(vd, struct cfe_node, video_dev);
+	struct cfe_device *cfe = node->cfe;
+	struct v4l2_mbus_framefmt *source_fmt;
+	struct v4l2_subdev_state *state;
+	struct v4l2_subdev *source_sd;
+	int ret = 0;
+
+	cfe_dbg(cfe, "%s: [%s] link \"%s\":%u -> \"%s\":%u\n", __func__,
+		node_desc[node->id].name,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index);
+
+	if (!media_entity_remote_source_pad_unique(link->sink->entity)) {
+		cfe_err(cfe, "video node %s pad not connected\n", vd->name);
+		return -ENOTCONN;
+	}
+
+	source_sd = media_entity_to_v4l2_subdev(link->source->entity);
+
+	state = v4l2_subdev_lock_and_get_active_state(source_sd);
+
+	source_fmt = v4l2_subdev_state_get_format(state, link->source->index);
+	if (!source_fmt) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (is_image_output_node(node)) {
+		struct v4l2_pix_format *pix_fmt = &node->vid_fmt.fmt.pix;
+		const struct cfe_fmt *fmt;
+
+		if (source_fmt->width != pix_fmt->width ||
+		    source_fmt->height != pix_fmt->height) {
+			cfe_err(cfe, "Wrong width or height %ux%u (remote pad set to %ux%u)\n",
+				pix_fmt->width, pix_fmt->height,
+				source_fmt->width, source_fmt->height);
+			ret = -EINVAL;
+			goto out;
+		}
+
+		fmt = find_format_by_code_and_fourcc(source_fmt->code,
+						     pix_fmt->pixelformat);
+		if (!fmt) {
+			cfe_err(cfe, "Format mismatch!\n");
+			ret = -EINVAL;
+			goto out;
+		}
+	} else if (is_csi2_node(node) && is_meta_output_node(node)) {
+		struct v4l2_meta_format *meta_fmt = &node->meta_fmt.fmt.meta;
+		const struct cfe_fmt *fmt;
+
+		if (source_fmt->width != meta_fmt->width ||
+		    source_fmt->height != meta_fmt->height) {
+			cfe_err(cfe, "Wrong width or height %ux%u (remote pad set to %ux%u)\n",
+				meta_fmt->width, meta_fmt->height,
+				source_fmt->width, source_fmt->height);
+			ret = -EINVAL;
+			goto out;
+		}
+
+		fmt = find_format_by_code_and_fourcc(source_fmt->code,
+						     meta_fmt->dataformat);
+		if (!fmt) {
+			cfe_err(cfe, "Format mismatch!\n");
+			ret = -EINVAL;
+			goto out;
+		}
+	}
+
+out:
+	v4l2_subdev_unlock_state(state);
+
+	return ret;
+}
+
+static const struct media_entity_operations cfe_media_entity_ops = {
+	.link_validate = cfe_video_link_validate,
+};
+
+static int cfe_video_link_notify(struct media_link *link, u32 flags,
+				 unsigned int notification)
+{
+	struct media_device *mdev = link->graph_obj.mdev;
+	struct cfe_device *cfe = container_of(mdev, struct cfe_device, mdev);
+	struct media_entity *fe = &cfe->fe.sd.entity;
+	struct media_entity *csi2 = &cfe->csi2.sd.entity;
+	unsigned long lock_flags;
+
+	if (notification != MEDIA_DEV_NOTIFY_POST_LINK_CH)
+		return 0;
+
+	cfe_dbg(cfe, "%s: %s[%u] -> %s[%u] 0x%x", __func__,
+		link->source->entity->name, link->source->index,
+		link->sink->entity->name, link->sink->index, flags);
+
+	spin_lock_irqsave(&cfe->state_lock, lock_flags);
+
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		if (link->sink->entity != &cfe->node[i].video_dev.entity &&
+		    link->source->entity != &cfe->node[i].video_dev.entity)
+			continue;
+
+		if (link->flags & MEDIA_LNK_FL_ENABLED)
+			set_state(cfe, NODE_ENABLED, i);
+		else
+			clear_state(cfe, NODE_ENABLED, i);
+
+		break;
+	}
+
+	spin_unlock_irqrestore(&cfe->state_lock, lock_flags);
+
+	if (link->source->entity != csi2)
+		return 0;
+	if (link->sink->entity != fe)
+		return 0;
+	if (link->sink->index != 0)
+		return 0;
+
+	cfe->fe_csi2_channel = -1;
+	if (link->flags & MEDIA_LNK_FL_ENABLED) {
+		if (link->source->index == node_desc[CSI2_CH0].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH0;
+		else if (link->source->index == node_desc[CSI2_CH1].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH1;
+		else if (link->source->index == node_desc[CSI2_CH2].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH2;
+		else if (link->source->index == node_desc[CSI2_CH3].link_pad)
+			cfe->fe_csi2_channel = CSI2_CH3;
+	}
+
+	if (is_fe_enabled(cfe))
+		cfe_dbg(cfe, "%s: Found CSI2:%d -> FE:0 link\n", __func__,
+			cfe->fe_csi2_channel);
+	else
+		cfe_dbg(cfe, "%s: Unable to find CSI2:x -> FE:0 link\n",
+			__func__);
+
+	return 0;
+}
+
+static const struct media_device_ops cfe_media_device_ops = {
+	.link_notify = cfe_video_link_notify,
+};
+
+static void cfe_release(struct kref *kref)
+{
+	struct cfe_device *cfe = container_of(kref, struct cfe_device, kref);
+
+	media_device_cleanup(&cfe->mdev);
+
+	kfree(cfe);
+}
+
+static void cfe_put(struct cfe_device *cfe)
+{
+	kref_put(&cfe->kref, cfe_release);
+}
+
+static void cfe_get(struct cfe_device *cfe)
+{
+	kref_get(&cfe->kref);
+}
+
+static void cfe_node_release(struct video_device *vdev)
+{
+	struct cfe_node *node = video_get_drvdata(vdev);
+
+	cfe_put(node->cfe);
+}
+
+static int cfe_register_node(struct cfe_device *cfe, int id)
+{
+	struct video_device *vdev;
+	const struct cfe_fmt *fmt;
+	struct vb2_queue *q;
+	struct cfe_node *node = &cfe->node[id];
+	int ret;
+
+	node->cfe = cfe;
+	node->id = id;
+
+	if (node_supports_image(node)) {
+		if (node_supports_image_output(node))
+			node->vid_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		else
+			node->vid_fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+		fmt = find_format_by_code(cfe_default_format.code);
+		if (!fmt) {
+			cfe_err(cfe, "Failed to find format code\n");
+			return -EINVAL;
+		}
+
+		node->vid_fmt.fmt.pix.pixelformat = fmt->fourcc;
+		v4l2_fill_pix_format(&node->vid_fmt.fmt.pix,
+				     &cfe_default_format);
+
+		ret = cfe_validate_fmt_vid_cap(node, &node->vid_fmt);
+		if (ret)
+			return ret;
+	}
+
+	if (node_supports_meta(node)) {
+		if (node_supports_meta_output(node))
+			node->meta_fmt.type = V4L2_BUF_TYPE_META_CAPTURE;
+		else
+			node->meta_fmt.type = V4L2_BUF_TYPE_META_OUTPUT;
+
+		ret = cfe_validate_fmt_meta(node, &node->meta_fmt);
+		if (ret)
+			return ret;
+	}
+
+	mutex_init(&node->lock);
+
+	q = &node->buffer_queue;
+	q->type = node_supports_image(node) ? node->vid_fmt.type :
+					      node->meta_fmt.type;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = node;
+	q->ops = &cfe_video_qops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = id == FE_CONFIG ? sizeof(struct cfe_config_buffer)
+					     : sizeof(struct cfe_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &node->lock;
+	q->min_queued_buffers = 1;
+	q->dev = &cfe->pdev->dev;
+
+	ret = vb2_queue_init(q);
+	if (ret) {
+		cfe_err(cfe, "vb2_queue_init() failed\n");
+		return ret;
+	}
+
+	INIT_LIST_HEAD(&node->dma_queue);
+
+	vdev = &node->video_dev;
+	vdev->release = cfe_node_release;
+	vdev->fops = &cfe_fops;
+	vdev->ioctl_ops = &cfe_ioctl_ops;
+	vdev->entity.ops = &cfe_media_entity_ops;
+	vdev->v4l2_dev = &cfe->v4l2_dev;
+	vdev->vfl_dir = (node_supports_image_output(node) ||
+			 node_supports_meta_output(node)) ?
+				VFL_DIR_RX :
+				VFL_DIR_TX;
+	vdev->queue = q;
+	vdev->lock = &node->lock;
+	vdev->device_caps = node_desc[id].caps;
+	vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
+
+	/* Define the device names */
+	snprintf(vdev->name, sizeof(vdev->name), "%s-%s", CFE_MODULE_NAME,
+		 node_desc[id].name);
+
+	video_set_drvdata(vdev, node);
+	node->pad.flags = node_desc[id].pad_flags;
+	media_entity_pads_init(&vdev->entity, 1, &node->pad);
+
+	if (!node_supports_image(node)) {
+		v4l2_disable_ioctl(&node->video_dev,
+				   VIDIOC_ENUM_FRAMEINTERVALS);
+		v4l2_disable_ioctl(&node->video_dev, VIDIOC_ENUM_FRAMESIZES);
+	}
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret) {
+		cfe_err(cfe, "Unable to register video device %s\n",
+			vdev->name);
+		return ret;
+	}
+
+	cfe_info(cfe, "Registered [%s] node id %d as /dev/video%u\n",
+		 vdev->name, id, vdev->num);
+
+	/*
+	 * Acquire a reference to cfe, which will be released when the video
+	 * device will be unregistered and userspace will have closed all open
+	 * file handles.
+	 */
+	cfe_get(cfe);
+	set_state(cfe, NODE_REGISTERED, id);
+
+	return 0;
+}
+
+static void cfe_unregister_nodes(struct cfe_device *cfe)
+{
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (check_state(cfe, NODE_REGISTERED, i)) {
+			clear_state(cfe, NODE_REGISTERED, i);
+			video_unregister_device(&node->video_dev);
+		}
+	}
+}
+
+static int cfe_link_node_pads(struct cfe_device *cfe)
+{
+	struct media_pad *remote_pad;
+	int ret;
+
+	/* Source -> CSI2 */
+
+	ret = v4l2_create_fwnode_links_to_pad(cfe->source_sd,
+					      &cfe->csi2.pad[CSI2_PAD_SINK],
+					      MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+
+	if (ret) {
+		cfe_err(cfe, "Failed to create links to the source: %d\n", ret);
+		return ret;
+	}
+
+	remote_pad = media_pad_remote_pad_unique(&cfe->csi2.pad[CSI2_PAD_SINK]);
+	if (IS_ERR(remote_pad)) {
+		ret = PTR_ERR(remote_pad);
+		cfe_err(cfe, "Failed to get unique remote source pad: %d\n",
+			ret);
+		return ret;
+	}
+
+	cfe->source_pad = remote_pad->index;
+
+	for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		struct cfe_node *node = &cfe->node[i];
+
+		if (!check_state(cfe, NODE_REGISTERED, i))
+			continue;
+
+		/* CSI2 channel # -> /dev/video# */
+		ret = media_create_pad_link(&cfe->csi2.sd.entity,
+					    node_desc[i].link_pad,
+					    &node->video_dev.entity, 0, 0);
+		if (ret)
+			return ret;
+
+		if (node_supports_image(node)) {
+			/* CSI2 channel # -> FE Input */
+			ret = media_create_pad_link(&cfe->csi2.sd.entity,
+						    node_desc[i].link_pad,
+						    &cfe->fe.sd.entity,
+						    FE_STREAM_PAD, 0);
+			if (ret)
+				return ret;
+		}
+	}
+
+	for (unsigned int i = CSI2_NUM_CHANNELS; i < NUM_NODES; i++) {
+		struct cfe_node *node = &cfe->node[i];
+		struct media_entity *src, *dst;
+		unsigned int src_pad, dst_pad;
+
+		if (node_desc[i].pad_flags & MEDIA_PAD_FL_SINK) {
+			/* FE -> /dev/video# */
+			src = &cfe->fe.sd.entity;
+			src_pad = node_desc[i].link_pad;
+			dst = &node->video_dev.entity;
+			dst_pad = 0;
+		} else {
+			/* /dev/video# -> FE */
+			dst = &cfe->fe.sd.entity;
+			dst_pad = node_desc[i].link_pad;
+			src = &node->video_dev.entity;
+			src_pad = 0;
+		}
+
+		ret = media_create_pad_link(src, src_pad, dst, dst_pad, 0);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int cfe_probe_complete(struct cfe_device *cfe)
+{
+	int ret;
+
+	cfe->v4l2_dev.notify = cfe_notify;
+
+	for (unsigned int i = 0; i < NUM_NODES; i++) {
+		ret = cfe_register_node(cfe, i);
+		if (ret) {
+			cfe_err(cfe, "Unable to register video node %u.\n", i);
+			goto unregister;
+		}
+	}
+
+	ret = cfe_link_node_pads(cfe);
+	if (ret) {
+		cfe_err(cfe, "Unable to link node pads.\n");
+		goto unregister;
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&cfe->v4l2_dev);
+	if (ret) {
+		cfe_err(cfe, "Unable to register subdev nodes.\n");
+		goto unregister;
+	}
+
+	return 0;
+
+unregister:
+	cfe_unregister_nodes(cfe);
+	return ret;
+}
+
+static int cfe_async_bound(struct v4l2_async_notifier *notifier,
+			   struct v4l2_subdev *subdev,
+			   struct v4l2_async_connection *asd)
+{
+	struct cfe_device *cfe = to_cfe_device(notifier->v4l2_dev);
+
+	if (cfe->source_sd) {
+		cfe_err(cfe, "Rejecting subdev %s (Already set!!)",
+			subdev->name);
+		return 0;
+	}
+
+	cfe->source_sd = subdev;
+
+	cfe_dbg(cfe, "Using source %s for capture\n", subdev->name);
+
+	return 0;
+}
+
+static int cfe_async_complete(struct v4l2_async_notifier *notifier)
+{
+	struct cfe_device *cfe = to_cfe_device(notifier->v4l2_dev);
+
+	return cfe_probe_complete(cfe);
+}
+
+static const struct v4l2_async_notifier_operations cfe_async_ops = {
+	.bound = cfe_async_bound,
+	.complete = cfe_async_complete,
+};
+
+static int cfe_register_async_nf(struct cfe_device *cfe)
+{
+	struct platform_device *pdev = cfe->pdev;
+	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct fwnode_handle *local_ep_fwnode;
+	struct v4l2_async_connection *asd;
+	int ret;
+
+	local_ep_fwnode = fwnode_graph_get_endpoint_by_id(pdev->dev.fwnode, 0,
+							  0, 0);
+	if (!local_ep_fwnode) {
+		cfe_err(cfe, "Failed to find local endpoint fwnode\n");
+		return -ENODEV;
+	}
+
+	/* Parse the local endpoint and validate its configuration. */
+	ret = v4l2_fwnode_endpoint_parse(local_ep_fwnode, &ep);
+	if (ret) {
+		cfe_err(cfe, "Failed to find remote endpoint fwnode\n");
+		goto err_put_local_fwnode;
+	}
+
+	for (unsigned int lane = 0; lane < ep.bus.mipi_csi2.num_data_lanes;
+	     lane++) {
+		if (ep.bus.mipi_csi2.data_lanes[lane] != lane + 1) {
+			cfe_err(cfe, "Data lanes reordering not supported\n");
+			ret = -EINVAL;
+			goto err_put_local_fwnode;
+		}
+	}
+
+	cfe->csi2.dphy.max_lanes = ep.bus.mipi_csi2.num_data_lanes;
+	cfe->csi2.bus_flags = ep.bus.mipi_csi2.flags;
+
+	/* Initialize and register the async notifier. */
+	v4l2_async_nf_init(&cfe->notifier, &cfe->v4l2_dev);
+	cfe->notifier.ops = &cfe_async_ops;
+
+	asd = v4l2_async_nf_add_fwnode_remote(&cfe->notifier, local_ep_fwnode,
+					      struct v4l2_async_connection);
+	if (IS_ERR(asd)) {
+		ret = PTR_ERR(asd);
+		cfe_err(cfe, "Error adding subdevice: %d\n", ret);
+		goto err_put_local_fwnode;
+	}
+
+	ret = v4l2_async_nf_register(&cfe->notifier);
+	if (ret) {
+		cfe_err(cfe, "Error registering async notifier: %d\n", ret);
+		goto err_nf_cleanup;
+	}
+
+	fwnode_handle_put(local_ep_fwnode);
+
+	return 0;
+
+err_nf_cleanup:
+	v4l2_async_nf_cleanup(&cfe->notifier);
+err_put_local_fwnode:
+	fwnode_handle_put(local_ep_fwnode);
+
+	return ret;
+}
+
+static int cfe_probe(struct platform_device *pdev)
+{
+	struct cfe_device *cfe;
+	char debugfs_name[32];
+	int ret;
+
+	cfe = kzalloc(sizeof(*cfe), GFP_KERNEL);
+	if (!cfe)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, cfe);
+
+	kref_init(&cfe->kref);
+	cfe->pdev = pdev;
+	cfe->fe_csi2_channel = -1;
+	spin_lock_init(&cfe->state_lock);
+
+	cfe->csi2.base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(cfe->csi2.base)) {
+		dev_err(&pdev->dev, "Failed to get dma io block\n");
+		ret = PTR_ERR(cfe->csi2.base);
+		goto err_cfe_put;
+	}
+
+	cfe->csi2.dphy.base = devm_platform_ioremap_resource(pdev, 1);
+	if (IS_ERR(cfe->csi2.dphy.base)) {
+		dev_err(&pdev->dev, "Failed to get host io block\n");
+		ret = PTR_ERR(cfe->csi2.dphy.base);
+		goto err_cfe_put;
+	}
+
+	cfe->mipi_cfg_base = devm_platform_ioremap_resource(pdev, 2);
+	if (IS_ERR(cfe->mipi_cfg_base)) {
+		dev_err(&pdev->dev, "Failed to get mipi cfg io block\n");
+		ret = PTR_ERR(cfe->mipi_cfg_base);
+		goto err_cfe_put;
+	}
+
+	cfe->fe.base = devm_platform_ioremap_resource(pdev, 3);
+	if (IS_ERR(cfe->fe.base)) {
+		dev_err(&pdev->dev, "Failed to get pisp fe io block\n");
+		ret = PTR_ERR(cfe->fe.base);
+		goto err_cfe_put;
+	}
+
+	ret = platform_get_irq(pdev, 0);
+	if (ret <= 0) {
+		ret = -EINVAL;
+		goto err_cfe_put;
+	}
+
+	ret = devm_request_irq(&pdev->dev, ret, cfe_isr, 0, "rp1-cfe", cfe);
+	if (ret) {
+		dev_err(&pdev->dev, "Unable to request interrupt\n");
+		ret = -EINVAL;
+		goto err_cfe_put;
+	}
+
+	ret = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(64));
+	if (ret) {
+		dev_err(&pdev->dev, "DMA enable failed\n");
+		goto err_cfe_put;
+	}
+
+	ret = vb2_dma_contig_set_max_seg_size(&pdev->dev, UINT_MAX);
+	if (ret)
+		goto err_cfe_put;
+
+	/* TODO: Enable clock only when running. */
+	cfe->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(cfe->clk)) {
+		ret = dev_err_probe(&pdev->dev, PTR_ERR(cfe->clk),
+				    "clock not found\n");
+		goto err_cfe_put;
+	}
+
+	cfe->mdev.dev = &pdev->dev;
+	cfe->mdev.ops = &cfe_media_device_ops;
+	strscpy(cfe->mdev.model, CFE_MODULE_NAME, sizeof(cfe->mdev.model));
+	strscpy(cfe->mdev.serial, "", sizeof(cfe->mdev.serial));
+	snprintf(cfe->mdev.bus_info, sizeof(cfe->mdev.bus_info), "platform:%s",
+		 dev_name(&pdev->dev));
+
+	media_device_init(&cfe->mdev);
+
+	cfe->v4l2_dev.mdev = &cfe->mdev;
+
+	ret = v4l2_device_register(&pdev->dev, &cfe->v4l2_dev);
+	if (ret) {
+		cfe_err(cfe, "Unable to register v4l2 device.\n");
+		goto err_cfe_put;
+	}
+
+	snprintf(debugfs_name, sizeof(debugfs_name), "rp1-cfe:%s",
+		 dev_name(&pdev->dev));
+	cfe->debugfs = debugfs_create_dir(debugfs_name, NULL);
+	debugfs_create_file("regs", 0440, cfe->debugfs, cfe,
+			    &mipi_cfg_regs_fops);
+
+	/* Enable the block power domain */
+	pm_runtime_enable(&pdev->dev);
+
+	ret = pm_runtime_resume_and_get(&cfe->pdev->dev);
+	if (ret)
+		goto err_runtime_disable;
+
+	cfe->csi2.v4l2_dev = &cfe->v4l2_dev;
+	ret = csi2_init(&cfe->csi2, cfe->debugfs);
+	if (ret) {
+		cfe_err(cfe, "Failed to init csi2 (%d)\n", ret);
+		goto err_runtime_put;
+	}
+
+	cfe->fe.v4l2_dev = &cfe->v4l2_dev;
+	ret = pisp_fe_init(&cfe->fe, cfe->debugfs);
+	if (ret) {
+		cfe_err(cfe, "Failed to init pisp fe (%d)\n", ret);
+		goto err_csi2_uninit;
+	}
+
+	cfe->mdev.hw_revision = cfe->fe.hw_revision;
+	ret = media_device_register(&cfe->mdev);
+	if (ret < 0) {
+		cfe_err(cfe, "Unable to register media-controller device.\n");
+		goto err_pisp_fe_uninit;
+	}
+
+	ret = cfe_register_async_nf(cfe);
+	if (ret) {
+		cfe_err(cfe, "Failed to connect subdevs\n");
+		goto err_media_unregister;
+	}
+
+	pm_runtime_put(&cfe->pdev->dev);
+
+	return 0;
+
+err_media_unregister:
+	media_device_unregister(&cfe->mdev);
+err_pisp_fe_uninit:
+	pisp_fe_uninit(&cfe->fe);
+err_csi2_uninit:
+	csi2_uninit(&cfe->csi2);
+err_runtime_put:
+	pm_runtime_put(&cfe->pdev->dev);
+err_runtime_disable:
+	pm_runtime_disable(&pdev->dev);
+	debugfs_remove(cfe->debugfs);
+	v4l2_device_unregister(&cfe->v4l2_dev);
+err_cfe_put:
+	cfe_put(cfe);
+
+	return ret;
+}
+
+static void cfe_remove(struct platform_device *pdev)
+{
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+
+	debugfs_remove(cfe->debugfs);
+
+	v4l2_async_nf_unregister(&cfe->notifier);
+	v4l2_async_nf_cleanup(&cfe->notifier);
+
+	media_device_unregister(&cfe->mdev);
+	cfe_unregister_nodes(cfe);
+
+	pisp_fe_uninit(&cfe->fe);
+	csi2_uninit(&cfe->csi2);
+
+	pm_runtime_disable(&pdev->dev);
+
+	v4l2_device_unregister(&cfe->v4l2_dev);
+
+	cfe_put(cfe);
+}
+
+static int cfe_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(cfe->clk);
+
+	return 0;
+}
+
+static int cfe_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct cfe_device *cfe = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = clk_prepare_enable(cfe->clk);
+	if (ret) {
+		dev_err(dev, "Unable to enable clock\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops cfe_pm_ops = {
+	SET_RUNTIME_PM_OPS(cfe_runtime_suspend, cfe_runtime_resume, NULL)
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				     pm_runtime_force_resume)
+};
+
+static const struct of_device_id cfe_of_match[] = {
+	{ .compatible = "raspberrypi,rp1-cfe" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, cfe_of_match);
+
+static struct platform_driver cfe_driver = {
+	.probe		= cfe_probe,
+	.remove		= cfe_remove,
+	.driver = {
+		.name	= CFE_MODULE_NAME,
+		.of_match_table = cfe_of_match,
+		.pm = &cfe_pm_ops,
+	},
+};
+
+module_platform_driver(cfe_driver);
+
+MODULE_AUTHOR("Naushir Patuck <naush@raspberrypi.com>");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
+MODULE_DESCRIPTION("Raspberry Pi RP1 Camera Front End driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(CFE_VERSION);
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/cfe.h b/drivers/media/platform/raspberrypi/rp1-cfe/cfe.h
new file mode 100644
index 000000000000..c63cc314be3c
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/cfe.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 CFE Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+#ifndef _RP1_CFE_
+#define _RP1_CFE_
+
+#include <linux/media-bus-format.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+extern bool cfe_debug_verbose;
+
+enum cfe_remap_types {
+	CFE_REMAP_16BIT,
+	CFE_REMAP_COMPRESSED,
+	CFE_NUM_REMAP,
+};
+
+#define CFE_FORMAT_FLAG_META_OUT	BIT(0)
+#define CFE_FORMAT_FLAG_META_CAP	BIT(1)
+#define CFE_FORMAT_FLAG_FE_OUT		BIT(2)
+
+struct cfe_fmt {
+	u32 fourcc;
+	u32 code;
+	u8 depth;
+	u8 csi_dt;
+	u32 remap[CFE_NUM_REMAP];
+	u32 flags;
+};
+
+extern const struct v4l2_mbus_framefmt cfe_default_format;
+
+const struct cfe_fmt *find_format_by_code(u32 code);
+const struct cfe_fmt *find_format_by_pix(u32 pixelformat);
+u32 cfe_find_16bit_code(u32 code);
+u32 cfe_find_compressed_code(u32 code);
+
+#endif
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/csi2.c b/drivers/media/platform/raspberrypi/rp1-cfe/csi2.c
new file mode 100644
index 000000000000..35c2ab1e2cd4
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/csi2.c
@@ -0,0 +1,586 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 CSI-2 Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+
+#include <linux/delay.h>
+#include <linux/moduleparam.h>
+#include <linux/pm_runtime.h>
+#include <linux/seq_file.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cfe.h"
+#include "csi2.h"
+
+#include "cfe-trace.h"
+
+static bool csi2_track_errors;
+module_param_named(track_csi2_errors, csi2_track_errors, bool, 0);
+MODULE_PARM_DESC(track_csi2_errors, "track csi-2 errors");
+
+#define csi2_dbg(csi2, fmt, arg...) dev_dbg((csi2)->v4l2_dev->dev, fmt, ##arg)
+#define csi2_err(csi2, fmt, arg...) dev_err((csi2)->v4l2_dev->dev, fmt, ##arg)
+
+/* CSI2-DMA registers */
+#define CSI2_STATUS		0x000
+#define CSI2_QOS		0x004
+#define CSI2_DISCARDS_OVERFLOW	0x008
+#define CSI2_DISCARDS_INACTIVE	0x00c
+#define CSI2_DISCARDS_UNMATCHED	0x010
+#define CSI2_DISCARDS_LEN_LIMIT	0x014
+
+#define CSI2_DISCARDS_AMOUNT_SHIFT	0
+#define CSI2_DISCARDS_AMOUNT_MASK	GENMASK(23, 0)
+#define CSI2_DISCARDS_DT_SHIFT		24
+#define CSI2_DISCARDS_DT_MASK		GENMASK(29, 24)
+#define CSI2_DISCARDS_VC_SHIFT		30
+#define CSI2_DISCARDS_VC_MASK		GENMASK(31, 30)
+
+#define CSI2_LLEV_PANICS	0x018
+#define CSI2_ULEV_PANICS	0x01c
+#define CSI2_IRQ_MASK		0x020
+#define CSI2_IRQ_MASK_IRQ_OVERFLOW		BIT(0)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW	BIT(1)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT	BIT(2)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED	BIT(3)
+#define CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE	BIT(4)
+#define CSI2_IRQ_MASK_IRQ_ALL                                              \
+	(CSI2_IRQ_MASK_IRQ_OVERFLOW | CSI2_IRQ_MASK_IRQ_DISCARD_OVERFLOW | \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_LENGTH_LIMIT |                          \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_UNMATCHED |                             \
+	 CSI2_IRQ_MASK_IRQ_DISCARD_INACTIVE)
+
+#define CSI2_CTRL		0x024
+#define CSI2_CH_CTRL(x)		((x) * 0x40 + 0x28)
+#define CSI2_CH_ADDR0(x)	((x) * 0x40 + 0x2c)
+#define CSI2_CH_ADDR1(x)	((x) * 0x40 + 0x3c)
+#define CSI2_CH_STRIDE(x)	((x) * 0x40 + 0x30)
+#define CSI2_CH_LENGTH(x)	((x) * 0x40 + 0x34)
+#define CSI2_CH_DEBUG(x)	((x) * 0x40 + 0x38)
+#define CSI2_CH_FRAME_SIZE(x)	((x) * 0x40 + 0x40)
+#define CSI2_CH_COMP_CTRL(x)	((x) * 0x40 + 0x44)
+#define CSI2_CH_FE_FRAME_ID(x)	((x) * 0x40 + 0x48)
+
+/* CSI2_STATUS */
+#define CSI2_STATUS_IRQ_FS(x)			(BIT(0) << (x))
+#define CSI2_STATUS_IRQ_FE(x)			(BIT(4) << (x))
+#define CSI2_STATUS_IRQ_FE_ACK(x)		(BIT(8) << (x))
+#define CSI2_STATUS_IRQ_LE(x)			(BIT(12) << (x))
+#define CSI2_STATUS_IRQ_LE_ACK(x)		(BIT(16) << (x))
+#define CSI2_STATUS_IRQ_CH_MASK(x) \
+	(CSI2_STATUS_IRQ_FS(x) | CSI2_STATUS_IRQ_FE(x) | \
+	 CSI2_STATUS_IRQ_FE_ACK(x) | CSI2_STATUS_IRQ_LE(x) | \
+	 CSI2_STATUS_IRQ_LE_ACK(x))
+#define CSI2_STATUS_IRQ_OVERFLOW		BIT(20)
+#define CSI2_STATUS_IRQ_DISCARD_OVERFLOW	BIT(21)
+#define CSI2_STATUS_IRQ_DISCARD_LEN_LIMIT	BIT(22)
+#define CSI2_STATUS_IRQ_DISCARD_UNMATCHED	BIT(23)
+#define CSI2_STATUS_IRQ_DISCARD_INACTIVE	BIT(24)
+
+/* CSI2_CTRL */
+#define CSI2_CTRL_EOP_IS_EOL			BIT(0)
+
+/* CSI2_CH_CTRL */
+#define CSI2_CH_CTRL_DMA_EN			BIT(0)
+#define CSI2_CH_CTRL_FORCE			BIT(3)
+#define CSI2_CH_CTRL_AUTO_ARM			BIT(4)
+#define CSI2_CH_CTRL_IRQ_EN_FS			BIT(13)
+#define CSI2_CH_CTRL_IRQ_EN_FE			BIT(14)
+#define CSI2_CH_CTRL_IRQ_EN_FE_ACK		BIT(15)
+#define CSI2_CH_CTRL_IRQ_EN_LE			BIT(16)
+#define CSI2_CH_CTRL_IRQ_EN_LE_ACK		BIT(17)
+#define CSI2_CH_CTRL_FLUSH_FE			BIT(28)
+#define CSI2_CH_CTRL_PACK_LINE			BIT(29)
+#define CSI2_CH_CTRL_PACK_BYTES			BIT(30)
+#define CSI2_CH_CTRL_CH_MODE_MASK		GENMASK(2, 1)
+#define CSI2_CH_CTRL_VC_MASK			GENMASK(6, 5)
+#define CSI2_CH_CTRL_DT_MASK			GENMASK(12, 7)
+#define CSI2_CH_CTRL_LC_MASK			GENMASK(27, 18)
+
+/* CHx_COMPRESSION_CONTROL */
+#define CSI2_CH_COMP_CTRL_OFFSET_MASK		GENMASK(15, 0)
+#define CSI2_CH_COMP_CTRL_SHIFT_MASK		GENMASK(19, 16)
+#define CSI2_CH_COMP_CTRL_MODE_MASK		GENMASK(25, 24)
+
+static inline u32 csi2_reg_read(struct csi2_device *csi2, u32 offset)
+{
+	return readl(csi2->base + offset);
+}
+
+static inline void csi2_reg_write(struct csi2_device *csi2, u32 offset, u32 val)
+{
+	writel(val, csi2->base + offset);
+}
+
+static inline void set_field(u32 *valp, u32 field, u32 mask)
+{
+	u32 val = *valp;
+
+	val &= ~mask;
+	val |= (field << __ffs(mask)) & mask;
+	*valp = val;
+}
+
+static int csi2_regs_show(struct seq_file *s, void *data)
+{
+	struct csi2_device *csi2 = s->private;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(csi2->v4l2_dev->dev);
+	if (ret)
+		return ret;
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", csi2_reg_read(csi2, reg))
+#define DUMP_CH(idx, reg) seq_printf(s, #reg "(%u) \t0x%08x\n", idx, \
+				     csi2_reg_read(csi2, reg(idx)))
+
+	DUMP(CSI2_STATUS);
+	DUMP(CSI2_DISCARDS_OVERFLOW);
+	DUMP(CSI2_DISCARDS_INACTIVE);
+	DUMP(CSI2_DISCARDS_UNMATCHED);
+	DUMP(CSI2_DISCARDS_LEN_LIMIT);
+	DUMP(CSI2_LLEV_PANICS);
+	DUMP(CSI2_ULEV_PANICS);
+	DUMP(CSI2_IRQ_MASK);
+	DUMP(CSI2_CTRL);
+
+	for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; ++i) {
+		DUMP_CH(i, CSI2_CH_CTRL);
+		DUMP_CH(i, CSI2_CH_ADDR0);
+		DUMP_CH(i, CSI2_CH_ADDR1);
+		DUMP_CH(i, CSI2_CH_STRIDE);
+		DUMP_CH(i, CSI2_CH_LENGTH);
+		DUMP_CH(i, CSI2_CH_DEBUG);
+		DUMP_CH(i, CSI2_CH_FRAME_SIZE);
+		DUMP_CH(i, CSI2_CH_COMP_CTRL);
+		DUMP_CH(i, CSI2_CH_FE_FRAME_ID);
+	}
+
+#undef DUMP
+#undef DUMP_CH
+
+	pm_runtime_put(csi2->v4l2_dev->dev);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(csi2_regs);
+
+static int csi2_errors_show(struct seq_file *s, void *data)
+{
+	struct csi2_device *csi2 = s->private;
+	unsigned long flags;
+	u32 discards_table[DISCARDS_TABLE_NUM_VCS][DISCARDS_TABLE_NUM_ENTRIES];
+	u32 discards_dt_table[DISCARDS_TABLE_NUM_ENTRIES];
+	u32 overflows;
+
+	spin_lock_irqsave(&csi2->errors_lock, flags);
+
+	memcpy(discards_table, csi2->discards_table, sizeof(discards_table));
+	memcpy(discards_dt_table, csi2->discards_dt_table,
+	       sizeof(discards_dt_table));
+	overflows = csi2->overflows;
+
+	csi2->overflows = 0;
+	memset(csi2->discards_table, 0, sizeof(discards_table));
+	memset(csi2->discards_dt_table, 0, sizeof(discards_dt_table));
+
+	spin_unlock_irqrestore(&csi2->errors_lock, flags);
+
+	seq_printf(s, "Overflows %u\n", overflows);
+	seq_puts(s, "Discards:\n");
+	seq_puts(s, "VC            OVLF        LEN  UNMATCHED   INACTIVE\n");
+
+	for (unsigned int vc = 0; vc < DISCARDS_TABLE_NUM_VCS; ++vc) {
+		seq_printf(s, "%u       %10u %10u %10u %10u\n", vc,
+			   discards_table[vc][DISCARDS_TABLE_OVERFLOW],
+			   discards_table[vc][DISCARDS_TABLE_LENGTH_LIMIT],
+			   discards_table[vc][DISCARDS_TABLE_UNMATCHED],
+			   discards_table[vc][DISCARDS_TABLE_INACTIVE]);
+	}
+
+	seq_printf(s, "Last DT %10u %10u %10u %10u\n",
+		   discards_dt_table[DISCARDS_TABLE_OVERFLOW],
+		   discards_dt_table[DISCARDS_TABLE_LENGTH_LIMIT],
+		   discards_dt_table[DISCARDS_TABLE_UNMATCHED],
+		   discards_dt_table[DISCARDS_TABLE_INACTIVE]);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(csi2_errors);
+
+static void csi2_isr_handle_errors(struct csi2_device *csi2, u32 status)
+{
+	spin_lock(&csi2->errors_lock);
+
+	if (status & CSI2_STATUS_IRQ_OVERFLOW)
+		csi2->overflows++;
+
+	for (unsigned int i = 0; i < DISCARDS_TABLE_NUM_ENTRIES; ++i) {
+		static const u32 discard_bits[] = {
+			CSI2_STATUS_IRQ_DISCARD_OVERFLOW,
+			CSI2_STATUS_IRQ_DISCARD_LEN_LIMIT,
+			CSI2_STATUS_IRQ_DISCARD_UNMATCHED,
+			CSI2_STATUS_IRQ_DISCARD_INACTIVE,
+		};
+		static const u8 discard_regs[] = {
+			CSI2_DISCARDS_OVERFLOW,
+			CSI2_DISCARDS_LEN_LIMIT,
+			CSI2_DISCARDS_UNMATCHED,
+			CSI2_DISCARDS_INACTIVE,
+		};
+		u32 amount;
+		u8 dt, vc;
+		u32 v;
+
+		if (!(status & discard_bits[i]))
+			continue;
+
+		v = csi2_reg_read(csi2, discard_regs[i]);
+		csi2_reg_write(csi2, discard_regs[i], 0);
+
+		amount = (v & CSI2_DISCARDS_AMOUNT_MASK) >>
+			 CSI2_DISCARDS_AMOUNT_SHIFT;
+		dt = (v & CSI2_DISCARDS_DT_MASK) >> CSI2_DISCARDS_DT_SHIFT;
+		vc = (v & CSI2_DISCARDS_VC_MASK) >> CSI2_DISCARDS_VC_SHIFT;
+
+		csi2->discards_table[vc][i] += amount;
+		csi2->discards_dt_table[i] = dt;
+	}
+
+	spin_unlock(&csi2->errors_lock);
+}
+
+void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof)
+{
+	u32 status;
+
+	status = csi2_reg_read(csi2, CSI2_STATUS);
+
+	/* Write value back to clear the interrupts */
+	csi2_reg_write(csi2, CSI2_STATUS, status);
+
+	for (unsigned int i = 0; i < CSI2_NUM_CHANNELS; i++) {
+		u32 dbg;
+
+		if ((status & CSI2_STATUS_IRQ_CH_MASK(i)) == 0)
+			continue;
+
+		dbg = csi2_reg_read(csi2, CSI2_CH_DEBUG(i));
+
+		trace_csi2_irq(i, status, dbg);
+
+		sof[i] = !!(status & CSI2_STATUS_IRQ_FS(i));
+		eof[i] = !!(status & CSI2_STATUS_IRQ_FE_ACK(i));
+	}
+
+	if (csi2_track_errors)
+		csi2_isr_handle_errors(csi2, status);
+}
+
+void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
+		     dma_addr_t dmaaddr, unsigned int stride, unsigned int size)
+{
+	u64 addr = dmaaddr;
+	/*
+	 * ADDRESS0 must be written last as it triggers the double buffering
+	 * mechanism for all buffer registers within the hardware.
+	 */
+	addr >>= 4;
+	csi2_reg_write(csi2, CSI2_CH_LENGTH(channel), size >> 4);
+	csi2_reg_write(csi2, CSI2_CH_STRIDE(channel), stride >> 4);
+	csi2_reg_write(csi2, CSI2_CH_ADDR1(channel), addr >> 32);
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), addr & 0xffffffff);
+}
+
+void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
+			  enum csi2_compression_mode mode, unsigned int shift,
+			  unsigned int offset)
+{
+	u32 compression = 0;
+
+	set_field(&compression, CSI2_CH_COMP_CTRL_OFFSET_MASK, offset);
+	set_field(&compression, CSI2_CH_COMP_CTRL_SHIFT_MASK, shift);
+	set_field(&compression, CSI2_CH_COMP_CTRL_MODE_MASK, mode);
+	csi2_reg_write(csi2, CSI2_CH_COMP_CTRL(channel), compression);
+}
+
+void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
+			enum csi2_mode mode, bool auto_arm, bool pack_bytes,
+			unsigned int width, unsigned int height,
+			u8 vc, u8 dt)
+{
+	u32 ctrl;
+
+	csi2_dbg(csi2, "%s [%u]\n", __func__, channel);
+
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), 0);
+	csi2_reg_write(csi2, CSI2_CH_DEBUG(channel), 0);
+	csi2_reg_write(csi2, CSI2_STATUS, CSI2_STATUS_IRQ_CH_MASK(channel));
+
+	/* Enable channel and FS/FE interrupts. */
+	ctrl = CSI2_CH_CTRL_DMA_EN | CSI2_CH_CTRL_IRQ_EN_FS |
+	       CSI2_CH_CTRL_IRQ_EN_FE_ACK | CSI2_CH_CTRL_PACK_LINE;
+	/* PACK_BYTES ensures no striding for embedded data. */
+	if (pack_bytes)
+		ctrl |= CSI2_CH_CTRL_PACK_BYTES;
+
+	if (auto_arm)
+		ctrl |= CSI2_CH_CTRL_AUTO_ARM;
+
+	if (width && height) {
+		set_field(&ctrl, mode, CSI2_CH_CTRL_CH_MODE_MASK);
+		csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel),
+			       (height << 16) | width);
+	} else {
+		set_field(&ctrl, 0x0, CSI2_CH_CTRL_CH_MODE_MASK);
+		csi2_reg_write(csi2, CSI2_CH_FRAME_SIZE(channel), 0);
+	}
+
+	set_field(&ctrl, vc, CSI2_CH_CTRL_VC_MASK);
+	set_field(&ctrl, dt, CSI2_CH_CTRL_DT_MASK);
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), ctrl);
+	csi2->num_lines[channel] = height;
+}
+
+void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel)
+{
+	csi2_dbg(csi2, "%s [%u]\n", __func__, channel);
+
+	/* Channel disable.  Use FORCE to allow stopping mid-frame. */
+	csi2_reg_write(csi2, CSI2_CH_CTRL(channel), CSI2_CH_CTRL_FORCE);
+	/* Latch the above change by writing to the ADDR0 register. */
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
+	/* Write this again, the HW needs it! */
+	csi2_reg_write(csi2, CSI2_CH_ADDR0(channel), 0);
+}
+
+void csi2_open_rx(struct csi2_device *csi2)
+{
+	csi2_reg_write(csi2, CSI2_IRQ_MASK,
+		       csi2_track_errors ? CSI2_IRQ_MASK_IRQ_ALL : 0);
+
+	dphy_start(&csi2->dphy);
+
+	csi2_reg_write(csi2, CSI2_CTRL, CSI2_CTRL_EOP_IS_EOL);
+}
+
+void csi2_close_rx(struct csi2_device *csi2)
+{
+	dphy_stop(&csi2->dphy);
+
+	csi2_reg_write(csi2, CSI2_IRQ_MASK, 0);
+}
+
+static int csi2_init_state(struct v4l2_subdev *sd,
+			   struct v4l2_subdev_state *state)
+{
+	struct v4l2_subdev_route routes[] = { {
+		.sink_pad = CSI2_PAD_SINK,
+		.sink_stream = 0,
+		.source_pad = CSI2_PAD_FIRST_SOURCE,
+		.source_stream = 0,
+		.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+	} };
+
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = ARRAY_SIZE(routes),
+		.routes = routes,
+	};
+
+	int ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, &routing,
+					       &cfe_default_format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int csi2_pad_set_fmt(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state,
+			    struct v4l2_subdev_format *format)
+{
+	if (format->pad == CSI2_PAD_SINK) {
+		/* Store the sink format and propagate it to the source. */
+
+		const struct cfe_fmt *cfe_fmt;
+
+		cfe_fmt = find_format_by_code(format->format.code);
+		if (!cfe_fmt) {
+			cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB10_1X10);
+			format->format.code = cfe_fmt->code;
+		}
+
+		struct v4l2_mbus_framefmt *fmt;
+
+		fmt = v4l2_subdev_state_get_format(state, format->pad,
+						   format->stream);
+		if (!fmt)
+			return -EINVAL;
+
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_opposite_stream_format(state,
+								   format->pad,
+								   format->stream);
+		if (!fmt)
+			return -EINVAL;
+
+		format->format.field = V4L2_FIELD_NONE;
+
+		*fmt = format->format;
+	} else {
+		/* Only allow changing the source pad mbus code. */
+
+		struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
+		u32 sink_code;
+		u32 code;
+
+		sink_fmt = v4l2_subdev_state_get_opposite_stream_format(state,
+									format->pad,
+									format->stream);
+		if (!sink_fmt)
+			return -EINVAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state, format->pad,
+							  format->stream);
+		if (!source_fmt)
+			return -EINVAL;
+
+		sink_code = sink_fmt->code;
+		code = format->format.code;
+
+		/*
+		 * Only allow changing the mbus code to:
+		 * - The sink's mbus code
+		 * - The 16-bit version of the sink's mbus code
+		 * - The compressed version of the sink's mbus code
+		 */
+		if (code == sink_code ||
+		    code == cfe_find_16bit_code(sink_code) ||
+		    code == cfe_find_compressed_code(sink_code))
+			source_fmt->code = code;
+
+		format->format.code = source_fmt->code;
+	}
+
+	return 0;
+}
+
+static int csi2_set_routing(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state,
+			    enum v4l2_subdev_format_whence which,
+			    struct v4l2_subdev_krouting *routing)
+{
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1 |
+					   V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING);
+	if (ret)
+		return ret;
+
+	/* Only stream ID 0 allowed on source pads */
+	for (unsigned int i = 0; i < routing->num_routes; ++i) {
+		const struct v4l2_subdev_route *route = &routing->routes[i];
+
+		if (route->source_stream != 0)
+			return -EINVAL;
+	}
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing,
+					       &cfe_default_format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops csi2_subdev_pad_ops = {
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = csi2_pad_set_fmt,
+	.set_routing = csi2_set_routing,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+static const struct media_entity_operations csi2_entity_ops = {
+	.link_validate = v4l2_subdev_link_validate,
+	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
+};
+
+static const struct v4l2_subdev_ops csi2_subdev_ops = {
+	.pad = &csi2_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops csi2_internal_ops = {
+	.init_state = csi2_init_state,
+};
+
+int csi2_init(struct csi2_device *csi2, struct dentry *debugfs)
+{
+	unsigned int ret;
+
+	spin_lock_init(&csi2->errors_lock);
+
+	csi2->dphy.dev = csi2->v4l2_dev->dev;
+	dphy_probe(&csi2->dphy);
+
+	debugfs_create_file("csi2_regs", 0440, debugfs, csi2, &csi2_regs_fops);
+
+	if (csi2_track_errors)
+		debugfs_create_file("csi2_errors", 0440, debugfs, csi2,
+				    &csi2_errors_fops);
+
+	csi2->pad[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+
+	for (unsigned int i = CSI2_PAD_FIRST_SOURCE;
+	     i < CSI2_PAD_FIRST_SOURCE + CSI2_PAD_NUM_SOURCES; i++)
+		csi2->pad[i].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&csi2->sd.entity, ARRAY_SIZE(csi2->pad),
+				     csi2->pad);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&csi2->sd, &csi2_subdev_ops);
+	csi2->sd.internal_ops = &csi2_internal_ops;
+	csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csi2->sd.entity.ops = &csi2_entity_ops;
+	csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
+	csi2->sd.owner = THIS_MODULE;
+	snprintf(csi2->sd.name, sizeof(csi2->sd.name), "csi2");
+
+	ret = v4l2_subdev_init_finalize(&csi2->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(csi2->v4l2_dev, &csi2->sd);
+	if (ret) {
+		csi2_err(csi2, "Failed register csi2 subdev (%d)\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&csi2->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&csi2->sd.entity);
+
+	return ret;
+}
+
+void csi2_uninit(struct csi2_device *csi2)
+{
+	v4l2_device_unregister_subdev(&csi2->sd);
+	v4l2_subdev_cleanup(&csi2->sd);
+	media_entity_cleanup(&csi2->sd.entity);
+}
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/csi2.h b/drivers/media/platform/raspberrypi/rp1-cfe/csi2.h
new file mode 100644
index 000000000000..a8ee5de565fb
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/csi2.h
@@ -0,0 +1,89 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * RP1 CSI-2 Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+
+#ifndef _RP1_CSI2_
+#define _RP1_CSI2_
+
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "dphy.h"
+
+#define CSI2_NUM_CHANNELS 4
+
+#define CSI2_PAD_SINK 0
+#define CSI2_PAD_FIRST_SOURCE 1
+#define CSI2_PAD_NUM_SOURCES 4
+#define CSI2_NUM_PADS 5
+
+#define DISCARDS_TABLE_NUM_VCS 4
+
+enum csi2_mode {
+	CSI2_MODE_NORMAL = 0,
+	CSI2_MODE_REMAP = 1,
+	CSI2_MODE_COMPRESSED = 2,
+	CSI2_MODE_FE_STREAMING = 3,
+};
+
+enum csi2_compression_mode {
+	CSI2_COMPRESSION_DELTA = 1,
+	CSI2_COMPRESSION_SIMPLE = 2,
+	CSI2_COMPRESSION_COMBINED = 3,
+};
+
+enum discards_table_index {
+	DISCARDS_TABLE_OVERFLOW = 0,
+	DISCARDS_TABLE_LENGTH_LIMIT,
+	DISCARDS_TABLE_UNMATCHED,
+	DISCARDS_TABLE_INACTIVE,
+	DISCARDS_TABLE_NUM_ENTRIES,
+};
+
+struct csi2_device {
+	/* Parent V4l2 device */
+	struct v4l2_device *v4l2_dev;
+
+	void __iomem *base;
+
+	struct dphy_data dphy;
+
+	enum v4l2_mbus_type bus_type;
+	unsigned int bus_flags;
+	unsigned int num_lines[CSI2_NUM_CHANNELS];
+
+	struct media_pad pad[CSI2_NUM_PADS];
+	struct v4l2_subdev sd;
+
+	/* lock for csi2 errors counters */
+	spinlock_t errors_lock;
+	u32 overflows;
+	u32 discards_table[DISCARDS_TABLE_NUM_VCS][DISCARDS_TABLE_NUM_ENTRIES];
+	u32 discards_dt_table[DISCARDS_TABLE_NUM_ENTRIES];
+};
+
+void csi2_isr(struct csi2_device *csi2, bool *sof, bool *eof);
+void csi2_set_buffer(struct csi2_device *csi2, unsigned int channel,
+		     dma_addr_t dmaaddr, unsigned int stride,
+		     unsigned int size);
+void csi2_set_compression(struct csi2_device *csi2, unsigned int channel,
+			  enum csi2_compression_mode mode, unsigned int shift,
+			  unsigned int offset);
+void csi2_start_channel(struct csi2_device *csi2, unsigned int channel,
+			enum csi2_mode mode, bool auto_arm,
+			bool pack_bytes, unsigned int width,
+			unsigned int height, u8 vc, u8 dt);
+void csi2_stop_channel(struct csi2_device *csi2, unsigned int channel);
+void csi2_open_rx(struct csi2_device *csi2);
+void csi2_close_rx(struct csi2_device *csi2);
+int csi2_init(struct csi2_device *csi2, struct dentry *debugfs);
+void csi2_uninit(struct csi2_device *csi2);
+
+#endif
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/dphy.c b/drivers/media/platform/raspberrypi/rp1-cfe/dphy.c
new file mode 100644
index 000000000000..b443f0f56ddc
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/dphy.c
@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * RP1 CSI-2 Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+
+#include <linux/delay.h>
+#include <linux/pm_runtime.h>
+
+#include "dphy.h"
+
+#define dphy_dbg(dphy, fmt, arg...) dev_dbg((dphy)->dev, fmt, ##arg)
+#define dphy_err(dphy, fmt, arg...) dev_err((dphy)->dev, fmt, ##arg)
+
+/* DW dphy Host registers */
+#define DPHY_VERSION		0x000
+#define DPHY_N_LANES		0x004
+#define DPHY_RESETN		0x008
+#define DPHY_PHY_SHUTDOWNZ	0x040
+#define DPHY_PHY_RSTZ		0x044
+#define DPHY_PHY_RX		0x048
+#define	DPHY_PHY_STOPSTATE	0x04c
+#define DPHY_PHY_TST_CTRL0	0x050
+#define DPHY_PHY_TST_CTRL1	0x054
+#define DPHY_PHY2_TST_CTRL0	0x058
+#define DPHY_PHY2_TST_CTRL1	0x05c
+
+/* DW dphy Host Transactions */
+#define DPHY_HS_RX_CTRL_LANE0_OFFSET	0x44
+#define DPHY_PLL_INPUT_DIV_OFFSET	0x17
+#define DPHY_PLL_LOOP_DIV_OFFSET	0x18
+#define DPHY_PLL_DIV_CTRL_OFFSET	0x19
+
+static u32 dw_csi2_host_read(struct dphy_data *dphy, u32 offset)
+{
+	return readl(dphy->base + offset);
+}
+
+static void dw_csi2_host_write(struct dphy_data *dphy, u32 offset, u32 data)
+{
+	writel(data, dphy->base + offset);
+}
+
+static void set_tstclr(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl0 = dw_csi2_host_read(dphy, DPHY_PHY_TST_CTRL0);
+
+	dw_csi2_host_write(dphy, DPHY_PHY_TST_CTRL0, (ctrl0 & ~1) | val);
+}
+
+static void set_tstclk(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl0 = dw_csi2_host_read(dphy, DPHY_PHY_TST_CTRL0);
+
+	dw_csi2_host_write(dphy, DPHY_PHY_TST_CTRL0, (ctrl0 & ~2) | (val << 1));
+}
+
+static uint8_t get_tstdout(struct dphy_data *dphy)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, DPHY_PHY_TST_CTRL1);
+
+	return ((ctrl1 >> 8) & 0xff);
+}
+
+static void set_testen(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, DPHY_PHY_TST_CTRL1);
+
+	dw_csi2_host_write(dphy, DPHY_PHY_TST_CTRL1,
+			   (ctrl1 & ~(1 << 16)) | (val << 16));
+}
+
+static void set_testdin(struct dphy_data *dphy, u32 val)
+{
+	u32 ctrl1 = dw_csi2_host_read(dphy, DPHY_PHY_TST_CTRL1);
+
+	dw_csi2_host_write(dphy, DPHY_PHY_TST_CTRL1, (ctrl1 & ~0xff) | val);
+}
+
+static uint8_t dphy_transaction(struct dphy_data *dphy, u8 test_code,
+				uint8_t test_data)
+{
+	/* See page 101 of the MIPI DPHY databook. */
+	set_tstclk(dphy, 1);
+	set_testen(dphy, 0);
+	set_testdin(dphy, test_code);
+	set_testen(dphy, 1);
+	set_tstclk(dphy, 0);
+	set_testen(dphy, 0);
+	set_testdin(dphy, test_data);
+	set_tstclk(dphy, 1);
+	return get_tstdout(dphy);
+}
+
+static void dphy_set_hsfreqrange(struct dphy_data *dphy, uint32_t mbps)
+{
+	/* See Table 5-1 on page 65 of dphy databook */
+	static const u16 hsfreqrange_table[][2] = {
+		{ 89, 0b000000 },   { 99, 0b010000 },	{ 109, 0b100000 },
+		{ 129, 0b000001 },  { 139, 0b010001 },	{ 149, 0b100001 },
+		{ 169, 0b000010 },  { 179, 0b010010 },	{ 199, 0b100010 },
+		{ 219, 0b000011 },  { 239, 0b010011 },	{ 249, 0b100011 },
+		{ 269, 0b000100 },  { 299, 0b010100 },	{ 329, 0b000101 },
+		{ 359, 0b010101 },  { 399, 0b100101 },	{ 449, 0b000110 },
+		{ 499, 0b010110 },  { 549, 0b000111 },	{ 599, 0b010111 },
+		{ 649, 0b001000 },  { 699, 0b011000 },	{ 749, 0b001001 },
+		{ 799, 0b011001 },  { 849, 0b101001 },	{ 899, 0b111001 },
+		{ 949, 0b001010 },  { 999, 0b011010 },	{ 1049, 0b101010 },
+		{ 1099, 0b111010 }, { 1149, 0b001011 }, { 1199, 0b011011 },
+		{ 1249, 0b101011 }, { 1299, 0b111011 }, { 1349, 0b001100 },
+		{ 1399, 0b011100 }, { 1449, 0b101100 }, { 1500, 0b111100 },
+	};
+	unsigned int i;
+
+	if (mbps < 80 || mbps > 1500)
+		dphy_err(dphy, "DPHY: Datarate %u Mbps out of range\n", mbps);
+
+	for (i = 0; i < ARRAY_SIZE(hsfreqrange_table) - 1; i++) {
+		if (mbps <= hsfreqrange_table[i][0])
+			break;
+	}
+
+	dphy_transaction(dphy, DPHY_HS_RX_CTRL_LANE0_OFFSET,
+			 hsfreqrange_table[i][1] << 1);
+}
+
+static void dphy_init(struct dphy_data *dphy)
+{
+	dw_csi2_host_write(dphy, DPHY_PHY_RSTZ, 0);
+	dw_csi2_host_write(dphy, DPHY_PHY_SHUTDOWNZ, 0);
+	set_tstclk(dphy, 1);
+	set_testen(dphy, 0);
+	set_tstclr(dphy, 1);
+	usleep_range(15, 20);
+	set_tstclr(dphy, 0);
+	usleep_range(15, 20);
+
+	dphy_set_hsfreqrange(dphy, dphy->dphy_rate);
+
+	usleep_range(5, 10);
+	dw_csi2_host_write(dphy, DPHY_PHY_SHUTDOWNZ, 1);
+	usleep_range(5, 10);
+	dw_csi2_host_write(dphy, DPHY_PHY_RSTZ, 1);
+}
+
+void dphy_start(struct dphy_data *dphy)
+{
+	dphy_dbg(dphy, "%s: Link rate %u Mbps, %u data lanes\n", __func__,
+		 dphy->dphy_rate, dphy->active_lanes);
+
+	dw_csi2_host_write(dphy, DPHY_N_LANES, (dphy->active_lanes - 1));
+	dphy_init(dphy);
+	dw_csi2_host_write(dphy, DPHY_RESETN, 0xffffffff);
+	usleep_range(10, 50);
+}
+
+void dphy_stop(struct dphy_data *dphy)
+{
+	dphy_dbg(dphy, "%s\n", __func__);
+
+	/* Set only one lane (lane 0) as active (ON) */
+	dw_csi2_host_write(dphy, DPHY_N_LANES, 0);
+	dw_csi2_host_write(dphy, DPHY_RESETN, 0);
+}
+
+void dphy_probe(struct dphy_data *dphy)
+{
+	u32 host_ver;
+	u8 host_ver_major, host_ver_minor;
+
+	host_ver = dw_csi2_host_read(dphy, DPHY_VERSION);
+	host_ver_major = (u8)((host_ver >> 24) - '0');
+	host_ver_minor = (u8)((host_ver >> 16) - '0');
+	host_ver_minor = host_ver_minor * 10;
+	host_ver_minor += (u8)((host_ver >> 8) - '0');
+
+	dphy_dbg(dphy, "DW dphy Host HW v%u.%u\n", host_ver_major,
+		 host_ver_minor);
+}
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/dphy.h b/drivers/media/platform/raspberrypi/rp1-cfe/dphy.h
new file mode 100644
index 000000000000..84fa370957cc
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/dphy.h
@@ -0,0 +1,27 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ * Copyright (c) 2023-2024 Ideas on Board Oy
+ */
+
+#ifndef _RP1_DPHY_
+#define _RP1_DPHY_
+
+#include <linux/io.h>
+#include <linux/types.h>
+
+struct dphy_data {
+	struct device *dev;
+
+	void __iomem *base;
+
+	u32 dphy_rate;
+	u32 max_lanes;
+	u32 active_lanes;
+};
+
+void dphy_probe(struct dphy_data *dphy);
+void dphy_start(struct dphy_data *dphy);
+void dphy_stop(struct dphy_data *dphy);
+
+#endif
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.c b/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.c
new file mode 100644
index 000000000000..05762b1be2bc
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.c
@@ -0,0 +1,605 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * PiSP Front End Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ */
+
+#include <linux/bitops.h>
+#include <linux/delay.h>
+#include <linux/moduleparam.h>
+#include <linux/pm_runtime.h>
+#include <linux/seq_file.h>
+
+#include <media/videobuf2-dma-contig.h>
+
+#include "cfe.h"
+#include "pisp-fe.h"
+
+#include "cfe-trace.h"
+
+#define FE_VERSION		0x000
+#define FE_CONTROL		0x004
+#define FE_STATUS		0x008
+#define FE_FRAME_STATUS		0x00c
+#define FE_ERROR_STATUS		0x010
+#define FE_OUTPUT_STATUS	0x014
+#define FE_INT_EN		0x018
+#define FE_INT_STATUS		0x01c
+
+/* CONTROL */
+#define FE_CONTROL_QUEUE	BIT(0)
+#define FE_CONTROL_ABORT	BIT(1)
+#define FE_CONTROL_RESET	BIT(2)
+#define FE_CONTROL_LATCH_REGS	BIT(3)
+
+/* INT_EN / INT_STATUS */
+#define FE_INT_EOF		BIT(0)
+#define FE_INT_SOF		BIT(1)
+#define FE_INT_LINES0		BIT(8)
+#define FE_INT_LINES1		BIT(9)
+#define FE_INT_STATS		BIT(16)
+#define FE_INT_QREADY		BIT(24)
+
+/* STATUS */
+#define FE_STATUS_QUEUED	BIT(0)
+#define FE_STATUS_WAITING	BIT(1)
+#define FE_STATUS_ACTIVE	BIT(2)
+
+#define PISP_FE_CONFIG_BASE_OFFSET	0x0040
+
+#define PISP_FE_ENABLE_STATS_CLUSTER \
+	(PISP_FE_ENABLE_STATS_CROP | PISP_FE_ENABLE_DECIMATE    | \
+	 PISP_FE_ENABLE_BLC        | PISP_FE_ENABLE_CDAF_STATS  | \
+	 PISP_FE_ENABLE_AWB_STATS  | PISP_FE_ENABLE_RGBY        | \
+	 PISP_FE_ENABLE_LSC        | PISP_FE_ENABLE_AGC_STATS)
+
+#define PISP_FE_ENABLE_OUTPUT_CLUSTER(i)				\
+	((PISP_FE_ENABLE_CROP0     | PISP_FE_ENABLE_DOWNSCALE0 |	\
+	  PISP_FE_ENABLE_COMPRESS0 | PISP_FE_ENABLE_OUTPUT0) << (4 * (i)))
+
+struct pisp_fe_config_param {
+	u32 dirty_flags;
+	u32 dirty_flags_extra;
+	size_t offset;
+	size_t size;
+};
+
+static const struct pisp_fe_config_param pisp_fe_config_map[] = {
+	/* *_dirty_flag_extra types */
+	{ 0, PISP_FE_DIRTY_GLOBAL,
+		offsetof(struct pisp_fe_config, global),
+		sizeof(struct pisp_fe_global_config) },
+	{ 0, PISP_FE_DIRTY_FLOATING,
+		offsetof(struct pisp_fe_config, floating_stats),
+		sizeof(struct pisp_fe_floating_stats_config) },
+	{ 0, PISP_FE_DIRTY_OUTPUT_AXI,
+		offsetof(struct pisp_fe_config, output_axi),
+		sizeof(struct pisp_fe_output_axi_config) },
+	/* *_dirty_flag types */
+	{ PISP_FE_ENABLE_INPUT, 0,
+		offsetof(struct pisp_fe_config, input),
+		sizeof(struct pisp_fe_input_config) },
+	{ PISP_FE_ENABLE_DECOMPRESS, 0,
+		offsetof(struct pisp_fe_config, decompress),
+		sizeof(struct pisp_decompress_config) },
+	{ PISP_FE_ENABLE_DECOMPAND, 0,
+		offsetof(struct pisp_fe_config, decompand),
+		sizeof(struct pisp_fe_decompand_config) },
+	{ PISP_FE_ENABLE_BLA, 0,
+		offsetof(struct pisp_fe_config, bla),
+		sizeof(struct pisp_bla_config) },
+	{ PISP_FE_ENABLE_DPC, 0,
+		offsetof(struct pisp_fe_config, dpc),
+		sizeof(struct pisp_fe_dpc_config) },
+	{ PISP_FE_ENABLE_STATS_CROP, 0,
+		offsetof(struct pisp_fe_config, stats_crop),
+		sizeof(struct pisp_fe_crop_config) },
+	{ PISP_FE_ENABLE_BLC, 0,
+		offsetof(struct pisp_fe_config, blc),
+		sizeof(struct pisp_bla_config) },
+	{ PISP_FE_ENABLE_CDAF_STATS, 0,
+		offsetof(struct pisp_fe_config, cdaf_stats),
+		sizeof(struct pisp_fe_cdaf_stats_config) },
+	{ PISP_FE_ENABLE_AWB_STATS, 0,
+		offsetof(struct pisp_fe_config, awb_stats),
+		sizeof(struct pisp_fe_awb_stats_config) },
+	{ PISP_FE_ENABLE_RGBY, 0,
+		offsetof(struct pisp_fe_config, rgby),
+		sizeof(struct pisp_fe_rgby_config) },
+	{ PISP_FE_ENABLE_LSC, 0,
+		offsetof(struct pisp_fe_config, lsc),
+		sizeof(struct pisp_fe_lsc_config) },
+	{ PISP_FE_ENABLE_AGC_STATS, 0,
+		offsetof(struct pisp_fe_config, agc_stats),
+		sizeof(struct pisp_agc_statistics) },
+	{ PISP_FE_ENABLE_CROP0, 0,
+		offsetof(struct pisp_fe_config, ch[0].crop),
+		sizeof(struct pisp_fe_crop_config) },
+	{ PISP_FE_ENABLE_DOWNSCALE0, 0,
+		offsetof(struct pisp_fe_config, ch[0].downscale),
+		sizeof(struct pisp_fe_downscale_config) },
+	{ PISP_FE_ENABLE_COMPRESS0, 0,
+		offsetof(struct pisp_fe_config, ch[0].compress),
+		sizeof(struct pisp_compress_config) },
+	{ PISP_FE_ENABLE_OUTPUT0, 0,
+		offsetof(struct pisp_fe_config, ch[0].output),
+		sizeof(struct pisp_fe_output_config) },
+	{ PISP_FE_ENABLE_CROP1, 0,
+		offsetof(struct pisp_fe_config, ch[1].crop),
+		sizeof(struct pisp_fe_crop_config) },
+	{ PISP_FE_ENABLE_DOWNSCALE1, 0,
+		offsetof(struct pisp_fe_config, ch[1].downscale),
+		sizeof(struct pisp_fe_downscale_config) },
+	{ PISP_FE_ENABLE_COMPRESS1, 0,
+		offsetof(struct pisp_fe_config, ch[1].compress),
+		sizeof(struct pisp_compress_config) },
+	{ PISP_FE_ENABLE_OUTPUT1, 0,
+		offsetof(struct pisp_fe_config, ch[1].output),
+		sizeof(struct pisp_fe_output_config) },
+};
+
+#define pisp_fe_dbg(fe, fmt, arg...) dev_dbg((fe)->v4l2_dev->dev, fmt, ##arg)
+#define pisp_fe_info(fe, fmt, arg...) dev_info((fe)->v4l2_dev->dev, fmt, ##arg)
+#define pisp_fe_err(fe, fmt, arg...) dev_err((fe)->v4l2_dev->dev, fmt, ##arg)
+
+static inline u32 pisp_fe_reg_read(struct pisp_fe_device *fe, u32 offset)
+{
+	return readl(fe->base + offset);
+}
+
+static inline void pisp_fe_reg_write(struct pisp_fe_device *fe, u32 offset,
+				     u32 val)
+{
+	writel(val, fe->base + offset);
+}
+
+static inline void pisp_fe_reg_write_relaxed(struct pisp_fe_device *fe,
+					     u32 offset, u32 val)
+{
+	writel_relaxed(val, fe->base + offset);
+}
+
+static int pisp_fe_regs_show(struct seq_file *s, void *data)
+{
+	struct pisp_fe_device *fe = s->private;
+	int ret;
+
+	ret = pm_runtime_resume_and_get(fe->v4l2_dev->dev);
+	if (ret)
+		return ret;
+
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
+
+#define DUMP(reg) seq_printf(s, #reg " \t0x%08x\n", pisp_fe_reg_read(fe, reg))
+	DUMP(FE_VERSION);
+	DUMP(FE_CONTROL);
+	DUMP(FE_STATUS);
+	DUMP(FE_FRAME_STATUS);
+	DUMP(FE_ERROR_STATUS);
+	DUMP(FE_OUTPUT_STATUS);
+	DUMP(FE_INT_EN);
+	DUMP(FE_INT_STATUS);
+#undef DUMP
+
+	pm_runtime_put(fe->v4l2_dev->dev);
+
+	return 0;
+}
+
+DEFINE_SHOW_ATTRIBUTE(pisp_fe_regs);
+
+static void pisp_fe_config_write(struct pisp_fe_device *fe,
+				 struct pisp_fe_config *config,
+				 unsigned int start_offset, unsigned int size)
+{
+	const unsigned int max_offset =
+		offsetof(struct pisp_fe_config, ch[PISP_FE_NUM_OUTPUTS]);
+	unsigned int end_offset;
+	u32 *cfg = (u32 *)config;
+
+	start_offset = min(start_offset, max_offset);
+	end_offset = min(start_offset + size, max_offset);
+
+	cfg += start_offset >> 2;
+	for (unsigned int i = start_offset; i < end_offset; i += 4, cfg++)
+		pisp_fe_reg_write_relaxed(fe, PISP_FE_CONFIG_BASE_OFFSET + i,
+					  *cfg);
+}
+
+void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof)
+{
+	u32 status, int_status, out_status, frame_status, error_status;
+
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_LATCH_REGS);
+	status = pisp_fe_reg_read(fe, FE_STATUS);
+	out_status = pisp_fe_reg_read(fe, FE_OUTPUT_STATUS);
+	frame_status = pisp_fe_reg_read(fe, FE_FRAME_STATUS);
+	error_status = pisp_fe_reg_read(fe, FE_ERROR_STATUS);
+
+	int_status = pisp_fe_reg_read(fe, FE_INT_STATUS);
+	pisp_fe_reg_write(fe, FE_INT_STATUS, int_status);
+
+	trace_fe_irq(status, out_status, frame_status, error_status,
+		     int_status);
+
+	/* We do not report interrupts for the input/stream pad. */
+	for (unsigned int i = 0; i < FE_NUM_PADS - 1; i++) {
+		sof[i] = !!(int_status & FE_INT_SOF);
+		eof[i] = !!(int_status & FE_INT_EOF);
+	}
+}
+
+static bool pisp_fe_validate_output(struct pisp_fe_config const *cfg,
+				    unsigned int c, struct v4l2_format const *f)
+{
+	unsigned int wbytes;
+
+	wbytes = cfg->ch[c].output.format.width;
+	if (cfg->ch[c].output.format.format & PISP_IMAGE_FORMAT_BPS_MASK)
+		wbytes *= 2;
+
+	/* Check output image dimensions are nonzero and not too big */
+	if (cfg->ch[c].output.format.width < 2 ||
+	    cfg->ch[c].output.format.height < 2 ||
+	    cfg->ch[c].output.format.height > f->fmt.pix.height ||
+	    cfg->ch[c].output.format.stride > f->fmt.pix.bytesperline ||
+	    wbytes > f->fmt.pix.bytesperline)
+		return false;
+
+	/* Check for zero-sized crops, which could cause lockup */
+	if ((cfg->global.enables & PISP_FE_ENABLE_CROP(c)) &&
+	    ((cfg->ch[c].crop.offset_x >= (cfg->input.format.width & ~1) ||
+	      cfg->ch[c].crop.offset_y >= cfg->input.format.height ||
+	      cfg->ch[c].crop.width < 2 || cfg->ch[c].crop.height < 2)))
+		return false;
+
+	if ((cfg->global.enables & PISP_FE_ENABLE_DOWNSCALE(c)) &&
+	    (cfg->ch[c].downscale.output_width < 2 ||
+	     cfg->ch[c].downscale.output_height < 2))
+		return false;
+
+	return true;
+}
+
+static bool pisp_fe_validate_stats(struct pisp_fe_config const *cfg)
+{
+	/* Check for zero-sized crop, which could cause lockup */
+	return (!(cfg->global.enables & PISP_FE_ENABLE_STATS_CROP) ||
+		(cfg->stats_crop.offset_x < (cfg->input.format.width & ~1) &&
+		 cfg->stats_crop.offset_y < cfg->input.format.height &&
+		 cfg->stats_crop.width >= 2 && cfg->stats_crop.height >= 2));
+}
+
+int pisp_fe_validate_config(struct pisp_fe_device *fe,
+			    struct pisp_fe_config *cfg,
+			    struct v4l2_format const *f0,
+			    struct v4l2_format const *f1)
+{
+	/*
+	 * Check the input is enabled, streaming and has nonzero size;
+	 * to avoid cases where the hardware might lock up or try to
+	 * read inputs from memory (which this driver doesn't support).
+	 */
+	if (!(cfg->global.enables & PISP_FE_ENABLE_INPUT) ||
+	    cfg->input.streaming != 1 || cfg->input.format.width < 2 ||
+	    cfg->input.format.height < 2) {
+		pisp_fe_err(fe, "%s: Input config not valid", __func__);
+		return -EINVAL;
+	}
+
+	for (unsigned int i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
+		if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i))) {
+			if (cfg->global.enables &
+					PISP_FE_ENABLE_OUTPUT_CLUSTER(i)) {
+				pisp_fe_err(fe, "%s: Output %u not valid",
+					    __func__, i);
+				return -EINVAL;
+			}
+			continue;
+		}
+
+		if (!pisp_fe_validate_output(cfg, i, i ? f1 : f0))
+			return -EINVAL;
+	}
+
+	if ((cfg->global.enables & PISP_FE_ENABLE_STATS_CLUSTER) &&
+	    !pisp_fe_validate_stats(cfg)) {
+		pisp_fe_err(fe, "%s: Stats config not valid", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
+			struct pisp_fe_config *cfg)
+{
+	u64 addr;
+	u32 status;
+
+	/*
+	 * Check output buffers exist and outputs are correctly configured.
+	 * If valid, set the buffer's DMA address; otherwise disable.
+	 */
+	for (unsigned int i = 0; i < PISP_FE_NUM_OUTPUTS; i++) {
+		struct vb2_buffer *buf = vb2_bufs[FE_OUTPUT0_PAD + i];
+
+		if (!(cfg->global.enables & PISP_FE_ENABLE_OUTPUT(i)))
+			continue;
+
+		addr = vb2_dma_contig_plane_dma_addr(buf, 0);
+		cfg->output_buffer[i].addr_lo = addr & 0xffffffff;
+		cfg->output_buffer[i].addr_hi = addr >> 32;
+	}
+
+	if (vb2_bufs[FE_STATS_PAD]) {
+		addr = vb2_dma_contig_plane_dma_addr(vb2_bufs[FE_STATS_PAD], 0);
+		cfg->stats_buffer.addr_lo = addr & 0xffffffff;
+		cfg->stats_buffer.addr_hi = addr >> 32;
+	}
+
+	/* Set up ILINES interrupts 3/4 of the way down each output */
+	cfg->ch[0].output.ilines =
+		max(0x80u, (3u * cfg->ch[0].output.format.height) >> 2);
+	cfg->ch[1].output.ilines =
+		max(0x80u, (3u * cfg->ch[1].output.format.height) >> 2);
+
+	/*
+	 * The hardware must have consumed the previous config by now.
+	 * This read of status also serves as a memory barrier before the
+	 * sequence of relaxed writes which follow.
+	 */
+	status = pisp_fe_reg_read(fe, FE_STATUS);
+	if (WARN_ON(status & FE_STATUS_QUEUED))
+		return;
+
+	/*
+	 * Unconditionally write buffers, global and input parameters.
+	 * Write cropping and output parameters whenever they are enabled.
+	 * Selectively write other parameters that have been marked as
+	 * changed through the dirty flags.
+	 */
+	pisp_fe_config_write(fe, cfg, 0,
+			     offsetof(struct pisp_fe_config, decompress));
+	cfg->dirty_flags_extra &= ~PISP_FE_DIRTY_GLOBAL;
+	cfg->dirty_flags &= ~PISP_FE_ENABLE_INPUT;
+	cfg->dirty_flags |= (cfg->global.enables &
+			     (PISP_FE_ENABLE_STATS_CROP        |
+			      PISP_FE_ENABLE_OUTPUT_CLUSTER(0) |
+			      PISP_FE_ENABLE_OUTPUT_CLUSTER(1)));
+	for (unsigned int i = 0; i < ARRAY_SIZE(pisp_fe_config_map); i++) {
+		const struct pisp_fe_config_param *p = &pisp_fe_config_map[i];
+
+		if (cfg->dirty_flags & p->dirty_flags ||
+		    cfg->dirty_flags_extra & p->dirty_flags_extra)
+			pisp_fe_config_write(fe, cfg, p->offset, p->size);
+	}
+
+	/* This final non-relaxed write serves as a memory barrier */
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_QUEUE);
+}
+
+void pisp_fe_start(struct pisp_fe_device *fe)
+{
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_RESET);
+	pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
+	pisp_fe_reg_write(fe, FE_INT_EN, FE_INT_EOF | FE_INT_SOF |
+					 FE_INT_LINES0 | FE_INT_LINES1);
+	fe->inframe_count = 0;
+}
+
+void pisp_fe_stop(struct pisp_fe_device *fe)
+{
+	pisp_fe_reg_write(fe, FE_INT_EN, 0);
+	pisp_fe_reg_write(fe, FE_CONTROL, FE_CONTROL_ABORT);
+	usleep_range(1000, 2000);
+	WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
+	pisp_fe_reg_write(fe, FE_INT_STATUS, ~0);
+}
+
+static int pisp_fe_init_state(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *state)
+{
+	struct v4l2_mbus_framefmt *fmt;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_CONFIG_PAD);
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+	fmt->width = sizeof(struct pisp_fe_config);
+	fmt->height = 1;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT0_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT1_PAD);
+	*fmt = cfe_default_format;
+	fmt->code = MEDIA_BUS_FMT_SRGGB16_1X16;
+
+	fmt = v4l2_subdev_state_get_format(state, FE_STATS_PAD);
+	fmt->code = MEDIA_BUS_FMT_FIXED;
+	fmt->width = sizeof(struct pisp_statistics);
+	fmt->height = 1;
+
+	return 0;
+}
+
+static int pisp_fe_pad_set_fmt(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
+			       struct v4l2_subdev_format *format)
+{
+	struct v4l2_mbus_framefmt *fmt;
+	const struct cfe_fmt *cfe_fmt;
+
+	/* TODO: format propagation to source pads */
+	/* TODO: format validation */
+
+	switch (format->pad) {
+	case FE_STREAM_PAD:
+		cfe_fmt = find_format_by_code(format->format.code);
+		if (!cfe_fmt || !(cfe_fmt->flags & CFE_FORMAT_FLAG_FE_OUT))
+			cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB16_1X16);
+
+		format->format.code = cfe_fmt->code;
+		format->format.field = V4L2_FIELD_NONE;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT0_PAD);
+		*fmt = format->format;
+
+		fmt = v4l2_subdev_state_get_format(state, FE_OUTPUT1_PAD);
+		*fmt = format->format;
+
+		return 0;
+
+	case FE_OUTPUT0_PAD:
+	case FE_OUTPUT1_PAD: {
+		/*
+		 * TODO: we should allow scaling and cropping by allowing the
+		 * user to set the size here.
+		 */
+		struct v4l2_mbus_framefmt *sink_fmt, *source_fmt;
+		u32 sink_code;
+		u32 code;
+
+		cfe_fmt = find_format_by_code(format->format.code);
+		if (!cfe_fmt || !(cfe_fmt->flags & CFE_FORMAT_FLAG_FE_OUT))
+			cfe_fmt = find_format_by_code(MEDIA_BUS_FMT_SRGGB16_1X16);
+
+		format->format.code = cfe_fmt->code;
+
+		sink_fmt = v4l2_subdev_state_get_format(state, FE_STREAM_PAD);
+		if (!sink_fmt)
+			return -EINVAL;
+
+		source_fmt = v4l2_subdev_state_get_format(state, format->pad);
+		if (!source_fmt)
+			return -EINVAL;
+
+		sink_code = sink_fmt->code;
+		code = format->format.code;
+
+		/*
+		 * If the source code from the user does not match the code in
+		 * the sink pad, check that the source code matches the
+		 * compressed version of the sink code.
+		 */
+
+		if (code != sink_code &&
+		    code == cfe_find_compressed_code(sink_code))
+			source_fmt->code = code;
+
+		return 0;
+	}
+
+	case FE_CONFIG_PAD:
+	case FE_STATS_PAD:
+	default:
+		return v4l2_subdev_get_fmt(sd, state, format);
+	}
+}
+
+static const struct v4l2_subdev_pad_ops pisp_fe_subdev_pad_ops = {
+	.get_fmt = v4l2_subdev_get_fmt,
+	.set_fmt = pisp_fe_pad_set_fmt,
+	.link_validate = v4l2_subdev_link_validate_default,
+};
+
+static int pisp_fe_link_validate(struct media_link *link)
+{
+	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(link->sink->entity);
+	struct pisp_fe_device *fe = container_of(sd, struct pisp_fe_device, sd);
+
+	pisp_fe_dbg(fe, "%s: link \"%s\":%u -> \"%s\":%u\n", __func__,
+		    link->source->entity->name, link->source->index,
+		    link->sink->entity->name, link->sink->index);
+
+	if (link->sink->index == FE_STREAM_PAD)
+		return v4l2_subdev_link_validate(link);
+
+	if (link->sink->index == FE_CONFIG_PAD)
+		return 0;
+
+	return -EINVAL;
+}
+
+static const struct media_entity_operations pisp_fe_entity_ops = {
+	.link_validate = pisp_fe_link_validate,
+};
+
+static const struct v4l2_subdev_ops pisp_fe_subdev_ops = {
+	.pad = &pisp_fe_subdev_pad_ops,
+};
+
+static const struct v4l2_subdev_internal_ops pisp_fe_internal_ops = {
+	.init_state = pisp_fe_init_state,
+};
+
+int pisp_fe_init(struct pisp_fe_device *fe, struct dentry *debugfs)
+{
+	int ret;
+
+	debugfs_create_file("fe_regs", 0440, debugfs, fe, &pisp_fe_regs_fops);
+
+	fe->hw_revision = pisp_fe_reg_read(fe, FE_VERSION);
+	pisp_fe_info(fe, "PiSP FE HW v%u.%u\n",
+		     (fe->hw_revision >> 24) & 0xff,
+		     (fe->hw_revision >> 20) & 0x0f);
+
+	fe->pad[FE_STREAM_PAD].flags =
+		MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
+	fe->pad[FE_CONFIG_PAD].flags = MEDIA_PAD_FL_SINK;
+	fe->pad[FE_OUTPUT0_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	fe->pad[FE_OUTPUT1_PAD].flags = MEDIA_PAD_FL_SOURCE;
+	fe->pad[FE_STATS_PAD].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&fe->sd.entity, ARRAY_SIZE(fe->pad),
+				     fe->pad);
+	if (ret)
+		return ret;
+
+	/* Initialize subdev */
+	v4l2_subdev_init(&fe->sd, &pisp_fe_subdev_ops);
+	fe->sd.internal_ops = &pisp_fe_internal_ops;
+	fe->sd.entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
+	fe->sd.entity.ops = &pisp_fe_entity_ops;
+	fe->sd.entity.name = "pisp-fe";
+	fe->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	fe->sd.owner = THIS_MODULE;
+	snprintf(fe->sd.name, sizeof(fe->sd.name), "pisp-fe");
+
+	ret = v4l2_subdev_init_finalize(&fe->sd);
+	if (ret)
+		goto err_entity_cleanup;
+
+	ret = v4l2_device_register_subdev(fe->v4l2_dev, &fe->sd);
+	if (ret) {
+		pisp_fe_err(fe, "Failed register pisp fe subdev (%d)\n", ret);
+		goto err_subdev_cleanup;
+	}
+
+	/* Must be in IDLE state (STATUS == 0) here. */
+	WARN_ON(pisp_fe_reg_read(fe, FE_STATUS));
+
+	return 0;
+
+err_subdev_cleanup:
+	v4l2_subdev_cleanup(&fe->sd);
+err_entity_cleanup:
+	media_entity_cleanup(&fe->sd.entity);
+
+	return ret;
+}
+
+void pisp_fe_uninit(struct pisp_fe_device *fe)
+{
+	v4l2_device_unregister_subdev(&fe->sd);
+	v4l2_subdev_cleanup(&fe->sd);
+	media_entity_cleanup(&fe->sd.entity);
+}
diff --git a/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.h b/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.h
new file mode 100644
index 000000000000..54d506e19cf2
--- /dev/null
+++ b/drivers/media/platform/raspberrypi/rp1-cfe/pisp-fe.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * PiSP Front End Driver
+ *
+ * Copyright (c) 2021-2024 Raspberry Pi Ltd.
+ */
+#ifndef _PISP_FE_H_
+#define _PISP_FE_H_
+
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/types.h>
+#include <linux/videodev2.h>
+
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include <linux/media/raspberrypi/pisp_fe_config.h>
+
+enum pisp_fe_pads {
+	FE_STREAM_PAD,
+	FE_CONFIG_PAD,
+	FE_OUTPUT0_PAD,
+	FE_OUTPUT1_PAD,
+	FE_STATS_PAD,
+	FE_NUM_PADS
+};
+
+struct pisp_fe_device {
+	/* Parent V4l2 device */
+	struct v4l2_device *v4l2_dev;
+	void __iomem *base;
+	u32 hw_revision;
+
+	u16 inframe_count;
+	struct media_pad pad[FE_NUM_PADS];
+	struct v4l2_subdev sd;
+};
+
+void pisp_fe_isr(struct pisp_fe_device *fe, bool *sof, bool *eof);
+int pisp_fe_validate_config(struct pisp_fe_device *fe,
+			    struct pisp_fe_config *cfg,
+			    struct v4l2_format const *f0,
+			    struct v4l2_format const *f1);
+void pisp_fe_submit_job(struct pisp_fe_device *fe, struct vb2_buffer **vb2_bufs,
+			struct pisp_fe_config *cfg);
+void pisp_fe_start(struct pisp_fe_device *fe);
+void pisp_fe_stop(struct pisp_fe_device *fe);
+int pisp_fe_init(struct pisp_fe_device *fe, struct dentry *debugfs);
+void pisp_fe_uninit(struct pisp_fe_device *fe);
+
+#endif
diff --git a/drivers/media/platform/renesas/Kconfig b/drivers/media/platform/renesas/Kconfig
index c7fc718a30a5..27a54fa79083 100644
--- a/drivers/media/platform/renesas/Kconfig
+++ b/drivers/media/platform/renesas/Kconfig
@@ -30,23 +30,6 @@ config VIDEO_RCAR_CSI2
 	  To compile this driver as a module, choose M here: the
 	  module will be called rcar-csi2.
 
-config VIDEO_RCAR_ISP
-	tristate "R-Car Image Signal Processor (ISP)"
-	depends on V4L_PLATFORM_DRIVERS
-	depends on VIDEO_DEV && OF
-	depends on ARCH_RENESAS || COMPILE_TEST
-	select MEDIA_CONTROLLER
-	select VIDEO_V4L2_SUBDEV_API
-	select RESET_CONTROLLER
-	select V4L2_FWNODE
-	help
-	  Support for Renesas R-Car Image Signal Processor (ISP).
-	  Enable this to support the Renesas R-Car Image Signal
-	  Processor (ISP).
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called rcar-isp.
-
 config VIDEO_SH_VOU
 	tristate "SuperH VOU video output driver"
 	depends on V4L_PLATFORM_DRIVERS
@@ -56,6 +39,7 @@ config VIDEO_SH_VOU
 	help
 	  Support for the Video Output Unit (VOU) on SuperH SoCs.
 
+source "drivers/media/platform/renesas/rcar-isp/Kconfig"
 source "drivers/media/platform/renesas/rcar-vin/Kconfig"
 source "drivers/media/platform/renesas/rzg2l-cru/Kconfig"
 
diff --git a/drivers/media/platform/renesas/Makefile b/drivers/media/platform/renesas/Makefile
index 50774a20330c..1127259c09d6 100644
--- a/drivers/media/platform/renesas/Makefile
+++ b/drivers/media/platform/renesas/Makefile
@@ -3,13 +3,13 @@
 # Makefile for the Renesas capture/playback device drivers.
 #
 
+obj-y += rcar-isp/
 obj-y += rcar-vin/
 obj-y += rzg2l-cru/
 obj-y += vsp1/
 
 obj-$(CONFIG_VIDEO_RCAR_CSI2) += rcar-csi2.o
 obj-$(CONFIG_VIDEO_RCAR_DRIF) += rcar_drif.o
-obj-$(CONFIG_VIDEO_RCAR_ISP) += rcar-isp.o
 obj-$(CONFIG_VIDEO_RENESAS_CEU) += renesas-ceu.o
 obj-$(CONFIG_VIDEO_RENESAS_FCP) += rcar-fcp.o
 obj-$(CONFIG_VIDEO_RENESAS_FDP1) += rcar_fdp1.o
diff --git a/drivers/media/platform/renesas/rcar-csi2.c b/drivers/media/platform/renesas/rcar-csi2.c
index 582d5e35db0e..9979de4f6ef1 100644
--- a/drivers/media/platform/renesas/rcar-csi2.c
+++ b/drivers/media/platform/renesas/rcar-csi2.c
@@ -8,6 +8,7 @@
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
+#include <linux/math64.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_graph.h>
@@ -15,6 +16,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/reset.h>
 #include <linux/sys_soc.h>
+#include <linux/units.h>
 
 #include <media/mipi-csi2.h>
 #include <media/v4l2-ctrls.h>
@@ -135,13 +137,23 @@ struct rcar_csi2;
 /* V4H BASE registers */
 #define V4H_N_LANES_REG					0x0004
 #define V4H_CSI2_RESETN_REG				0x0008
+
 #define V4H_PHY_MODE_REG				0x001c
+#define V4H_PHY_MODE_DPHY				0
+#define V4H_PHY_MODE_CPHY				1
+
 #define V4H_PHY_SHUTDOWNZ_REG				0x0040
 #define V4H_DPHY_RSTZ_REG				0x0044
 #define V4H_FLDC_REG					0x0804
 #define V4H_FLDD_REG					0x0808
 #define V4H_IDIC_REG					0x0810
+
 #define V4H_PHY_EN_REG					0x2000
+#define V4H_PHY_EN_ENABLE_3				BIT(7)
+#define V4H_PHY_EN_ENABLE_2				BIT(6)
+#define V4H_PHY_EN_ENABLE_1				BIT(5)
+#define V4H_PHY_EN_ENABLE_0				BIT(4)
+#define V4H_PHY_EN_ENABLE_CLK				BIT(0)
 
 #define V4H_ST_PHYST_REG				0x2814
 #define V4H_ST_PHYST_ST_PHY_READY			BIT(31)
@@ -173,17 +185,19 @@ struct rcar_csi2;
 #define V4H_CORE_DIG_IOCTRL_RW_AFE_CB_CTRL_2_REG(n)	(0x23840 + ((n) * 2)) /* n = 0 - 11 */
 #define V4H_CORE_DIG_RW_COMMON_REG(n)			(0x23880 + ((n) * 2)) /* n = 0 - 15 */
 #define V4H_CORE_DIG_ANACTRL_RW_COMMON_ANACTRL_REG(n)	(0x239e0 + ((n) * 2)) /* n = 0 - 3 */
-#define V4H_CORE_DIG_CLANE_1_RW_CFG_0_REG		0x2a400
 #define V4H_CORE_DIG_CLANE_1_RW_HS_TX_6_REG		0x2a60c
 
 /* V4H C-PHY */
 #define V4H_CORE_DIG_RW_TRIO0_REG(n)			(0x22100 + ((n) * 2)) /* n = 0 - 3 */
 #define V4H_CORE_DIG_RW_TRIO1_REG(n)			(0x22500 + ((n) * 2)) /* n = 0 - 3 */
 #define V4H_CORE_DIG_RW_TRIO2_REG(n)			(0x22900 + ((n) * 2)) /* n = 0 - 3 */
+#define V4H_CORE_DIG_CLANE_0_RW_CFG_0_REG		0x2a000
 #define V4H_CORE_DIG_CLANE_0_RW_LP_0_REG		0x2a080
 #define V4H_CORE_DIG_CLANE_0_RW_HS_RX_REG(n)		(0x2a100 + ((n) * 2)) /* n = 0 - 6 */
+#define V4H_CORE_DIG_CLANE_1_RW_CFG_0_REG		0x2a400
 #define V4H_CORE_DIG_CLANE_1_RW_LP_0_REG		0x2a480
 #define V4H_CORE_DIG_CLANE_1_RW_HS_RX_REG(n)		(0x2a500 + ((n) * 2)) /* n = 0 - 6 */
+#define V4H_CORE_DIG_CLANE_2_RW_CFG_0_REG		0x2a800
 #define V4H_CORE_DIG_CLANE_2_RW_LP_0_REG		0x2a880
 #define V4H_CORE_DIG_CLANE_2_RW_HS_RX_REG(n)		(0x2a900 + ((n) * 2)) /* n = 0 - 6 */
 
@@ -237,17 +251,37 @@ static const struct rcsi2_cphy_setting cphy_setting_table_r8a779g0[] = {
 	{ /* sentinel */ },
 };
 
+/* V4M registers */
+#define V4M_OVR1_REG					0x0848
+#define V4M_OVR1_FORCERXMODE_3				BIT(12)
+#define V4M_OVR1_FORCERXMODE_2				BIT(11)
+#define V4M_OVR1_FORCERXMODE_1				BIT(10)
+#define V4M_OVR1_FORCERXMODE_0				BIT(9)
+
+#define V4M_FRXM_REG					0x2004
+#define V4M_FRXM_FORCERXMODE_3				BIT(3)
+#define V4M_FRXM_FORCERXMODE_2				BIT(2)
+#define V4M_FRXM_FORCERXMODE_1				BIT(1)
+#define V4M_FRXM_FORCERXMODE_0				BIT(0)
+
+#define V4M_PHYPLL_REG					0x02050
+#define V4M_CSI0CLKFCPR_REG				0x02054
+#define V4M_PHTW_REG					0x02060
+#define V4M_PHTR_REG					0x02064
+#define V4M_PHTC_REG					0x02068
+
 struct phtw_value {
-	u16 data;
-	u16 code;
+	u8 data;
+	u8 code;
 };
 
-struct rcsi2_mbps_reg {
+struct rcsi2_mbps_info {
 	u16 mbps;
-	u16 reg;
+	u8 reg;
+	u16 osc_freq; /* V4M */
 };
 
-static const struct rcsi2_mbps_reg phtw_mbps_v3u[] = {
+static const struct rcsi2_mbps_info phtw_mbps_v3u[] = {
 	{ .mbps = 1500, .reg = 0xcc },
 	{ .mbps = 1550, .reg = 0x1d },
 	{ .mbps = 1600, .reg = 0x27 },
@@ -272,7 +306,7 @@ static const struct rcsi2_mbps_reg phtw_mbps_v3u[] = {
 	{ /* sentinel */ },
 };
 
-static const struct rcsi2_mbps_reg phtw_mbps_h3_v3h_m3n[] = {
+static const struct rcsi2_mbps_info phtw_mbps_h3_v3h_m3n[] = {
 	{ .mbps =   80, .reg = 0x86 },
 	{ .mbps =   90, .reg = 0x86 },
 	{ .mbps =  100, .reg = 0x87 },
@@ -292,7 +326,7 @@ static const struct rcsi2_mbps_reg phtw_mbps_h3_v3h_m3n[] = {
 	{ /* sentinel */ },
 };
 
-static const struct rcsi2_mbps_reg phtw_mbps_v3m_e3[] = {
+static const struct rcsi2_mbps_info phtw_mbps_v3m_e3[] = {
 	{ .mbps =   80, .reg = 0x00 },
 	{ .mbps =   90, .reg = 0x20 },
 	{ .mbps =  100, .reg = 0x40 },
@@ -336,7 +370,7 @@ static const struct rcsi2_mbps_reg phtw_mbps_v3m_e3[] = {
 #define PHYPLL_REG			0x68
 #define PHYPLL_HSFREQRANGE(n)		((n) << 16)
 
-static const struct rcsi2_mbps_reg hsfreqrange_v3u[] = {
+static const struct rcsi2_mbps_info hsfreqrange_v3u[] = {
 	{ .mbps =   80, .reg = 0x00 },
 	{ .mbps =   90, .reg = 0x10 },
 	{ .mbps =  100, .reg = 0x20 },
@@ -402,7 +436,7 @@ static const struct rcsi2_mbps_reg hsfreqrange_v3u[] = {
 	{ /* sentinel */ },
 };
 
-static const struct rcsi2_mbps_reg hsfreqrange_h3_v3h_m3n[] = {
+static const struct rcsi2_mbps_info hsfreqrange_h3_v3h_m3n[] = {
 	{ .mbps =   80, .reg = 0x00 },
 	{ .mbps =   90, .reg = 0x10 },
 	{ .mbps =  100, .reg = 0x20 },
@@ -449,7 +483,7 @@ static const struct rcsi2_mbps_reg hsfreqrange_h3_v3h_m3n[] = {
 	{ /* sentinel */ },
 };
 
-static const struct rcsi2_mbps_reg hsfreqrange_m3w[] = {
+static const struct rcsi2_mbps_info hsfreqrange_m3w[] = {
 	{ .mbps =   80,	.reg = 0x00 },
 	{ .mbps =   90,	.reg = 0x10 },
 	{ .mbps =  100,	.reg = 0x20 },
@@ -496,6 +530,73 @@ static const struct rcsi2_mbps_reg hsfreqrange_m3w[] = {
 	{ /* sentinel */ },
 };
 
+static const struct rcsi2_mbps_info hsfreqrange_v4m[] = {
+	{ .mbps =   80, .reg = 0x00, .osc_freq = 0x01a9 },
+	{ .mbps =   90, .reg = 0x10, .osc_freq = 0x01a9 },
+	{ .mbps =  100, .reg = 0x20, .osc_freq = 0x01a9 },
+	{ .mbps =  110, .reg = 0x30, .osc_freq = 0x01a9 },
+	{ .mbps =  120, .reg = 0x01, .osc_freq = 0x01a9 },
+	{ .mbps =  130, .reg = 0x11, .osc_freq = 0x01a9 },
+	{ .mbps =  140, .reg = 0x21, .osc_freq = 0x01a9 },
+	{ .mbps =  150, .reg = 0x31, .osc_freq = 0x01a9 },
+	{ .mbps =  160, .reg = 0x02, .osc_freq = 0x01a9 },
+	{ .mbps =  170, .reg = 0x12, .osc_freq = 0x01a9 },
+	{ .mbps =  180, .reg = 0x22, .osc_freq = 0x01a9 },
+	{ .mbps =  190, .reg = 0x32, .osc_freq = 0x01a9 },
+	{ .mbps =  205, .reg = 0x03, .osc_freq = 0x01a9 },
+	{ .mbps =  220, .reg = 0x13, .osc_freq = 0x01a9 },
+	{ .mbps =  235, .reg = 0x23, .osc_freq = 0x01a9 },
+	{ .mbps =  250, .reg = 0x33, .osc_freq = 0x01a9 },
+	{ .mbps =  275, .reg = 0x04, .osc_freq = 0x01a9 },
+	{ .mbps =  300, .reg = 0x14, .osc_freq = 0x01a9 },
+	{ .mbps =  325, .reg = 0x25, .osc_freq = 0x01a9 },
+	{ .mbps =  350, .reg = 0x35, .osc_freq = 0x01a9 },
+	{ .mbps =  400, .reg = 0x05, .osc_freq = 0x01a9 },
+	{ .mbps =  450, .reg = 0x16, .osc_freq = 0x01a9 },
+	{ .mbps =  500, .reg = 0x26, .osc_freq = 0x01a9 },
+	{ .mbps =  550, .reg = 0x37, .osc_freq = 0x01a9 },
+	{ .mbps =  600, .reg = 0x07, .osc_freq = 0x01a9 },
+	{ .mbps =  650, .reg = 0x18, .osc_freq = 0x01a9 },
+	{ .mbps =  700, .reg = 0x28, .osc_freq = 0x01a9 },
+	{ .mbps =  750, .reg = 0x39, .osc_freq = 0x01a9 },
+	{ .mbps =  800, .reg = 0x09, .osc_freq = 0x01a9 },
+	{ .mbps =  850, .reg = 0x19, .osc_freq = 0x01a9 },
+	{ .mbps =  900, .reg = 0x29, .osc_freq = 0x01a9 },
+	{ .mbps =  950, .reg = 0x3a, .osc_freq = 0x01a9 },
+	{ .mbps = 1000, .reg = 0x0a, .osc_freq = 0x01a9 },
+	{ .mbps = 1050, .reg = 0x1a, .osc_freq = 0x01a9 },
+	{ .mbps = 1100, .reg = 0x2a, .osc_freq = 0x01a9 },
+	{ .mbps = 1150, .reg = 0x3b, .osc_freq = 0x01a9 },
+	{ .mbps = 1200, .reg = 0x0b, .osc_freq = 0x01a9 },
+	{ .mbps = 1250, .reg = 0x1b, .osc_freq = 0x01a9 },
+	{ .mbps = 1300, .reg = 0x2b, .osc_freq = 0x01a9 },
+	{ .mbps = 1350, .reg = 0x3c, .osc_freq = 0x01a9 },
+	{ .mbps = 1400, .reg = 0x0c, .osc_freq = 0x01a9 },
+	{ .mbps = 1450, .reg = 0x1c, .osc_freq = 0x01a9 },
+	{ .mbps = 1500, .reg = 0x2c, .osc_freq = 0x01a9 },
+	{ .mbps = 1550, .reg = 0x3d, .osc_freq = 0x0108 },
+	{ .mbps = 1600, .reg = 0x0d, .osc_freq = 0x0110 },
+	{ .mbps = 1650, .reg = 0x1d, .osc_freq = 0x0119 },
+	{ .mbps = 1700, .reg = 0x2e, .osc_freq = 0x0121 },
+	{ .mbps = 1750, .reg = 0x3e, .osc_freq = 0x012a },
+	{ .mbps = 1800, .reg = 0x0e, .osc_freq = 0x0132 },
+	{ .mbps = 1850, .reg = 0x1e, .osc_freq = 0x013b },
+	{ .mbps = 1900, .reg = 0x2f, .osc_freq = 0x0143 },
+	{ .mbps = 1950, .reg = 0x3f, .osc_freq = 0x014c },
+	{ .mbps = 2000, .reg = 0x0f, .osc_freq = 0x0154 },
+	{ .mbps = 2050, .reg = 0x40, .osc_freq = 0x015d },
+	{ .mbps = 2100, .reg = 0x41, .osc_freq = 0x0165 },
+	{ .mbps = 2150, .reg = 0x42, .osc_freq = 0x016e },
+	{ .mbps = 2200, .reg = 0x43, .osc_freq = 0x0176 },
+	{ .mbps = 2250, .reg = 0x44, .osc_freq = 0x017f },
+	{ .mbps = 2300, .reg = 0x45, .osc_freq = 0x0187 },
+	{ .mbps = 2350, .reg = 0x46, .osc_freq = 0x0190 },
+	{ .mbps = 2400, .reg = 0x47, .osc_freq = 0x0198 },
+	{ .mbps = 2450, .reg = 0x48, .osc_freq = 0x01a1 },
+	{ .mbps = 2500, .reg = 0x49, .osc_freq = 0x01a9 },
+	{ /* sentinel */ },
+};
+
 /* PHY ESC Error Monitor */
 #define PHEERM_REG			0x74
 
@@ -538,6 +639,10 @@ static const struct rcar_csi2_format rcar_csi2_formats[] = {
 		.datatype = MIPI_CSI2_DT_YUV422_8B,
 		.bpp = 20,
 	}, {
+		.code = MEDIA_BUS_FMT_Y8_1X8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.bpp = 8,
+	}, {
 		.code = MEDIA_BUS_FMT_Y10_1X10,
 		.datatype = MIPI_CSI2_DT_RAW10,
 		.bpp = 10,
@@ -558,9 +663,37 @@ static const struct rcar_csi2_format rcar_csi2_formats[] = {
 		.datatype = MIPI_CSI2_DT_RAW8,
 		.bpp = 8,
 	}, {
-		.code = MEDIA_BUS_FMT_Y8_1X8,
-		.datatype = MIPI_CSI2_DT_RAW8,
-		.bpp = 8,
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.bpp = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.bpp = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.bpp = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.bpp = 10,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.bpp = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.bpp = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.bpp = 12,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.bpp = 12,
 	},
 };
 
@@ -575,6 +708,21 @@ static const struct rcar_csi2_format *rcsi2_code_to_fmt(unsigned int code)
 	return NULL;
 }
 
+struct rcsi2_cphy_line_order {
+	enum v4l2_mbus_csi2_cphy_line_orders_type order;
+	u16 cfg;
+	u16 ctrl29;
+};
+
+static const struct rcsi2_cphy_line_order rcsi2_cphy_line_orders[] = {
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_ABC, .cfg = 0x0, .ctrl29 = 0x0 },
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_ACB, .cfg = 0xa, .ctrl29 = 0x1 },
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_BAC, .cfg = 0xc, .ctrl29 = 0x1 },
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_BCA, .cfg = 0x5, .ctrl29 = 0x0 },
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_CAB, .cfg = 0x3, .ctrl29 = 0x0 },
+	{ .order = V4L2_MBUS_CSI2_CPHY_LINE_ORDER_CBA, .cfg = 0x9, .ctrl29 = 0x1 }
+};
+
 enum rcar_csi2_pads {
 	RCAR_CSI2_SINK,
 	RCAR_CSI2_SOURCE_VC0,
@@ -584,12 +732,19 @@ enum rcar_csi2_pads {
 	NR_OF_RCAR_CSI2_PAD,
 };
 
+struct rcsi2_register_layout {
+	unsigned int phtw;
+	unsigned int phypll;
+};
+
 struct rcar_csi2_info {
+	const struct rcsi2_register_layout *regs;
 	int (*init_phtw)(struct rcar_csi2 *priv, unsigned int mbps);
 	int (*phy_post_init)(struct rcar_csi2 *priv);
-	int (*start_receiver)(struct rcar_csi2 *priv);
+	int (*start_receiver)(struct rcar_csi2 *priv,
+			      struct v4l2_subdev_state *state);
 	void (*enter_standby)(struct rcar_csi2 *priv);
-	const struct rcsi2_mbps_reg *hsfreqrange;
+	const struct rcsi2_mbps_info *hsfreqrange;
 	unsigned int csi0clkfreqrange;
 	unsigned int num_channels;
 	bool clear_ulps;
@@ -613,13 +768,12 @@ struct rcar_csi2 {
 
 	int channel_vc[4];
 
-	struct mutex lock; /* Protects mf and stream_count. */
-	struct v4l2_mbus_framefmt mf;
 	int stream_count;
 
 	bool cphy;
 	unsigned short lanes;
 	unsigned char lane_swap[4];
+	enum v4l2_mbus_csi2_cphy_line_orders_type line_orders[3];
 };
 
 static inline struct rcar_csi2 *sd_to_csi2(struct v4l2_subdev *sd)
@@ -632,6 +786,16 @@ static inline struct rcar_csi2 *notifier_to_csi2(struct v4l2_async_notifier *n)
 	return container_of(n, struct rcar_csi2, notifier);
 }
 
+static unsigned int rcsi2_num_pads(const struct rcar_csi2 *priv)
+{
+	/* Used together with R-Car ISP: one sink and one source pad. */
+	if (priv->info->use_isp)
+		return 2;
+
+	/* Used together with R-Car VIN: one sink and four source pads. */
+	return 5;
+}
+
 static u32 rcsi2_read(struct rcar_csi2 *priv, unsigned int reg)
 {
 	return ioread32(priv->base + reg);
@@ -642,11 +806,88 @@ static void rcsi2_write(struct rcar_csi2 *priv, unsigned int reg, u32 data)
 	iowrite32(data, priv->base + reg);
 }
 
+static u16 rcsi2_read16(struct rcar_csi2 *priv, unsigned int reg)
+{
+	return ioread16(priv->base + reg);
+}
+
 static void rcsi2_write16(struct rcar_csi2 *priv, unsigned int reg, u16 data)
 {
 	iowrite16(data, priv->base + reg);
 }
 
+static void rcsi2_modify16(struct rcar_csi2 *priv, unsigned int reg, u16 data, u16 mask)
+{
+	u16 val;
+
+	val = rcsi2_read16(priv, reg) & ~mask;
+	rcsi2_write16(priv, reg, val | data);
+}
+
+static int rcsi2_phtw_write(struct rcar_csi2 *priv, u8 data, u8 code)
+{
+	unsigned int timeout;
+
+	rcsi2_write(priv, priv->info->regs->phtw,
+		    PHTW_DWEN | PHTW_TESTDIN_DATA(data) |
+		    PHTW_CWEN | PHTW_TESTDIN_CODE(code));
+
+	/* Wait for DWEN and CWEN to be cleared by hardware. */
+	for (timeout = 0; timeout <= 20; timeout++) {
+		if (!(rcsi2_read(priv, priv->info->regs->phtw) & (PHTW_DWEN | PHTW_CWEN)))
+			return 0;
+
+		usleep_range(1000, 2000);
+	}
+
+	dev_err(priv->dev, "Timeout waiting for PHTW_DWEN and/or PHTW_CWEN\n");
+
+	return -ETIMEDOUT;
+}
+
+static int rcsi2_phtw_write_array(struct rcar_csi2 *priv,
+				  const struct phtw_value *values,
+				  unsigned int size)
+{
+	int ret;
+
+	for (unsigned int i = 0; i < size; i++) {
+		ret = rcsi2_phtw_write(priv, values[i].data, values[i].code);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static const struct rcsi2_mbps_info *
+rcsi2_mbps_to_info(struct rcar_csi2 *priv,
+		   const struct rcsi2_mbps_info *infotable, unsigned int mbps)
+{
+	const struct rcsi2_mbps_info *info;
+	const struct rcsi2_mbps_info *prev = NULL;
+
+	if (mbps < infotable->mbps)
+		dev_warn(priv->dev, "%u Mbps less than min PHY speed %u Mbps",
+			 mbps, infotable->mbps);
+
+	for (info = infotable; info->mbps != 0; info++) {
+		if (info->mbps >= mbps)
+			break;
+		prev = info;
+	}
+
+	if (!info->mbps) {
+		dev_err(priv->dev, "Unsupported PHY speed (%u Mbps)", mbps);
+		return NULL;
+	}
+
+	if (prev && ((mbps - prev->mbps) <= (info->mbps - mbps)))
+		info = prev;
+
+	return info;
+}
+
 static void rcsi2_enter_standby_gen3(struct rcar_csi2 *priv)
 {
 	rcsi2_write(priv, PHYCNT_REG, 0);
@@ -699,29 +940,13 @@ static int rcsi2_wait_phy_start(struct rcar_csi2 *priv,
 
 static int rcsi2_set_phypll(struct rcar_csi2 *priv, unsigned int mbps)
 {
-	const struct rcsi2_mbps_reg *hsfreq;
-	const struct rcsi2_mbps_reg *hsfreq_prev = NULL;
-
-	if (mbps < priv->info->hsfreqrange->mbps)
-		dev_warn(priv->dev, "%u Mbps less than min PHY speed %u Mbps",
-			 mbps, priv->info->hsfreqrange->mbps);
+	const struct rcsi2_mbps_info *info;
 
-	for (hsfreq = priv->info->hsfreqrange; hsfreq->mbps != 0; hsfreq++) {
-		if (hsfreq->mbps >= mbps)
-			break;
-		hsfreq_prev = hsfreq;
-	}
-
-	if (!hsfreq->mbps) {
-		dev_err(priv->dev, "Unsupported PHY speed (%u Mbps)", mbps);
+	info = rcsi2_mbps_to_info(priv, priv->info->hsfreqrange, mbps);
+	if (!info)
 		return -ERANGE;
-	}
 
-	if (hsfreq_prev &&
-	    ((mbps - hsfreq_prev->mbps) <= (hsfreq->mbps - mbps)))
-		hsfreq = hsfreq_prev;
-
-	rcsi2_write(priv, PHYPLL_REG, PHYPLL_HSFREQRANGE(hsfreq->reg));
+	rcsi2_write(priv, priv->info->regs->phypll, PHYPLL_HSFREQRANGE(info->reg));
 
 	return 0;
 }
@@ -730,7 +955,7 @@ static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp,
 			   unsigned int lanes)
 {
 	struct v4l2_subdev *source;
-	struct v4l2_ctrl *ctrl;
+	s64 freq;
 	u64 mbps;
 
 	if (!priv->remote)
@@ -738,21 +963,17 @@ static int rcsi2_calc_mbps(struct rcar_csi2 *priv, unsigned int bpp,
 
 	source = priv->remote;
 
-	/* Read the pixel rate control from remote. */
-	ctrl = v4l2_ctrl_find(source->ctrl_handler, V4L2_CID_PIXEL_RATE);
-	if (!ctrl) {
-		dev_err(priv->dev, "no pixel rate control in subdev %s\n",
-			source->name);
-		return -EINVAL;
+	freq = v4l2_get_link_freq(source->ctrl_handler, bpp, 2 * lanes);
+	if (freq < 0) {
+		int ret = (int)freq;
+
+		dev_err(priv->dev, "failed to get link freq for %s: %d\n",
+			source->name, ret);
+
+		return ret;
 	}
 
-	/*
-	 * Calculate the phypll in mbps.
-	 * link_freq = (pixel_rate * bits_per_sample) / (2 * nr_of_lanes)
-	 * bps = link_freq * 2
-	 */
-	mbps = v4l2_ctrl_g_ctrl_int64(ctrl) * bpp;
-	do_div(mbps, lanes * 1000000);
+	mbps = div_u64(freq * 2, MEGA);
 
 	/* Adjust for C-PHY, divide by 2.8. */
 	if (priv->cphy)
@@ -808,20 +1029,25 @@ static int rcsi2_get_active_lanes(struct rcar_csi2 *priv,
 	return 0;
 }
 
-static int rcsi2_start_receiver_gen3(struct rcar_csi2 *priv)
+static int rcsi2_start_receiver_gen3(struct rcar_csi2 *priv,
+				     struct v4l2_subdev_state *state)
 {
 	const struct rcar_csi2_format *format;
 	u32 phycnt, vcdt = 0, vcdt2 = 0, fld = 0;
+	const struct v4l2_mbus_framefmt *fmt;
 	unsigned int lanes;
 	unsigned int i;
 	int mbps, ret;
 
+	/* Use the format on the sink pad to compute the receiver config. */
+	fmt = v4l2_subdev_state_get_format(state, RCAR_CSI2_SINK);
+
 	dev_dbg(priv->dev, "Input size (%ux%u%c)\n",
-		priv->mf.width, priv->mf.height,
-		priv->mf.field == V4L2_FIELD_NONE ? 'p' : 'i');
+		fmt->width, fmt->height,
+		fmt->field == V4L2_FIELD_NONE ? 'p' : 'i');
 
 	/* Code is validated in set_fmt. */
-	format = rcsi2_code_to_fmt(priv->mf.code);
+	format = rcsi2_code_to_fmt(fmt->code);
 	if (!format)
 		return -EINVAL;
 
@@ -849,16 +1075,10 @@ static int rcsi2_start_receiver_gen3(struct rcar_csi2 *priv)
 			vcdt2 |= vcdt_part << ((i % 2) * 16);
 	}
 
-	if (priv->mf.field == V4L2_FIELD_ALTERNATE) {
+	if (fmt->field == V4L2_FIELD_ALTERNATE)
 		fld = FLD_DET_SEL(1) | FLD_FLD_EN4 | FLD_FLD_EN3 | FLD_FLD_EN2
 			| FLD_FLD_EN;
 
-		if (priv->mf.height == 240)
-			fld |= FLD_FLD_NUM(0);
-		else
-			fld |= FLD_FLD_NUM(1);
-	}
-
 	/*
 	 * Get the number of active data lanes inspecting the remote mbus
 	 * configuration.
@@ -947,6 +1167,26 @@ static int rcsi2_start_receiver_gen3(struct rcar_csi2 *priv)
 	return 0;
 }
 
+static void rsci2_set_line_order(struct rcar_csi2 *priv,
+				 enum v4l2_mbus_csi2_cphy_line_orders_type order,
+				 unsigned int cfgreg, unsigned int ctrlreg)
+{
+	const struct rcsi2_cphy_line_order *info = NULL;
+
+	for (unsigned int i = 0; i < ARRAY_SIZE(rcsi2_cphy_line_orders); i++) {
+		if (rcsi2_cphy_line_orders[i].order == order) {
+			info = &rcsi2_cphy_line_orders[i];
+			break;
+		}
+	}
+
+	if (!info)
+		return;
+
+	rcsi2_modify16(priv, cfgreg, info->cfg, 0x000f);
+	rcsi2_modify16(priv, ctrlreg, info->ctrl29, 0x0100);
+}
+
 static int rcsi2_wait_phy_start_v4h(struct rcar_csi2 *priv, u32 match)
 {
 	unsigned int timeout;
@@ -1024,12 +1264,18 @@ static int rcsi2_c_phy_setting_v4h(struct rcar_csi2 *priv, int msps)
 	rcsi2_write16(priv, V4H_CORE_DIG_RW_TRIO1_REG(1), conf->trio1);
 	rcsi2_write16(priv, V4H_CORE_DIG_RW_TRIO2_REG(1), conf->trio1);
 
-	/*
-	 * Configure pin-swap.
-	 * TODO: This registers is not documented yet, the values should depend
-	 * on the 'clock-lanes' and 'data-lanes' devicetree properties.
-	 */
-	rcsi2_write16(priv, V4H_CORE_DIG_CLANE_1_RW_CFG_0_REG, 0xf5);
+	/* Configure data line order. */
+	rsci2_set_line_order(priv, priv->line_orders[0],
+			     V4H_CORE_DIG_CLANE_0_RW_CFG_0_REG,
+			     V4H_CORE_DIG_IOCTRL_RW_AFE_LANE0_CTRL_2_REG(9));
+	rsci2_set_line_order(priv, priv->line_orders[1],
+			     V4H_CORE_DIG_CLANE_1_RW_CFG_0_REG,
+			     V4H_CORE_DIG_IOCTRL_RW_AFE_LANE1_CTRL_2_REG(9));
+	rsci2_set_line_order(priv, priv->line_orders[2],
+			     V4H_CORE_DIG_CLANE_2_RW_CFG_0_REG,
+			     V4H_CORE_DIG_IOCTRL_RW_AFE_LANE2_CTRL_2_REG(9));
+
+	/* TODO: This registers is not documented. */
 	rcsi2_write16(priv, V4H_CORE_DIG_CLANE_1_RW_HS_TX_6_REG, 0x5000);
 
 	/* Leave Shutdown mode */
@@ -1049,15 +1295,18 @@ static int rcsi2_c_phy_setting_v4h(struct rcar_csi2 *priv, int msps)
 	return 0;
 }
 
-static int rcsi2_start_receiver_v4h(struct rcar_csi2 *priv)
+static int rcsi2_start_receiver_v4h(struct rcar_csi2 *priv,
+				    struct v4l2_subdev_state *state)
 {
 	const struct rcar_csi2_format *format;
+	const struct v4l2_mbus_framefmt *fmt;
 	unsigned int lanes;
 	int msps;
 	int ret;
 
-	/* Calculate parameters */
-	format = rcsi2_code_to_fmt(priv->mf.code);
+	/* Use the format on the sink pad to compute the receiver config. */
+	fmt = v4l2_subdev_state_get_format(state, RCAR_CSI2_SINK);
+	format = rcsi2_code_to_fmt(fmt->code);
 	if (!format)
 		return -EINVAL;
 
@@ -1075,11 +1324,11 @@ static int rcsi2_start_receiver_v4h(struct rcar_csi2 *priv)
 	rcsi2_write(priv, V4H_PHY_SHUTDOWNZ_REG, 0);
 
 	/* PHY static setting */
-	rcsi2_write(priv, V4H_PHY_EN_REG, BIT(0));
+	rcsi2_write(priv, V4H_PHY_EN_REG, V4H_PHY_EN_ENABLE_CLK);
 	rcsi2_write(priv, V4H_FLDC_REG, 0);
 	rcsi2_write(priv, V4H_FLDD_REG, 0);
 	rcsi2_write(priv, V4H_IDIC_REG, 0);
-	rcsi2_write(priv, V4H_PHY_MODE_REG, BIT(0));
+	rcsi2_write(priv, V4H_PHY_MODE_REG, V4H_PHY_MODE_CPHY);
 	rcsi2_write(priv, V4H_N_LANES_REG, lanes - 1);
 
 	/* Reset CSI2 */
@@ -1114,7 +1363,201 @@ static int rcsi2_start_receiver_v4h(struct rcar_csi2 *priv)
 	return 0;
 }
 
-static int rcsi2_start(struct rcar_csi2 *priv)
+static int rcsi2_d_phy_setting_v4m(struct rcar_csi2 *priv, int data_rate)
+{
+	unsigned int timeout;
+	int ret;
+
+	static const struct phtw_value step1[] = {
+		{ .data = 0x00, .code = 0x00 },
+		{ .data = 0x00, .code = 0x1e },
+	};
+
+	/* Shutdown and reset PHY. */
+	rcsi2_write(priv, V4H_DPHY_RSTZ_REG, BIT(0));
+	rcsi2_write(priv, V4H_PHY_SHUTDOWNZ_REG, BIT(0));
+
+	/* Start internal calibration (POR). */
+	ret = rcsi2_phtw_write_array(priv, step1, ARRAY_SIZE(step1));
+	if (ret)
+		return ret;
+
+	/* Wait for POR to complete. */
+	for (timeout = 10; timeout > 0; timeout--) {
+		if ((rcsi2_read(priv, V4M_PHTR_REG) & 0xf0000) == 0x70000)
+			break;
+		usleep_range(1000, 2000);
+	}
+
+	if (!timeout) {
+		dev_err(priv->dev, "D-PHY calibration failed\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int rcsi2_set_osc_freq(struct rcar_csi2 *priv, unsigned int mbps)
+{
+	const struct rcsi2_mbps_info *info;
+	struct phtw_value steps[] = {
+		{ .data = 0x00, .code = 0x00 },
+		{ .code = 0xe2 }, /* Data filled in below. */
+		{ .code = 0xe3 }, /* Data filled in below. */
+		{ .data = 0x01, .code = 0xe4 },
+	};
+
+	info = rcsi2_mbps_to_info(priv, priv->info->hsfreqrange, mbps);
+	if (!info)
+		return -ERANGE;
+
+	/* Fill in data for command. */
+	steps[1].data = (info->osc_freq & 0x00ff) >> 0;
+	steps[2].data = (info->osc_freq & 0x0f00) >> 8;
+
+	return rcsi2_phtw_write_array(priv, steps, ARRAY_SIZE(steps));
+}
+
+static int rcsi2_init_common_v4m(struct rcar_csi2 *priv, unsigned int mbps)
+{
+	int ret;
+
+	static const struct phtw_value step1[] = {
+		{ .data = 0x00, .code = 0x00 },
+		{ .data = 0x3c, .code = 0x08 },
+	};
+
+	static const struct phtw_value step2[] = {
+		{ .data = 0x00, .code = 0x00 },
+		{ .data = 0x80, .code = 0xe0 },
+		{ .data = 0x31, .code = 0xe1 },
+		{ .data = 0x06, .code = 0x00 },
+		{ .data = 0x11, .code = 0x11 },
+		{ .data = 0x08, .code = 0x00 },
+		{ .data = 0x11, .code = 0x11 },
+		{ .data = 0x0a, .code = 0x00 },
+		{ .data = 0x11, .code = 0x11 },
+		{ .data = 0x0c, .code = 0x00 },
+		{ .data = 0x11, .code = 0x11 },
+		{ .data = 0x01, .code = 0x00 },
+		{ .data = 0x31, .code = 0xaa },
+		{ .data = 0x05, .code = 0x00 },
+		{ .data = 0x05, .code = 0x09 },
+		{ .data = 0x07, .code = 0x00 },
+		{ .data = 0x05, .code = 0x09 },
+		{ .data = 0x09, .code = 0x00 },
+		{ .data = 0x05, .code = 0x09 },
+		{ .data = 0x0b, .code = 0x00 },
+		{ .data = 0x05, .code = 0x09 },
+	};
+
+	static const struct phtw_value step3[] = {
+		{ .data = 0x01, .code = 0x00 },
+		{ .data = 0x06, .code = 0xab },
+	};
+
+	if (priv->info->hsfreqrange) {
+		ret = rcsi2_set_phypll(priv, mbps);
+		if (ret)
+			return ret;
+
+		ret = rcsi2_set_osc_freq(priv, mbps);
+		if (ret)
+			return ret;
+	}
+
+	if (mbps <= 1500) {
+		ret = rcsi2_phtw_write_array(priv, step1, ARRAY_SIZE(step1));
+		if (ret)
+			return ret;
+	}
+
+	if (priv->info->csi0clkfreqrange)
+		rcsi2_write(priv, V4M_CSI0CLKFCPR_REG,
+			    CSI0CLKFREQRANGE(priv->info->csi0clkfreqrange));
+
+	rcsi2_write(priv, V4H_PHY_EN_REG, V4H_PHY_EN_ENABLE_CLK |
+		    V4H_PHY_EN_ENABLE_0 | V4H_PHY_EN_ENABLE_1 |
+		    V4H_PHY_EN_ENABLE_2 | V4H_PHY_EN_ENABLE_3);
+
+	if (mbps > 1500) {
+		ret = rcsi2_phtw_write_array(priv, step2, ARRAY_SIZE(step2));
+		if (ret)
+			return ret;
+	}
+
+	return rcsi2_phtw_write_array(priv, step3, ARRAY_SIZE(step3));
+}
+
+static int rcsi2_start_receiver_v4m(struct rcar_csi2 *priv,
+				    struct v4l2_subdev_state *state)
+{
+	const struct rcar_csi2_format *format;
+	const struct v4l2_mbus_framefmt *fmt;
+	unsigned int lanes;
+	int mbps;
+	int ret;
+
+	/* Calculate parameters */
+	fmt = v4l2_subdev_state_get_format(state, RCAR_CSI2_SINK);
+	format = rcsi2_code_to_fmt(fmt->code);
+	if (!format)
+		return -EINVAL;
+
+	ret = rcsi2_get_active_lanes(priv, &lanes);
+	if (ret)
+		return ret;
+
+	mbps = rcsi2_calc_mbps(priv, format->bpp, lanes);
+	if (mbps < 0)
+		return mbps;
+
+	/* Reset LINK and PHY */
+	rcsi2_write(priv, V4H_CSI2_RESETN_REG, 0);
+	rcsi2_write(priv, V4H_DPHY_RSTZ_REG, 0);
+	rcsi2_write(priv, V4H_PHY_SHUTDOWNZ_REG, 0);
+	rcsi2_write(priv, V4M_PHTC_REG, PHTC_TESTCLR);
+
+	/* PHY static setting */
+	rcsi2_write(priv, V4H_PHY_EN_REG, V4H_PHY_EN_ENABLE_CLK);
+	rcsi2_write(priv, V4H_FLDC_REG, 0);
+	rcsi2_write(priv, V4H_FLDD_REG, 0);
+	rcsi2_write(priv, V4H_IDIC_REG, 0);
+	rcsi2_write(priv, V4H_PHY_MODE_REG, V4H_PHY_MODE_DPHY);
+	rcsi2_write(priv, V4H_N_LANES_REG, lanes - 1);
+
+	rcsi2_write(priv, V4M_FRXM_REG,
+		    V4M_FRXM_FORCERXMODE_0 | V4M_FRXM_FORCERXMODE_1 |
+		    V4M_FRXM_FORCERXMODE_2 | V4M_FRXM_FORCERXMODE_3);
+	rcsi2_write(priv, V4M_OVR1_REG,
+		    V4M_OVR1_FORCERXMODE_0 | V4M_OVR1_FORCERXMODE_1 |
+		    V4M_OVR1_FORCERXMODE_2 | V4M_OVR1_FORCERXMODE_3);
+
+	/* Reset CSI2 */
+	rcsi2_write(priv, V4M_PHTC_REG, 0);
+	rcsi2_write(priv, V4H_CSI2_RESETN_REG, BIT(0));
+
+	/* Common settings */
+	ret = rcsi2_init_common_v4m(priv, mbps);
+	if (ret)
+		return ret;
+
+	/* D-PHY settings */
+	ret = rcsi2_d_phy_setting_v4m(priv, mbps);
+	if (ret)
+		return ret;
+
+	rcsi2_wait_phy_start_v4h(priv, V4H_ST_PHYST_ST_STOPSTATE_0 |
+				 V4H_ST_PHYST_ST_STOPSTATE_1 |
+				 V4H_ST_PHYST_ST_STOPSTATE_2 |
+				 V4H_ST_PHYST_ST_STOPSTATE_3);
+
+	rcsi2_write(priv, V4M_FRXM_REG, 0);
+
+	return 0;
+}
+
+static int rcsi2_start(struct rcar_csi2 *priv, struct v4l2_subdev_state *state)
 {
 	int ret;
 
@@ -1122,13 +1565,14 @@ static int rcsi2_start(struct rcar_csi2 *priv)
 	if (ret < 0)
 		return ret;
 
-	ret = priv->info->start_receiver(priv);
+	ret = priv->info->start_receiver(priv, state);
 	if (ret) {
 		rcsi2_enter_standby(priv);
 		return ret;
 	}
 
-	ret = v4l2_subdev_call(priv->remote, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(priv->remote, priv->remote_pad,
+					 BIT_ULL(0));
 	if (ret) {
 		rcsi2_enter_standby(priv);
 		return ret;
@@ -1140,92 +1584,115 @@ static int rcsi2_start(struct rcar_csi2 *priv)
 static void rcsi2_stop(struct rcar_csi2 *priv)
 {
 	rcsi2_enter_standby(priv);
-	v4l2_subdev_call(priv->remote, video, s_stream, 0);
+	v4l2_subdev_disable_streams(priv->remote, priv->remote_pad, BIT_ULL(0));
 }
 
-static int rcsi2_s_stream(struct v4l2_subdev *sd, int enable)
+static int rcsi2_enable_streams(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state, u32 source_pad,
+				u64 source_streams_mask)
 {
 	struct rcar_csi2 *priv = sd_to_csi2(sd);
 	int ret = 0;
 
-	mutex_lock(&priv->lock);
+	if (source_streams_mask != 1)
+		return -EINVAL;
 
-	if (!priv->remote) {
-		ret = -ENODEV;
-		goto out;
-	}
+	if (!priv->remote)
+		return -ENODEV;
 
-	if (enable && priv->stream_count == 0) {
-		ret = rcsi2_start(priv);
+	if (priv->stream_count == 0) {
+		ret = rcsi2_start(priv, state);
 		if (ret)
-			goto out;
-	} else if (!enable && priv->stream_count == 1) {
-		rcsi2_stop(priv);
+			return ret;
 	}
 
-	priv->stream_count += enable ? 1 : -1;
-out:
-	mutex_unlock(&priv->lock);
+	priv->stream_count += 1;
 
 	return ret;
 }
 
-static int rcsi2_set_pad_format(struct v4l2_subdev *sd,
-				struct v4l2_subdev_state *sd_state,
-				struct v4l2_subdev_format *format)
+static int rcsi2_disable_streams(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *state,
+				 u32 source_pad, u64 source_streams_mask)
 {
 	struct rcar_csi2 *priv = sd_to_csi2(sd);
-	struct v4l2_mbus_framefmt *framefmt;
+	int ret = 0;
 
-	mutex_lock(&priv->lock);
+	if (source_streams_mask != 1)
+		return -EINVAL;
 
-	if (!rcsi2_code_to_fmt(format->format.code))
-		format->format.code = rcar_csi2_formats[0].code;
+	if (!priv->remote)
+		return -ENODEV;
 
-	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		priv->mf = format->format;
-	} else {
-		framefmt = v4l2_subdev_state_get_format(sd_state, 0);
-		*framefmt = format->format;
-	}
+	if (priv->stream_count == 1)
+		rcsi2_stop(priv);
 
-	mutex_unlock(&priv->lock);
+	priv->stream_count -= 1;
 
-	return 0;
+	return ret;
 }
 
-static int rcsi2_get_pad_format(struct v4l2_subdev *sd,
-				struct v4l2_subdev_state *sd_state,
+static int rcsi2_set_pad_format(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state,
 				struct v4l2_subdev_format *format)
 {
 	struct rcar_csi2 *priv = sd_to_csi2(sd);
+	unsigned int num_pads = rcsi2_num_pads(priv);
 
-	mutex_lock(&priv->lock);
+	if (format->pad > RCAR_CSI2_SINK)
+		return v4l2_subdev_get_fmt(sd, state, format);
 
-	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-		format->format = priv->mf;
-	else
-		format->format = *v4l2_subdev_state_get_format(sd_state, 0);
+	if (!rcsi2_code_to_fmt(format->format.code))
+		format->format.code = rcar_csi2_formats[0].code;
+
+	*v4l2_subdev_state_get_format(state, format->pad) = format->format;
 
-	mutex_unlock(&priv->lock);
+	/* Propagate the format to the source pads. */
+	for (unsigned int i = RCAR_CSI2_SOURCE_VC0; i < num_pads; i++)
+		*v4l2_subdev_state_get_format(state, i) = format->format;
 
 	return 0;
 }
 
-static const struct v4l2_subdev_video_ops rcar_csi2_video_ops = {
-	.s_stream = rcsi2_s_stream,
-};
-
 static const struct v4l2_subdev_pad_ops rcar_csi2_pad_ops = {
+	.enable_streams = rcsi2_enable_streams,
+	.disable_streams = rcsi2_disable_streams,
+
 	.set_fmt = rcsi2_set_pad_format,
-	.get_fmt = rcsi2_get_pad_format,
+	.get_fmt = v4l2_subdev_get_fmt,
 };
 
 static const struct v4l2_subdev_ops rcar_csi2_subdev_ops = {
-	.video	= &rcar_csi2_video_ops,
 	.pad	= &rcar_csi2_pad_ops,
 };
 
+static int rcsi2_init_state(struct v4l2_subdev *sd,
+			    struct v4l2_subdev_state *state)
+{
+	struct rcar_csi2 *priv = sd_to_csi2(sd);
+	unsigned int num_pads = rcsi2_num_pads(priv);
+
+	static const struct v4l2_mbus_framefmt rcar_csi2_default_fmt = {
+		.width		= 1920,
+		.height		= 1080,
+		.code		= MEDIA_BUS_FMT_RGB888_1X24,
+		.colorspace	= V4L2_COLORSPACE_SRGB,
+		.field		= V4L2_FIELD_NONE,
+		.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
+		.quantization	= V4L2_QUANTIZATION_DEFAULT,
+		.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
+	};
+
+	for (unsigned int i = RCAR_CSI2_SINK; i < num_pads; i++)
+		*v4l2_subdev_state_get_format(state, i) = rcar_csi2_default_fmt;
+
+	return 0;
+}
+
+static const struct v4l2_subdev_internal_ops rcar_csi2_internal_ops = {
+	.init_state = rcsi2_init_state,
+};
+
 static irqreturn_t rcsi2_irq(int irq, void *data)
 {
 	struct rcar_csi2 *priv = data;
@@ -1251,14 +1718,17 @@ static irqreturn_t rcsi2_irq(int irq, void *data)
 
 static irqreturn_t rcsi2_irq_thread(int irq, void *data)
 {
+	struct v4l2_subdev_state *state;
 	struct rcar_csi2 *priv = data;
 
-	mutex_lock(&priv->lock);
+	state = v4l2_subdev_lock_and_get_active_state(&priv->subdev);
+
 	rcsi2_stop(priv);
 	usleep_range(1000, 2000);
-	if (rcsi2_start(priv))
+	if (rcsi2_start(priv, state))
 		dev_warn(priv->dev, "Failed to restart CSI-2 receiver\n");
-	mutex_unlock(&priv->lock);
+
+	v4l2_subdev_unlock_state(state);
 
 	return IRQ_HANDLED;
 }
@@ -1366,6 +1836,9 @@ static int rcsi2_parse_v4l2(struct rcar_csi2 *priv,
 		}
 	}
 
+	for (i = 0; i < ARRAY_SIZE(priv->line_orders); i++)
+		priv->line_orders[i] = vep->bus.mipi_csi2.line_orders[i];
+
 	return 0;
 }
 
@@ -1425,64 +1898,16 @@ static int rcsi2_parse_dt(struct rcar_csi2 *priv)
  * NOTE: Magic values are from the datasheet and lack documentation.
  */
 
-static int rcsi2_phtw_write(struct rcar_csi2 *priv, u16 data, u16 code)
-{
-	unsigned int timeout;
-
-	rcsi2_write(priv, PHTW_REG,
-		    PHTW_DWEN | PHTW_TESTDIN_DATA(data) |
-		    PHTW_CWEN | PHTW_TESTDIN_CODE(code));
-
-	/* Wait for DWEN and CWEN to be cleared by hardware. */
-	for (timeout = 0; timeout <= 20; timeout++) {
-		if (!(rcsi2_read(priv, PHTW_REG) & (PHTW_DWEN | PHTW_CWEN)))
-			return 0;
-
-		usleep_range(1000, 2000);
-	}
-
-	dev_err(priv->dev, "Timeout waiting for PHTW_DWEN and/or PHTW_CWEN\n");
-
-	return -ETIMEDOUT;
-}
-
-static int rcsi2_phtw_write_array(struct rcar_csi2 *priv,
-				  const struct phtw_value *values)
-{
-	const struct phtw_value *value;
-	int ret;
-
-	for (value = values; value->data || value->code; value++) {
-		ret = rcsi2_phtw_write(priv, value->data, value->code);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-
 static int rcsi2_phtw_write_mbps(struct rcar_csi2 *priv, unsigned int mbps,
-				 const struct rcsi2_mbps_reg *values, u16 code)
+				 const struct rcsi2_mbps_info *values, u8 code)
 {
-	const struct rcsi2_mbps_reg *value;
-	const struct rcsi2_mbps_reg *prev_value = NULL;
-
-	for (value = values; value->mbps; value++) {
-		if (value->mbps >= mbps)
-			break;
-		prev_value = value;
-	}
+	const struct rcsi2_mbps_info *info;
 
-	if (prev_value &&
-	    ((mbps - prev_value->mbps) <= (value->mbps - mbps)))
-		value = prev_value;
-
-	if (!value->mbps) {
-		dev_err(priv->dev, "Unsupported PHY speed (%u Mbps)", mbps);
+	info = rcsi2_mbps_to_info(priv, values, mbps);
+	if (!info)
 		return -ERANGE;
-	}
 
-	return rcsi2_phtw_write(priv, value->reg, code);
+	return rcsi2_phtw_write(priv, info->reg, code);
 }
 
 static int __rcsi2_init_phtw_h3_v3h_m3n(struct rcar_csi2 *priv,
@@ -1494,7 +1919,6 @@ static int __rcsi2_init_phtw_h3_v3h_m3n(struct rcar_csi2 *priv,
 		{ .data = 0x11, .code = 0xe4 },
 		{ .data = 0x01, .code = 0xe5 },
 		{ .data = 0x10, .code = 0x04 },
-		{ /* sentinel */ },
 	};
 
 	static const struct phtw_value step2[] = {
@@ -1503,12 +1927,11 @@ static int __rcsi2_init_phtw_h3_v3h_m3n(struct rcar_csi2 *priv,
 		{ .data = 0x4b, .code = 0xac },
 		{ .data = 0x03, .code = 0x00 },
 		{ .data = 0x80, .code = 0x07 },
-		{ /* sentinel */ },
 	};
 
 	int ret;
 
-	ret = rcsi2_phtw_write_array(priv, step1);
+	ret = rcsi2_phtw_write_array(priv, step1, ARRAY_SIZE(step1));
 	if (ret)
 		return ret;
 
@@ -1523,7 +1946,7 @@ static int __rcsi2_init_phtw_h3_v3h_m3n(struct rcar_csi2 *priv,
 			return ret;
 	}
 
-	return rcsi2_phtw_write_array(priv, step2);
+	return rcsi2_phtw_write_array(priv, step2, ARRAY_SIZE(step2));
 }
 
 static int rcsi2_init_phtw_h3_v3h_m3n(struct rcar_csi2 *priv, unsigned int mbps)
@@ -1549,10 +1972,9 @@ static int rcsi2_phy_post_init_v3m_e3(struct rcar_csi2 *priv)
 		{ .data = 0xee, .code = 0x54 },
 		{ .data = 0xee, .code = 0x84 },
 		{ .data = 0xee, .code = 0x94 },
-		{ /* sentinel */ },
 	};
 
-	return rcsi2_phtw_write_array(priv, step1);
+	return rcsi2_phtw_write_array(priv, step1, ARRAY_SIZE(step1));
 }
 
 static int rcsi2_init_phtw_v3u(struct rcar_csi2 *priv,
@@ -1561,20 +1983,17 @@ static int rcsi2_init_phtw_v3u(struct rcar_csi2 *priv,
 	/* In case of 1500Mbps or less */
 	static const struct phtw_value step1[] = {
 		{ .data = 0xcc, .code = 0xe2 },
-		{ /* sentinel */ },
 	};
 
 	static const struct phtw_value step2[] = {
 		{ .data = 0x01, .code = 0xe3 },
 		{ .data = 0x11, .code = 0xe4 },
 		{ .data = 0x01, .code = 0xe5 },
-		{ /* sentinel */ },
 	};
 
 	/* In case of 1500Mbps or less */
 	static const struct phtw_value step3[] = {
 		{ .data = 0x38, .code = 0x08 },
-		{ /* sentinel */ },
 	};
 
 	static const struct phtw_value step4[] = {
@@ -1582,29 +2001,28 @@ static int rcsi2_init_phtw_v3u(struct rcar_csi2 *priv,
 		{ .data = 0x4b, .code = 0xac },
 		{ .data = 0x03, .code = 0x00 },
 		{ .data = 0x80, .code = 0x07 },
-		{ /* sentinel */ },
 	};
 
 	int ret;
 
 	if (mbps != 0 && mbps <= 1500)
-		ret = rcsi2_phtw_write_array(priv, step1);
+		ret = rcsi2_phtw_write_array(priv, step1, ARRAY_SIZE(step1));
 	else
 		ret = rcsi2_phtw_write_mbps(priv, mbps, phtw_mbps_v3u, 0xe2);
 	if (ret)
 		return ret;
 
-	ret = rcsi2_phtw_write_array(priv, step2);
+	ret = rcsi2_phtw_write_array(priv, step2, ARRAY_SIZE(step2));
 	if (ret)
 		return ret;
 
 	if (mbps != 0 && mbps <= 1500) {
-		ret = rcsi2_phtw_write_array(priv, step3);
+		ret = rcsi2_phtw_write_array(priv, step3, ARRAY_SIZE(step3));
 		if (ret)
 			return ret;
 	}
 
-	ret = rcsi2_phtw_write_array(priv, step4);
+	ret = rcsi2_phtw_write_array(priv, step4, ARRAY_SIZE(step4));
 	if (ret)
 		return ret;
 
@@ -1688,7 +2106,13 @@ static int rcsi2_probe_resources(struct rcar_csi2 *priv,
 	return PTR_ERR_OR_ZERO(priv->rstc);
 }
 
+static const struct rcsi2_register_layout rcsi2_registers_gen3 = {
+	.phtw = PHTW_REG,
+	.phypll = PHYPLL_REG,
+};
+
 static const struct rcar_csi2_info rcar_csi2_info_r8a7795 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_h3_v3h_m3n,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
@@ -1700,6 +2124,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a7795 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a7795es2 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_h3es2,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
@@ -1711,6 +2136,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a7795es2 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a7796 = {
+	.regs = &rcsi2_registers_gen3,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
 	.hsfreqrange = hsfreqrange_m3w,
@@ -1719,6 +2145,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a7796 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a77961 = {
+	.regs = &rcsi2_registers_gen3,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
 	.hsfreqrange = hsfreqrange_m3w,
@@ -1727,6 +2154,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a77961 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a77965 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_h3_v3h_m3n,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
@@ -1738,6 +2166,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a77965 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a77970 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_v3m_e3,
 	.phy_post_init = rcsi2_phy_post_init_v3m_e3,
 	.start_receiver = rcsi2_start_receiver_gen3,
@@ -1747,6 +2176,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a77970 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a77980 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_h3_v3h_m3n,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
@@ -1757,6 +2187,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a77980 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a77990 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_v3m_e3,
 	.phy_post_init = rcsi2_phy_post_init_v3m_e3,
 	.start_receiver = rcsi2_start_receiver_gen3,
@@ -1766,6 +2197,7 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a77990 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a779a0 = {
+	.regs = &rcsi2_registers_gen3,
 	.init_phtw = rcsi2_init_phtw_v3u,
 	.start_receiver = rcsi2_start_receiver_gen3,
 	.enter_standby = rcsi2_enter_standby_gen3,
@@ -1777,11 +2209,26 @@ static const struct rcar_csi2_info rcar_csi2_info_r8a779a0 = {
 };
 
 static const struct rcar_csi2_info rcar_csi2_info_r8a779g0 = {
+	.regs = &rcsi2_registers_gen3,
 	.start_receiver = rcsi2_start_receiver_v4h,
 	.use_isp = true,
 	.support_cphy = true,
 };
 
+static const struct rcsi2_register_layout rcsi2_registers_v4m = {
+	.phtw = V4M_PHTW_REG,
+	.phypll = V4M_PHYPLL_REG,
+};
+
+static const struct rcar_csi2_info rcar_csi2_info_r8a779h0 = {
+	.regs = &rcsi2_registers_v4m,
+	.start_receiver = rcsi2_start_receiver_v4m,
+	.hsfreqrange = hsfreqrange_v4m,
+	.csi0clkfreqrange = 0x0c,
+	.use_isp = true,
+	.support_dphy = true,
+};
+
 static const struct of_device_id rcar_csi2_of_table[] = {
 	{
 		.compatible = "renesas,r8a774a1-csi2",
@@ -1835,6 +2282,10 @@ static const struct of_device_id rcar_csi2_of_table[] = {
 		.compatible = "renesas,r8a779g0-csi2",
 		.data = &rcar_csi2_info_r8a779g0,
 	},
+	{
+		.compatible = "renesas,r8a779h0-csi2",
+		.data = &rcar_csi2_info_r8a779h0,
+	},
 	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, rcar_csi2_of_table);
@@ -1870,23 +2321,23 @@ static int rcsi2_probe(struct platform_device *pdev)
 
 	priv->dev = &pdev->dev;
 
-	mutex_init(&priv->lock);
 	priv->stream_count = 0;
 
 	ret = rcsi2_probe_resources(priv, pdev);
 	if (ret) {
 		dev_err(priv->dev, "Failed to get resources\n");
-		goto error_mutex;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, priv);
 
 	ret = rcsi2_parse_dt(priv);
 	if (ret)
-		goto error_mutex;
+		return ret;
 
 	priv->subdev.owner = THIS_MODULE;
 	priv->subdev.dev = &pdev->dev;
+	priv->subdev.internal_ops = &rcar_csi2_internal_ops;
 	v4l2_subdev_init(&priv->subdev, &rcar_csi2_subdev_ops);
 	v4l2_set_subdevdata(&priv->subdev, &pdev->dev);
 	snprintf(priv->subdev.name, sizeof(priv->subdev.name), "%s %s",
@@ -1896,7 +2347,7 @@ static int rcsi2_probe(struct platform_device *pdev)
 	priv->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 	priv->subdev.entity.ops = &rcar_csi2_entity_ops;
 
-	num_pads = priv->info->use_isp ? 2 : NR_OF_RCAR_CSI2_PAD;
+	num_pads = rcsi2_num_pads(priv);
 
 	priv->pads[RCAR_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
 	for (i = RCAR_CSI2_SOURCE_VC0; i < num_pads; i++)
@@ -1912,19 +2363,25 @@ static int rcsi2_probe(struct platform_device *pdev)
 
 	pm_runtime_enable(&pdev->dev);
 
+	ret = v4l2_subdev_init_finalize(&priv->subdev);
+	if (ret)
+		goto error_pm_runtime;
+
 	ret = v4l2_async_register_subdev(&priv->subdev);
 	if (ret < 0)
-		goto error_async;
+		goto error_subdev;
 
 	dev_info(priv->dev, "%d lanes found\n", priv->lanes);
 
 	return 0;
 
+error_subdev:
+	v4l2_subdev_cleanup(&priv->subdev);
+error_pm_runtime:
+	pm_runtime_disable(&pdev->dev);
 error_async:
 	v4l2_async_nf_unregister(&priv->notifier);
 	v4l2_async_nf_cleanup(&priv->notifier);
-error_mutex:
-	mutex_destroy(&priv->lock);
 
 	return ret;
 }
@@ -1936,14 +2393,13 @@ static void rcsi2_remove(struct platform_device *pdev)
 	v4l2_async_nf_unregister(&priv->notifier);
 	v4l2_async_nf_cleanup(&priv->notifier);
 	v4l2_async_unregister_subdev(&priv->subdev);
+	v4l2_subdev_cleanup(&priv->subdev);
 
 	pm_runtime_disable(&pdev->dev);
-
-	mutex_destroy(&priv->lock);
 }
 
 static struct platform_driver rcar_csi2_pdrv = {
-	.remove_new = rcsi2_remove,
+	.remove = rcsi2_remove,
 	.probe	= rcsi2_probe,
 	.driver	= {
 		.name	= "rcar-csi2",
diff --git a/drivers/media/platform/renesas/rcar-fcp.c b/drivers/media/platform/renesas/rcar-fcp.c
index bcef7b87da7c..cee9bbce4e3a 100644
--- a/drivers/media/platform/renesas/rcar-fcp.c
+++ b/drivers/media/platform/renesas/rcar-fcp.c
@@ -164,7 +164,7 @@ MODULE_DEVICE_TABLE(of, rcar_fcp_of_match);
 
 static struct platform_driver rcar_fcp_platform_driver = {
 	.probe		= rcar_fcp_probe,
-	.remove_new	= rcar_fcp_remove,
+	.remove		= rcar_fcp_remove,
 	.driver		= {
 		.name	= "rcar-fcp",
 		.of_match_table = rcar_fcp_of_match,
diff --git a/drivers/media/platform/renesas/rcar-isp/Kconfig b/drivers/media/platform/renesas/rcar-isp/Kconfig
new file mode 100644
index 000000000000..242f6a23851f
--- /dev/null
+++ b/drivers/media/platform/renesas/rcar-isp/Kconfig
@@ -0,0 +1,18 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config VIDEO_RCAR_ISP
+	tristate "R-Car Image Signal Processor (ISP)"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV && OF
+	depends on ARCH_RENESAS || COMPILE_TEST
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select RESET_CONTROLLER
+	select V4L2_FWNODE
+	help
+	  Support for Renesas R-Car Image Signal Processor (ISP).
+	  Enable this to support the Renesas R-Car Image Signal
+	  Processor (ISP).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rcar-isp.
diff --git a/drivers/media/platform/renesas/rcar-isp/Makefile b/drivers/media/platform/renesas/rcar-isp/Makefile
new file mode 100644
index 000000000000..b542118c831e
--- /dev/null
+++ b/drivers/media/platform/renesas/rcar-isp/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+rcar-isp-objs = csisp.o
+
+obj-$(CONFIG_VIDEO_RCAR_ISP) += rcar-isp.o
diff --git a/drivers/media/platform/renesas/rcar-isp.c b/drivers/media/platform/renesas/rcar-isp/csisp.c
index 4512ac338ca5..1eb29a0b774a 100644
--- a/drivers/media/platform/renesas/rcar-isp.c
+++ b/drivers/media/platform/renesas/rcar-isp/csisp.c
@@ -76,6 +76,54 @@ static const struct rcar_isp_format rcar_isp_formats[] = {
 		.code = MEDIA_BUS_FMT_YUYV10_2X10,
 		.datatype = MIPI_CSI2_DT_YUV422_8B,
 		.procmode = 0x0c,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.procmode = 0x00,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.procmode = 0x00,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.procmode = 0x00,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.procmode = 0x00,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.procmode = 0x01,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.procmode = 0x01,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.procmode = 0x01,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB10_1X10,
+		.datatype = MIPI_CSI2_DT_RAW10,
+		.procmode = 0x01,
+	}, {
+		.code = MEDIA_BUS_FMT_SBGGR12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.procmode = 0x02,
+	}, {
+		.code = MEDIA_BUS_FMT_SGBRG12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.procmode = 0x02,
+	}, {
+		.code = MEDIA_BUS_FMT_SGRBG12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.procmode = 0x02,
+	}, {
+		.code = MEDIA_BUS_FMT_SRGGB12_1X12,
+		.datatype = MIPI_CSI2_DT_RAW12,
+		.procmode = 0x02,
 	},
 };
 
@@ -111,7 +159,7 @@ enum rcar_isp_pads {
 
 struct rcar_isp {
 	struct device *dev;
-	void __iomem *base;
+	void __iomem *csbase;
 	struct reset_control *rstc;
 
 	enum rcar_isp_input csi_input;
@@ -121,9 +169,8 @@ struct rcar_isp {
 
 	struct v4l2_async_notifier notifier;
 	struct v4l2_subdev *remote;
+	unsigned int remote_pad;
 
-	struct mutex lock; /* Protects mf and stream_count. */
-	struct v4l2_mbus_framefmt mf;
 	int stream_count;
 };
 
@@ -137,14 +184,14 @@ static inline struct rcar_isp *notifier_to_isp(struct v4l2_async_notifier *n)
 	return container_of(n, struct rcar_isp, notifier);
 }
 
-static void risp_write(struct rcar_isp *isp, u32 offset, u32 value)
+static void risp_write_cs(struct rcar_isp *isp, u32 offset, u32 value)
 {
-	iowrite32(value, isp->base + offset);
+	iowrite32(value, isp->csbase + offset);
 }
 
-static u32 risp_read(struct rcar_isp *isp, u32 offset)
+static u32 risp_read_cs(struct rcar_isp *isp, u32 offset)
 {
-	return ioread32(isp->base + offset);
+	return ioread32(isp->csbase + offset);
 }
 
 static int risp_power_on(struct rcar_isp *isp)
@@ -170,14 +217,19 @@ static void risp_power_off(struct rcar_isp *isp)
 	pm_runtime_put(isp->dev);
 }
 
-static int risp_start(struct rcar_isp *isp)
+static int risp_start(struct rcar_isp *isp, struct v4l2_subdev_state *state)
 {
+	const struct v4l2_mbus_framefmt *fmt;
 	const struct rcar_isp_format *format;
 	unsigned int vc;
 	u32 sel_csi = 0;
 	int ret;
 
-	format = risp_code_to_fmt(isp->mf.code);
+	fmt = v4l2_subdev_state_get_format(state, RCAR_ISP_SINK);
+	if (!fmt)
+		return -EINVAL;
+
+	format = risp_code_to_fmt(fmt->code);
 	if (!format) {
 		dev_err(isp->dev, "Unsupported bus format\n");
 		return -EINVAL;
@@ -193,33 +245,34 @@ static int risp_start(struct rcar_isp *isp)
 	if (isp->csi_input == RISP_CSI_INPUT1)
 		sel_csi = ISPINPUTSEL0_SEL_CSI0;
 
-	risp_write(isp, ISPINPUTSEL0_REG,
-		   risp_read(isp, ISPINPUTSEL0_REG) | sel_csi);
+	risp_write_cs(isp, ISPINPUTSEL0_REG,
+		      risp_read_cs(isp, ISPINPUTSEL0_REG) | sel_csi);
 
 	/* Configure Channel Selector. */
 	for (vc = 0; vc < 4; vc++) {
 		u8 ch = vc + 4;
 		u8 dt = format->datatype;
 
-		risp_write(isp, ISPCS_FILTER_ID_CH_REG(ch), BIT(vc));
-		risp_write(isp, ISPCS_DT_CODE03_CH_REG(ch),
-			   ISPCS_DT_CODE03_EN3 | ISPCS_DT_CODE03_DT3(dt) |
-			   ISPCS_DT_CODE03_EN2 | ISPCS_DT_CODE03_DT2(dt) |
-			   ISPCS_DT_CODE03_EN1 | ISPCS_DT_CODE03_DT1(dt) |
-			   ISPCS_DT_CODE03_EN0 | ISPCS_DT_CODE03_DT0(dt));
+		risp_write_cs(isp, ISPCS_FILTER_ID_CH_REG(ch), BIT(vc));
+		risp_write_cs(isp, ISPCS_DT_CODE03_CH_REG(ch),
+			      ISPCS_DT_CODE03_EN3 | ISPCS_DT_CODE03_DT3(dt) |
+			      ISPCS_DT_CODE03_EN2 | ISPCS_DT_CODE03_DT2(dt) |
+			      ISPCS_DT_CODE03_EN1 | ISPCS_DT_CODE03_DT1(dt) |
+			      ISPCS_DT_CODE03_EN0 | ISPCS_DT_CODE03_DT0(dt));
 	}
 
 	/* Setup processing method. */
-	risp_write(isp, ISPPROCMODE_DT_REG(format->datatype),
-		   ISPPROCMODE_DT_PROC_MODE_VC3(format->procmode) |
-		   ISPPROCMODE_DT_PROC_MODE_VC2(format->procmode) |
-		   ISPPROCMODE_DT_PROC_MODE_VC1(format->procmode) |
-		   ISPPROCMODE_DT_PROC_MODE_VC0(format->procmode));
+	risp_write_cs(isp, ISPPROCMODE_DT_REG(format->datatype),
+		      ISPPROCMODE_DT_PROC_MODE_VC3(format->procmode) |
+		      ISPPROCMODE_DT_PROC_MODE_VC2(format->procmode) |
+		      ISPPROCMODE_DT_PROC_MODE_VC1(format->procmode) |
+		      ISPPROCMODE_DT_PROC_MODE_VC0(format->procmode));
 
 	/* Start ISP. */
-	risp_write(isp, ISPSTART_REG, ISPSTART_START);
+	risp_write_cs(isp, ISPSTART_REG, ISPSTART_START);
 
-	ret = v4l2_subdev_call(isp->remote, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(isp->remote, isp->remote_pad,
+					 BIT_ULL(0));
 	if (ret)
 		risp_power_off(isp);
 
@@ -228,95 +281,87 @@ static int risp_start(struct rcar_isp *isp)
 
 static void risp_stop(struct rcar_isp *isp)
 {
-	v4l2_subdev_call(isp->remote, video, s_stream, 0);
+	v4l2_subdev_disable_streams(isp->remote, isp->remote_pad, BIT_ULL(0));
 
 	/* Stop ISP. */
-	risp_write(isp, ISPSTART_REG, ISPSTART_STOP);
+	risp_write_cs(isp, ISPSTART_REG, ISPSTART_STOP);
 
 	risp_power_off(isp);
 }
 
-static int risp_s_stream(struct v4l2_subdev *sd, int enable)
+static int risp_enable_streams(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state, u32 source_pad,
+			       u64 source_streams_mask)
 {
 	struct rcar_isp *isp = sd_to_isp(sd);
 	int ret = 0;
 
-	mutex_lock(&isp->lock);
+	if (source_streams_mask != 1)
+		return -EINVAL;
 
-	if (!isp->remote) {
-		ret = -ENODEV;
-		goto out;
-	}
+	if (!isp->remote)
+		return -ENODEV;
 
-	if (enable && isp->stream_count == 0) {
-		ret = risp_start(isp);
+	if (isp->stream_count == 0) {
+		ret = risp_start(isp, state);
 		if (ret)
-			goto out;
-	} else if (!enable && isp->stream_count == 1) {
-		risp_stop(isp);
+			return ret;
 	}
 
-	isp->stream_count += enable ? 1 : -1;
-out:
-	mutex_unlock(&isp->lock);
+	isp->stream_count += 1;
 
 	return ret;
 }
 
-static const struct v4l2_subdev_video_ops risp_video_ops = {
-	.s_stream = risp_s_stream,
-};
-
-static int risp_set_pad_format(struct v4l2_subdev *sd,
-			       struct v4l2_subdev_state *sd_state,
-			       struct v4l2_subdev_format *format)
+static int risp_disable_streams(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state, u32 source_pad,
+				u64 source_streams_mask)
 {
 	struct rcar_isp *isp = sd_to_isp(sd);
-	struct v4l2_mbus_framefmt *framefmt;
 
-	mutex_lock(&isp->lock);
+	if (source_streams_mask != 1)
+		return -EINVAL;
 
-	if (!risp_code_to_fmt(format->format.code))
-		format->format.code = rcar_isp_formats[0].code;
+	if (!isp->remote)
+		return -ENODEV;
 
-	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		isp->mf = format->format;
-	} else {
-		framefmt = v4l2_subdev_state_get_format(sd_state, 0);
-		*framefmt = format->format;
-	}
+	if (isp->stream_count == 1)
+		risp_stop(isp);
 
-	mutex_unlock(&isp->lock);
+	isp->stream_count -= 1;
 
 	return 0;
 }
 
-static int risp_get_pad_format(struct v4l2_subdev *sd,
-			       struct v4l2_subdev_state *sd_state,
+static int risp_set_pad_format(struct v4l2_subdev *sd,
+			       struct v4l2_subdev_state *state,
 			       struct v4l2_subdev_format *format)
 {
-	struct rcar_isp *isp = sd_to_isp(sd);
+	struct v4l2_mbus_framefmt *framefmt;
 
-	mutex_lock(&isp->lock);
+	if (format->pad > RCAR_ISP_SINK)
+		return v4l2_subdev_get_fmt(sd, state, format);
 
-	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-		format->format = isp->mf;
-	else
-		format->format = *v4l2_subdev_state_get_format(sd_state, 0);
+	if (!risp_code_to_fmt(format->format.code))
+		format->format.code = rcar_isp_formats[0].code;
 
-	mutex_unlock(&isp->lock);
+	for (unsigned int i = 0; i < RCAR_ISP_NUM_PADS; i++) {
+		framefmt = v4l2_subdev_state_get_format(state, i);
+		*framefmt = format->format;
+	}
 
 	return 0;
 }
 
 static const struct v4l2_subdev_pad_ops risp_pad_ops = {
+	.enable_streams = risp_enable_streams,
+	.disable_streams = risp_disable_streams,
 	.set_fmt = risp_set_pad_format,
-	.get_fmt = risp_get_pad_format,
+	.get_fmt = v4l2_subdev_get_fmt,
 	.link_validate = v4l2_subdev_link_validate_default,
 };
 
 static const struct v4l2_subdev_ops rcar_isp_subdev_ops = {
-	.video	= &risp_video_ops,
 	.pad	= &risp_pad_ops,
 };
 
@@ -339,6 +384,7 @@ static int risp_notify_bound(struct v4l2_async_notifier *notifier,
 	}
 
 	isp->remote = subdev;
+	isp->remote_pad = pad;
 
 	dev_dbg(isp->dev, "Bound %s pad: %d\n", subdev->name, pad);
 
@@ -419,9 +465,20 @@ static const struct media_entity_operations risp_entity_ops = {
 static int risp_probe_resources(struct rcar_isp *isp,
 				struct platform_device *pdev)
 {
-	isp->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
-	if (IS_ERR(isp->base))
-		return PTR_ERR(isp->base);
+	struct resource *res;
+
+	/*
+	 * For backward compatibility allow cs base to be the only reg if no
+	 * reg-names are set in DT.
+	 */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "cs");
+	if (!res)
+		isp->csbase = devm_platform_ioremap_resource(pdev, 0);
+	else
+		isp->csbase = devm_ioremap_resource(&pdev->dev, res);
+
+	if (IS_ERR(isp->csbase))
+		return PTR_ERR(isp->csbase);
 
 	isp->rstc = devm_reset_control_get(&pdev->dev, NULL);
 
@@ -431,7 +488,9 @@ static int risp_probe_resources(struct rcar_isp *isp,
 static const struct of_device_id risp_of_id_table[] = {
 	{ .compatible = "renesas,r8a779a0-isp" },
 	{ .compatible = "renesas,r8a779g0-isp" },
-	{ /* sentinel */ },
+	/* Keep above for compatibility with old DTB files. */
+	{ .compatible = "renesas,rcar-gen4-isp" },
+	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, risp_of_id_table);
 
@@ -447,12 +506,10 @@ static int risp_probe(struct platform_device *pdev)
 
 	isp->dev = &pdev->dev;
 
-	mutex_init(&isp->lock);
-
 	ret = risp_probe_resources(isp, pdev);
 	if (ret) {
 		dev_err(isp->dev, "Failed to get resources\n");
-		goto error_mutex;
+		return ret;
 	}
 
 	platform_set_drvdata(pdev, isp);
@@ -483,20 +540,25 @@ static int risp_probe(struct platform_device *pdev)
 	if (ret)
 		goto error_notifier;
 
+	ret = v4l2_subdev_init_finalize(&isp->subdev);
+	if (ret)
+		goto error_notifier;
+
 	ret = v4l2_async_register_subdev(&isp->subdev);
 	if (ret < 0)
-		goto error_notifier;
+		goto error_subdev;
 
 	dev_info(isp->dev, "Using CSI-2 input: %u\n", isp->csi_input);
 
 	return 0;
+
+error_subdev:
+	v4l2_subdev_cleanup(&isp->subdev);
 error_notifier:
 	v4l2_async_nf_unregister(&isp->notifier);
 	v4l2_async_nf_cleanup(&isp->notifier);
 error_pm:
 	pm_runtime_disable(&pdev->dev);
-error_mutex:
-	mutex_destroy(&isp->lock);
 
 	return ret;
 }
@@ -509,10 +571,9 @@ static void risp_remove(struct platform_device *pdev)
 	v4l2_async_nf_cleanup(&isp->notifier);
 
 	v4l2_async_unregister_subdev(&isp->subdev);
+	v4l2_subdev_cleanup(&isp->subdev);
 
 	pm_runtime_disable(&pdev->dev);
-
-	mutex_destroy(&isp->lock);
 }
 
 static struct platform_driver rcar_isp_driver = {
@@ -522,7 +583,7 @@ static struct platform_driver rcar_isp_driver = {
 		.of_match_table = risp_of_id_table,
 	},
 	.probe = risp_probe,
-	.remove_new = risp_remove,
+	.remove = risp_remove,
 };
 
 module_platform_driver(rcar_isp_driver);
diff --git a/drivers/media/platform/renesas/rcar-vin/rcar-core.c b/drivers/media/platform/renesas/rcar-vin/rcar-core.c
index 809c3a38cc4a..846ae7989b1d 100644
--- a/drivers/media/platform/renesas/rcar-vin/rcar-core.c
+++ b/drivers/media/platform/renesas/rcar-vin/rcar-core.c
@@ -1080,13 +1080,11 @@ static int __maybe_unused rvin_suspend(struct device *dev)
 {
 	struct rvin_dev *vin = dev_get_drvdata(dev);
 
-	if (vin->state != RUNNING)
+	if (!vin->running)
 		return 0;
 
 	rvin_stop_streaming(vin);
 
-	vin->state = SUSPENDED;
-
 	return 0;
 }
 
@@ -1094,7 +1092,7 @@ static int __maybe_unused rvin_resume(struct device *dev)
 {
 	struct rvin_dev *vin = dev_get_drvdata(dev);
 
-	if (vin->state != SUSPENDED)
+	if (!vin->running)
 		return 0;
 
 	/*
@@ -1274,20 +1272,12 @@ static const struct rvin_info rcar_info_r8a77995 = {
 	.scaler = rvin_scaler_gen3,
 };
 
-static const struct rvin_info rcar_info_r8a779a0 = {
-	.model = RCAR_GEN3,
-	.use_mc = true,
-	.use_isp = true,
-	.nv12 = true,
-	.max_width = 4096,
-	.max_height = 4096,
-};
-
-static const struct rvin_info rcar_info_r8a779g0 = {
-	.model = RCAR_GEN3,
+static const struct rvin_info rcar_info_gen4 = {
+	.model = RCAR_GEN4,
 	.use_mc = true,
 	.use_isp = true,
 	.nv12 = true,
+	.raw10 = true,
 	.max_width = 4096,
 	.max_height = 4096,
 };
@@ -1354,12 +1344,18 @@ static const struct of_device_id rvin_of_id_table[] = {
 		.data = &rcar_info_r8a77995,
 	},
 	{
+		/* Keep to be compatible with old DTS files. */
 		.compatible = "renesas,vin-r8a779a0",
-		.data = &rcar_info_r8a779a0,
+		.data = &rcar_info_gen4,
 	},
 	{
+		/* Keep to be compatible with old DTS files. */
 		.compatible = "renesas,vin-r8a779g0",
-		.data = &rcar_info_r8a779g0,
+		.data = &rcar_info_gen4,
+	},
+	{
+		.compatible = "renesas,rcar-gen4-vin",
+		.data = &rcar_info_gen4,
 	},
 	{ /* Sentinel */ },
 };
@@ -1446,7 +1442,7 @@ static struct platform_driver rcar_vin_driver = {
 		.of_match_table = rvin_of_id_table,
 	},
 	.probe = rcar_vin_probe,
-	.remove_new = rcar_vin_remove,
+	.remove = rcar_vin_remove,
 };
 
 module_platform_driver(rcar_vin_driver);
diff --git a/drivers/media/platform/renesas/rcar-vin/rcar-dma.c b/drivers/media/platform/renesas/rcar-vin/rcar-dma.c
index e2c40abc6d3d..5c08ee2c9807 100644
--- a/drivers/media/platform/renesas/rcar-vin/rcar-dma.c
+++ b/drivers/media/platform/renesas/rcar-vin/rcar-dma.c
@@ -94,6 +94,7 @@
 #define VNMC_INF_YUV16		(5 << 16)
 #define VNMC_INF_RGB888		(6 << 16)
 #define VNMC_INF_RGB666		(7 << 16)
+#define VNMC_EXINF_RAW8		(1 << 12) /* Gen4 specific */
 #define VNMC_VUP		(1 << 10)
 #define VNMC_IM_ODD		(0 << 3)
 #define VNMC_IM_ODD_EVEN	(1 << 3)
@@ -123,7 +124,9 @@
 /* Video n Data Mode Register bits */
 #define VNDMR_A8BIT(n)		(((n) & 0xff) << 24)
 #define VNDMR_A8BIT_MASK	(0xff << 24)
+#define VNDMR_RMODE_RAW10	(2 << 19)
 #define VNDMR_YMODE_Y8		(1 << 12)
+#define VNDMR_YC_THR		(1 << 11)
 #define VNDMR_EXRGB		(1 << 8)
 #define VNDMR_BPSM		(1 << 4)
 #define VNDMR_ABIT		(1 << 2)
@@ -640,8 +643,6 @@ void rvin_scaler_gen3(struct rvin_dev *vin)
 	case V4L2_FIELD_INTERLACED_TB:
 	case V4L2_FIELD_INTERLACED_BT:
 	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_SEQ_TB:
-	case V4L2_FIELD_SEQ_BT:
 		clip_size |= vin->compose.height / 2;
 		break;
 	default:
@@ -677,22 +678,6 @@ void rvin_crop_scale_comp(struct rvin_dev *vin)
 
 	fmt = rvin_format_from_pixel(vin, vin->format.pixelformat);
 	stride = vin->format.bytesperline / fmt->bpp;
-
-	/* For RAW8 format bpp is 1, but the hardware process RAW8
-	 * format in 2 pixel unit hence configure VNIS_REG as stride / 2.
-	 */
-	switch (vin->format.pixelformat) {
-	case V4L2_PIX_FMT_SBGGR8:
-	case V4L2_PIX_FMT_SGBRG8:
-	case V4L2_PIX_FMT_SGRBG8:
-	case V4L2_PIX_FMT_SRGGB8:
-	case V4L2_PIX_FMT_GREY:
-		stride /= 2;
-		break;
-	default:
-		break;
-	}
-
 	rvin_write(vin, stride, VNIS_REG);
 }
 
@@ -725,8 +710,6 @@ static int rvin_setup(struct rvin_dev *vin)
 	case V4L2_FIELD_INTERLACED_BT:
 		vnmc = VNMC_IM_FULL | VNMC_FOC;
 		break;
-	case V4L2_FIELD_SEQ_TB:
-	case V4L2_FIELD_SEQ_BT:
 	case V4L2_FIELD_NONE:
 	case V4L2_FIELD_ALTERNATE:
 		vnmc = VNMC_IM_ODD_EVEN;
@@ -742,12 +725,22 @@ static int rvin_setup(struct rvin_dev *vin)
 	 */
 	switch (vin->mbus_code) {
 	case MEDIA_BUS_FMT_YUYV8_1X16:
-		/* BT.601/BT.1358 16bit YCbCr422 */
-		vnmc |= VNMC_INF_YUV16;
+		if (vin->is_csi)
+			/* YCbCr422 8-bit */
+			vnmc |= VNMC_INF_YUV8_BT601;
+		else
+			/* BT.601/BT.1358 16bit YCbCr422 */
+			vnmc |= VNMC_INF_YUV16;
 		input_is_yuv = true;
 		break;
 	case MEDIA_BUS_FMT_UYVY8_1X16:
-		vnmc |= VNMC_INF_YUV16 | VNMC_YCAL;
+		if (vin->is_csi)
+			/* YCbCr422 8-bit */
+			vnmc |= VNMC_INF_YUV8_BT601;
+		else
+			/* BT.601/BT.1358 16bit YCbCr422 */
+			vnmc |= VNMC_INF_YUV16;
+		vnmc |= VNMC_YCAL;
 		input_is_yuv = true;
 		break;
 	case MEDIA_BUS_FMT_UYVY8_2X8:
@@ -779,36 +772,21 @@ static int rvin_setup(struct rvin_dev *vin)
 	case MEDIA_BUS_FMT_SRGGB8_1X8:
 	case MEDIA_BUS_FMT_Y8_1X8:
 		vnmc |= VNMC_INF_RAW8;
+		if (vin->info->model == RCAR_GEN4)
+			vnmc |= VNMC_EXINF_RAW8;
+		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		vnmc |= VNMC_INF_RGB666;
 		break;
 	default:
 		break;
 	}
 
-	/* Make sure input interface and input format is valid. */
-	if (vin->info->model == RCAR_GEN3) {
-		switch (vnmc & VNMC_INF_MASK) {
-		case VNMC_INF_YUV8_BT656:
-		case VNMC_INF_YUV10_BT656:
-		case VNMC_INF_YUV16:
-		case VNMC_INF_RGB666:
-			if (vin->is_csi) {
-				vin_err(vin, "Invalid setting in MIPI CSI2\n");
-				return -EINVAL;
-			}
-			break;
-		case VNMC_INF_RAW8:
-			if (!vin->is_csi) {
-				vin_err(vin, "Invalid setting in Digital Pins\n");
-				return -EINVAL;
-			}
-			break;
-		default:
-			break;
-		}
-	}
-
 	/* Enable VSYNC Field Toggle mode after one VSYNC input */
-	if (vin->info->model == RCAR_GEN3)
+	if (vin->info->model == RCAR_GEN3 || vin->info->model == RCAR_GEN4)
 		dmr2 = VNDMR2_FTEV;
 	else
 		dmr2 = VNDMR2_FTEV | VNDMR2_VLV(1);
@@ -888,6 +866,12 @@ static int rvin_setup(struct rvin_dev *vin)
 			dmr = 0;
 		}
 		break;
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+		dmr = VNDMR_RMODE_RAW10;
+		break;
 	default:
 		vin_err(vin, "Invalid pixelformat (0x%x)\n",
 			vin->format.pixelformat);
@@ -902,7 +886,7 @@ static int rvin_setup(struct rvin_dev *vin)
 		if (input_is_yuv == output_is_yuv)
 			vnmc |= VNMC_BPS;
 
-		if (vin->info->model == RCAR_GEN3) {
+		if (vin->info->model == RCAR_GEN3 || vin->info->model == RCAR_GEN4) {
 			/* Select between CSI-2 and parallel input */
 			if (vin->is_csi)
 				vnmc &= ~VNMC_DPINE;
@@ -997,33 +981,13 @@ static void rvin_fill_hw_slot(struct rvin_dev *vin, int slot)
 	struct rvin_buffer *buf;
 	struct vb2_v4l2_buffer *vbuf;
 	dma_addr_t phys_addr;
-	int prev;
 
 	/* A already populated slot shall never be overwritten. */
 	if (WARN_ON(vin->buf_hw[slot].buffer))
 		return;
 
-	prev = (slot == 0 ? HW_BUFFER_NUM : slot) - 1;
-
-	if (vin->buf_hw[prev].type == HALF_TOP) {
-		vbuf = vin->buf_hw[prev].buffer;
-		vin->buf_hw[slot].buffer = vbuf;
-		vin->buf_hw[slot].type = HALF_BOTTOM;
-		switch (vin->format.pixelformat) {
-		case V4L2_PIX_FMT_NV12:
-		case V4L2_PIX_FMT_NV16:
-			phys_addr = vin->buf_hw[prev].phys +
-				vin->format.sizeimage / 4;
-			break;
-		default:
-			phys_addr = vin->buf_hw[prev].phys +
-				vin->format.sizeimage / 2;
-			break;
-		}
-	} else if ((vin->state != STOPPED && vin->state != RUNNING) ||
-		   list_empty(&vin->buf_list)) {
+	if (list_empty(&vin->buf_list)) {
 		vin->buf_hw[slot].buffer = NULL;
-		vin->buf_hw[slot].type = FULL;
 		phys_addr = vin->scratch_phys;
 	} else {
 		/* Keep track of buffer we give to HW */
@@ -1032,16 +996,12 @@ static void rvin_fill_hw_slot(struct rvin_dev *vin, int slot)
 		list_del_init(to_buf_list(vbuf));
 		vin->buf_hw[slot].buffer = vbuf;
 
-		vin->buf_hw[slot].type =
-			V4L2_FIELD_IS_SEQUENTIAL(vin->format.field) ?
-			HALF_TOP : FULL;
-
 		/* Setup DMA */
 		phys_addr = vb2_dma_contig_plane_dma_addr(&vbuf->vb2_buf, 0);
 	}
 
-	vin_dbg(vin, "Filling HW slot: %d type: %d buffer: %p\n",
-		slot, vin->buf_hw[slot].type, vin->buf_hw[slot].buffer);
+	vin_dbg(vin, "Filling HW slot: %d buffer: %p\n",
+		slot, vin->buf_hw[slot].buffer);
 
 	vin->buf_hw[slot].phys = phys_addr;
 	rvin_set_slot_addr(vin, slot, phys_addr);
@@ -1049,15 +1009,12 @@ static void rvin_fill_hw_slot(struct rvin_dev *vin, int slot)
 
 static int rvin_capture_start(struct rvin_dev *vin)
 {
-	int slot, ret;
+	int ret;
 
-	for (slot = 0; slot < HW_BUFFER_NUM; slot++) {
+	for (unsigned int slot = 0; slot < HW_BUFFER_NUM; slot++) {
 		vin->buf_hw[slot].buffer = NULL;
-		vin->buf_hw[slot].type = FULL;
-	}
-
-	for (slot = 0; slot < HW_BUFFER_NUM; slot++)
 		rvin_fill_hw_slot(vin, slot);
+	}
 
 	ret = rvin_setup(vin);
 	if (ret)
@@ -1070,8 +1027,6 @@ static int rvin_capture_start(struct rvin_dev *vin)
 	/* Continuous Frame Capture Mode */
 	rvin_write(vin, VNFC_C_FRAME, VNFC_REG);
 
-	vin->state = STARTING;
-
 	return 0;
 }
 
@@ -1112,9 +1067,9 @@ static irqreturn_t rvin_irq(int irq, void *data)
 	if (!(int_status & VNINTS_FIS))
 		goto done;
 
-	/* Nothing to do if capture status is 'STOPPED' */
-	if (vin->state == STOPPED) {
-		vin_dbg(vin, "IRQ while state stopped\n");
+	/* Nothing to do if not running. */
+	if (!vin->running) {
+		vin_dbg(vin, "IRQ while not running, ignoring\n");
 		goto done;
 	}
 
@@ -1126,28 +1081,17 @@ static irqreturn_t rvin_irq(int irq, void *data)
 	 * To hand buffers back in a known order to userspace start
 	 * to capture first from slot 0.
 	 */
-	if (vin->state == STARTING) {
+	if (!vin->sequence) {
 		if (slot != 0) {
 			vin_dbg(vin, "Starting sync slot: %d\n", slot);
 			goto done;
 		}
 
 		vin_dbg(vin, "Capture start synced!\n");
-		vin->state = RUNNING;
 	}
 
 	/* Capture frame */
 	if (vin->buf_hw[slot].buffer) {
-		/*
-		 * Nothing to do but refill the hardware slot if
-		 * capture only filled first half of vb2 buffer.
-		 */
-		if (vin->buf_hw[slot].type == HALF_TOP) {
-			vin->buf_hw[slot].buffer = NULL;
-			rvin_fill_hw_slot(vin, slot);
-			goto done;
-		}
-
 		vin->buf_hw[slot].buffer->field =
 			rvin_get_active_field(vin, vnms);
 		vin->buf_hw[slot].buffer->sequence = vin->sequence;
@@ -1270,6 +1214,22 @@ static int rvin_mc_validate_format(struct rvin_dev *vin, struct v4l2_subdev *sd,
 		if (vin->format.pixelformat != V4L2_PIX_FMT_GREY)
 			return -EPIPE;
 		break;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		if (vin->format.pixelformat != V4L2_PIX_FMT_SBGGR10)
+			return -EPIPE;
+		break;
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		if (vin->format.pixelformat != V4L2_PIX_FMT_SGBRG10)
+			return -EPIPE;
+		break;
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+		if (vin->format.pixelformat != V4L2_PIX_FMT_SGRBG10)
+			return -EPIPE;
+		break;
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		if (vin->format.pixelformat != V4L2_PIX_FMT_SRGGB10)
+			return -EPIPE;
+		break;
 	default:
 		return -EPIPE;
 	}
@@ -1282,8 +1242,6 @@ static int rvin_mc_validate_format(struct rvin_dev *vin, struct v4l2_subdev *sd,
 	case V4L2_FIELD_INTERLACED_TB:
 	case V4L2_FIELD_INTERLACED_BT:
 	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_SEQ_TB:
-	case V4L2_FIELD_SEQ_BT:
 		/* Supported natively */
 		break;
 	case V4L2_FIELD_ALTERNATE:
@@ -1296,8 +1254,6 @@ static int rvin_mc_validate_format(struct rvin_dev *vin, struct v4l2_subdev *sd,
 		case V4L2_FIELD_INTERLACED_TB:
 		case V4L2_FIELD_INTERLACED_BT:
 		case V4L2_FIELD_INTERLACED:
-		case V4L2_FIELD_SEQ_TB:
-		case V4L2_FIELD_SEQ_BT:
 			/* Use VIN hardware to combine the two fields */
 			fmt.format.height *= 2;
 			break;
@@ -1311,7 +1267,7 @@ static int rvin_mc_validate_format(struct rvin_dev *vin, struct v4l2_subdev *sd,
 
 	if (rvin_scaler_needed(vin)) {
 		/* Gen3 can't scale NV12 */
-		if (vin->info->model == RCAR_GEN3 &&
+		if ((vin->info->model == RCAR_GEN3 || vin->info->model == RCAR_GEN4) &&
 		    vin->format.pixelformat == V4L2_PIX_FMT_NV12)
 			return -EPIPE;
 
@@ -1357,7 +1313,7 @@ static int rvin_set_stream(struct rvin_dev *vin, int on)
 
 	if (!on) {
 		video_device_pipeline_stop(&vin->vdev);
-		return v4l2_subdev_call(sd, video, s_stream, 0);
+		return v4l2_subdev_disable_streams(sd, pad->index, BIT_ULL(0));
 	}
 
 	ret = rvin_mc_validate_format(vin, sd, pad);
@@ -1368,7 +1324,7 @@ static int rvin_set_stream(struct rvin_dev *vin, int on)
 	if (ret)
 		return ret;
 
-	ret = v4l2_subdev_call(sd, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(sd, pad->index, BIT_ULL(0));
 	if (ret == -ENOIOCTLCMD)
 		ret = 0;
 	if (ret)
@@ -1394,6 +1350,8 @@ int rvin_start_streaming(struct rvin_dev *vin)
 	if (ret)
 		rvin_set_stream(vin, 0);
 
+	vin->running = true;
+
 	spin_unlock_irqrestore(&vin->qlock, flags);
 
 	return ret;
@@ -1426,44 +1384,21 @@ err_scratch:
 
 void rvin_stop_streaming(struct rvin_dev *vin)
 {
-	unsigned int i, retries;
 	unsigned long flags;
-	bool buffersFreed;
 
 	spin_lock_irqsave(&vin->qlock, flags);
 
-	if (vin->state == STOPPED) {
+	if (!vin->running) {
 		spin_unlock_irqrestore(&vin->qlock, flags);
 		return;
 	}
 
-	vin->state = STOPPING;
-
-	/* Wait until only scratch buffer is used, max 3 interrupts. */
-	retries = 0;
-	while (retries++ < RVIN_RETRIES) {
-		buffersFreed = true;
-		for (i = 0; i < HW_BUFFER_NUM; i++)
-			if (vin->buf_hw[i].buffer)
-				buffersFreed = false;
-
-		if (buffersFreed)
-			break;
-
-		spin_unlock_irqrestore(&vin->qlock, flags);
-		msleep(RVIN_TIMEOUT_MS);
-		spin_lock_irqsave(&vin->qlock, flags);
-	}
-
 	/* Wait for streaming to stop */
-	retries = 0;
-	while (retries++ < RVIN_RETRIES) {
-
+	for (unsigned int i = 0; i < RVIN_RETRIES; i++) {
 		rvin_capture_stop(vin);
 
 		/* Check if HW is stopped */
 		if (!rvin_capture_active(vin)) {
-			vin->state = STOPPED;
 			break;
 		}
 
@@ -1472,32 +1407,25 @@ void rvin_stop_streaming(struct rvin_dev *vin)
 		spin_lock_irqsave(&vin->qlock, flags);
 	}
 
-	if (!buffersFreed || vin->state != STOPPED) {
-		/*
-		 * If this happens something have gone horribly wrong.
-		 * Set state to stopped to prevent the interrupt handler
-		 * to make things worse...
-		 */
-		vin_err(vin, "Failed stop HW, something is seriously broken\n");
-		vin->state = STOPPED;
-	}
+	if (rvin_capture_active(vin))
+		vin_err(vin, "Hardware did not stop\n");
 
-	spin_unlock_irqrestore(&vin->qlock, flags);
+	vin->running = false;
 
-	/* If something went wrong, free buffers with an error. */
-	if (!buffersFreed) {
-		return_unused_buffers(vin, VB2_BUF_STATE_ERROR);
-		for (i = 0; i < HW_BUFFER_NUM; i++) {
-			if (vin->buf_hw[i].buffer)
-				vb2_buffer_done(&vin->buf_hw[i].buffer->vb2_buf,
-						VB2_BUF_STATE_ERROR);
-		}
-	}
+	spin_unlock_irqrestore(&vin->qlock, flags);
 
 	rvin_set_stream(vin, 0);
 
 	/* disable interrupts */
 	rvin_disable_interrupts(vin);
+
+	/* Return unprocessed buffers from hardware. */
+	for (unsigned int i = 0; i < HW_BUFFER_NUM; i++) {
+		if (vin->buf_hw[i].buffer)
+			vb2_buffer_done(&vin->buf_hw[i].buffer->vb2_buf,
+					VB2_BUF_STATE_ERROR);
+	}
+
 }
 
 static void rvin_stop_streaming_vq(struct vb2_queue *vq)
@@ -1519,8 +1447,6 @@ static const struct vb2_ops rvin_qops = {
 	.buf_queue		= rvin_buffer_queue,
 	.start_streaming	= rvin_start_streaming_vq,
 	.stop_streaming		= rvin_stop_streaming_vq,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 void rvin_dma_unregister(struct rvin_dev *vin)
@@ -1545,8 +1471,6 @@ int rvin_dma_register(struct rvin_dev *vin, int irq)
 
 	spin_lock_init(&vin->qlock);
 
-	vin->state = STOPPED;
-
 	for (i = 0; i < HW_BUFFER_NUM; i++)
 		vin->buf_hw[i].buffer = NULL;
 
@@ -1649,7 +1573,7 @@ void rvin_set_alpha(struct rvin_dev *vin, unsigned int alpha)
 
 	vin->alpha = alpha;
 
-	if (vin->state == STOPPED)
+	if (!vin->running)
 		goto out;
 
 	switch (vin->format.pixelformat) {
diff --git a/drivers/media/platform/renesas/rcar-vin/rcar-v4l2.c b/drivers/media/platform/renesas/rcar-vin/rcar-v4l2.c
index 073f70c6ac68..db091af57c19 100644
--- a/drivers/media/platform/renesas/rcar-vin/rcar-v4l2.c
+++ b/drivers/media/platform/renesas/rcar-vin/rcar-v4l2.c
@@ -86,6 +86,22 @@ static const struct rvin_video_format rvin_formats[] = {
 		.fourcc			= V4L2_PIX_FMT_GREY,
 		.bpp			= 1,
 	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SBGGR10,
+		.bpp			= 2,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SGBRG10,
+		.bpp			= 2,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SGRBG10,
+		.bpp			= 2,
+	},
+	{
+		.fourcc			= V4L2_PIX_FMT_SRGGB10,
+		.bpp			= 2,
+	},
 };
 
 const struct rvin_video_format *rvin_format_from_pixel(struct rvin_dev *vin,
@@ -106,6 +122,13 @@ const struct rvin_video_format *rvin_format_from_pixel(struct rvin_dev *vin,
 		if (!vin->info->nv12 || !(BIT(vin->id) & 0x3333))
 			return NULL;
 		break;
+	case V4L2_PIX_FMT_SBGGR10:
+	case V4L2_PIX_FMT_SGBRG10:
+	case V4L2_PIX_FMT_SGRBG10:
+	case V4L2_PIX_FMT_SRGGB10:
+		if (!vin->info->raw10)
+			return NULL;
+		break;
 	default:
 		break;
 	}
@@ -138,9 +161,6 @@ static u32 rvin_format_bytesperline(struct rvin_dev *vin,
 		break;
 	}
 
-	if (V4L2_FIELD_IS_SEQUENTIAL(pix->field))
-		align = 0x80;
-
 	return ALIGN(pix->width, align) * fmt->bpp;
 }
 
@@ -171,8 +191,6 @@ static void rvin_format_align(struct rvin_dev *vin, struct v4l2_pix_format *pix)
 	case V4L2_FIELD_INTERLACED_BT:
 	case V4L2_FIELD_INTERLACED:
 	case V4L2_FIELD_ALTERNATE:
-	case V4L2_FIELD_SEQ_TB:
-	case V4L2_FIELD_SEQ_BT:
 		break;
 	default:
 		pix->field = RVIN_DEFAULT_FIELD;
@@ -407,6 +425,26 @@ static int rvin_enum_fmt_vid_cap(struct file *file, void *priv,
 			return -EINVAL;
 		f->pixelformat = V4L2_PIX_FMT_SRGGB8;
 		return 0;
+	case MEDIA_BUS_FMT_SBGGR10_1X10:
+		if (f->index)
+			return -EINVAL;
+		f->pixelformat = V4L2_PIX_FMT_SBGGR10;
+		return 0;
+	case MEDIA_BUS_FMT_SGBRG10_1X10:
+		if (f->index)
+			return -EINVAL;
+		f->pixelformat = V4L2_PIX_FMT_SGBRG10;
+		return 0;
+	case MEDIA_BUS_FMT_SGRBG10_1X10:
+		if (f->index)
+			return -EINVAL;
+		f->pixelformat = V4L2_PIX_FMT_SGRBG10;
+		return 0;
+	case MEDIA_BUS_FMT_SRGGB10_1X10:
+		if (f->index)
+			return -EINVAL;
+		f->pixelformat = V4L2_PIX_FMT_SRGGB10;
+		return 0;
 	default:
 		return -EINVAL;
 	}
@@ -461,8 +499,6 @@ static int rvin_remote_rectangle(struct rvin_dev *vin, struct v4l2_rect *rect)
 		case V4L2_FIELD_INTERLACED_TB:
 		case V4L2_FIELD_INTERLACED_BT:
 		case V4L2_FIELD_INTERLACED:
-		case V4L2_FIELD_SEQ_TB:
-		case V4L2_FIELD_SEQ_BT:
 			rect->height *= 2;
 			break;
 		}
@@ -548,8 +584,8 @@ static int rvin_s_selection(struct file *file, void *fh,
 
 		vin->crop = s->r = r;
 
-		vin_dbg(vin, "Cropped %dx%d@%d:%d of %dx%d\n",
-			r.width, r.height, r.left, r.top,
+		vin_dbg(vin, "Cropped (%d,%d)/%ux%u of %dx%d\n",
+			r.left, r.top, r.width, r.height,
 			max_rect.width, max_rect.height);
 		break;
 	case V4L2_SEL_TGT_COMPOSE:
@@ -573,8 +609,8 @@ static int rvin_s_selection(struct file *file, void *fh,
 
 		vin->compose = s->r = r;
 
-		vin_dbg(vin, "Compose %dx%d@%d:%d in %dx%d\n",
-			r.width, r.height, r.left, r.top,
+		vin_dbg(vin, "Compose (%d,%d)/%ux%u in %dx%d\n",
+			r.left, r.top, r.width, r.height,
 			vin->format.width, vin->format.height);
 		break;
 	default:
@@ -730,7 +766,8 @@ static int rvin_s_dv_timings(struct file *file, void *priv_fh,
 	struct v4l2_subdev *sd = vin_to_source(vin);
 	int ret;
 
-	ret = v4l2_subdev_call(sd, video, s_dv_timings, timings);
+	ret = v4l2_subdev_call(sd, pad, s_dv_timings,
+			       vin->parallel.sink_pad, timings);
 	if (ret)
 		return ret;
 
@@ -744,7 +781,8 @@ static int rvin_g_dv_timings(struct file *file, void *priv_fh,
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 
-	return v4l2_subdev_call(sd, video, g_dv_timings, timings);
+	return v4l2_subdev_call(sd, pad, g_dv_timings,
+				vin->parallel.sink_pad, timings);
 }
 
 static int rvin_query_dv_timings(struct file *file, void *priv_fh,
@@ -753,7 +791,8 @@ static int rvin_query_dv_timings(struct file *file, void *priv_fh,
 	struct rvin_dev *vin = video_drvdata(file);
 	struct v4l2_subdev *sd = vin_to_source(vin);
 
-	return v4l2_subdev_call(sd, video, query_dv_timings, timings);
+	return v4l2_subdev_call(sd, pad, query_dv_timings,
+				vin->parallel.sink_pad, timings);
 }
 
 static int rvin_dv_timings_cap(struct file *file, void *priv_fh,
diff --git a/drivers/media/platform/renesas/rcar-vin/rcar-vin.h b/drivers/media/platform/renesas/rcar-vin/rcar-vin.h
index 792336dada44..83d1b2734c41 100644
--- a/drivers/media/platform/renesas/rcar-vin/rcar-vin.h
+++ b/drivers/media/platform/renesas/rcar-vin/rcar-vin.h
@@ -39,6 +39,7 @@ enum model_id {
 	RCAR_M1,
 	RCAR_GEN2,
 	RCAR_GEN3,
+	RCAR_GEN4,
 };
 
 enum rvin_csi_id {
@@ -59,40 +60,7 @@ enum rvin_isp_id {
 
 #define RVIN_REMOTES_MAX \
 	(((unsigned int)RVIN_CSI_MAX) > ((unsigned int)RVIN_ISP_MAX) ? \
-	 RVIN_CSI_MAX : RVIN_ISP_MAX)
-
-/**
- * enum rvin_dma_state - DMA states
- * @STOPPED:   No operation in progress
- * @STARTING:  Capture starting up
- * @RUNNING:   Operation in progress have buffers
- * @STOPPING:  Stopping operation
- * @SUSPENDED: Capture is suspended
- */
-enum rvin_dma_state {
-	STOPPED = 0,
-	STARTING,
-	RUNNING,
-	STOPPING,
-	SUSPENDED,
-};
-
-/**
- * enum rvin_buffer_type
- *
- * Describes how a buffer is given to the hardware. To be able
- * to capture SEQ_TB/BT it's needed to capture to the same vb2
- * buffer twice so the type of buffer needs to be kept.
- *
- * @FULL: One capture fills the whole vb2 buffer
- * @HALF_TOP: One capture fills the top half of the vb2 buffer
- * @HALF_BOTTOM: One capture fills the bottom half of the vb2 buffer
- */
-enum rvin_buffer_type {
-	FULL,
-	HALF_TOP,
-	HALF_BOTTOM,
-};
+	 (unsigned int)RVIN_CSI_MAX : (unsigned int)RVIN_ISP_MAX)
 
 /**
  * struct rvin_video_format - Data format stored in memory
@@ -151,7 +119,8 @@ struct rvin_group_route {
  * @model:		VIN model
  * @use_mc:		use media controller instead of controlling subdevice
  * @use_isp:		the VIN is connected to the ISP and not to the CSI-2
- * @nv12:		support outputing NV12 pixel format
+ * @nv12:		support outputting NV12 pixel format
+ * @raw10:		support outputting RAW10 pixel format
  * @max_width:		max input width the VIN supports
  * @max_height:		max input height the VIN supports
  * @routes:		list of possible routes from the CSI-2 recivers to
@@ -163,6 +132,7 @@ struct rvin_info {
 	bool use_mc;
 	bool use_isp;
 	bool nv12;
+	bool raw10;
 
 	unsigned int max_width;
 	unsigned int max_height;
@@ -192,11 +162,11 @@ struct rvin_info {
  * @scratch:		cpu address for scratch buffer
  * @scratch_phys:	physical address of the scratch buffer
  *
- * @qlock:		protects @buf_hw, @buf_list, @sequence and @state
+ * @qlock:		Protects @buf_hw, @buf_list, @sequence and @running
  * @buf_hw:		Keeps track of buffers given to HW slot
  * @buf_list:		list of queued buffers
  * @sequence:		V4L2 buffers sequence number
- * @state:		keeps track of operation state
+ * @running:		Keeps track of if the VIN is running
  *
  * @is_csi:		flag to mark the VIN as using a CSI-2 subdevice
  * @chsel:		Cached value of the current CSI-2 channel selection
@@ -235,12 +205,11 @@ struct rvin_dev {
 	spinlock_t qlock;
 	struct {
 		struct vb2_v4l2_buffer *buffer;
-		enum rvin_buffer_type type;
 		dma_addr_t phys;
 	} buf_hw[HW_BUFFER_NUM];
 	struct list_head buf_list;
 	unsigned int sequence;
-	enum rvin_dma_state state;
+	bool running;
 
 	bool is_csi;
 	unsigned int chsel;
diff --git a/drivers/media/platform/renesas/rcar_drif.c b/drivers/media/platform/renesas/rcar_drif.c
index f21d05054341..fc8b6bbef793 100644
--- a/drivers/media/platform/renesas/rcar_drif.c
+++ b/drivers/media/platform/renesas/rcar_drif.c
@@ -861,8 +861,6 @@ static const struct vb2_ops rcar_drif_vb2_ops = {
 	.buf_queue              = rcar_drif_buf_queue,
 	.start_streaming        = rcar_drif_start_streaming,
 	.stop_streaming         = rcar_drif_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int rcar_drif_querycap(struct file *file, void *fh,
@@ -1071,7 +1069,6 @@ static int rcar_drif_sdr_register(struct rcar_drif_sdr *sdr)
 	sdr->vdev->release = video_device_release;
 	sdr->vdev->lock = &sdr->v4l2_mutex;
 	sdr->vdev->queue = &sdr->vb_queue;
-	sdr->vdev->queue->lock = &sdr->vb_queue_mutex;
 	sdr->vdev->ctrl_handler = &sdr->ctrl_hdl;
 	sdr->vdev->v4l2_dev = &sdr->v4l2_dev;
 	sdr->vdev->device_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_TUNER |
@@ -1316,6 +1313,7 @@ static int rcar_drif_sdr_probe(struct rcar_drif_sdr *sdr)
 	sdr->vb_queue.ops = &rcar_drif_vb2_ops;
 	sdr->vb_queue.mem_ops = &vb2_vmalloc_memops;
 	sdr->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	sdr->vb_queue.lock = &sdr->vb_queue_mutex;
 
 	/* Init videobuf2 queue */
 	ret = vb2_queue_init(&sdr->vb_queue);
@@ -1475,7 +1473,7 @@ static struct platform_driver rcar_drif_driver = {
 		.pm = &rcar_drif_pm_ops,
 		},
 	.probe = rcar_drif_probe,
-	.remove_new = rcar_drif_remove,
+	.remove = rcar_drif_remove,
 };
 
 module_platform_driver(rcar_drif_driver);
diff --git a/drivers/media/platform/renesas/rcar_fdp1.c b/drivers/media/platform/renesas/rcar_fdp1.c
index a2565b269f3b..5d453a7a8988 100644
--- a/drivers/media/platform/renesas/rcar_fdp1.c
+++ b/drivers/media/platform/renesas/rcar_fdp1.c
@@ -2032,8 +2032,6 @@ static const struct vb2_ops fdp1_qops = {
 	.buf_queue	 = fdp1_buf_queue,
 	.start_streaming = fdp1_start_streaming,
 	.stop_streaming  = fdp1_stop_streaming,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -2444,7 +2442,7 @@ MODULE_DEVICE_TABLE(of, fdp1_dt_ids);
 
 static struct platform_driver fdp1_pdrv = {
 	.probe		= fdp1_probe,
-	.remove_new	= fdp1_remove,
+	.remove		= fdp1_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.of_match_table = fdp1_dt_ids,
diff --git a/drivers/media/platform/renesas/rcar_jpu.c b/drivers/media/platform/renesas/rcar_jpu.c
index fff349e45067..81038df71bb5 100644
--- a/drivers/media/platform/renesas/rcar_jpu.c
+++ b/drivers/media/platform/renesas/rcar_jpu.c
@@ -14,7 +14,7 @@
  *      3) V4L2_CID_JPEG_ACTIVE_MARKER
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/interrupt.h>
@@ -1171,8 +1171,6 @@ static const struct vb2_ops jpu_qops = {
 	.buf_finish		= jpu_buf_finish,
 	.start_streaming	= jpu_start_streaming,
 	.stop_streaming		= jpu_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int jpu_queue_init(void *priv, struct vb2_queue *src_vq,
@@ -1736,7 +1734,7 @@ static const struct dev_pm_ops jpu_pm_ops = {
 
 static struct platform_driver jpu_driver = {
 	.probe = jpu_probe,
-	.remove_new = jpu_remove,
+	.remove = jpu_remove,
 	.driver = {
 		.of_match_table = jpu_dt_ids,
 		.name = DRV_NAME,
diff --git a/drivers/media/platform/renesas/renesas-ceu.c b/drivers/media/platform/renesas/renesas-ceu.c
index 167760276796..8cceafe491b1 100644
--- a/drivers/media/platform/renesas/renesas-ceu.c
+++ b/drivers/media/platform/renesas/renesas-ceu.c
@@ -761,8 +761,6 @@ static const struct vb2_ops ceu_vb2_ops = {
 	.queue_setup		= ceu_vb2_setup,
 	.buf_queue		= ceu_vb2_queue,
 	.buf_prepare		= ceu_vb2_prepare,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= ceu_start_streaming,
 	.stop_streaming		= ceu_stop_streaming,
 };
@@ -1723,7 +1721,7 @@ static struct platform_driver ceu_driver = {
 		.of_match_table = of_match_ptr(ceu_of_match),
 	},
 	.probe		= ceu_probe,
-	.remove_new	= ceu_remove,
+	.remove		= ceu_remove,
 };
 
 module_platform_driver(ceu_driver);
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-core.c b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-core.c
index 280efd2a8185..5fa73ab2db53 100644
--- a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-core.c
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-core.c
@@ -22,6 +22,7 @@
 #include <media/v4l2-mc.h>
 
 #include "rzg2l-cru.h"
+#include "rzg2l-cru-regs.h"
 
 static inline struct rzg2l_cru_dev *notifier_to_cru(struct v4l2_async_notifier *n)
 {
@@ -72,7 +73,6 @@ static int rzg2l_cru_group_notify_complete(struct v4l2_async_notifier *notifier)
 			source->name, sink->name);
 		return ret;
 	}
-	cru->csi.channel = 0;
 	cru->ip.remote = cru->csi.subdev;
 
 	/* Create media device link between CRU IP <-> CRU OUTPUT */
@@ -209,7 +209,7 @@ static int rzg2l_cru_media_init(struct rzg2l_cru_dev *cru)
 	const struct of_device_id *match;
 	int ret;
 
-	cru->pad.flags = MEDIA_PAD_FL_SINK;
+	cru->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
 	ret = media_entity_pads_init(&cru->vdev.entity, 1, &cru->pad);
 	if (ret)
 		return ret;
@@ -241,10 +241,11 @@ static int rzg2l_cru_media_init(struct rzg2l_cru_dev *cru)
 
 static int rzg2l_cru_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct rzg2l_cru_dev *cru;
-	int ret;
+	int irq, ret;
 
-	cru = devm_kzalloc(&pdev->dev, sizeof(*cru), GFP_KERNEL);
+	cru = devm_kzalloc(dev, sizeof(*cru), GFP_KERNEL);
 	if (!cru)
 		return -ENOMEM;
 
@@ -252,27 +253,32 @@ static int rzg2l_cru_probe(struct platform_device *pdev)
 	if (IS_ERR(cru->base))
 		return PTR_ERR(cru->base);
 
-	cru->presetn = devm_reset_control_get_shared(&pdev->dev, "presetn");
+	cru->presetn = devm_reset_control_get_shared(dev, "presetn");
 	if (IS_ERR(cru->presetn))
-		return dev_err_probe(&pdev->dev, PTR_ERR(cru->presetn),
+		return dev_err_probe(dev, PTR_ERR(cru->presetn),
 				     "Failed to get cpg presetn\n");
 
-	cru->aresetn = devm_reset_control_get_exclusive(&pdev->dev, "aresetn");
+	cru->aresetn = devm_reset_control_get_exclusive(dev, "aresetn");
 	if (IS_ERR(cru->aresetn))
-		return dev_err_probe(&pdev->dev, PTR_ERR(cru->aresetn),
+		return dev_err_probe(dev, PTR_ERR(cru->aresetn),
 				     "Failed to get cpg aresetn\n");
 
-	cru->vclk = devm_clk_get(&pdev->dev, "video");
+	cru->vclk = devm_clk_get(dev, "video");
 	if (IS_ERR(cru->vclk))
-		return dev_err_probe(&pdev->dev, PTR_ERR(cru->vclk),
+		return dev_err_probe(dev, PTR_ERR(cru->vclk),
 				     "Failed to get video clock\n");
 
-	cru->dev = &pdev->dev;
-	cru->info = of_device_get_match_data(&pdev->dev);
+	cru->dev = dev;
+	cru->info = of_device_get_match_data(dev);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
 
-	cru->image_conv_irq = platform_get_irq(pdev, 0);
-	if (cru->image_conv_irq < 0)
-		return cru->image_conv_irq;
+	ret = devm_request_irq(dev, irq, cru->info->irq_handler, 0,
+			       KBUILD_MODNAME, cru);
+	if (ret)
+		return dev_err_probe(dev, ret, "failed to request irq\n");
 
 	platform_set_drvdata(pdev, cru);
 
@@ -281,8 +287,10 @@ static int rzg2l_cru_probe(struct platform_device *pdev)
 		return ret;
 
 	cru->num_buf = RZG2L_CRU_HW_BUFFER_DEFAULT;
-	pm_suspend_ignore_children(&pdev->dev, true);
-	pm_runtime_enable(&pdev->dev);
+	pm_suspend_ignore_children(dev, true);
+	ret = devm_pm_runtime_enable(dev);
+	if (ret)
+		goto error_dma_unregister;
 
 	ret = rzg2l_cru_media_init(cru);
 	if (ret)
@@ -292,7 +300,6 @@ static int rzg2l_cru_probe(struct platform_device *pdev)
 
 error_dma_unregister:
 	rzg2l_cru_dma_unregister(cru);
-	pm_runtime_disable(&pdev->dev);
 
 	return ret;
 }
@@ -301,8 +308,6 @@ static void rzg2l_cru_remove(struct platform_device *pdev)
 {
 	struct rzg2l_cru_dev *cru = platform_get_drvdata(pdev);
 
-	pm_runtime_disable(&pdev->dev);
-
 	v4l2_async_nf_unregister(&cru->notifier);
 	v4l2_async_nf_cleanup(&cru->notifier);
 
@@ -313,8 +318,112 @@ static void rzg2l_cru_remove(struct platform_device *pdev)
 	rzg2l_cru_dma_unregister(cru);
 }
 
+static const u16 rzg3e_cru_regs[] = {
+	[CRUnCTRL] = 0x0,
+	[CRUnIE] = 0x4,
+	[CRUnIE2] = 0x8,
+	[CRUnINTS] = 0xc,
+	[CRUnINTS2] = 0x10,
+	[CRUnRST] = 0x18,
+	[AMnMB1ADDRL] = 0x40,
+	[AMnMB1ADDRH] = 0x44,
+	[AMnMB2ADDRL] = 0x48,
+	[AMnMB2ADDRH] = 0x4c,
+	[AMnMB3ADDRL] = 0x50,
+	[AMnMB3ADDRH] = 0x54,
+	[AMnMB4ADDRL] = 0x58,
+	[AMnMB4ADDRH] = 0x5c,
+	[AMnMB5ADDRL] = 0x60,
+	[AMnMB5ADDRH] = 0x64,
+	[AMnMB6ADDRL] = 0x68,
+	[AMnMB6ADDRH] = 0x6c,
+	[AMnMB7ADDRL] = 0x70,
+	[AMnMB7ADDRH] = 0x74,
+	[AMnMB8ADDRL] = 0x78,
+	[AMnMB8ADDRH] = 0x7c,
+	[AMnMBVALID] = 0x88,
+	[AMnMADRSL] = 0x8c,
+	[AMnMADRSH] = 0x90,
+	[AMnAXIATTR] = 0xec,
+	[AMnFIFOPNTR] = 0xf8,
+	[AMnAXISTP] = 0x110,
+	[AMnAXISTPACK] = 0x114,
+	[AMnIS] = 0x128,
+	[ICnEN] = 0x1f0,
+	[ICnSVCNUM] = 0x1f8,
+	[ICnSVC] = 0x1fc,
+	[ICnIPMC_C0] = 0x200,
+	[ICnMS] = 0x2d8,
+	[ICnDMR] = 0x304,
+};
+
+static const struct rzg2l_cru_info rzg3e_cru_info = {
+	.max_width = 4095,
+	.max_height = 4095,
+	.image_conv = ICnIPMC_C0,
+	.has_stride = true,
+	.regs = rzg3e_cru_regs,
+	.irq_handler = rzg3e_cru_irq,
+	.enable_interrupts = rzg3e_cru_enable_interrupts,
+	.disable_interrupts = rzg3e_cru_disable_interrupts,
+	.fifo_empty = rz3e_fifo_empty,
+	.csi_setup = rzg3e_cru_csi2_setup,
+};
+
+static const u16 rzg2l_cru_regs[] = {
+	[CRUnCTRL] = 0x0,
+	[CRUnIE] = 0x4,
+	[CRUnINTS] = 0x8,
+	[CRUnRST] = 0xc,
+	[AMnMB1ADDRL] = 0x100,
+	[AMnMB1ADDRH] = 0x104,
+	[AMnMB2ADDRL] = 0x108,
+	[AMnMB2ADDRH] = 0x10c,
+	[AMnMB3ADDRL] = 0x110,
+	[AMnMB3ADDRH] = 0x114,
+	[AMnMB4ADDRL] = 0x118,
+	[AMnMB4ADDRH] = 0x11c,
+	[AMnMB5ADDRL] = 0x120,
+	[AMnMB5ADDRH] = 0x124,
+	[AMnMB6ADDRL] = 0x128,
+	[AMnMB6ADDRH] = 0x12c,
+	[AMnMB7ADDRL] = 0x130,
+	[AMnMB7ADDRH] = 0x134,
+	[AMnMB8ADDRL] = 0x138,
+	[AMnMB8ADDRH] = 0x13c,
+	[AMnMBVALID] = 0x148,
+	[AMnMBS] = 0x14c,
+	[AMnAXIATTR] = 0x158,
+	[AMnFIFOPNTR] = 0x168,
+	[AMnAXISTP] = 0x174,
+	[AMnAXISTPACK] = 0x178,
+	[ICnEN] = 0x200,
+	[ICnMC] = 0x208,
+	[ICnMS] = 0x254,
+	[ICnDMR] = 0x26c,
+};
+
+static const struct rzg2l_cru_info rzgl2_cru_info = {
+	.max_width = 2800,
+	.max_height = 4095,
+	.image_conv = ICnMC,
+	.regs = rzg2l_cru_regs,
+	.irq_handler = rzg2l_cru_irq,
+	.enable_interrupts = rzg2l_cru_enable_interrupts,
+	.disable_interrupts = rzg2l_cru_disable_interrupts,
+	.fifo_empty = rzg2l_fifo_empty,
+	.csi_setup = rzg2l_cru_csi2_setup,
+};
+
 static const struct of_device_id rzg2l_cru_of_id_table[] = {
-	{ .compatible = "renesas,rzg2l-cru", },
+	{
+		.compatible = "renesas,r9a09g047-cru",
+		.data = &rzg3e_cru_info,
+	},
+	{
+		.compatible = "renesas,rzg2l-cru",
+		.data = &rzgl2_cru_info,
+	},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, rzg2l_cru_of_id_table);
@@ -325,7 +434,7 @@ static struct platform_driver rzg2l_cru_driver = {
 		.of_match_table = rzg2l_cru_of_id_table,
 	},
 	.probe = rzg2l_cru_probe,
-	.remove_new = rzg2l_cru_remove,
+	.remove = rzg2l_cru_remove,
 };
 
 module_platform_driver(rzg2l_cru_driver);
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru-regs.h b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru-regs.h
new file mode 100644
index 000000000000..a5a57369ef0e
--- /dev/null
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru-regs.h
@@ -0,0 +1,111 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * rzg2l-cru-regs.h--RZ/G2L (and alike SoCs) CRU Registers Definitions
+ *
+ * Copyright (C) 2024 Renesas Electronics Corp.
+ */
+
+#ifndef __RZG2L_CRU_REGS_H__
+#define __RZG2L_CRU_REGS_H__
+
+/* HW CRU Registers Definition */
+
+#define CRUnCTRL_VINSEL(x)		((x) << 0)
+
+#define CRUnIE_EFE			BIT(17)
+
+#define CRUnIE2_FSxE(x)			BIT(((x) * 3))
+#define CRUnIE2_FExE(x)			BIT(((x) * 3) + 1)
+
+#define CRUnINTS_SFS			BIT(16)
+
+#define CRUnINTS2_FSxS(x)		BIT(((x) * 3))
+
+#define CRUnRST_VRESETN			BIT(0)
+
+/* Memory Bank Base Address (Lower) Register for CRU Image Data */
+#define AMnMBxADDRL(x)			(AMnMB1ADDRL + (x) * 2)
+
+/* Memory Bank Base Address (Higher) Register for CRU Image Data */
+#define AMnMBxADDRH(x)			(AMnMB1ADDRH + (x) * 2)
+
+#define AMnMBVALID_MBVALID(x)		GENMASK(x, 0)
+
+#define AMnMBS_MBSTS			0x7
+
+#define AMnAXIATTR_AXILEN_MASK		GENMASK(3, 0)
+#define AMnAXIATTR_AXILEN		(0xf)
+
+#define AMnFIFOPNTR_FIFOWPNTR		GENMASK(7, 0)
+#define AMnFIFOPNTR_FIFOWPNTR_B0	AMnFIFOPNTR_FIFOWPNTR
+#define AMnFIFOPNTR_FIFOWPNTR_B1	GENMASK(15, 8)
+#define AMnFIFOPNTR_FIFORPNTR_Y		GENMASK(23, 16)
+#define AMnFIFOPNTR_FIFORPNTR_B0	AMnFIFOPNTR_FIFORPNTR_Y
+#define AMnFIFOPNTR_FIFORPNTR_B1	GENMASK(31, 24)
+
+#define AMnIS_IS_MASK			GENMASK(14, 7)
+#define AMnIS_IS(x)			((x) << 7)
+
+#define AMnAXISTP_AXI_STOP		BIT(0)
+
+#define AMnAXISTPACK_AXI_STOP_ACK	BIT(0)
+
+#define ICnEN_ICEN			BIT(0)
+
+#define ICnSVC_SVC0(x)			(x)
+#define ICnSVC_SVC1(x)			((x) << 4)
+#define ICnSVC_SVC2(x)			((x) << 8)
+#define ICnSVC_SVC3(x)			((x) << 12)
+
+#define ICnMC_CSCTHR			BIT(5)
+#define ICnMC_INF(x)			((x) << 16)
+#define ICnMC_VCSEL(x)			((x) << 22)
+#define ICnMC_INF_MASK			GENMASK(21, 16)
+
+#define ICnMS_IA			BIT(2)
+
+#define ICnDMR_YCMODE_UYVY		(1 << 4)
+
+enum rzg2l_cru_common_regs {
+	CRUnCTRL,	/* CRU Control */
+	CRUnIE,		/* CRU Interrupt Enable */
+	CRUnIE2,	/* CRU Interrupt Enable(2) */
+	CRUnINTS,	/* CRU Interrupt Status */
+	CRUnINTS2,	/* CRU Interrupt Status(2) */
+	CRUnRST,	/* CRU Reset */
+	AMnMB1ADDRL,	/* Bank 1 Address (Lower) for CRU Image Data */
+	AMnMB1ADDRH,	/* Bank 1 Address (Higher) for CRU Image Data */
+	AMnMB2ADDRL,    /* Bank 2 Address (Lower) for CRU Image Data */
+	AMnMB2ADDRH,    /* Bank 2 Address (Higher) for CRU Image Data */
+	AMnMB3ADDRL,    /* Bank 3 Address (Lower) for CRU Image Data */
+	AMnMB3ADDRH,    /* Bank 3 Address (Higher) for CRU Image Data */
+	AMnMB4ADDRL,    /* Bank 4 Address (Lower) for CRU Image Data */
+	AMnMB4ADDRH,    /* Bank 4 Address (Higher) for CRU Image Data */
+	AMnMB5ADDRL,    /* Bank 5 Address (Lower) for CRU Image Data */
+	AMnMB5ADDRH,    /* Bank 5 Address (Higher) for CRU Image Data */
+	AMnMB6ADDRL,    /* Bank 6 Address (Lower) for CRU Image Data */
+	AMnMB6ADDRH,    /* Bank 6 Address (Higher) for CRU Image Data */
+	AMnMB7ADDRL,    /* Bank 7 Address (Lower) for CRU Image Data */
+	AMnMB7ADDRH,    /* Bank 7 Address (Higher) for CRU Image Data */
+	AMnMB8ADDRL,    /* Bank 8 Address (Lower) for CRU Image Data */
+	AMnMB8ADDRH,    /* Bank 8 Address (Higher) for CRU Image Data */
+	AMnMBVALID,	/* Memory Bank Enable for CRU Image Data */
+	AMnMBS,		/* Memory Bank Status for CRU Image Data */
+	AMnMADRSL,	/* VD Memory Address Lower Status Register */
+	AMnMADRSH,	/* VD Memory Address Higher Status Register */
+	AMnAXIATTR,	/* AXI Master Transfer Setting Register for CRU Image Data */
+	AMnFIFOPNTR,	/* AXI Master FIFO Pointer for CRU Image Data */
+	AMnAXISTP,	/* AXI Master Transfer Stop for CRU Image Data */
+	AMnAXISTPACK,	/* AXI Master Transfer Stop Status for CRU Image Data */
+	AMnIS,		/* Image Stride Setting Register */
+	ICnEN,		/* CRU Image Processing Enable */
+	ICnSVCNUM,	/* CRU SVC Number Register */
+	ICnSVC,		/* CRU VC Select Register */
+	ICnMC,		/* CRU Image Processing Main Control */
+	ICnIPMC_C0,	/* CRU Image Converter Main Control 0 */
+	ICnMS,		/* CRU Module Status */
+	ICnDMR,		/* CRU Data Output Mode */
+	RZG2L_CRU_MAX_REG,
+};
+
+#endif /* __RZG2L_CRU_REGS_H__ */
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru.h b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru.h
index a5a99b004322..c30f3b281284 100644
--- a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru.h
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-cru.h
@@ -8,6 +8,7 @@
 #ifndef __RZG2L_CRU__
 #define __RZG2L_CRU__
 
+#include <linux/irqreturn.h>
 #include <linux/reset.h>
 
 #include <media/v4l2-async.h>
@@ -26,9 +27,14 @@
 #define RZG2L_CRU_CSI2_VCHANNEL		4
 
 #define RZG2L_CRU_MIN_INPUT_WIDTH	320
-#define RZG2L_CRU_MAX_INPUT_WIDTH	2800
 #define RZG2L_CRU_MIN_INPUT_HEIGHT	240
-#define RZG2L_CRU_MAX_INPUT_HEIGHT	4095
+
+enum rzg2l_csi2_pads {
+	RZG2L_CRU_IP_SINK = 0,
+	RZG2L_CRU_IP_SOURCE,
+};
+
+struct rzg2l_cru_dev;
 
 /**
  * enum rzg2l_cru_dma_state - DMA states
@@ -47,7 +53,6 @@ enum rzg2l_cru_dma_state {
 struct rzg2l_cru_csi {
 	struct v4l2_async_connection *asd;
 	struct v4l2_subdev *subdev;
-	u32 channel;
 };
 
 struct rzg2l_cru_ip {
@@ -58,6 +63,39 @@ struct rzg2l_cru_ip {
 };
 
 /**
+ * struct rzg2l_cru_ip_format - CRU IP format
+ * @code: Media bus code
+ * @datatype: MIPI CSI2 data type
+ * @format: 4CC format identifier (V4L2_PIX_FMT_*)
+ * @icndmr: ICnDMR register value
+ * @bpp: bytes per pixel
+ * @yuv: Flag to indicate whether the format is YUV-based.
+ */
+struct rzg2l_cru_ip_format {
+	u32 code;
+	u32 datatype;
+	u32 format;
+	u32 icndmr;
+	u8 bpp;
+	bool yuv;
+};
+
+struct rzg2l_cru_info {
+	unsigned int max_width;
+	unsigned int max_height;
+	u16 image_conv;
+	const u16 *regs;
+	bool has_stride;
+	irqreturn_t (*irq_handler)(int irq, void *data);
+	void (*enable_interrupts)(struct rzg2l_cru_dev *cru);
+	void (*disable_interrupts)(struct rzg2l_cru_dev *cru);
+	bool (*fifo_empty)(struct rzg2l_cru_dev *cru);
+	void (*csi_setup)(struct rzg2l_cru_dev *cru,
+			  const struct rzg2l_cru_ip_format *ip_fmt,
+			  u8 csi_vc);
+};
+
+/**
  * struct rzg2l_cru_dev - Renesas CRU device structure
  * @dev:		(OF) device
  * @base:		device I/O register space remapped to virtual memory
@@ -68,11 +106,11 @@ struct rzg2l_cru_ip {
  *
  * @vclk:		CRU Main clock
  *
- * @image_conv_irq:	Holds image conversion interrupt number
- *
  * @vdev:		V4L2 video device associated with CRU
  * @v4l2_dev:		V4L2 device
  * @num_buf:		Holds the current number of buffers enabled
+ * @svc_channel:	SVC0/1/2/3 to use for RZ/G3E
+ * @buf_addr:		Memory addresses where current video data is written.
  * @notifier:		V4L2 asynchronous subdevs notifier
  *
  * @ip:			Image processing subdev info
@@ -105,12 +143,13 @@ struct rzg2l_cru_dev {
 
 	struct clk *vclk;
 
-	int image_conv_irq;
-
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
 	u8 num_buf;
 
+	u8 svc_channel;
+	dma_addr_t buf_addr[RZG2L_CRU_HW_BUFFER_DEFAULT];
+
 	struct v4l2_async_notifier notifier;
 
 	struct rzg2l_cru_ip ip;
@@ -141,6 +180,8 @@ void rzg2l_cru_dma_unregister(struct rzg2l_cru_dev *cru);
 
 int rzg2l_cru_video_register(struct rzg2l_cru_dev *cru);
 void rzg2l_cru_video_unregister(struct rzg2l_cru_dev *cru);
+irqreturn_t rzg2l_cru_irq(int irq, void *data);
+irqreturn_t rzg3e_cru_irq(int irq, void *data);
 
 const struct v4l2_format_info *rzg2l_cru_format_from_pixel(u32 format);
 
@@ -148,4 +189,22 @@ int rzg2l_cru_ip_subdev_register(struct rzg2l_cru_dev *cru);
 void rzg2l_cru_ip_subdev_unregister(struct rzg2l_cru_dev *cru);
 struct v4l2_mbus_framefmt *rzg2l_cru_ip_get_src_fmt(struct rzg2l_cru_dev *cru);
 
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_code_to_fmt(unsigned int code);
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_format_to_fmt(u32 format);
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_index_to_fmt(u32 index);
+
+void rzg2l_cru_enable_interrupts(struct rzg2l_cru_dev *cru);
+void rzg2l_cru_disable_interrupts(struct rzg2l_cru_dev *cru);
+void rzg3e_cru_enable_interrupts(struct rzg2l_cru_dev *cru);
+void rzg3e_cru_disable_interrupts(struct rzg2l_cru_dev *cru);
+
+bool rzg2l_fifo_empty(struct rzg2l_cru_dev *cru);
+bool rz3e_fifo_empty(struct rzg2l_cru_dev *cru);
+void rzg2l_cru_csi2_setup(struct rzg2l_cru_dev *cru,
+			  const struct rzg2l_cru_ip_format *ip_fmt,
+			  u8 csi_vc);
+void rzg3e_cru_csi2_setup(struct rzg2l_cru_dev *cru,
+			  const struct rzg2l_cru_ip_format *ip_fmt,
+			  u8 csi_vc);
+
 #endif
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-csi2.c b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-csi2.c
index e68fcdaea207..9243306e2aa9 100644
--- a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-csi2.c
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-csi2.c
@@ -85,6 +85,15 @@
 					 CSIDPHYSKW0_UTIL_DL2_SKW_ADJ(1) | \
 					 CSIDPHYSKW0_UTIL_DL3_SKW_ADJ(1))
 
+/* DPHY registers on RZ/V2H(P) SoC */
+#define CRUm_S_TIMCTL			0x41c
+#define CRUm_S_TIMCTL_S_HSSETTLECTL(x)	((x) << 8)
+
+#define CRUm_S_DPHYCTL_MSB		0x434
+#define CRUm_S_DPHYCTL_MSB_DESKEW	BIT(1)
+
+#define CRUm_SWAPCTL			0x438
+
 #define VSRSTS_RETRIES			20
 
 #define RZG2L_CSI2_MIN_WIDTH		320
@@ -107,6 +116,7 @@ struct rzg2l_csi2 {
 	void __iomem *base;
 	struct reset_control *presetn;
 	struct reset_control *cmn_rstb;
+	const struct rzg2l_csi2_info *info;
 	struct clk *sysclk;
 	struct clk *vclk;
 	unsigned long vclk_rate;
@@ -123,6 +133,12 @@ struct rzg2l_csi2 {
 	bool dphy_enabled;
 };
 
+struct rzg2l_csi2_info {
+	int (*dphy_enable)(struct rzg2l_csi2 *csi2);
+	int (*dphy_disable)(struct rzg2l_csi2 *csi2);
+	bool has_system_clk;
+};
+
 struct rzg2l_csi2_timings {
 	u32 t_init;
 	u32 tclk_miss;
@@ -133,6 +149,30 @@ struct rzg2l_csi2_timings {
 	u32 max_hsfreq;
 };
 
+struct rzv2h_csi2_s_hssettlectl {
+	unsigned int hsfreq;
+	u16 s_hssettlectl;
+};
+
+static const struct rzv2h_csi2_s_hssettlectl rzv2h_s_hssettlectl[] = {
+	{   90,  1 }, {  130,  2 }, {  180,  3 },
+	{  220,  4 }, {  270,  5 }, {  310,  6 },
+	{  360,  7 }, {  400,  8 }, {  450,  9 },
+	{  490, 10 }, {  540, 11 }, {  580, 12 },
+	{  630, 13 }, {  670, 14 }, {  720, 15 },
+	{  760, 16 }, {  810, 17 }, {  850, 18 },
+	{  900, 19 }, {  940, 20 }, {  990, 21 },
+	{ 1030, 22 }, { 1080, 23 }, { 1120, 24 },
+	{ 1170, 25 }, { 1220, 26 }, { 1260, 27 },
+	{ 1310, 28 }, { 1350, 29 }, { 1400, 30 },
+	{ 1440, 31 }, { 1490, 32 }, { 1530, 33 },
+	{ 1580, 34 }, { 1620, 35 }, { 1670, 36 },
+	{ 1710, 37 }, { 1760, 38 }, { 1800, 39 },
+	{ 1850, 40 }, { 1890, 41 }, { 1940, 42 },
+	{ 1980, 43 }, { 2030, 44 }, { 2070, 45 },
+	{ 2100, 46 },
+};
+
 static const struct rzg2l_csi2_timings rzg2l_csi2_global_timings[] = {
 	{
 		.max_hsfreq = 80,
@@ -183,12 +223,15 @@ static const struct rzg2l_csi2_timings rzg2l_csi2_global_timings[] = {
 
 struct rzg2l_csi2_format {
 	u32 code;
-	unsigned int datatype;
 	unsigned int bpp;
 };
 
 static const struct rzg2l_csi2_format rzg2l_csi2_formats[] = {
-	{ .code = MEDIA_BUS_FMT_UYVY8_1X16,	.datatype = 0x1e, .bpp = 16 },
+	{ .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16 },
+	{ .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, },
+	{ .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, },
+	{ .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, },
+	{ .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, },
 };
 
 static inline struct rzg2l_csi2 *sd_to_csi2(struct v4l2_subdev *sd)
@@ -352,14 +395,20 @@ static int rzg2l_csi2_dphy_enable(struct rzg2l_csi2 *csi2)
 	return ret;
 }
 
+static const struct rzg2l_csi2_info rzg2l_csi2_info = {
+	.dphy_enable = rzg2l_csi2_dphy_enable,
+	.dphy_disable = rzg2l_csi2_dphy_disable,
+	.has_system_clk = true,
+};
+
 static int rzg2l_csi2_dphy_setting(struct v4l2_subdev *sd, bool on)
 {
 	struct rzg2l_csi2 *csi2 = sd_to_csi2(sd);
 
 	if (on)
-		return rzg2l_csi2_dphy_enable(csi2);
+		return csi2->info->dphy_enable(csi2);
 
-	return rzg2l_csi2_dphy_disable(csi2);
+	return csi2->info->dphy_disable(csi2);
 }
 
 static int rzg2l_csi2_mipi_link_enable(struct rzg2l_csi2 *csi2)
@@ -418,6 +467,64 @@ static int rzg2l_csi2_mipi_link_disable(struct rzg2l_csi2 *csi2)
 	return 0;
 }
 
+static int rzv2h_csi2_dphy_disable(struct rzg2l_csi2 *csi2)
+{
+	int ret;
+
+	/* Reset the CRU (D-PHY) */
+	ret = reset_control_assert(csi2->cmn_rstb);
+	if (ret)
+		return ret;
+
+	csi2->dphy_enabled = false;
+
+	return 0;
+}
+
+static int rzv2h_csi2_dphy_enable(struct rzg2l_csi2 *csi2)
+{
+	unsigned int i;
+	u16 hssettle;
+	int mbps;
+
+	mbps = rzg2l_csi2_calc_mbps(csi2);
+	if (mbps < 0)
+		return mbps;
+
+	csi2->hsfreq = mbps;
+
+	for (i = 0; i < ARRAY_SIZE(rzv2h_s_hssettlectl); i++) {
+		if (csi2->hsfreq <= rzv2h_s_hssettlectl[i].hsfreq)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(rzv2h_s_hssettlectl))
+		return -EINVAL;
+
+	rzg2l_csi2_write(csi2, CRUm_SWAPCTL, 0);
+
+	hssettle = rzv2h_s_hssettlectl[i].s_hssettlectl;
+	rzg2l_csi2_write(csi2, CRUm_S_TIMCTL,
+			 CRUm_S_TIMCTL_S_HSSETTLECTL(hssettle));
+
+	if (csi2->hsfreq > 1500)
+		rzg2l_csi2_set(csi2, CRUm_S_DPHYCTL_MSB,
+			       CRUm_S_DPHYCTL_MSB_DESKEW);
+	else
+		rzg2l_csi2_clr(csi2, CRUm_S_DPHYCTL_MSB,
+			       CRUm_S_DPHYCTL_MSB_DESKEW);
+
+	csi2->dphy_enabled = true;
+
+	return 0;
+}
+
+static const struct rzg2l_csi2_info rzv2h_csi2_info = {
+	.dphy_enable = rzv2h_csi2_dphy_enable,
+	.dphy_disable = rzv2h_csi2_dphy_disable,
+	.has_system_clk = false,
+};
+
 static int rzg2l_csi2_mipi_link_setting(struct v4l2_subdev *sd, bool on)
 {
 	struct rzg2l_csi2 *csi2 = sd_to_csi2(sd);
@@ -574,6 +681,9 @@ static int rzg2l_csi2_enum_frame_size(struct v4l2_subdev *sd,
 	if (fse->index != 0)
 		return -EINVAL;
 
+	if (!rzg2l_csi2_code_to_fmt(fse->code))
+		return -EINVAL;
+
 	fse->min_width = RZG2L_CSI2_MIN_WIDTH;
 	fse->min_height = RZG2L_CSI2_MIN_HEIGHT;
 	fse->max_width = RZG2L_CSI2_MAX_WIDTH;
@@ -582,6 +692,25 @@ static int rzg2l_csi2_enum_frame_size(struct v4l2_subdev *sd,
 	return 0;
 }
 
+static int rzg2l_csi2_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
+				     struct v4l2_mbus_frame_desc *fd)
+{
+	struct rzg2l_csi2 *csi2 = sd_to_csi2(sd);
+	struct media_pad *remote_pad;
+
+	if (!csi2->remote_source)
+		return -ENODEV;
+
+	remote_pad = media_pad_remote_pad_unique(&csi2->pads[RZG2L_CSI2_SINK]);
+	if (IS_ERR(remote_pad)) {
+		dev_err(csi2->dev, "can't get source pad of %s (%ld)\n",
+			csi2->remote_source->name, PTR_ERR(remote_pad));
+		return PTR_ERR(remote_pad);
+	}
+	return v4l2_subdev_call(csi2->remote_source, pad, get_frame_desc,
+				remote_pad->index, fd);
+}
+
 static const struct v4l2_subdev_video_ops rzg2l_csi2_video_ops = {
 	.s_stream = rzg2l_csi2_s_stream,
 	.pre_streamon = rzg2l_csi2_pre_streamon,
@@ -593,6 +722,7 @@ static const struct v4l2_subdev_pad_ops rzg2l_csi2_pad_ops = {
 	.enum_frame_size = rzg2l_csi2_enum_frame_size,
 	.set_fmt = rzg2l_csi2_set_format,
 	.get_fmt = v4l2_subdev_get_fmt,
+	.get_frame_desc = rzg2l_csi2_get_frame_desc,
 };
 
 static const struct v4l2_subdev_ops rzg2l_csi2_subdev_ops = {
@@ -738,39 +868,46 @@ static const struct media_entity_operations rzg2l_csi2_entity_ops = {
 
 static int rzg2l_csi2_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct rzg2l_csi2 *csi2;
 	int ret;
 
-	csi2 = devm_kzalloc(&pdev->dev, sizeof(*csi2), GFP_KERNEL);
+	csi2 = devm_kzalloc(dev, sizeof(*csi2), GFP_KERNEL);
 	if (!csi2)
 		return -ENOMEM;
 
+	csi2->info = of_device_get_match_data(dev);
+	if (!csi2->info)
+		return dev_err_probe(dev, -EINVAL, "Failed to get OF match data\n");
+
 	csi2->base = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(csi2->base))
 		return PTR_ERR(csi2->base);
 
-	csi2->cmn_rstb = devm_reset_control_get_exclusive(&pdev->dev, "cmn-rstb");
+	csi2->cmn_rstb = devm_reset_control_get_exclusive(dev, "cmn-rstb");
 	if (IS_ERR(csi2->cmn_rstb))
-		return dev_err_probe(&pdev->dev, PTR_ERR(csi2->cmn_rstb),
+		return dev_err_probe(dev, PTR_ERR(csi2->cmn_rstb),
 				     "Failed to get cpg cmn-rstb\n");
 
-	csi2->presetn = devm_reset_control_get_shared(&pdev->dev, "presetn");
+	csi2->presetn = devm_reset_control_get_shared(dev, "presetn");
 	if (IS_ERR(csi2->presetn))
-		return dev_err_probe(&pdev->dev, PTR_ERR(csi2->presetn),
+		return dev_err_probe(dev, PTR_ERR(csi2->presetn),
 				     "Failed to get cpg presetn\n");
 
-	csi2->sysclk = devm_clk_get(&pdev->dev, "system");
-	if (IS_ERR(csi2->sysclk))
-		return dev_err_probe(&pdev->dev, PTR_ERR(csi2->sysclk),
-				     "Failed to get system clk\n");
+	if (csi2->info->has_system_clk) {
+		csi2->sysclk = devm_clk_get(dev, "system");
+		if (IS_ERR(csi2->sysclk))
+			return dev_err_probe(dev, PTR_ERR(csi2->sysclk),
+					     "Failed to get system clk\n");
+	}
 
-	csi2->vclk = devm_clk_get(&pdev->dev, "video");
+	csi2->vclk = devm_clk_get(dev, "video");
 	if (IS_ERR(csi2->vclk))
-		return dev_err_probe(&pdev->dev, PTR_ERR(csi2->vclk),
+		return dev_err_probe(dev, PTR_ERR(csi2->vclk),
 				     "Failed to get video clock\n");
 	csi2->vclk_rate = clk_get_rate(csi2->vclk);
 
-	csi2->dev = &pdev->dev;
+	csi2->dev = dev;
 
 	platform_set_drvdata(pdev, csi2);
 
@@ -778,33 +915,38 @@ static int rzg2l_csi2_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	pm_runtime_enable(&pdev->dev);
+	ret = devm_pm_runtime_enable(dev);
+	if (ret)
+		return ret;
 
 	ret = rzg2l_validate_csi2_lanes(csi2);
 	if (ret)
-		goto error_pm;
+		return ret;
 
-	csi2->subdev.dev = &pdev->dev;
+	csi2->subdev.dev = dev;
 	v4l2_subdev_init(&csi2->subdev, &rzg2l_csi2_subdev_ops);
 	csi2->subdev.internal_ops = &rzg2l_csi2_internal_ops;
-	v4l2_set_subdevdata(&csi2->subdev, &pdev->dev);
+	v4l2_set_subdevdata(&csi2->subdev, dev);
 	snprintf(csi2->subdev.name, sizeof(csi2->subdev.name),
-		 "csi-%s", dev_name(&pdev->dev));
+		 "csi-%s", dev_name(dev));
 	csi2->subdev.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	csi2->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 	csi2->subdev.entity.ops = &rzg2l_csi2_entity_ops;
 
-	csi2->pads[RZG2L_CSI2_SINK].flags = MEDIA_PAD_FL_SINK;
+	csi2->pads[RZG2L_CSI2_SINK].flags = MEDIA_PAD_FL_SINK |
+					    MEDIA_PAD_FL_MUST_CONNECT;
 	/*
 	 * TODO: RZ/G2L CSI2 supports 4 virtual channels, as virtual
 	 * channels should be implemented by streams API which is under
 	 * development lets hardcode to VC0 for now.
 	 */
-	csi2->pads[RZG2L_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_pads_init(&csi2->subdev.entity, 2, csi2->pads);
+	csi2->pads[RZG2L_CSI2_SOURCE].flags = MEDIA_PAD_FL_SOURCE |
+					      MEDIA_PAD_FL_MUST_CONNECT;
+	ret = media_entity_pads_init(&csi2->subdev.entity, ARRAY_SIZE(csi2->pads),
+				     csi2->pads);
 	if (ret)
-		goto error_pm;
+		return ret;
 
 	ret = v4l2_subdev_init_finalize(&csi2->subdev);
 	if (ret < 0)
@@ -822,8 +964,6 @@ error_async:
 	v4l2_async_nf_unregister(&csi2->notifier);
 	v4l2_async_nf_cleanup(&csi2->notifier);
 	media_entity_cleanup(&csi2->subdev.entity);
-error_pm:
-	pm_runtime_disable(&pdev->dev);
 
 	return ret;
 }
@@ -837,7 +977,6 @@ static void rzg2l_csi2_remove(struct platform_device *pdev)
 	v4l2_async_unregister_subdev(&csi2->subdev);
 	v4l2_subdev_cleanup(&csi2->subdev);
 	media_entity_cleanup(&csi2->subdev.entity);
-	pm_runtime_disable(&pdev->dev);
 }
 
 static int rzg2l_csi2_pm_runtime_suspend(struct device *dev)
@@ -862,12 +1001,20 @@ static const struct dev_pm_ops rzg2l_csi2_pm_ops = {
 };
 
 static const struct of_device_id rzg2l_csi2_of_table[] = {
-	{ .compatible = "renesas,rzg2l-csi2", },
+	{
+		.compatible = "renesas,r9a09g057-csi2",
+		.data = &rzv2h_csi2_info,
+	},
+	{
+		.compatible = "renesas,rzg2l-csi2",
+		.data = &rzg2l_csi2_info,
+	},
 	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(of, rzg2l_csi2_of_table);
 
 static struct platform_driver rzg2l_csi2_pdrv = {
-	.remove_new = rzg2l_csi2_remove,
+	.remove = rzg2l_csi2_remove,
 	.probe	= rzg2l_csi2_probe,
 	.driver	= {
 		.name = "rzg2l-csi2",
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-ip.c b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-ip.c
index ac8ebae4ed07..7836c7cd53dc 100644
--- a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-ip.c
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-ip.c
@@ -6,24 +6,55 @@
  */
 
 #include <linux/delay.h>
-#include "rzg2l-cru.h"
+#include <media/mipi-csi2.h>
 
-struct rzg2l_cru_ip_format {
-	u32 code;
-	unsigned int datatype;
-	unsigned int bpp;
-};
+#include "rzg2l-cru.h"
+#include "rzg2l-cru-regs.h"
 
 static const struct rzg2l_cru_ip_format rzg2l_cru_ip_formats[] = {
-	{ .code = MEDIA_BUS_FMT_UYVY8_1X16,	.datatype = 0x1e, .bpp = 16 },
-};
-
-enum rzg2l_csi2_pads {
-	RZG2L_CRU_IP_SINK = 0,
-	RZG2L_CRU_IP_SOURCE,
+	{
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.datatype = MIPI_CSI2_DT_YUV422_8B,
+		.format = V4L2_PIX_FMT_UYVY,
+		.bpp = 2,
+		.icndmr = ICnDMR_YCMODE_UYVY,
+		.yuv = true,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SBGGR8_1X8,
+		.format = V4L2_PIX_FMT_SBGGR8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.bpp = 1,
+		.icndmr = 0,
+		.yuv = false,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGBRG8_1X8,
+		.format = V4L2_PIX_FMT_SGBRG8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.bpp = 1,
+		.icndmr = 0,
+		.yuv = false,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SGRBG8_1X8,
+		.format = V4L2_PIX_FMT_SGRBG8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.bpp = 1,
+		.icndmr = 0,
+		.yuv = false,
+	},
+	{
+		.code = MEDIA_BUS_FMT_SRGGB8_1X8,
+		.format = V4L2_PIX_FMT_SRGGB8,
+		.datatype = MIPI_CSI2_DT_RAW8,
+		.bpp = 1,
+		.icndmr = 0,
+		.yuv = false,
+	},
 };
 
-static const struct rzg2l_cru_ip_format *rzg2l_cru_ip_code_to_fmt(unsigned int code)
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_code_to_fmt(unsigned int code)
 {
 	unsigned int i;
 
@@ -34,6 +65,26 @@ static const struct rzg2l_cru_ip_format *rzg2l_cru_ip_code_to_fmt(unsigned int c
 	return NULL;
 }
 
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_format_to_fmt(u32 format)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(rzg2l_cru_ip_formats); i++) {
+		if (rzg2l_cru_ip_formats[i].format == format)
+			return &rzg2l_cru_ip_formats[i];
+	}
+
+	return NULL;
+}
+
+const struct rzg2l_cru_ip_format *rzg2l_cru_ip_index_to_fmt(u32 index)
+{
+	if (index >= ARRAY_SIZE(rzg2l_cru_ip_formats))
+		return NULL;
+
+	return &rzg2l_cru_ip_formats[index];
+}
+
 struct v4l2_mbus_framefmt *rzg2l_cru_ip_get_src_fmt(struct rzg2l_cru_dev *cru)
 {
 	struct v4l2_subdev_state *state;
@@ -97,6 +148,8 @@ static int rzg2l_cru_ip_set_format(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *state,
 				   struct v4l2_subdev_format *fmt)
 {
+	struct rzg2l_cru_dev *cru = v4l2_get_subdevdata(sd);
+	const struct rzg2l_cru_info *info = cru->info;
 	struct v4l2_mbus_framefmt *src_format;
 	struct v4l2_mbus_framefmt *sink_format;
 
@@ -119,9 +172,9 @@ static int rzg2l_cru_ip_set_format(struct v4l2_subdev *sd,
 	sink_format->ycbcr_enc = fmt->format.ycbcr_enc;
 	sink_format->quantization = fmt->format.quantization;
 	sink_format->width = clamp_t(u32, fmt->format.width,
-				     RZG2L_CRU_MIN_INPUT_WIDTH, RZG2L_CRU_MAX_INPUT_WIDTH);
+				     RZG2L_CRU_MIN_INPUT_WIDTH, info->max_width);
 	sink_format->height = clamp_t(u32, fmt->format.height,
-				      RZG2L_CRU_MIN_INPUT_HEIGHT, RZG2L_CRU_MAX_INPUT_HEIGHT);
+				      RZG2L_CRU_MIN_INPUT_HEIGHT, info->max_height);
 
 	fmt->format = *sink_format;
 
@@ -146,16 +199,19 @@ static int rzg2l_cru_ip_enum_frame_size(struct v4l2_subdev *sd,
 					struct v4l2_subdev_state *state,
 					struct v4l2_subdev_frame_size_enum *fse)
 {
+	struct rzg2l_cru_dev *cru = v4l2_get_subdevdata(sd);
+	const struct rzg2l_cru_info *info = cru->info;
+
 	if (fse->index != 0)
 		return -EINVAL;
 
-	if (fse->code != MEDIA_BUS_FMT_UYVY8_1X16)
+	if (!rzg2l_cru_ip_code_to_fmt(fse->code))
 		return -EINVAL;
 
 	fse->min_width = RZG2L_CRU_MIN_INPUT_WIDTH;
 	fse->min_height = RZG2L_CRU_MIN_INPUT_HEIGHT;
-	fse->max_width = RZG2L_CRU_MAX_INPUT_WIDTH;
-	fse->max_height = RZG2L_CRU_MAX_INPUT_HEIGHT;
+	fse->max_width = info->max_width;
+	fse->max_height = info->max_height;
 
 	return 0;
 }
@@ -217,8 +273,10 @@ int rzg2l_cru_ip_subdev_register(struct rzg2l_cru_dev *cru)
 	ip->subdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
 	ip->subdev.entity.ops = &rzg2l_cru_ip_entity_ops;
 
-	ip->pads[0].flags = MEDIA_PAD_FL_SINK;
-	ip->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+	ip->pads[RZG2L_CRU_IP_SINK].flags = MEDIA_PAD_FL_SINK |
+					    MEDIA_PAD_FL_MUST_CONNECT;
+	ip->pads[RZG2L_CRU_IP_SOURCE].flags = MEDIA_PAD_FL_SOURCE |
+					      MEDIA_PAD_FL_MUST_CONNECT;
 
 	ret = media_entity_pads_init(&ip->subdev.entity, 2, ip->pads);
 	if (ret)
diff --git a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-video.c b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-video.c
index b16b8af6e8f8..067c6af14e95 100644
--- a/drivers/media/platform/renesas/rzg2l-cru/rzg2l-video.c
+++ b/drivers/media/platform/renesas/rzg2l-cru/rzg2l-video.c
@@ -15,75 +15,12 @@
 #include <linux/delay.h>
 #include <linux/pm_runtime.h>
 
+#include <media/mipi-csi2.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-dma-contig.h>
 
 #include "rzg2l-cru.h"
-
-/* HW CRU Registers Definition */
-
-/* CRU Control Register */
-#define CRUnCTRL			0x0
-#define CRUnCTRL_VINSEL(x)		((x) << 0)
-
-/* CRU Interrupt Enable Register */
-#define CRUnIE				0x4
-#define CRUnIE_EFE			BIT(17)
-
-/* CRU Interrupt Status Register */
-#define CRUnINTS			0x8
-#define CRUnINTS_SFS			BIT(16)
-
-/* CRU Reset Register */
-#define CRUnRST				0xc
-#define CRUnRST_VRESETN			BIT(0)
-
-/* Memory Bank Base Address (Lower) Register for CRU Image Data */
-#define AMnMBxADDRL(x)			(0x100 + ((x) * 8))
-
-/* Memory Bank Base Address (Higher) Register for CRU Image Data */
-#define AMnMBxADDRH(x)			(0x104 + ((x) * 8))
-
-/* Memory Bank Enable Register for CRU Image Data */
-#define AMnMBVALID			0x148
-#define AMnMBVALID_MBVALID(x)		GENMASK(x, 0)
-
-/* Memory Bank Status Register for CRU Image Data */
-#define AMnMBS				0x14c
-#define AMnMBS_MBSTS			0x7
-
-/* AXI Master FIFO Pointer Register for CRU Image Data */
-#define AMnFIFOPNTR			0x168
-#define AMnFIFOPNTR_FIFOWPNTR		GENMASK(7, 0)
-#define AMnFIFOPNTR_FIFORPNTR_Y		GENMASK(23, 16)
-
-/* AXI Master Transfer Stop Register for CRU Image Data */
-#define AMnAXISTP			0x174
-#define AMnAXISTP_AXI_STOP		BIT(0)
-
-/* AXI Master Transfer Stop Status Register for CRU Image Data */
-#define AMnAXISTPACK			0x178
-#define AMnAXISTPACK_AXI_STOP_ACK	BIT(0)
-
-/* CRU Image Processing Enable Register */
-#define ICnEN				0x200
-#define ICnEN_ICEN			BIT(0)
-
-/* CRU Image Processing Main Control Register */
-#define ICnMC				0x208
-#define ICnMC_CSCTHR			BIT(5)
-#define ICnMC_INF_YUV8_422		(0x1e << 16)
-#define ICnMC_INF_USER			(0x30 << 16)
-#define ICnMC_VCSEL(x)			((x) << 22)
-#define ICnMC_INF_MASK			GENMASK(21, 16)
-
-/* CRU Module Status Register */
-#define ICnMS				0x254
-#define ICnMS_IA			BIT(2)
-
-/* CRU Data Output Mode Register */
-#define ICnDMR				0x26c
-#define ICnDMR_YCMODE_UYVY		(1 << 4)
+#include "rzg2l-cru-regs.h"
 
 #define RZG2L_TIMEOUT_MS		100
 #define RZG2L_RETRIES			10
@@ -94,6 +31,9 @@
 #define RZG2L_CRU_DEFAULT_FIELD		V4L2_FIELD_NONE
 #define RZG2L_CRU_DEFAULT_COLORSPACE	V4L2_COLORSPACE_SRGB
 
+#define RZG2L_CRU_STRIDE_MAX		32640
+#define RZG2L_CRU_STRIDE_ALIGN		128
+
 struct rzg2l_cru_buffer {
 	struct vb2_v4l2_buffer vb;
 	struct list_head list;
@@ -105,16 +45,66 @@ struct rzg2l_cru_buffer {
 /* -----------------------------------------------------------------------------
  * DMA operations
  */
-static void rzg2l_cru_write(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
+static void __rzg2l_cru_write(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
 {
-	iowrite32(value, cru->base + offset);
+	const u16 *regs = cru->info->regs;
+
+	/*
+	 * CRUnCTRL is a first register on all CRU supported SoCs so validate
+	 * rest of the registers have valid offset being set in cru->info->regs.
+	 */
+	if (WARN_ON(offset >= RZG2L_CRU_MAX_REG) ||
+	    WARN_ON(offset != CRUnCTRL && regs[offset] == 0))
+		return;
+
+	iowrite32(value, cru->base + regs[offset]);
 }
 
-static u32 rzg2l_cru_read(struct rzg2l_cru_dev *cru, u32 offset)
+static u32 __rzg2l_cru_read(struct rzg2l_cru_dev *cru, u32 offset)
 {
-	return ioread32(cru->base + offset);
+	const u16 *regs = cru->info->regs;
+
+	/*
+	 * CRUnCTRL is a first register on all CRU supported SoCs so validate
+	 * rest of the registers have valid offset being set in cru->info->regs.
+	 */
+	if (WARN_ON(offset >= RZG2L_CRU_MAX_REG) ||
+	    WARN_ON(offset != CRUnCTRL && regs[offset] == 0))
+		return 0;
+
+	return ioread32(cru->base + regs[offset]);
 }
 
+static __always_inline void
+__rzg2l_cru_write_constant(struct rzg2l_cru_dev *cru, u32 offset, u32 value)
+{
+	const u16 *regs = cru->info->regs;
+
+	BUILD_BUG_ON(offset >= RZG2L_CRU_MAX_REG);
+
+	iowrite32(value, cru->base + regs[offset]);
+}
+
+static __always_inline u32
+__rzg2l_cru_read_constant(struct rzg2l_cru_dev *cru, u32 offset)
+{
+	const u16 *regs = cru->info->regs;
+
+	BUILD_BUG_ON(offset >= RZG2L_CRU_MAX_REG);
+
+	return ioread32(cru->base + regs[offset]);
+}
+
+#define rzg2l_cru_write(cru, offset, value) \
+	(__builtin_constant_p(offset) ? \
+	 __rzg2l_cru_write_constant(cru, offset, value) : \
+	 __rzg2l_cru_write(cru, offset, value))
+
+#define rzg2l_cru_read(cru, offset) \
+	(__builtin_constant_p(offset) ? \
+	 __rzg2l_cru_read_constant(cru, offset) : \
+	 __rzg2l_cru_read(cru, offset))
+
 /* Need to hold qlock before calling */
 static void return_unused_buffers(struct rzg2l_cru_dev *cru,
 				  enum vb2_buffer_state state)
@@ -184,46 +174,6 @@ static void rzg2l_cru_buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&cru->qlock, flags);
 }
 
-static int rzg2l_cru_mc_validate_format(struct rzg2l_cru_dev *cru,
-					struct v4l2_subdev *sd,
-					struct media_pad *pad)
-{
-	struct v4l2_subdev_format fmt = {
-		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
-	};
-
-	fmt.pad = pad->index;
-	if (v4l2_subdev_call_state_active(sd, pad, get_fmt, &fmt))
-		return -EPIPE;
-
-	switch (fmt.format.code) {
-	case MEDIA_BUS_FMT_UYVY8_1X16:
-		break;
-	default:
-		return -EPIPE;
-	}
-
-	switch (fmt.format.field) {
-	case V4L2_FIELD_TOP:
-	case V4L2_FIELD_BOTTOM:
-	case V4L2_FIELD_NONE:
-	case V4L2_FIELD_INTERLACED_TB:
-	case V4L2_FIELD_INTERLACED_BT:
-	case V4L2_FIELD_INTERLACED:
-	case V4L2_FIELD_SEQ_TB:
-	case V4L2_FIELD_SEQ_BT:
-		break;
-	default:
-		return -EPIPE;
-	}
-
-	if (fmt.format.width != cru->format.width ||
-	    fmt.format.height != cru->format.height)
-		return -EPIPE;
-
-	return 0;
-}
-
 static void rzg2l_cru_set_slot_addr(struct rzg2l_cru_dev *cru,
 				    int slot, dma_addr_t addr)
 {
@@ -237,6 +187,8 @@ static void rzg2l_cru_set_slot_addr(struct rzg2l_cru_dev *cru,
 	/* Currently, we just use the buffer in 32 bits address */
 	rzg2l_cru_write(cru, AMnMBxADDRL(slot), addr);
 	rzg2l_cru_write(cru, AMnMBxADDRH(slot), 0);
+
+	cru->buf_addr[slot] = addr;
 }
 
 /*
@@ -277,7 +229,9 @@ static void rzg2l_cru_fill_hw_slot(struct rzg2l_cru_dev *cru, int slot)
 
 static void rzg2l_cru_initialize_axi(struct rzg2l_cru_dev *cru)
 {
+	const struct rzg2l_cru_info *info = cru->info;
 	unsigned int slot;
+	u32 amnaxiattr;
 
 	/*
 	 * Set image data memory banks.
@@ -287,70 +241,118 @@ static void rzg2l_cru_initialize_axi(struct rzg2l_cru_dev *cru)
 
 	for (slot = 0; slot < cru->num_buf; slot++)
 		rzg2l_cru_fill_hw_slot(cru, slot);
+
+	if (info->has_stride) {
+		u32 stride = cru->format.bytesperline;
+		u32 amnis;
+
+		stride /= RZG2L_CRU_STRIDE_ALIGN;
+		amnis = rzg2l_cru_read(cru, AMnIS) & ~AMnIS_IS_MASK;
+		rzg2l_cru_write(cru, AMnIS, amnis | AMnIS_IS(stride));
+	}
+
+	/* Set AXI burst max length to recommended setting */
+	amnaxiattr = rzg2l_cru_read(cru, AMnAXIATTR) & ~AMnAXIATTR_AXILEN_MASK;
+	amnaxiattr |= AMnAXIATTR_AXILEN;
+	rzg2l_cru_write(cru, AMnAXIATTR, amnaxiattr);
 }
 
-static void rzg2l_cru_csi2_setup(struct rzg2l_cru_dev *cru, bool *input_is_yuv,
-				 struct v4l2_mbus_framefmt *ip_sd_fmt)
+void rzg3e_cru_csi2_setup(struct rzg2l_cru_dev *cru,
+			  const struct rzg2l_cru_ip_format *ip_fmt,
+			  u8 csi_vc)
 {
-	u32 icnmc;
+	const struct rzg2l_cru_info *info = cru->info;
+	u32 icnmc = ICnMC_INF(ip_fmt->datatype);
 
-	switch (ip_sd_fmt->code) {
-	case MEDIA_BUS_FMT_UYVY8_1X16:
-		icnmc = ICnMC_INF_YUV8_422;
-		*input_is_yuv = true;
-		break;
-	default:
-		*input_is_yuv = false;
-		icnmc = ICnMC_INF_USER;
-		break;
-	}
+	icnmc |= rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_INF_MASK;
+
+	/* Set virtual channel CSI2 */
+	icnmc |= ICnMC_VCSEL(csi_vc);
+
+	rzg2l_cru_write(cru, ICnSVCNUM, csi_vc);
+	rzg2l_cru_write(cru, ICnSVC, ICnSVC_SVC0(0) | ICnSVC_SVC1(1) |
+			ICnSVC_SVC2(2) | ICnSVC_SVC3(3));
+	rzg2l_cru_write(cru, info->image_conv, icnmc);
+}
+
+void rzg2l_cru_csi2_setup(struct rzg2l_cru_dev *cru,
+			  const struct rzg2l_cru_ip_format *ip_fmt,
+			  u8 csi_vc)
+{
+	const struct rzg2l_cru_info *info = cru->info;
+	u32 icnmc = ICnMC_INF(ip_fmt->datatype);
 
-	icnmc |= (rzg2l_cru_read(cru, ICnMC) & ~ICnMC_INF_MASK);
+	icnmc |= rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_INF_MASK;
 
 	/* Set virtual channel CSI2 */
-	icnmc |= ICnMC_VCSEL(cru->csi.channel);
+	icnmc |= ICnMC_VCSEL(csi_vc);
 
-	rzg2l_cru_write(cru, ICnMC, icnmc);
+	rzg2l_cru_write(cru, info->image_conv, icnmc);
 }
 
 static int rzg2l_cru_initialize_image_conv(struct rzg2l_cru_dev *cru,
-					   struct v4l2_mbus_framefmt *ip_sd_fmt)
+					   struct v4l2_mbus_framefmt *ip_sd_fmt,
+					   u8 csi_vc)
 {
-	bool output_is_yuv = false;
-	bool input_is_yuv = false;
-	u32 icndmr;
+	const struct rzg2l_cru_info *info = cru->info;
+	const struct rzg2l_cru_ip_format *cru_video_fmt;
+	const struct rzg2l_cru_ip_format *cru_ip_fmt;
 
-	rzg2l_cru_csi2_setup(cru, &input_is_yuv, ip_sd_fmt);
+	cru_ip_fmt = rzg2l_cru_ip_code_to_fmt(ip_sd_fmt->code);
+	info->csi_setup(cru, cru_ip_fmt, csi_vc);
 
 	/* Output format */
-	switch (cru->format.pixelformat) {
-	case V4L2_PIX_FMT_UYVY:
-		icndmr = ICnDMR_YCMODE_UYVY;
-		output_is_yuv = true;
-		break;
-	default:
+	cru_video_fmt = rzg2l_cru_ip_format_to_fmt(cru->format.pixelformat);
+	if (!cru_video_fmt) {
 		dev_err(cru->dev, "Invalid pixelformat (0x%x)\n",
 			cru->format.pixelformat);
 		return -EINVAL;
 	}
 
 	/* If input and output use same colorspace, do bypass mode */
-	if (output_is_yuv == input_is_yuv)
-		rzg2l_cru_write(cru, ICnMC,
-				rzg2l_cru_read(cru, ICnMC) | ICnMC_CSCTHR);
+	if (cru_ip_fmt->yuv == cru_video_fmt->yuv)
+		rzg2l_cru_write(cru, info->image_conv,
+				rzg2l_cru_read(cru, info->image_conv) | ICnMC_CSCTHR);
 	else
-		rzg2l_cru_write(cru, ICnMC,
-				rzg2l_cru_read(cru, ICnMC) & (~ICnMC_CSCTHR));
+		rzg2l_cru_write(cru, info->image_conv,
+				rzg2l_cru_read(cru, info->image_conv) & ~ICnMC_CSCTHR);
 
 	/* Set output data format */
-	rzg2l_cru_write(cru, ICnDMR, icndmr);
+	rzg2l_cru_write(cru, ICnDMR, cru_video_fmt->icndmr);
 
 	return 0;
 }
 
-void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
+bool rz3e_fifo_empty(struct rzg2l_cru_dev *cru)
+{
+	u32 amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);
+
+	if ((((amnfifopntr & AMnFIFOPNTR_FIFORPNTR_B1) >> 24) ==
+	     ((amnfifopntr & AMnFIFOPNTR_FIFOWPNTR_B1) >> 8)) &&
+	    (((amnfifopntr & AMnFIFOPNTR_FIFORPNTR_B0) >> 16) ==
+	     (amnfifopntr & AMnFIFOPNTR_FIFOWPNTR_B0)))
+		return true;
+
+	return false;
+}
+
+bool rzg2l_fifo_empty(struct rzg2l_cru_dev *cru)
 {
 	u32 amnfifopntr, amnfifopntr_w, amnfifopntr_r_y;
+
+	amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);
+
+	amnfifopntr_w = amnfifopntr & AMnFIFOPNTR_FIFOWPNTR;
+	amnfifopntr_r_y =
+		(amnfifopntr & AMnFIFOPNTR_FIFORPNTR_Y) >> 16;
+	if (amnfifopntr_w == amnfifopntr_r_y)
+		return true;
+
+	return amnfifopntr_w == amnfifopntr_r_y;
+}
+
+void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
+{
 	unsigned int retries = 0;
 	unsigned long flags;
 	u32 icnms;
@@ -358,8 +360,7 @@ void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
 	spin_lock_irqsave(&cru->qlock, flags);
 
 	/* Disable and clear the interrupt */
-	rzg2l_cru_write(cru, CRUnIE, 0);
-	rzg2l_cru_write(cru, CRUnINTS, 0x001F0F0F);
+	cru->info->disable_interrupts(cru);
 
 	/* Stop the operation of image conversion */
 	rzg2l_cru_write(cru, ICnEN, 0);
@@ -379,12 +380,7 @@ void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
 
 	/* Wait until the FIFO becomes empty */
 	for (retries = 5; retries > 0; retries--) {
-		amnfifopntr = rzg2l_cru_read(cru, AMnFIFOPNTR);
-
-		amnfifopntr_w = amnfifopntr & AMnFIFOPNTR_FIFOWPNTR;
-		amnfifopntr_r_y =
-			(amnfifopntr & AMnFIFOPNTR_FIFORPNTR_Y) >> 16;
-		if (amnfifopntr_w == amnfifopntr_r_y)
+		if (cru->info->fifo_empty(cru))
 			break;
 
 		usleep_range(10, 20);
@@ -422,12 +418,73 @@ void rzg2l_cru_stop_image_processing(struct rzg2l_cru_dev *cru)
 	spin_unlock_irqrestore(&cru->qlock, flags);
 }
 
+static int rzg2l_cru_get_virtual_channel(struct rzg2l_cru_dev *cru)
+{
+	struct v4l2_mbus_frame_desc fd = { };
+	struct media_pad *remote_pad;
+	int ret;
+
+	remote_pad = media_pad_remote_pad_unique(&cru->ip.pads[RZG2L_CRU_IP_SINK]);
+	ret = v4l2_subdev_call(cru->ip.remote, pad, get_frame_desc, remote_pad->index, &fd);
+	if (ret < 0 && ret != -ENOIOCTLCMD) {
+		dev_err(cru->dev, "get_frame_desc failed on IP remote subdev\n");
+		return ret;
+	}
+	/* If remote subdev does not implement .get_frame_desc default to VC0. */
+	if (ret == -ENOIOCTLCMD)
+		return 0;
+
+	if (fd.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
+		dev_err(cru->dev, "get_frame_desc returned invalid bus type %d\n", fd.type);
+		return -EINVAL;
+	}
+
+	if (!fd.num_entries) {
+		dev_err(cru->dev, "get_frame_desc returned zero entries\n");
+		return -EINVAL;
+	}
+
+	return fd.entry[0].bus.csi2.vc;
+}
+
+void rzg3e_cru_enable_interrupts(struct rzg2l_cru_dev *cru)
+{
+	rzg2l_cru_write(cru, CRUnIE2, CRUnIE2_FSxE(cru->svc_channel));
+	rzg2l_cru_write(cru, CRUnIE2, CRUnIE2_FExE(cru->svc_channel));
+}
+
+void rzg3e_cru_disable_interrupts(struct rzg2l_cru_dev *cru)
+{
+	rzg2l_cru_write(cru, CRUnIE, 0);
+	rzg2l_cru_write(cru, CRUnIE2, 0);
+	rzg2l_cru_write(cru, CRUnINTS, rzg2l_cru_read(cru, CRUnINTS));
+	rzg2l_cru_write(cru, CRUnINTS2, rzg2l_cru_read(cru, CRUnINTS2));
+}
+
+void rzg2l_cru_enable_interrupts(struct rzg2l_cru_dev *cru)
+{
+	rzg2l_cru_write(cru, CRUnIE, CRUnIE_EFE);
+}
+
+void rzg2l_cru_disable_interrupts(struct rzg2l_cru_dev *cru)
+{
+	rzg2l_cru_write(cru, CRUnIE, 0);
+	rzg2l_cru_write(cru, CRUnINTS, 0x001f000f);
+}
+
 int rzg2l_cru_start_image_processing(struct rzg2l_cru_dev *cru)
 {
 	struct v4l2_mbus_framefmt *fmt = rzg2l_cru_ip_get_src_fmt(cru);
 	unsigned long flags;
+	u8 csi_vc;
 	int ret;
 
+	ret = rzg2l_cru_get_virtual_channel(cru);
+	if (ret < 0)
+		return ret;
+	csi_vc = ret;
+	cru->svc_channel = csi_vc;
+
 	spin_lock_irqsave(&cru->qlock, flags);
 
 	/* Select a video input */
@@ -437,21 +494,20 @@ int rzg2l_cru_start_image_processing(struct rzg2l_cru_dev *cru)
 	rzg2l_cru_write(cru, CRUnRST, CRUnRST_VRESETN);
 
 	/* Disable and clear the interrupt before using */
-	rzg2l_cru_write(cru, CRUnIE, 0);
-	rzg2l_cru_write(cru, CRUnINTS, 0x001f000f);
+	cru->info->disable_interrupts(cru);
 
 	/* Initialize the AXI master */
 	rzg2l_cru_initialize_axi(cru);
 
 	/* Initialize image convert */
-	ret = rzg2l_cru_initialize_image_conv(cru, fmt);
+	ret = rzg2l_cru_initialize_image_conv(cru, fmt, csi_vc);
 	if (ret) {
 		spin_unlock_irqrestore(&cru->qlock, flags);
 		return ret;
 	}
 
 	/* Enable interrupt */
-	rzg2l_cru_write(cru, CRUnIE, CRUnIE_EFE);
+	cru->info->enable_interrupts(cru);
 
 	/* Enable image processing reception */
 	rzg2l_cru_write(cru, ICnEN, ICnEN_ICEN);
@@ -492,10 +548,6 @@ static int rzg2l_cru_set_stream(struct rzg2l_cru_dev *cru, int on)
 		return stream_off_ret;
 	}
 
-	ret = rzg2l_cru_mc_validate_format(cru, sd, pad);
-	if (ret)
-		return ret;
-
 	pipe = media_entity_pipeline(&sd->entity) ? : &cru->vdev.pipe;
 	ret = video_device_pipeline_start(&cru->vdev, pipe);
 	if (ret)
@@ -527,7 +579,7 @@ static void rzg2l_cru_stop_streaming(struct rzg2l_cru_dev *cru)
 	rzg2l_cru_set_stream(cru, 0);
 }
 
-static irqreturn_t rzg2l_cru_irq(int irq, void *data)
+irqreturn_t rzg2l_cru_irq(int irq, void *data)
 {
 	struct rzg2l_cru_dev *cru = data;
 	unsigned int handled = 0;
@@ -610,6 +662,104 @@ done:
 	return IRQ_RETVAL(handled);
 }
 
+static int rzg3e_cru_get_current_slot(struct rzg2l_cru_dev *cru)
+{
+	u64 amnmadrs;
+	int slot;
+
+	/*
+	 * When AMnMADRSL is read, AMnMADRSH of the higher-order
+	 * address also latches the address.
+	 *
+	 * AMnMADRSH must be read after AMnMADRSL has been read.
+	 */
+	amnmadrs = rzg2l_cru_read(cru, AMnMADRSL);
+	amnmadrs |= (u64)rzg2l_cru_read(cru, AMnMADRSH) << 32;
+
+	/* Ensure amnmadrs is within this buffer range */
+	for (slot = 0; slot < cru->num_buf; slot++) {
+		if (amnmadrs >= cru->buf_addr[slot] &&
+		    amnmadrs < cru->buf_addr[slot] + cru->format.sizeimage)
+			return slot;
+	}
+
+	dev_err(cru->dev, "Invalid MB address 0x%llx (out of range)\n", amnmadrs);
+	return -EINVAL;
+}
+
+irqreturn_t rzg3e_cru_irq(int irq, void *data)
+{
+	struct rzg2l_cru_dev *cru = data;
+	u32 irq_status;
+	int slot;
+
+	scoped_guard(spinlock, &cru->qlock) {
+		irq_status = rzg2l_cru_read(cru, CRUnINTS2);
+		if (!irq_status)
+			return IRQ_NONE;
+
+		dev_dbg(cru->dev, "CRUnINTS2 0x%x\n", irq_status);
+
+		rzg2l_cru_write(cru, CRUnINTS2, rzg2l_cru_read(cru, CRUnINTS2));
+
+		/* Nothing to do if capture status is 'RZG2L_CRU_DMA_STOPPED' */
+		if (cru->state == RZG2L_CRU_DMA_STOPPED) {
+			dev_dbg(cru->dev, "IRQ while state stopped\n");
+			return IRQ_HANDLED;
+		}
+
+		if (cru->state == RZG2L_CRU_DMA_STOPPING) {
+			if (irq_status & CRUnINTS2_FSxS(0) ||
+			    irq_status & CRUnINTS2_FSxS(1) ||
+			    irq_status & CRUnINTS2_FSxS(2) ||
+			    irq_status & CRUnINTS2_FSxS(3))
+				dev_dbg(cru->dev, "IRQ while state stopping\n");
+			return IRQ_HANDLED;
+		}
+
+		slot = rzg3e_cru_get_current_slot(cru);
+		if (slot < 0)
+			return IRQ_HANDLED;
+
+		dev_dbg(cru->dev, "Current written slot: %d\n", slot);
+		cru->buf_addr[slot] = 0;
+
+		/*
+		 * To hand buffers back in a known order to userspace start
+		 * to capture first from slot 0.
+		 */
+		if (cru->state == RZG2L_CRU_DMA_STARTING) {
+			if (slot != 0) {
+				dev_dbg(cru->dev, "Starting sync slot: %d\n", slot);
+				return IRQ_HANDLED;
+			}
+			dev_dbg(cru->dev, "Capture start synced!\n");
+			cru->state = RZG2L_CRU_DMA_RUNNING;
+		}
+
+		/* Capture frame */
+		if (cru->queue_buf[slot]) {
+			struct vb2_v4l2_buffer *buf = cru->queue_buf[slot];
+
+			buf->field = cru->format.field;
+			buf->sequence = cru->sequence;
+			buf->vb2_buf.timestamp = ktime_get_ns();
+			vb2_buffer_done(&buf->vb2_buf, VB2_BUF_STATE_DONE);
+			cru->queue_buf[slot] = NULL;
+		} else {
+			/* Scratch buffer was used, dropping frame. */
+			dev_dbg(cru->dev, "Dropping frame %u\n", cru->sequence);
+		}
+
+		cru->sequence++;
+
+		/* Prepare for next frame */
+		rzg2l_cru_fill_hw_slot(cru, slot);
+	}
+
+	return IRQ_HANDLED;
+}
+
 static int rzg2l_cru_start_streaming_vq(struct vb2_queue *vq, unsigned int count)
 {
 	struct rzg2l_cru_dev *cru = vb2_get_drv_priv(vq);
@@ -637,21 +787,14 @@ static int rzg2l_cru_start_streaming_vq(struct vb2_queue *vq, unsigned int count
 		goto assert_aresetn;
 	}
 
-	ret = request_irq(cru->image_conv_irq, rzg2l_cru_irq,
-			  IRQF_SHARED, KBUILD_MODNAME, cru);
-	if (ret) {
-		dev_err(cru->dev, "failed to request irq\n");
-		goto assert_presetn;
-	}
-
-	/* Allocate scratch buffer. */
+	/* Allocate scratch buffer */
 	cru->scratch = dma_alloc_coherent(cru->dev, cru->format.sizeimage,
 					  &cru->scratch_phys, GFP_KERNEL);
 	if (!cru->scratch) {
 		return_unused_buffers(cru, VB2_BUF_STATE_QUEUED);
 		dev_err(cru->dev, "Failed to allocate scratch buffer\n");
 		ret = -ENOMEM;
-		goto free_image_conv_irq;
+		goto assert_presetn;
 	}
 
 	cru->sequence = 0;
@@ -670,9 +813,6 @@ out:
 	if (ret)
 		dma_free_coherent(cru->dev, cru->format.sizeimage, cru->scratch,
 				  cru->scratch_phys);
-free_image_conv_irq:
-	free_irq(cru->image_conv_irq, cru);
-
 assert_presetn:
 	reset_control_assert(cru->presetn);
 
@@ -698,7 +838,6 @@ static void rzg2l_cru_stop_streaming_vq(struct vb2_queue *vq)
 	dma_free_coherent(cru->dev, cru->format.sizeimage,
 			  cru->scratch, cru->scratch_phys);
 
-	free_irq(cru->image_conv_irq, cru);
 	return_unused_buffers(cru, VB2_BUF_STATE_ERROR);
 
 	reset_control_assert(cru->presetn);
@@ -712,8 +851,6 @@ static const struct vb2_ops rzg2l_cru_qops = {
 	.buf_queue		= rzg2l_cru_buffer_queue,
 	.start_streaming	= rzg2l_cru_start_streaming_vq,
 	.stop_streaming		= rzg2l_cru_stop_streaming_vq,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 void rzg2l_cru_dma_unregister(struct rzg2l_cru_dev *cru)
@@ -775,46 +912,17 @@ error:
  * V4L2 stuff
  */
 
-static const struct v4l2_format_info rzg2l_cru_formats[] = {
-	{
-		.format = V4L2_PIX_FMT_UYVY,
-		.bpp[0] = 2,
-	},
-};
-
-const struct v4l2_format_info *rzg2l_cru_format_from_pixel(u32 format)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(rzg2l_cru_formats); i++)
-		if (rzg2l_cru_formats[i].format == format)
-			return rzg2l_cru_formats + i;
-
-	return NULL;
-}
-
-static u32 rzg2l_cru_format_bytesperline(struct v4l2_pix_format *pix)
-{
-	const struct v4l2_format_info *fmt;
-
-	fmt = rzg2l_cru_format_from_pixel(pix->pixelformat);
-
-	if (WARN_ON(!fmt))
-		return -EINVAL;
-
-	return pix->width * fmt->bpp[0];
-}
-
-static u32 rzg2l_cru_format_sizeimage(struct v4l2_pix_format *pix)
-{
-	return pix->bytesperline * pix->height;
-}
-
 static void rzg2l_cru_format_align(struct rzg2l_cru_dev *cru,
 				   struct v4l2_pix_format *pix)
 {
-	if (!rzg2l_cru_format_from_pixel(pix->pixelformat))
+	const struct rzg2l_cru_info *info = cru->info;
+	const struct rzg2l_cru_ip_format *fmt;
+
+	fmt = rzg2l_cru_ip_format_to_fmt(pix->pixelformat);
+	if (!fmt) {
 		pix->pixelformat = RZG2L_CRU_DEFAULT_FORMAT;
+		fmt = rzg2l_cru_ip_format_to_fmt(pix->pixelformat);
+	}
 
 	switch (pix->field) {
 	case V4L2_FIELD_TOP:
@@ -830,11 +938,18 @@ static void rzg2l_cru_format_align(struct rzg2l_cru_dev *cru,
 	}
 
 	/* Limit to CRU capabilities */
-	v4l_bound_align_image(&pix->width, 320, RZG2L_CRU_MAX_INPUT_WIDTH, 1,
-			      &pix->height, 240, RZG2L_CRU_MAX_INPUT_HEIGHT, 2, 0);
+	v4l_bound_align_image(&pix->width, 320, info->max_width, 1,
+			      &pix->height, 240, info->max_height, 2, 0);
+
+	if (info->has_stride) {
+		u32 stride = clamp(pix->bytesperline, pix->width * fmt->bpp,
+				   RZG2L_CRU_STRIDE_MAX);
+		pix->bytesperline = round_up(stride, RZG2L_CRU_STRIDE_ALIGN);
+	} else {
+		pix->bytesperline = pix->width * fmt->bpp;
+	}
 
-	pix->bytesperline = rzg2l_cru_format_bytesperline(pix);
-	pix->sizeimage = rzg2l_cru_format_sizeimage(pix);
+	pix->sizeimage = pix->bytesperline * pix->height;
 
 	dev_dbg(cru->dev, "Format %ux%u bpl: %u size: %u\n",
 		pix->width, pix->height, pix->bytesperline, pix->sizeimage);
@@ -905,10 +1020,13 @@ static int rzg2l_cru_g_fmt_vid_cap(struct file *file, void *priv,
 static int rzg2l_cru_enum_fmt_vid_cap(struct file *file, void *priv,
 				      struct v4l2_fmtdesc *f)
 {
-	if (f->index >= ARRAY_SIZE(rzg2l_cru_formats))
+	const struct rzg2l_cru_ip_format *fmt;
+
+	fmt = rzg2l_cru_ip_index_to_fmt(f->index);
+	if (!fmt)
 		return -EINVAL;
 
-	f->pixelformat = rzg2l_cru_formats[f->index].format;
+	f->pixelformat = fmt->format;
 
 	return 0;
 }
@@ -984,6 +1102,43 @@ static const struct v4l2_file_operations rzg2l_cru_fops = {
 	.read		= vb2_fop_read,
 };
 
+/* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+static int rzg2l_cru_video_link_validate(struct media_link *link)
+{
+	struct v4l2_subdev_format fmt = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+	};
+	const struct rzg2l_cru_ip_format *video_fmt;
+	struct v4l2_subdev *subdev;
+	struct rzg2l_cru_dev *cru;
+	int ret;
+
+	subdev = media_entity_to_v4l2_subdev(link->source->entity);
+	fmt.pad = link->source->index;
+	ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
+	if (ret < 0)
+		return ret == -ENOIOCTLCMD ? -EINVAL : ret;
+
+	cru = container_of(media_entity_to_video_device(link->sink->entity),
+			   struct rzg2l_cru_dev, vdev);
+	video_fmt = rzg2l_cru_ip_format_to_fmt(cru->format.pixelformat);
+
+	if (fmt.format.width != cru->format.width ||
+	    fmt.format.height != cru->format.height ||
+	    fmt.format.field != cru->format.field ||
+	    video_fmt->code != fmt.format.code)
+		return -EPIPE;
+
+	return 0;
+}
+
+static const struct media_entity_operations rzg2l_cru_video_media_ops = {
+	.link_validate = rzg2l_cru_video_link_validate,
+};
+
 static void rzg2l_cru_v4l2_init(struct rzg2l_cru_dev *cru)
 {
 	struct video_device *vdev = &cru->vdev;
@@ -995,6 +1150,7 @@ static void rzg2l_cru_v4l2_init(struct rzg2l_cru_dev *cru)
 	vdev->lock = &cru->lock;
 	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
 	vdev->device_caps |= V4L2_CAP_IO_MC;
+	vdev->entity.ops = &rzg2l_cru_video_media_ops;
 	vdev->fops = &rzg2l_cru_fops;
 	vdev->ioctl_ops = &rzg2l_cru_ioctl_ops;
 
diff --git a/drivers/media/platform/renesas/sh_vou.c b/drivers/media/platform/renesas/sh_vou.c
index 1e74dd601c2b..4ad7ae188d5b 100644
--- a/drivers/media/platform/renesas/sh_vou.c
+++ b/drivers/media/platform/renesas/sh_vou.c
@@ -360,8 +360,6 @@ static const struct vb2_ops sh_vou_qops = {
 	.buf_queue		= sh_vou_buf_queue,
 	.start_streaming	= sh_vou_start_streaming,
 	.stop_streaming		= sh_vou_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* Video IOCTLs */
@@ -1359,7 +1357,7 @@ static void sh_vou_remove(struct platform_device *pdev)
 }
 
 static struct platform_driver sh_vou = {
-	.remove_new = sh_vou_remove,
+	.remove = sh_vou_remove,
 	.driver  = {
 		.name	= "sh-vou",
 	},
diff --git a/drivers/media/platform/renesas/vsp1/Makefile b/drivers/media/platform/renesas/vsp1/Makefile
index 4bb4dcbef7b5..de8c802e1d1a 100644
--- a/drivers/media/platform/renesas/vsp1/Makefile
+++ b/drivers/media/platform/renesas/vsp1/Makefile
@@ -5,6 +5,6 @@ vsp1-y					+= vsp1_rpf.o vsp1_rwpf.o vsp1_wpf.o
 vsp1-y					+= vsp1_clu.o vsp1_hsit.o vsp1_lut.o
 vsp1-y					+= vsp1_brx.o vsp1_sru.o vsp1_uds.o
 vsp1-y					+= vsp1_hgo.o vsp1_hgt.o vsp1_histo.o
-vsp1-y					+= vsp1_lif.o vsp1_uif.o
+vsp1-y					+= vsp1_iif.o vsp1_lif.o vsp1_uif.o
 
 obj-$(CONFIG_VIDEO_RENESAS_VSP1)	+= vsp1.o
diff --git a/drivers/media/platform/renesas/vsp1/vsp1.h b/drivers/media/platform/renesas/vsp1/vsp1.h
index 2f6f0c6ae555..f97a1a31bfab 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1.h
@@ -32,6 +32,7 @@ struct vsp1_clu;
 struct vsp1_hgo;
 struct vsp1_hgt;
 struct vsp1_hsit;
+struct vsp1_iif;
 struct vsp1_lif;
 struct vsp1_lut;
 struct vsp1_rwpf;
@@ -56,6 +57,8 @@ struct vsp1_uif;
 #define VSP1_HAS_BRS		BIT(9)
 #define VSP1_HAS_EXT_DL		BIT(10)
 #define VSP1_HAS_NON_ZERO_LBA	BIT(11)
+#define VSP1_HAS_IIF		BIT(12)
+#define VSP1_HAS_HSIT		BIT(13)
 
 struct vsp1_device_info {
 	u32 version;
@@ -91,6 +94,7 @@ struct vsp1_device {
 	struct vsp1_hgt *hgt;
 	struct vsp1_hsit *hsi;
 	struct vsp1_hsit *hst;
+	struct vsp1_iif *iif;
 	struct vsp1_lif *lif[VSP1_MAX_LIF];
 	struct vsp1_lut *lut;
 	struct vsp1_rwpf *rpf[VSP1_MAX_RPF];
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_brx.c b/drivers/media/platform/renesas/vsp1/vsp1_brx.c
index a8535c6e2c46..5fc2e5a3bb30 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_brx.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_brx.c
@@ -15,6 +15,7 @@
 #include "vsp1.h"
 #include "vsp1_brx.h"
 #include "vsp1_dl.h"
+#include "vsp1_entity.h"
 #include "vsp1_pipe.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_video.h"
@@ -96,13 +97,6 @@ static int brx_enum_frame_size(struct v4l2_subdev *subdev,
 	return 0;
 }
 
-static struct v4l2_rect *brx_get_compose(struct vsp1_brx *brx,
-					 struct v4l2_subdev_state *sd_state,
-					 unsigned int pad)
-{
-	return v4l2_subdev_state_get_compose(sd_state, pad);
-}
-
 static void brx_try_format(struct vsp1_brx *brx,
 			   struct v4l2_subdev_state *sd_state,
 			   unsigned int pad, struct v4l2_mbus_framefmt *fmt)
@@ -115,20 +109,26 @@ static void brx_try_format(struct vsp1_brx *brx,
 		if (fmt->code != MEDIA_BUS_FMT_ARGB8888_1X32 &&
 		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
 			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
+
+		vsp1_entity_adjust_color_space(fmt);
 		break;
 
 	default:
 		/* The BRx can't perform format conversion. */
-		format = vsp1_entity_get_pad_format(&brx->entity, sd_state,
-						    BRX_PAD_SINK(0));
+		format = v4l2_subdev_state_get_format(sd_state,
+						      BRX_PAD_SINK(0));
 		fmt->code = format->code;
+
+		fmt->colorspace = format->colorspace;
+		fmt->xfer_func = format->xfer_func;
+		fmt->ycbcr_enc = format->ycbcr_enc;
+		fmt->quantization = format->quantization;
 		break;
 	}
 
 	fmt->width = clamp(fmt->width, BRX_MIN_SIZE, BRX_MAX_SIZE);
 	fmt->height = clamp(fmt->height, BRX_MIN_SIZE, BRX_MAX_SIZE);
 	fmt->field = V4L2_FIELD_NONE;
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
 }
 
 static int brx_set_format(struct v4l2_subdev *subdev,
@@ -150,14 +150,14 @@ static int brx_set_format(struct v4l2_subdev *subdev,
 
 	brx_try_format(brx, state, fmt->pad, &fmt->format);
 
-	format = vsp1_entity_get_pad_format(&brx->entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 	*format = fmt->format;
 
 	/* Reset the compose rectangle. */
 	if (fmt->pad != brx->entity.source_pad) {
 		struct v4l2_rect *compose;
 
-		compose = brx_get_compose(brx, state, fmt->pad);
+		compose = v4l2_subdev_state_get_compose(state, fmt->pad);
 		compose->left = 0;
 		compose->top = 0;
 		compose->width = format->width;
@@ -169,8 +169,7 @@ static int brx_set_format(struct v4l2_subdev *subdev,
 		unsigned int i;
 
 		for (i = 0; i <= brx->entity.source_pad; ++i) {
-			format = vsp1_entity_get_pad_format(&brx->entity,
-							    state, i);
+			format = v4l2_subdev_state_get_format(state, i);
 			format->code = fmt->format.code;
 		}
 	}
@@ -205,7 +204,7 @@ static int brx_get_selection(struct v4l2_subdev *subdev,
 			return -EINVAL;
 
 		mutex_lock(&brx->entity.lock);
-		sel->r = *brx_get_compose(brx, state, sel->pad);
+		sel->r = *v4l2_subdev_state_get_compose(state, sel->pad);
 		mutex_unlock(&brx->entity.lock);
 		return 0;
 
@@ -242,8 +241,7 @@ static int brx_set_selection(struct v4l2_subdev *subdev,
 	 * The compose rectangle top left corner must be inside the output
 	 * frame.
 	 */
-	format = vsp1_entity_get_pad_format(&brx->entity, state,
-					    brx->entity.source_pad);
+	format = v4l2_subdev_state_get_format(state, brx->entity.source_pad);
 	sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
 	sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
 
@@ -251,11 +249,11 @@ static int brx_set_selection(struct v4l2_subdev *subdev,
 	 * Scaling isn't supported, the compose rectangle size must be identical
 	 * to the sink format size.
 	 */
-	format = vsp1_entity_get_pad_format(&brx->entity, state, sel->pad);
+	format = v4l2_subdev_state_get_format(state, sel->pad);
 	sel->r.width = format->width;
 	sel->r.height = format->height;
 
-	compose = brx_get_compose(brx, state, sel->pad);
+	compose = v4l2_subdev_state_get_compose(state, sel->pad);
 	*compose = sel->r;
 
 done:
@@ -281,6 +279,7 @@ static const struct v4l2_subdev_ops brx_ops = {
  */
 
 static void brx_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -290,8 +289,7 @@ static void brx_configure_stream(struct vsp1_entity *entity,
 	unsigned int flags;
 	unsigned int i;
 
-	format = vsp1_entity_get_pad_format(&brx->entity, brx->entity.state,
-					    brx->entity.source_pad);
+	format = v4l2_subdev_state_get_format(state, brx->entity.source_pad);
 
 	/*
 	 * The hardware is extremely flexible but we have no userspace API to
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_clu.c b/drivers/media/platform/renesas/vsp1/vsp1_clu.c
index 625776a9bda4..98645bd2a983 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_clu.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_clu.c
@@ -170,6 +170,7 @@ static const struct v4l2_subdev_ops clu_ops = {
  */
 
 static void clu_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -181,8 +182,7 @@ static void clu_configure_stream(struct vsp1_entity *entity,
 	 * The yuv_mode can't be changed during streaming. Cache it internally
 	 * for future runtime configuration calls.
 	 */
-	format = vsp1_entity_get_pad_format(&clu->entity, clu->entity.state,
-					    CLU_PAD_SINK);
+	format = v4l2_subdev_state_get_format(state, CLU_PAD_SINK);
 	clu->yuv_mode = format->code == MEDIA_BUS_FMT_AYUV8_1X32;
 }
 
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_dl.c b/drivers/media/platform/renesas/vsp1/vsp1_dl.c
index ad3fa1c9cc73..bb8228b19824 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_dl.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_dl.c
@@ -1099,7 +1099,12 @@ struct vsp1_dl_manager *vsp1_dlm_create(struct vsp1_device *vsp1,
 		return NULL;
 
 	dlm->index = index;
-	dlm->singleshot = vsp1->info->uapi;
+	/*
+	 * uapi = single shot mode;
+	 * DRM = continuous mode;
+	 * VSPX = single shot mode;
+	 */
+	dlm->singleshot = vsp1->info->uapi || vsp1->iif;
 	dlm->vsp1 = vsp1;
 
 	spin_lock_init(&dlm->lock);
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_drm.c b/drivers/media/platform/renesas/vsp1/vsp1_drm.c
index 9b087bd8df7d..fe55e8747b05 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_drm.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_drm.c
@@ -118,26 +118,26 @@ static int vsp1_du_pipeline_setup_rpf(struct vsp1_device *vsp1,
 				      struct vsp1_entity *uif,
 				      unsigned int brx_input)
 {
+	const struct vsp1_drm_input *input = &vsp1->drm->inputs[rpf->entity.index];
 	struct v4l2_subdev_selection sel = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
 	struct v4l2_subdev_format format = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 	};
-	const struct v4l2_rect *crop;
 	int ret;
 
 	/*
 	 * Configure the format on the RPF sink pad and propagate it up to the
 	 * BRx sink pad.
 	 */
-	crop = &vsp1->drm->inputs[rpf->entity.index].crop;
-
 	format.pad = RWPF_PAD_SINK;
-	format.format.width = crop->width + crop->left;
-	format.format.height = crop->height + crop->top;
+	format.format.width = input->crop.width + input->crop.left;
+	format.format.height = input->crop.height + input->crop.top;
 	format.format.code = rpf->fmtinfo->mbus;
 	format.format.field = V4L2_FIELD_NONE;
+	format.format.ycbcr_enc = input->ycbcr_enc;
+	format.format.quantization = input->quantization;
 
 	ret = v4l2_subdev_call(&rpf->entity.subdev, pad, set_fmt, NULL,
 			       &format);
@@ -151,7 +151,7 @@ static int vsp1_du_pipeline_setup_rpf(struct vsp1_device *vsp1,
 
 	sel.pad = RWPF_PAD_SINK;
 	sel.target = V4L2_SEL_TGT_CROP;
-	sel.r = *crop;
+	sel.r = input->crop;
 
 	ret = v4l2_subdev_call(&rpf->entity.subdev, pad, set_selection, NULL,
 			       &sel);
@@ -317,7 +317,10 @@ static int vsp1_du_pipeline_setup_brx(struct vsp1_device *vsp1,
 			list_add_tail(&released_brx->list_pipe,
 				      &pipe->entities);
 
-		/* Add the BRx to the pipeline. */
+		/*
+		 * Add the BRx to the pipeline, inserting it just before the
+		 * WPF.
+		 */
 		dev_dbg(vsp1->dev, "%s: pipe %u: acquired %s\n",
 			__func__, pipe->lif->index, BRX_NAME(brx));
 
@@ -326,7 +329,8 @@ static int vsp1_du_pipeline_setup_brx(struct vsp1_device *vsp1,
 		pipe->brx->sink = &pipe->output->entity;
 		pipe->brx->sink_pad = 0;
 
-		list_add_tail(&pipe->brx->list_pipe, &pipe->entities);
+		list_add_tail(&pipe->brx->list_pipe,
+			      &pipe->output->entity.list_pipe);
 	}
 
 	/*
@@ -420,7 +424,7 @@ static int vsp1_du_pipeline_setup_inputs(struct vsp1_device *vsp1,
 
 		if (!rpf->entity.pipe) {
 			rpf->entity.pipe = pipe;
-			list_add_tail(&rpf->entity.list_pipe, &pipe->entities);
+			list_add(&rpf->entity.list_pipe, &pipe->entities);
 		}
 
 		brx->inputs[i].rpf = rpf;
@@ -546,6 +550,9 @@ static void vsp1_du_pipeline_configure(struct vsp1_pipeline *pipe)
 	struct vsp1_dl_body *dlb;
 	unsigned int dl_flags = 0;
 
+	vsp1_pipeline_calculate_partition(pipe, &pipe->part_table[0],
+					  drm_pipe->width, 0);
+
 	if (drm_pipe->force_brx_release)
 		dl_flags |= VSP1_DL_FRAME_END_INTERNAL;
 	if (pipe->output->writeback)
@@ -567,9 +574,11 @@ static void vsp1_du_pipeline_configure(struct vsp1_pipeline *pipe)
 		}
 
 		vsp1_entity_route_setup(entity, pipe, dlb);
-		vsp1_entity_configure_stream(entity, pipe, dl, dlb);
+		vsp1_entity_configure_stream(entity, entity->state, pipe,
+					     dl, dlb);
 		vsp1_entity_configure_frame(entity, pipe, dl, dlb);
-		vsp1_entity_configure_partition(entity, pipe, dl, dlb);
+		vsp1_entity_configure_partition(entity, pipe,
+						&pipe->part_table[0], dl, dlb);
 	}
 
 	vsp1_dl_list_commit(dl, dl_flags);
@@ -584,8 +593,8 @@ static int vsp1_du_pipeline_set_rwpf_format(struct vsp1_device *vsp1,
 
 	fmtinfo = vsp1_get_format_info(vsp1, pixelformat);
 	if (!fmtinfo) {
-		dev_dbg(vsp1->dev, "Unsupported pixel format %08x\n",
-			pixelformat);
+		dev_dbg(vsp1->dev, "Unsupported pixel format %p4cc\n",
+			&pixelformat);
 		return -EINVAL;
 	}
 
@@ -733,6 +742,8 @@ int vsp1_du_setup_lif(struct device *dev, unsigned int pipe_index,
 	if (ret < 0)
 		goto unlock;
 
+	vsp1_pipeline_dump(pipe, "LIF setup");
+
 	/* Enable the VSP1. */
 	ret = vsp1_device_get(vsp1);
 	if (ret < 0)
@@ -815,12 +826,14 @@ int vsp1_du_atomic_update(struct device *dev, unsigned int pipe_index,
 {
 	struct vsp1_device *vsp1 = dev_get_drvdata(dev);
 	struct vsp1_drm_pipeline *drm_pipe = &vsp1->drm->pipe[pipe_index];
+	struct vsp1_drm_input *input;
 	struct vsp1_rwpf *rpf;
 	int ret;
 
 	if (rpf_index >= vsp1->info->rpf_count)
 		return -EINVAL;
 
+	input = &vsp1->drm->inputs[rpf_index];
 	rpf = vsp1->rpf[rpf_index];
 
 	if (!cfg) {
@@ -838,11 +851,11 @@ int vsp1_du_atomic_update(struct device *dev, unsigned int pipe_index,
 	}
 
 	dev_dbg(vsp1->dev,
-		"%s: RPF%u: (%u,%u)/%ux%u -> (%u,%u)/%ux%u (%08x), pitch %u dma { %pad, %pad, %pad } zpos %u\n",
+		"%s: RPF%u: (%u,%u)/%ux%u -> (%u,%u)/%ux%u (%p4cc), pitch %u dma { %pad, %pad, %pad } zpos %u\n",
 		__func__, rpf_index,
 		cfg->src.left, cfg->src.top, cfg->src.width, cfg->src.height,
 		cfg->dst.left, cfg->dst.top, cfg->dst.width, cfg->dst.height,
-		cfg->pixelformat, cfg->pitch, &cfg->mem[0], &cfg->mem[1],
+		&cfg->pixelformat, cfg->pitch, &cfg->mem[0], &cfg->mem[1],
 		&cfg->mem[2], cfg->zpos);
 
 	/*
@@ -862,9 +875,11 @@ int vsp1_du_atomic_update(struct device *dev, unsigned int pipe_index,
 
 	rpf->format.flags = cfg->premult ? V4L2_PIX_FMT_FLAG_PREMUL_ALPHA : 0;
 
-	vsp1->drm->inputs[rpf_index].crop = cfg->src;
-	vsp1->drm->inputs[rpf_index].compose = cfg->dst;
-	vsp1->drm->inputs[rpf_index].zpos = cfg->zpos;
+	input->crop = cfg->src;
+	input->compose = cfg->dst;
+	input->zpos = cfg->zpos;
+	input->ycbcr_enc = cfg->color_encoding;
+	input->quantization = cfg->color_range;
 
 	drm_pipe->pipe.inputs[rpf_index] = rpf;
 
@@ -906,6 +921,9 @@ void vsp1_du_atomic_flush(struct device *dev, unsigned int pipe_index,
 	}
 
 	vsp1_du_pipeline_setup_inputs(vsp1, pipe);
+
+	vsp1_pipeline_dump(pipe, "atomic update");
+
 	vsp1_du_pipeline_configure(pipe);
 
 done:
@@ -959,6 +977,9 @@ int vsp1_drm_init(struct vsp1_device *vsp1)
 
 		vsp1_pipeline_init(pipe);
 
+		pipe->partitions = 1;
+		pipe->part_table = &drm_pipe->partition;
+
 		pipe->frame_end = vsp1_du_pipeline_frame_end;
 
 		/*
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_drm.h b/drivers/media/platform/renesas/vsp1/vsp1_drm.h
index ab8b7e3161a2..07a5d0adbd08 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_drm.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1_drm.h
@@ -20,6 +20,7 @@
 /**
  * struct vsp1_drm_pipeline - State for the API exposed to the DRM driver
  * @pipe: the VSP1 pipeline used for display
+ * @partition: the pre-calculated partition used by the pipeline
  * @width: output display width
  * @height: output display height
  * @force_brx_release: when set, release the BRx during the next reconfiguration
@@ -31,6 +32,7 @@
  */
 struct vsp1_drm_pipeline {
 	struct vsp1_pipeline pipe;
+	struct vsp1_partition partition;
 
 	unsigned int width;
 	unsigned int height;
@@ -50,17 +52,19 @@ struct vsp1_drm_pipeline {
  * struct vsp1_drm - State for the API exposed to the DRM driver
  * @pipe: the VSP1 DRM pipeline used for display
  * @lock: protects the BRU and BRS allocation
- * @inputs: source crop rectangle, destination compose rectangle and z-order
- *	position for every input (indexed by RPF index)
+ * @inputs: source crop rectangle, destination compose rectangle, z-order
+ *	position and colorspace for every input (indexed by RPF index)
  */
 struct vsp1_drm {
 	struct vsp1_drm_pipeline pipe[VSP1_MAX_LIF];
 	struct mutex lock;
 
-	struct {
+	struct vsp1_drm_input {
 		struct v4l2_rect crop;
 		struct v4l2_rect compose;
 		unsigned int zpos;
+		enum v4l2_ycbcr_encoding ycbcr_enc;
+		enum v4l2_quantization quantization;
 	} inputs[VSP1_MAX_RPF];
 };
 
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_drv.c b/drivers/media/platform/renesas/vsp1/vsp1_drv.c
index 1aac44d68731..8270a9d207cb 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_drv.c
@@ -29,6 +29,7 @@
 #include "vsp1_hgo.h"
 #include "vsp1_hgt.h"
 #include "vsp1_hsit.h"
+#include "vsp1_iif.h"
 #include "vsp1_lif.h"
 #include "vsp1_lut.h"
 #include "vsp1_pipe.h"
@@ -302,22 +303,6 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 		list_add_tail(&vsp1->clu->entity.list_dev, &vsp1->entities);
 	}
 
-	vsp1->hsi = vsp1_hsit_create(vsp1, true);
-	if (IS_ERR(vsp1->hsi)) {
-		ret = PTR_ERR(vsp1->hsi);
-		goto done;
-	}
-
-	list_add_tail(&vsp1->hsi->entity.list_dev, &vsp1->entities);
-
-	vsp1->hst = vsp1_hsit_create(vsp1, false);
-	if (IS_ERR(vsp1->hst)) {
-		ret = PTR_ERR(vsp1->hst);
-		goto done;
-	}
-
-	list_add_tail(&vsp1->hst->entity.list_dev, &vsp1->entities);
-
 	if (vsp1_feature(vsp1, VSP1_HAS_HGO) && vsp1->info->uapi) {
 		vsp1->hgo = vsp1_hgo_create(vsp1);
 		if (IS_ERR(vsp1->hgo)) {
@@ -340,6 +325,34 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 			      &vsp1->entities);
 	}
 
+	if (vsp1_feature(vsp1, VSP1_HAS_IIF)) {
+		vsp1->iif = vsp1_iif_create(vsp1);
+		if (IS_ERR(vsp1->iif)) {
+			ret = PTR_ERR(vsp1->iif);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->iif->entity.list_dev, &vsp1->entities);
+	}
+
+	if (vsp1_feature(vsp1, VSP1_HAS_HSIT)) {
+		vsp1->hsi = vsp1_hsit_create(vsp1, true);
+		if (IS_ERR(vsp1->hsi)) {
+			ret = PTR_ERR(vsp1->hsi);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->hsi->entity.list_dev, &vsp1->entities);
+
+		vsp1->hst = vsp1_hsit_create(vsp1, false);
+		if (IS_ERR(vsp1->hst)) {
+			ret = PTR_ERR(vsp1->hst);
+			goto done;
+		}
+
+		list_add_tail(&vsp1->hst->entity.list_dev, &vsp1->entities);
+	}
+
 	/*
 	 * The LIFs are only supported when used in conjunction with the DU, in
 	 * which case the userspace API is disabled. If the userspace API is
@@ -683,8 +696,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.model = "VSP1-S",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_HGO
-			  | VSP1_HAS_HGT | VSP1_HAS_LUT | VSP1_HAS_SRU
-			  | VSP1_HAS_WPF_VFLIP,
+			  | VSP1_HAS_HGT | VSP1_HAS_HSIT | VSP1_HAS_LUT
+			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
 		.uds_count = 3,
 		.wpf_count = 4,
@@ -694,7 +707,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.version = VI6_IP_VERSION_MODEL_VSPR_H2,
 		.model = "VSP1-R",
 		.gen = 2,
-		.features = VSP1_HAS_BRU | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
+		.features = VSP1_HAS_BRU | VSP1_HAS_HSIT | VSP1_HAS_SRU
+			   | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
 		.uds_count = 3,
 		.wpf_count = 4,
@@ -704,7 +718,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.version = VI6_IP_VERSION_MODEL_VSPD_GEN2,
 		.model = "VSP1-D",
 		.gen = 2,
-		.features = VSP1_HAS_BRU | VSP1_HAS_HGO | VSP1_HAS_LUT,
+		.features = VSP1_HAS_BRU | VSP1_HAS_HGO | VSP1_HAS_HSIT
+			  | VSP1_HAS_LUT,
 		.lif_count = 1,
 		.rpf_count = 4,
 		.uds_count = 1,
@@ -716,8 +731,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.model = "VSP1-S",
 		.gen = 2,
 		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_HGO
-			  | VSP1_HAS_HGT | VSP1_HAS_LUT | VSP1_HAS_SRU
-			  | VSP1_HAS_WPF_VFLIP,
+			  | VSP1_HAS_HGT | VSP1_HAS_HSIT | VSP1_HAS_LUT
+			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 5,
 		.uds_count = 1,
 		.wpf_count = 4,
@@ -727,8 +742,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.version = VI6_IP_VERSION_MODEL_VSPS_V2H,
 		.model = "VSP1V-S",
 		.gen = 2,
-		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT
-			  | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
+		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_HSIT
+			  | VSP1_HAS_LUT | VSP1_HAS_SRU | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 4,
 		.uds_count = 1,
 		.wpf_count = 4,
@@ -738,7 +753,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.version = VI6_IP_VERSION_MODEL_VSPD_V2H,
 		.model = "VSP1V-D",
 		.gen = 2,
-		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_LUT,
+		.features = VSP1_HAS_BRU | VSP1_HAS_CLU | VSP1_HAS_HSIT
+			  | VSP1_HAS_LUT,
 		.lif_count = 1,
 		.rpf_count = 4,
 		.uds_count = 1,
@@ -750,8 +766,8 @@ static const struct vsp1_device_info vsp1_device_infos[] = {
 		.model = "VSP2-I",
 		.gen = 3,
 		.features = VSP1_HAS_CLU | VSP1_HAS_HGO | VSP1_HAS_HGT
-			  | VSP1_HAS_LUT | VSP1_HAS_SRU | VSP1_HAS_WPF_HFLIP
-			  | VSP1_HAS_WPF_VFLIP,
+			  | VSP1_HAS_HSIT | VSP1_HAS_LUT | VSP1_HAS_SRU
+			  | VSP1_HAS_WPF_HFLIP | VSP1_HAS_WPF_VFLIP,
 		.rpf_count = 1,
 		.uds_count = 1,
 		.wpf_count = 1,
@@ -1005,7 +1021,7 @@ MODULE_DEVICE_TABLE(of, vsp1_of_match);
 
 static struct platform_driver vsp1_platform_driver = {
 	.probe		= vsp1_probe,
-	.remove_new	= vsp1_remove,
+	.remove		= vsp1_remove,
 	.driver		= {
 		.name	= "vsp1",
 		.pm	= &vsp1_pm_ops,
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_entity.c b/drivers/media/platform/renesas/vsp1/vsp1_entity.c
index 0a5a7f9cc870..a6680d531872 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_entity.c
@@ -63,19 +63,25 @@ void vsp1_entity_route_setup(struct vsp1_entity *entity,
 	/*
 	 * The ILV and BRS share the same data path route. The extra BRSSEL bit
 	 * selects between the ILV and BRS.
+	 *
+	 * The BRU and IIF share the same data path route. The extra IIFSEL bit
+	 * selects between the IIF and BRU.
 	 */
 	if (source->type == VSP1_ENTITY_BRS)
 		route |= VI6_DPR_ROUTE_BRSSEL;
+	else if (source->type == VSP1_ENTITY_IIF)
+		route |= VI6_DPR_ROUTE_IIFSEL;
 	vsp1_dl_body_write(dlb, source->route->reg, route);
 }
 
 void vsp1_entity_configure_stream(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe,
 				  struct vsp1_dl_list *dl,
 				  struct vsp1_dl_body *dlb)
 {
 	if (entity->ops->configure_stream)
-		entity->ops->configure_stream(entity, pipe, dl, dlb);
+		entity->ops->configure_stream(entity, state, pipe, dl, dlb);
 }
 
 void vsp1_entity_configure_frame(struct vsp1_entity *entity,
@@ -89,11 +95,27 @@ void vsp1_entity_configure_frame(struct vsp1_entity *entity,
 
 void vsp1_entity_configure_partition(struct vsp1_entity *entity,
 				     struct vsp1_pipeline *pipe,
+				     const struct vsp1_partition *partition,
 				     struct vsp1_dl_list *dl,
 				     struct vsp1_dl_body *dlb)
 {
 	if (entity->ops->configure_partition)
-		entity->ops->configure_partition(entity, pipe, dl, dlb);
+		entity->ops->configure_partition(entity, pipe, partition,
+						 dl, dlb);
+}
+
+void vsp1_entity_adjust_color_space(struct v4l2_mbus_framefmt *format)
+{
+	u8 xfer_func = format->xfer_func;
+	u8 ycbcr_enc = format->ycbcr_enc;
+	u8 quantization = format->quantization;
+
+	vsp1_adjust_color_space(format->code, &format->colorspace, &xfer_func,
+				&ycbcr_enc, &quantization);
+
+	format->xfer_func = xfer_func;
+	format->ycbcr_enc = ycbcr_enc;
+	format->quantization = quantization;
 }
 
 /* -----------------------------------------------------------------------------
@@ -127,49 +149,6 @@ vsp1_entity_get_state(struct vsp1_entity *entity,
 	}
 }
 
-/**
- * vsp1_entity_get_pad_format - Get a pad format from storage for an entity
- * @entity: the entity
- * @sd_state: the state storage
- * @pad: the pad number
- *
- * Return the format stored in the given configuration for an entity's pad. The
- * configuration can be an ACTIVE or TRY configuration.
- */
-struct v4l2_mbus_framefmt *
-vsp1_entity_get_pad_format(struct vsp1_entity *entity,
-			   struct v4l2_subdev_state *sd_state,
-			   unsigned int pad)
-{
-	return v4l2_subdev_state_get_format(sd_state, pad);
-}
-
-/**
- * vsp1_entity_get_pad_selection - Get a pad selection from storage for entity
- * @entity: the entity
- * @sd_state: the state storage
- * @pad: the pad number
- * @target: the selection target
- *
- * Return the selection rectangle stored in the given configuration for an
- * entity's pad. The configuration can be an ACTIVE or TRY configuration. The
- * selection target can be COMPOSE or CROP.
- */
-struct v4l2_rect *
-vsp1_entity_get_pad_selection(struct vsp1_entity *entity,
-			      struct v4l2_subdev_state *sd_state,
-			      unsigned int pad, unsigned int target)
-{
-	switch (target) {
-	case V4L2_SEL_TGT_COMPOSE:
-		return v4l2_subdev_state_get_compose(sd_state, pad);
-	case V4L2_SEL_TGT_CROP:
-		return v4l2_subdev_state_get_crop(sd_state, pad);
-	default:
-		return NULL;
-	}
-}
-
 /*
  * vsp1_subdev_get_pad_format - Subdev pad get_fmt handler
  * @subdev: V4L2 subdevice
@@ -191,7 +170,7 @@ int vsp1_subdev_get_pad_format(struct v4l2_subdev *subdev,
 		return -EINVAL;
 
 	mutex_lock(&entity->lock);
-	fmt->format = *vsp1_entity_get_pad_format(entity, state, fmt->pad);
+	fmt->format = *v4l2_subdev_state_get_format(state, fmt->pad);
 	mutex_unlock(&entity->lock);
 
 	return 0;
@@ -238,7 +217,7 @@ int vsp1_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
 			return -EINVAL;
 
 		mutex_lock(&entity->lock);
-		format = vsp1_entity_get_pad_format(entity, state, 0);
+		format = v4l2_subdev_state_get_format(state, 0);
 		code->code = format->code;
 		mutex_unlock(&entity->lock);
 	}
@@ -276,7 +255,7 @@ int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
 	if (!state)
 		return -EINVAL;
 
-	format = vsp1_entity_get_pad_format(entity, state, fse->pad);
+	format = v4l2_subdev_state_get_format(state, fse->pad);
 
 	mutex_lock(&entity->lock);
 
@@ -346,7 +325,7 @@ int vsp1_subdev_set_pad_format(struct v4l2_subdev *subdev,
 		goto done;
 	}
 
-	format = vsp1_entity_get_pad_format(entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 
 	if (fmt->pad == entity->source_pad) {
 		/* The output format can't be modified. */
@@ -369,24 +348,28 @@ int vsp1_subdev_set_pad_format(struct v4l2_subdev *subdev,
 	format->height = clamp_t(unsigned int, fmt->format.height,
 				 min_height, max_height);
 	format->field = V4L2_FIELD_NONE;
-	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	format->colorspace = fmt->format.colorspace;
+	format->xfer_func = fmt->format.xfer_func;
+	format->ycbcr_enc = fmt->format.ycbcr_enc;
+	format->quantization = fmt->format.quantization;
+
+	vsp1_entity_adjust_color_space(format);
 
 	fmt->format = *format;
 
 	/* Propagate the format to the source pad. */
-	format = vsp1_entity_get_pad_format(entity, state, entity->source_pad);
+	format = v4l2_subdev_state_get_format(state, entity->source_pad);
 	*format = fmt->format;
 
 	/* Reset the crop and compose rectangles. */
-	selection = vsp1_entity_get_pad_selection(entity, state, fmt->pad,
-						  V4L2_SEL_TGT_CROP);
+	selection = v4l2_subdev_state_get_crop(state, fmt->pad);
 	selection->left = 0;
 	selection->top = 0;
 	selection->width = format->width;
 	selection->height = format->height;
 
-	selection = vsp1_entity_get_pad_selection(entity, state, fmt->pad,
-						  V4L2_SEL_TGT_COMPOSE);
+	selection = v4l2_subdev_state_get_compose(state, fmt->pad);
 	selection->left = 0;
 	selection->top = 0;
 	selection->width = format->width;
@@ -570,6 +553,9 @@ struct media_pad *vsp1_entity_remote_pad(struct media_pad *pad)
 	  { VI6_DPR_NODE_WPF(idx) }, VI6_DPR_NODE_WPF(idx) }
 
 static const struct vsp1_route vsp1_routes[] = {
+	{ VSP1_ENTITY_IIF, 0, VI6_DPR_BRU_ROUTE,
+	  { VI6_DPR_NODE_BRU_IN(0), VI6_DPR_NODE_BRU_IN(1),
+	    VI6_DPR_NODE_BRU_IN(3) }, VI6_DPR_NODE_WPF(0) },
 	{ VSP1_ENTITY_BRS, 0, VI6_DPR_ILV_BRS_ROUTE,
 	  { VI6_DPR_NODE_BRS_IN(0), VI6_DPR_NODE_BRS_IN(1) }, 0 },
 	{ VSP1_ENTITY_BRU, 0, VI6_DPR_BRU_ROUTE,
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_entity.h b/drivers/media/platform/renesas/vsp1/vsp1_entity.h
index 735f32dde4b5..b7c72d0b7f8e 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_entity.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1_entity.h
@@ -19,7 +19,6 @@ struct vsp1_dl_body;
 struct vsp1_dl_list;
 struct vsp1_pipeline;
 struct vsp1_partition;
-struct vsp1_partition_window;
 
 enum vsp1_entity_type {
 	VSP1_ENTITY_BRS,
@@ -29,6 +28,7 @@ enum vsp1_entity_type {
 	VSP1_ENTITY_HGT,
 	VSP1_ENTITY_HSI,
 	VSP1_ENTITY_HST,
+	VSP1_ENTITY_IIF,
 	VSP1_ENTITY_LIF,
 	VSP1_ENTITY_LUT,
 	VSP1_ENTITY_RPF,
@@ -78,19 +78,30 @@ struct vsp1_route {
  *		configuration.
  */
 struct vsp1_entity_operations {
-	void (*destroy)(struct vsp1_entity *);
-	void (*configure_stream)(struct vsp1_entity *, struct vsp1_pipeline *,
-				 struct vsp1_dl_list *, struct vsp1_dl_body *);
-	void (*configure_frame)(struct vsp1_entity *, struct vsp1_pipeline *,
-				struct vsp1_dl_list *, struct vsp1_dl_body *);
-	void (*configure_partition)(struct vsp1_entity *,
-				    struct vsp1_pipeline *,
-				    struct vsp1_dl_list *,
-				    struct vsp1_dl_body *);
-	unsigned int (*max_width)(struct vsp1_entity *, struct vsp1_pipeline *);
-	void (*partition)(struct vsp1_entity *, struct vsp1_pipeline *,
-			  struct vsp1_partition *, unsigned int,
-			  struct vsp1_partition_window *);
+	void (*destroy)(struct vsp1_entity *entity);
+	void (*configure_stream)(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
+				 struct vsp1_pipeline *pipe,
+				 struct vsp1_dl_list *dl,
+				 struct vsp1_dl_body *dlb);
+	void (*configure_frame)(struct vsp1_entity *entity,
+				struct vsp1_pipeline *pipe,
+				struct vsp1_dl_list *dl,
+				struct vsp1_dl_body *dlb);
+	void (*configure_partition)(struct vsp1_entity *entity,
+				    struct vsp1_pipeline *pipe,
+				    const struct vsp1_partition *partition,
+				    struct vsp1_dl_list *dl,
+				    struct vsp1_dl_body *dlb);
+	unsigned int (*max_width)(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
+				  struct vsp1_pipeline *pipe);
+	void (*partition)(struct vsp1_entity *entity,
+			  struct v4l2_subdev_state *state,
+			  struct vsp1_pipeline *pipe,
+			  struct vsp1_partition *partition,
+			  unsigned int index,
+			  struct v4l2_rect *window);
 };
 
 struct vsp1_entity {
@@ -138,20 +149,13 @@ struct v4l2_subdev_state *
 vsp1_entity_get_state(struct vsp1_entity *entity,
 		      struct v4l2_subdev_state *sd_state,
 		      enum v4l2_subdev_format_whence which);
-struct v4l2_mbus_framefmt *
-vsp1_entity_get_pad_format(struct vsp1_entity *entity,
-			   struct v4l2_subdev_state *sd_state,
-			   unsigned int pad);
-struct v4l2_rect *
-vsp1_entity_get_pad_selection(struct vsp1_entity *entity,
-			      struct v4l2_subdev_state *sd_state,
-			      unsigned int pad, unsigned int target);
 
 void vsp1_entity_route_setup(struct vsp1_entity *entity,
 			     struct vsp1_pipeline *pipe,
 			     struct vsp1_dl_body *dlb);
 
 void vsp1_entity_configure_stream(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe,
 				  struct vsp1_dl_list *dl,
 				  struct vsp1_dl_body *dlb);
@@ -163,9 +167,12 @@ void vsp1_entity_configure_frame(struct vsp1_entity *entity,
 
 void vsp1_entity_configure_partition(struct vsp1_entity *entity,
 				     struct vsp1_pipeline *pipe,
+				     const struct vsp1_partition *partition,
 				     struct vsp1_dl_list *dl,
 				     struct vsp1_dl_body *dlb);
 
+void vsp1_entity_adjust_color_space(struct v4l2_mbus_framefmt *format);
+
 struct media_pad *vsp1_entity_remote_pad(struct media_pad *pad);
 
 int vsp1_subdev_get_pad_format(struct v4l2_subdev *subdev,
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_hgo.c b/drivers/media/platform/renesas/vsp1/vsp1_hgo.c
index 40c571a987ef..2c8ce7175a4e 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_hgo.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_hgo.c
@@ -130,6 +130,7 @@ static const struct v4l2_ctrl_config hgo_num_bins_control = {
  */
 
 static void hgo_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -140,11 +141,8 @@ static void hgo_configure_stream(struct vsp1_entity *entity,
 	unsigned int hratio;
 	unsigned int vratio;
 
-	crop = vsp1_entity_get_pad_selection(entity, entity->state,
-					     HISTO_PAD_SINK, V4L2_SEL_TGT_CROP);
-	compose = vsp1_entity_get_pad_selection(entity, entity->state,
-						HISTO_PAD_SINK,
-						V4L2_SEL_TGT_COMPOSE);
+	crop = v4l2_subdev_state_get_crop(state, HISTO_PAD_SINK);
+	compose = v4l2_subdev_state_get_compose(state, HISTO_PAD_SINK);
 
 	vsp1_hgo_write(hgo, dlb, VI6_HGO_REGRST, VI6_HGO_REGRST_RCLEA);
 
@@ -194,6 +192,16 @@ struct vsp1_hgo *vsp1_hgo_create(struct vsp1_device *vsp1)
 	if (hgo == NULL)
 		return ERR_PTR(-ENOMEM);
 
+	/* Initialize the video device and queue for statistics data. */
+	ret = vsp1_histogram_init(vsp1, &hgo->histo, VSP1_ENTITY_HGO, "hgo",
+				  &hgo_entity_ops, hgo_mbus_formats,
+				  ARRAY_SIZE(hgo_mbus_formats),
+				  HGO_DATA_SIZE, V4L2_META_FMT_VSP1_HGO);
+	if (ret < 0) {
+		vsp1_entity_destroy(&hgo->histo.entity);
+		return ERR_PTR(ret);
+	}
+
 	/* Initialize the control handler. */
 	v4l2_ctrl_handler_init(&hgo->ctrls.handler,
 			       vsp1->info->gen >= 3 ? 2 : 1);
@@ -209,15 +217,5 @@ struct vsp1_hgo *vsp1_hgo_create(struct vsp1_device *vsp1)
 
 	hgo->histo.entity.subdev.ctrl_handler = &hgo->ctrls.handler;
 
-	/* Initialize the video device and queue for statistics data. */
-	ret = vsp1_histogram_init(vsp1, &hgo->histo, VSP1_ENTITY_HGO, "hgo",
-				  &hgo_entity_ops, hgo_mbus_formats,
-				  ARRAY_SIZE(hgo_mbus_formats),
-				  HGO_DATA_SIZE, V4L2_META_FMT_VSP1_HGO);
-	if (ret < 0) {
-		vsp1_entity_destroy(&hgo->histo.entity);
-		return ERR_PTR(ret);
-	}
-
 	return hgo;
 }
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_hgt.c b/drivers/media/platform/renesas/vsp1/vsp1_hgt.c
index 8281b86874ab..858f330d44fa 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_hgt.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_hgt.c
@@ -126,6 +126,7 @@ static const struct v4l2_ctrl_config hgt_hue_areas = {
  */
 
 static void hgt_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -139,11 +140,8 @@ static void hgt_configure_stream(struct vsp1_entity *entity,
 	u8 upper;
 	unsigned int i;
 
-	crop = vsp1_entity_get_pad_selection(entity, entity->state,
-					     HISTO_PAD_SINK, V4L2_SEL_TGT_CROP);
-	compose = vsp1_entity_get_pad_selection(entity, entity->state,
-						HISTO_PAD_SINK,
-						V4L2_SEL_TGT_COMPOSE);
+	crop = v4l2_subdev_state_get_crop(state, HISTO_PAD_SINK);
+	compose = v4l2_subdev_state_get_compose(state, HISTO_PAD_SINK);
 
 	vsp1_hgt_write(hgt, dlb, VI6_HGT_REGRST, VI6_HGT_REGRST_RCLEA);
 
@@ -193,12 +191,6 @@ struct vsp1_hgt *vsp1_hgt_create(struct vsp1_device *vsp1)
 	if (hgt == NULL)
 		return ERR_PTR(-ENOMEM);
 
-	/* Initialize the control handler. */
-	v4l2_ctrl_handler_init(&hgt->ctrls, 1);
-	v4l2_ctrl_new_custom(&hgt->ctrls, &hgt_hue_areas, NULL);
-
-	hgt->histo.entity.subdev.ctrl_handler = &hgt->ctrls;
-
 	/* Initialize the video device and queue for statistics data. */
 	ret = vsp1_histogram_init(vsp1, &hgt->histo, VSP1_ENTITY_HGT, "hgt",
 				  &hgt_entity_ops, hgt_mbus_formats,
@@ -209,6 +201,12 @@ struct vsp1_hgt *vsp1_hgt_create(struct vsp1_device *vsp1)
 		return ERR_PTR(ret);
 	}
 
+	/* Initialize the control handler. */
+	v4l2_ctrl_handler_init(&hgt->ctrls, 1);
+	v4l2_ctrl_new_custom(&hgt->ctrls, &hgt_hue_areas, NULL);
+
+	hgt->histo.entity.subdev.ctrl_handler = &hgt->ctrls;
+
 	v4l2_ctrl_handler_setup(&hgt->ctrls);
 
 	return hgt;
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_histo.c b/drivers/media/platform/renesas/vsp1/vsp1_histo.c
index 71155282ca11..c762202877ba 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_histo.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_histo.c
@@ -36,9 +36,8 @@ struct vsp1_histogram_buffer *
 vsp1_histogram_buffer_get(struct vsp1_histogram *histo)
 {
 	struct vsp1_histogram_buffer *buf = NULL;
-	unsigned long flags;
 
-	spin_lock_irqsave(&histo->irqlock, flags);
+	spin_lock(&histo->irqlock);
 
 	if (list_empty(&histo->irqqueue))
 		goto done;
@@ -49,7 +48,7 @@ vsp1_histogram_buffer_get(struct vsp1_histogram *histo)
 	histo->readout = true;
 
 done:
-	spin_unlock_irqrestore(&histo->irqlock, flags);
+	spin_unlock(&histo->irqlock);
 	return buf;
 }
 
@@ -58,7 +57,6 @@ void vsp1_histogram_buffer_complete(struct vsp1_histogram *histo,
 				    size_t size)
 {
 	struct vsp1_pipeline *pipe = histo->entity.pipe;
-	unsigned long flags;
 
 	/*
 	 * The pipeline pointer is guaranteed to be valid as this function is
@@ -70,10 +68,10 @@ void vsp1_histogram_buffer_complete(struct vsp1_histogram *histo,
 	vb2_set_plane_payload(&buf->buf.vb2_buf, 0, size);
 	vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
 
-	spin_lock_irqsave(&histo->irqlock, flags);
+	spin_lock(&histo->irqlock);
 	histo->readout = false;
 	wake_up(&histo->wait_queue);
-	spin_unlock_irqrestore(&histo->irqlock, flags);
+	spin_unlock(&histo->irqlock);
 }
 
 /* -----------------------------------------------------------------------------
@@ -124,11 +122,10 @@ static void histo_buffer_queue(struct vb2_buffer *vb)
 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 	struct vsp1_histogram *histo = vb2_get_drv_priv(vb->vb2_queue);
 	struct vsp1_histogram_buffer *buf = to_vsp1_histogram_buffer(vbuf);
-	unsigned long flags;
 
-	spin_lock_irqsave(&histo->irqlock, flags);
+	spin_lock_irq(&histo->irqlock);
 	list_add_tail(&buf->queue, &histo->irqqueue);
-	spin_unlock_irqrestore(&histo->irqlock, flags);
+	spin_unlock_irq(&histo->irqlock);
 }
 
 static int histo_start_streaming(struct vb2_queue *vq, unsigned int count)
@@ -140,9 +137,8 @@ static void histo_stop_streaming(struct vb2_queue *vq)
 {
 	struct vsp1_histogram *histo = vb2_get_drv_priv(vq);
 	struct vsp1_histogram_buffer *buffer;
-	unsigned long flags;
 
-	spin_lock_irqsave(&histo->irqlock, flags);
+	spin_lock_irq(&histo->irqlock);
 
 	/* Remove all buffers from the IRQ queue. */
 	list_for_each_entry(buffer, &histo->irqqueue, queue)
@@ -152,15 +148,13 @@ static void histo_stop_streaming(struct vb2_queue *vq)
 	/* Wait for the buffer being read out (if any) to complete. */
 	wait_event_lock_irq(histo->wait_queue, !histo->readout, histo->irqlock);
 
-	spin_unlock_irqrestore(&histo->irqlock, flags);
+	spin_unlock_irq(&histo->irqlock);
 }
 
 static const struct vb2_ops histo_video_queue_qops = {
 	.queue_setup = histo_queue_setup,
 	.buf_prepare = histo_buffer_prepare,
 	.buf_queue = histo_buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = histo_start_streaming,
 	.stop_streaming = histo_stop_streaming,
 };
@@ -222,9 +216,7 @@ static int histo_get_selection(struct v4l2_subdev *subdev,
 	switch (sel->target) {
 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-		crop = vsp1_entity_get_pad_selection(&histo->entity, state,
-						     HISTO_PAD_SINK,
-						     V4L2_SEL_TGT_CROP);
+		crop = v4l2_subdev_state_get_crop(state, HISTO_PAD_SINK);
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = crop->width;
@@ -233,8 +225,7 @@ static int histo_get_selection(struct v4l2_subdev *subdev,
 
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 	case V4L2_SEL_TGT_CROP_DEFAULT:
-		format = vsp1_entity_get_pad_format(&histo->entity, state,
-						    HISTO_PAD_SINK);
+		format = v4l2_subdev_state_get_format(state, HISTO_PAD_SINK);
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = format->width;
@@ -242,9 +233,11 @@ static int histo_get_selection(struct v4l2_subdev *subdev,
 		break;
 
 	case V4L2_SEL_TGT_COMPOSE:
+		sel->r = *v4l2_subdev_state_get_compose(state, sel->pad);
+		break;
+
 	case V4L2_SEL_TGT_CROP:
-		sel->r = *vsp1_entity_get_pad_selection(&histo->entity, state,
-							sel->pad, sel->target);
+		sel->r = *v4l2_subdev_state_get_crop(state, sel->pad);
 		break;
 
 	default:
@@ -261,13 +254,10 @@ static int histo_set_crop(struct v4l2_subdev *subdev,
 			  struct v4l2_subdev_state *sd_state,
 			  struct v4l2_subdev_selection *sel)
 {
-	struct vsp1_histogram *histo = subdev_to_histo(subdev);
 	struct v4l2_mbus_framefmt *format;
-	struct v4l2_rect *selection;
 
 	/* The crop rectangle must be inside the input frame. */
-	format = vsp1_entity_get_pad_format(&histo->entity, sd_state,
-					    HISTO_PAD_SINK);
+	format = v4l2_subdev_state_get_format(sd_state, HISTO_PAD_SINK);
 	sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
 	sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
 	sel->r.width = clamp_t(unsigned int, sel->r.width, HISTO_MIN_SIZE,
@@ -276,14 +266,8 @@ static int histo_set_crop(struct v4l2_subdev *subdev,
 				format->height - sel->r.top);
 
 	/* Set the crop rectangle and reset the compose rectangle. */
-	selection = vsp1_entity_get_pad_selection(&histo->entity, sd_state,
-						  sel->pad, V4L2_SEL_TGT_CROP);
-	*selection = sel->r;
-
-	selection = vsp1_entity_get_pad_selection(&histo->entity, sd_state,
-						  sel->pad,
-						  V4L2_SEL_TGT_COMPOSE);
-	*selection = sel->r;
+	*v4l2_subdev_state_get_crop(sd_state, sel->pad) = sel->r;
+	*v4l2_subdev_state_get_compose(sd_state, sel->pad) = sel->r;
 
 	return 0;
 }
@@ -292,7 +276,6 @@ static int histo_set_compose(struct v4l2_subdev *subdev,
 			     struct v4l2_subdev_state *sd_state,
 			     struct v4l2_subdev_selection *sel)
 {
-	struct vsp1_histogram *histo = subdev_to_histo(subdev);
 	struct v4l2_rect *compose;
 	struct v4l2_rect *crop;
 	unsigned int ratio;
@@ -305,9 +288,7 @@ static int histo_set_compose(struct v4l2_subdev *subdev,
 	sel->r.left = 0;
 	sel->r.top = 0;
 
-	crop = vsp1_entity_get_pad_selection(&histo->entity, sd_state,
-					     sel->pad,
-					     V4L2_SEL_TGT_CROP);
+	crop = v4l2_subdev_state_get_crop(sd_state, sel->pad);
 
 	/*
 	 * Clamp the width and height to acceptable values first and then
@@ -332,9 +313,7 @@ static int histo_set_compose(struct v4l2_subdev *subdev,
 	ratio = 1 << (crop->height * 2 / sel->r.height / 3);
 	sel->r.height = crop->height / ratio;
 
-	compose = vsp1_entity_get_pad_selection(&histo->entity, sd_state,
-						sel->pad,
-						V4L2_SEL_TGT_COMPOSE);
+	compose = v4l2_subdev_state_get_compose(sd_state, sel->pad);
 	*compose = sel->r;
 
 	return 0;
@@ -371,31 +350,22 @@ done:
 	return ret;
 }
 
-static int histo_get_format(struct v4l2_subdev *subdev,
+static int histo_set_format(struct v4l2_subdev *subdev,
 			    struct v4l2_subdev_state *sd_state,
 			    struct v4l2_subdev_format *fmt)
 {
+	struct vsp1_histogram *histo = subdev_to_histo(subdev);
+
 	if (fmt->pad == HISTO_PAD_SOURCE) {
 		fmt->format.code = MEDIA_BUS_FMT_FIXED;
 		fmt->format.width = 0;
 		fmt->format.height = 0;
 		fmt->format.field = V4L2_FIELD_NONE;
 		fmt->format.colorspace = V4L2_COLORSPACE_RAW;
+
 		return 0;
 	}
 
-	return vsp1_subdev_get_pad_format(subdev, sd_state, fmt);
-}
-
-static int histo_set_format(struct v4l2_subdev *subdev,
-			    struct v4l2_subdev_state *sd_state,
-			    struct v4l2_subdev_format *fmt)
-{
-	struct vsp1_histogram *histo = subdev_to_histo(subdev);
-
-	if (fmt->pad != HISTO_PAD_SINK)
-		return histo_get_format(subdev, sd_state, fmt);
-
 	return vsp1_subdev_set_pad_format(subdev, sd_state, fmt,
 					  histo->formats, histo->num_formats,
 					  HISTO_MIN_SIZE, HISTO_MIN_SIZE,
@@ -405,7 +375,7 @@ static int histo_set_format(struct v4l2_subdev *subdev,
 static const struct v4l2_subdev_pad_ops histo_pad_ops = {
 	.enum_mbus_code = histo_enum_mbus_code,
 	.enum_frame_size = histo_enum_frame_size,
-	.get_fmt = histo_get_format,
+	.get_fmt = vsp1_subdev_get_pad_format,
 	.set_fmt = histo_set_format,
 	.get_selection = histo_get_selection,
 	.set_selection = histo_set_selection,
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_hsit.c b/drivers/media/platform/renesas/vsp1/vsp1_hsit.c
index bc1299c29ac9..1fcd1967d3b2 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_hsit.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_hsit.c
@@ -14,6 +14,7 @@
 
 #include "vsp1.h"
 #include "vsp1_dl.h"
+#include "vsp1_entity.h"
 #include "vsp1_hsit.h"
 
 #define HSIT_MIN_SIZE				4U
@@ -78,7 +79,7 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
 		goto done;
 	}
 
-	format = vsp1_entity_get_pad_format(&hsit->entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 
 	if (fmt->pad == HSIT_PAD_SOURCE) {
 		/*
@@ -96,17 +97,24 @@ static int hsit_set_format(struct v4l2_subdev *subdev,
 	format->height = clamp_t(unsigned int, fmt->format.height,
 				 HSIT_MIN_SIZE, HSIT_MAX_SIZE);
 	format->field = V4L2_FIELD_NONE;
-	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	format->colorspace = fmt->format.colorspace;
+	format->xfer_func = fmt->format.xfer_func;
+	format->ycbcr_enc = fmt->format.ycbcr_enc;
+	format->quantization = fmt->format.quantization;
+
+	vsp1_entity_adjust_color_space(format);
 
 	fmt->format = *format;
 
 	/* Propagate the format to the source pad. */
-	format = vsp1_entity_get_pad_format(&hsit->entity, state,
-					    HSIT_PAD_SOURCE);
+	format = v4l2_subdev_state_get_format(state, HSIT_PAD_SOURCE);
 	*format = fmt->format;
 	format->code = hsit->inverse ? MEDIA_BUS_FMT_ARGB8888_1X32
 		     : MEDIA_BUS_FMT_AHSV8888_1X32;
 
+	vsp1_entity_adjust_color_space(format);
+
 done:
 	mutex_unlock(&hsit->entity.lock);
 	return ret;
@@ -128,6 +136,7 @@ static const struct v4l2_subdev_ops hsit_ops = {
  */
 
 static void hsit_configure_stream(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe,
 				  struct vsp1_dl_list *dl,
 				  struct vsp1_dl_body *dlb)
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_iif.c b/drivers/media/platform/renesas/vsp1/vsp1_iif.c
new file mode 100644
index 000000000000..5dd62bebbe8c
--- /dev/null
+++ b/drivers/media/platform/renesas/vsp1/vsp1_iif.c
@@ -0,0 +1,121 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * vsp1_iif.c  --  R-Car VSP1 IIF (ISP Interface)
+ *
+ * Copyright (C) 2025 Ideas On Board Oy
+ * Copyright (C) 2025 Renesas Corporation
+ */
+
+#include "vsp1.h"
+#include "vsp1_dl.h"
+#include "vsp1_iif.h"
+
+#define IIF_MIN_WIDTH				128U
+#define IIF_MIN_HEIGHT				32U
+#define IIF_MAX_WIDTH				5120U
+#define IIF_MAX_HEIGHT				4096U
+
+/* -----------------------------------------------------------------------------
+ * Device Access
+ */
+
+static inline void vsp1_iif_write(struct vsp1_dl_body *dlb, u32 reg, u32 data)
+{
+	vsp1_dl_body_write(dlb, reg, data);
+}
+
+/* -----------------------------------------------------------------------------
+ * V4L2 Subdevice Operations
+ */
+
+static const unsigned int iif_codes[] = {
+	MEDIA_BUS_FMT_Y8_1X8,
+	MEDIA_BUS_FMT_Y10_1X10,
+	MEDIA_BUS_FMT_Y12_1X12,
+	MEDIA_BUS_FMT_Y16_1X16,
+	MEDIA_BUS_FMT_METADATA_FIXED
+};
+
+static int iif_enum_mbus_code(struct v4l2_subdev *subdev,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_mbus_code_enum *code)
+{
+	return vsp1_subdev_enum_mbus_code(subdev, sd_state, code, iif_codes,
+					  ARRAY_SIZE(iif_codes));
+}
+
+static int iif_enum_frame_size(struct v4l2_subdev *subdev,
+			       struct v4l2_subdev_state *sd_state,
+			       struct v4l2_subdev_frame_size_enum *fse)
+{
+	return vsp1_subdev_enum_frame_size(subdev, sd_state, fse,
+					   IIF_MIN_WIDTH, IIF_MIN_HEIGHT,
+					   IIF_MAX_WIDTH, IIF_MAX_HEIGHT);
+}
+
+static int iif_set_format(struct v4l2_subdev *subdev,
+			  struct v4l2_subdev_state *sd_state,
+			  struct v4l2_subdev_format *fmt)
+{
+	return vsp1_subdev_set_pad_format(subdev, sd_state, fmt, iif_codes,
+					  ARRAY_SIZE(iif_codes),
+					  IIF_MIN_WIDTH, IIF_MIN_HEIGHT,
+					  IIF_MAX_WIDTH, IIF_MAX_HEIGHT);
+}
+
+static const struct v4l2_subdev_pad_ops iif_pad_ops = {
+	.enum_mbus_code = iif_enum_mbus_code,
+	.enum_frame_size = iif_enum_frame_size,
+	.get_fmt = vsp1_subdev_get_pad_format,
+	.set_fmt = iif_set_format,
+};
+
+static const struct v4l2_subdev_ops iif_ops = {
+	.pad    = &iif_pad_ops,
+};
+
+/* -----------------------------------------------------------------------------
+ * VSP1 Entity Operations
+ */
+
+static void iif_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
+				 struct vsp1_pipeline *pipe,
+				 struct vsp1_dl_list *dl,
+				 struct vsp1_dl_body *dlb)
+{
+	vsp1_iif_write(dlb, VI6_IIF_CTRL, VI6_IIF_CTRL_CTRL);
+}
+
+static const struct vsp1_entity_operations iif_entity_ops = {
+	.configure_stream = iif_configure_stream,
+};
+
+/* -----------------------------------------------------------------------------
+ * Initialization and Cleanup
+ */
+
+struct vsp1_iif *vsp1_iif_create(struct vsp1_device *vsp1)
+{
+	struct vsp1_iif *iif;
+	int ret;
+
+	iif = devm_kzalloc(vsp1->dev, sizeof(*iif), GFP_KERNEL);
+	if (!iif)
+		return ERR_PTR(-ENOMEM);
+
+	iif->entity.ops = &iif_entity_ops;
+	iif->entity.type = VSP1_ENTITY_IIF;
+
+	/*
+	 * The IIF is never exposed to userspace, but media entity registration
+	 * requires a function to be set. Use PROC_VIDEO_PIXEL_FORMATTER just to
+	 * avoid triggering a WARN_ON(), the value won't be seen anywhere.
+	 */
+	ret = vsp1_entity_init(vsp1, &iif->entity, "iif", 3, &iif_ops,
+			       MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	return iif;
+}
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_iif.h b/drivers/media/platform/renesas/vsp1/vsp1_iif.h
new file mode 100644
index 000000000000..46f327851c35
--- /dev/null
+++ b/drivers/media/platform/renesas/vsp1/vsp1_iif.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * vsp1_iif.h  --  R-Car VSP1 IIF (ISP Interface)
+ *
+ * Copyright (C) 2025 Ideas On Board Oy
+ * Copyright (C) 2025 Renesas Corporation
+ */
+#ifndef __VSP1_IIF_H__
+#define __VSP1_IIF_H__
+
+#include <media/v4l2-subdev.h>
+
+#include "vsp1_entity.h"
+
+#define VSPX_IIF_SINK_PAD_IMG		0
+#define VSPX_IIF_SINK_PAD_CONFIG	2
+
+struct vsp1_iif {
+	struct vsp1_entity entity;
+};
+
+static inline struct vsp1_iif *to_iif(struct v4l2_subdev *subdev)
+{
+	return container_of(subdev, struct vsp1_iif, entity.subdev);
+}
+
+struct vsp1_iif *vsp1_iif_create(struct vsp1_device *vsp1);
+
+#endif /* __VSP1_IIF_H__ */
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_lif.c b/drivers/media/platform/renesas/vsp1/vsp1_lif.c
index b1d21a54837b..b3d83d1c5306 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_lif.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_lif.c
@@ -83,6 +83,7 @@ static const struct v4l2_subdev_ops lif_ops = {
  */
 
 static void lif_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -93,8 +94,7 @@ static void lif_configure_stream(struct vsp1_entity *entity,
 	unsigned int obth;
 	unsigned int lbth;
 
-	format = vsp1_entity_get_pad_format(&lif->entity, lif->entity.state,
-					    LIF_PAD_SOURCE);
+	format = v4l2_subdev_state_get_format(state, LIF_PAD_SOURCE);
 
 	switch (entity->vsp1->version & VI6_IP_VERSION_MODEL_MASK) {
 	case VI6_IP_VERSION_MODEL_VSPD_GEN2:
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_lut.c b/drivers/media/platform/renesas/vsp1/vsp1_lut.c
index 451d24ab0b56..dd264e6532e0 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_lut.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_lut.c
@@ -146,6 +146,7 @@ static const struct v4l2_subdev_ops lut_ops = {
  */
 
 static void lut_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_pipe.c b/drivers/media/platform/renesas/vsp1/vsp1_pipe.c
index 68d05243c3ee..3cbb768cf6ad 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_pipe.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_pipe.c
@@ -138,14 +138,6 @@ static const struct vsp1_format_info vsp1_video_formats[] = {
 	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
 	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
 	  1, { 32, 0, 0 }, false, false, 1, 1, false },
-	{ V4L2_PIX_FMT_HSV24, MEDIA_BUS_FMT_AHSV8888_1X32,
-	  VI6_FMT_RGB_888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
-	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
-	  1, { 24, 0, 0 }, false, false, 1, 1, false },
-	{ V4L2_PIX_FMT_HSV32, MEDIA_BUS_FMT_AHSV8888_1X32,
-	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
-	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
-	  1, { 32, 0, 0 }, false, false, 1, 1, false },
 	{ V4L2_PIX_FMT_RGBX1010102, MEDIA_BUS_FMT_ARGB8888_1X32,
 	  VI6_FMT_RGB10_RGB10A2_A2RGB10,
 	  VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS,
@@ -162,10 +154,6 @@ static const struct vsp1_format_info vsp1_video_formats[] = {
 	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
 	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
 	  1, { 16, 0, 0 }, false, false, 2, 1, false },
-	{ V4L2_PIX_FMT_VYUY, MEDIA_BUS_FMT_AYUV8_1X32,
-	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
-	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
-	  1, { 16, 0, 0 }, false, true, 2, 1, false },
 	{ V4L2_PIX_FMT_YUYV, MEDIA_BUS_FMT_AYUV8_1X32,
 	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
 	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
@@ -222,6 +210,24 @@ static const struct vsp1_format_info vsp1_video_formats[] = {
 	  1, { 32, 0, 0 }, false, false, 2, 1, false },
 };
 
+static const struct vsp1_format_info vsp1_video_gen2_formats[] = {
+	{ V4L2_PIX_FMT_VYUY, MEDIA_BUS_FMT_AYUV8_1X32,
+	  VI6_FMT_YUYV_422, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 16, 0, 0 }, false, true, 2, 1, false },
+};
+
+static const struct vsp1_format_info vsp1_video_hsit_formats[] = {
+	{ V4L2_PIX_FMT_HSV24, MEDIA_BUS_FMT_AHSV8888_1X32,
+	  VI6_FMT_RGB_888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 24, 0, 0 }, false, false, 1, 1, false },
+	{ V4L2_PIX_FMT_HSV32, MEDIA_BUS_FMT_AHSV8888_1X32,
+	  VI6_FMT_ARGB_8888, VI6_RPF_DSWAP_P_LLS | VI6_RPF_DSWAP_P_LWS |
+	  VI6_RPF_DSWAP_P_WDS | VI6_RPF_DSWAP_P_BTS,
+	  1, { 32, 0, 0 }, false, false, 1, 1, false },
+};
+
 /**
  * vsp1_get_format_info - Retrieve format information for a 4CC
  * @vsp1: the VSP1 device
@@ -235,26 +241,164 @@ const struct vsp1_format_info *vsp1_get_format_info(struct vsp1_device *vsp1,
 {
 	unsigned int i;
 
-	/* Special case, the VYUY and HSV formats are supported on Gen2 only. */
-	if (vsp1->info->gen != 2) {
-		switch (fourcc) {
-		case V4L2_PIX_FMT_VYUY:
-		case V4L2_PIX_FMT_HSV24:
-		case V4L2_PIX_FMT_HSV32:
-			return NULL;
+	for (i = 0; i < ARRAY_SIZE(vsp1_video_formats); ++i) {
+		const struct vsp1_format_info *info = &vsp1_video_formats[i];
+
+		if (info->fourcc == fourcc)
+			return info;
+	}
+
+	if (vsp1->info->gen == 2) {
+		for (i = 0; i < ARRAY_SIZE(vsp1_video_gen2_formats); ++i) {
+			const struct vsp1_format_info *info =
+				&vsp1_video_gen2_formats[i];
+
+			if (info->fourcc == fourcc)
+				return info;
+		}
+	}
+
+	if (vsp1_feature(vsp1, VSP1_HAS_HSIT)) {
+		for (i = 0; i < ARRAY_SIZE(vsp1_video_hsit_formats); ++i) {
+			const struct vsp1_format_info *info =
+				&vsp1_video_hsit_formats[i];
+
+			if (info->fourcc == fourcc)
+				return info;
+		}
+	}
+
+	return NULL;
+}
+
+/**
+ * vsp1_get_format_info_by_index - Enumerate format information
+ * @vsp1: the VSP1 device
+ * @index: the format index
+ * @code: media bus code to limit enumeration
+ *
+ * Return a pointer to the format information structure corresponding to the
+ * given index, or NULL if the index exceeds the supported formats list. If the
+ * @code parameter is not zero, only formats compatible with the media bus code
+ * will be enumerated.
+ */
+const struct vsp1_format_info *
+vsp1_get_format_info_by_index(struct vsp1_device *vsp1, unsigned int index,
+			      u32 code)
+{
+	unsigned int i;
+
+	if (!code) {
+		if (index < ARRAY_SIZE(vsp1_video_formats))
+			return &vsp1_video_formats[index];
+
+		if (vsp1->info->gen == 2) {
+			index -= ARRAY_SIZE(vsp1_video_formats);
+			if (index < ARRAY_SIZE(vsp1_video_gen2_formats))
+				return &vsp1_video_gen2_formats[index];
+		}
+
+		if (vsp1_feature(vsp1, VSP1_HAS_HSIT)) {
+			index -= ARRAY_SIZE(vsp1_video_gen2_formats);
+			if (index < ARRAY_SIZE(vsp1_video_hsit_formats))
+				return &vsp1_video_hsit_formats[index];
 		}
+
+		return NULL;
 	}
 
 	for (i = 0; i < ARRAY_SIZE(vsp1_video_formats); ++i) {
 		const struct vsp1_format_info *info = &vsp1_video_formats[i];
 
-		if (info->fourcc == fourcc)
-			return info;
+		if (info->mbus == code) {
+			if (!index)
+				return info;
+			index--;
+		}
+	}
+
+	if (vsp1->info->gen == 2) {
+		for (i = 0; i < ARRAY_SIZE(vsp1_video_gen2_formats); ++i) {
+			const struct vsp1_format_info *info =
+				&vsp1_video_gen2_formats[i];
+
+			if (info->mbus == code) {
+				if (!index)
+					return info;
+				index--;
+			}
+		}
+	}
+
+	if (vsp1_feature(vsp1, VSP1_HAS_HSIT)) {
+		for (i = 0; i < ARRAY_SIZE(vsp1_video_hsit_formats); ++i) {
+			const struct vsp1_format_info *info =
+				&vsp1_video_hsit_formats[i];
+
+			if (info->mbus == code) {
+				if (!index)
+					return info;
+				index--;
+			}
+		}
 	}
 
 	return NULL;
 }
 
+/**
+ * vsp1_adjust_color_space - Adjust color space fields in a format
+ * @code: the media bus code
+ * @colorspace: the colorspace
+ * @xfer_func: the transfer function
+ * @encoding: the encoding
+ * @quantization: the quantization
+ *
+ * This function adjusts all color space fields of a video device of subdev
+ * format structure, taking into account the requested format, requested color
+ * space and limitations of the VSP1. It should be used in the video device and
+ * subdev set format handlers.
+ *
+ * The colorspace and xfer_func fields are freely configurable, as they are out
+ * of scope for VSP processing. The encoding and quantization is hardcoded for
+ * non-YUV formats, and can be configured for YUV formats.
+ */
+void vsp1_adjust_color_space(u32 code, u32 *colorspace, u8 *xfer_func,
+			     u8 *encoding, u8 *quantization)
+{
+	if (*colorspace == V4L2_COLORSPACE_DEFAULT ||
+	    *colorspace >= V4L2_COLORSPACE_LAST)
+		*colorspace = code == MEDIA_BUS_FMT_AYUV8_1X32
+			    ? V4L2_COLORSPACE_SMPTE170M
+			    : V4L2_COLORSPACE_SRGB;
+
+	if (*xfer_func == V4L2_XFER_FUNC_DEFAULT ||
+	    *xfer_func >= V4L2_XFER_FUNC_LAST)
+		*xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(*colorspace);
+
+	switch (code) {
+	case MEDIA_BUS_FMT_ARGB8888_1X32:
+	default:
+		*encoding = V4L2_YCBCR_ENC_601;
+		*quantization = V4L2_QUANTIZATION_FULL_RANGE;
+		break;
+
+	case MEDIA_BUS_FMT_AHSV8888_1X32:
+		*encoding = V4L2_HSV_ENC_256;
+		*quantization = V4L2_QUANTIZATION_FULL_RANGE;
+		break;
+
+	case MEDIA_BUS_FMT_AYUV8_1X32:
+		if (*encoding != V4L2_YCBCR_ENC_601 &&
+		    *encoding != V4L2_YCBCR_ENC_709)
+			*encoding = V4L2_YCBCR_ENC_601;
+		if (*quantization != V4L2_QUANTIZATION_FULL_RANGE &&
+		    *quantization != V4L2_QUANTIZATION_LIM_RANGE)
+			*quantization = V4L2_QUANTIZATION_LIM_RANGE;
+		break;
+	}
+}
+
 /* -----------------------------------------------------------------------------
  * Pipeline Management
  */
@@ -286,6 +430,7 @@ void vsp1_pipeline_reset(struct vsp1_pipeline *pipe)
 	pipe->brx = NULL;
 	pipe->hgo = NULL;
 	pipe->hgt = NULL;
+	pipe->iif = NULL;
 	pipe->lif = NULL;
 	pipe->uds = NULL;
 }
@@ -301,6 +446,28 @@ void vsp1_pipeline_init(struct vsp1_pipeline *pipe)
 	pipe->state = VSP1_PIPELINE_STOPPED;
 }
 
+void __vsp1_pipeline_dump(struct _ddebug *dbg, struct vsp1_pipeline *pipe,
+			  const char *msg)
+{
+	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
+	struct vsp1_entity *entity;
+	bool first = true;
+
+	printk(KERN_DEBUG "%s: %s: pipe: ", dev_name(vsp1->dev), msg);
+
+	list_for_each_entry(entity, &pipe->entities, list_pipe) {
+		const char *name;
+
+		name = strchrnul(entity->subdev.name, ' ');
+		name = name ? name + 1 : entity->subdev.name;
+
+		pr_cont("%s%s", first ? "" : ", ", name);
+		first = false;
+	}
+
+	pr_cont("\n");
+}
+
 /* Must be called with the pipe irqlock held. */
 void vsp1_pipeline_run(struct vsp1_pipeline *pipe)
 {
@@ -444,6 +611,10 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
 	vsp1_uds_set_alpha(pipe->uds, dlb, alpha);
 }
 
+/* -----------------------------------------------------------------------------
+ * VSP1 Partition Algorithm support
+ */
+
 /*
  * Propagate the partition calculations through the pipeline
  *
@@ -452,17 +623,82 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
  * source. Each entity must produce the partition required for the previous
  * entity in the pipeline.
  */
-void vsp1_pipeline_propagate_partition(struct vsp1_pipeline *pipe,
-				       struct vsp1_partition *partition,
-				       unsigned int index,
-				       struct vsp1_partition_window *window)
+static void vsp1_pipeline_propagate_partition(struct vsp1_pipeline *pipe,
+					      struct vsp1_partition *partition,
+					      unsigned int index,
+					      struct v4l2_rect *window)
 {
 	struct vsp1_entity *entity;
 
 	list_for_each_entry_reverse(entity, &pipe->entities, list_pipe) {
 		if (entity->ops->partition)
-			entity->ops->partition(entity, pipe, partition, index,
-					       window);
+			entity->ops->partition(entity, entity->state, pipe,
+					       partition, index, window);
 	}
 }
 
+/*
+ * vsp1_pipeline_calculate_partition - Calculate pipeline configuration for a
+ * partition
+ *
+ * @pipe: the pipeline
+ * @partition: partition that will hold the calculated values
+ * @div_size: pre-determined maximum partition division size
+ * @index: partition index
+ */
+void vsp1_pipeline_calculate_partition(struct vsp1_pipeline *pipe,
+				       struct vsp1_partition *partition,
+				       unsigned int div_size,
+				       unsigned int index)
+{
+	const struct v4l2_mbus_framefmt *format;
+	struct v4l2_rect window;
+	unsigned int modulus;
+
+	/*
+	 * Partitions are computed on the size before rotation, use the format
+	 * at the WPF sink.
+	 */
+	format = v4l2_subdev_state_get_format(pipe->output->entity.state,
+					      RWPF_PAD_SINK);
+
+	/* Initialise the partition with sane starting conditions. */
+	window.left = index * div_size;
+	window.width = div_size;
+	window.top = 0;
+	window.height = format->height;
+
+	modulus = format->width % div_size;
+
+	/*
+	 * We need to prevent the last partition from being smaller than the
+	 * *minimum* width of the hardware capabilities.
+	 *
+	 * If the modulus is less than half of the partition size,
+	 * the penultimate partition is reduced to half, which is added
+	 * to the final partition: |1234|1234|1234|12|341|
+	 * to prevent this:        |1234|1234|1234|1234|1|.
+	 */
+	if (modulus) {
+		/*
+		 * pipe->partitions is 1 based, whilst index is a 0 based index.
+		 * Normalise this locally.
+		 */
+		unsigned int partitions = pipe->partitions - 1;
+
+		if (modulus < div_size / 2) {
+			if (index == partitions - 1) {
+				/* Halve the penultimate partition. */
+				window.width = div_size / 2;
+			} else if (index == partitions) {
+				/* Increase the final partition. */
+				window.width = (div_size / 2) + modulus;
+				window.left -= div_size / 2;
+			}
+		} else if (index == partitions) {
+			window.width = modulus;
+		}
+	}
+
+	vsp1_pipeline_propagate_partition(pipe, partition, index, &window);
+}
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_pipe.h b/drivers/media/platform/renesas/vsp1/vsp1_pipe.h
index 674b5748d929..7f623b8cbe5c 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_pipe.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1_pipe.h
@@ -9,6 +9,7 @@
 #ifndef __VSP1_PIPE_H__
 #define __VSP1_PIPE_H__
 
+#include <linux/dynamic_debug.h>
 #include <linux/kref.h>
 #include <linux/list.h>
 #include <linux/spinlock.h>
@@ -54,17 +55,6 @@ enum vsp1_pipeline_state {
 };
 
 /*
- * struct vsp1_partition_window - Partition window coordinates
- * @left: horizontal coordinate of the partition start in pixels relative to the
- *	  left edge of the image
- * @width: partition width in pixels
- */
-struct vsp1_partition_window {
-	unsigned int left;
-	unsigned int width;
-};
-
-/*
  * struct vsp1_partition - A description of a slice for the partition algorithm
  * @rpf: The RPF partition window configuration
  * @uds_sink: The UDS input partition window configuration
@@ -73,11 +63,11 @@ struct vsp1_partition_window {
  * @wpf: The WPF partition window configuration
  */
 struct vsp1_partition {
-	struct vsp1_partition_window rpf;
-	struct vsp1_partition_window uds_sink;
-	struct vsp1_partition_window uds_source;
-	struct vsp1_partition_window sru;
-	struct vsp1_partition_window wpf;
+	struct v4l2_rect rpf[VSP1_MAX_RPF];
+	struct v4l2_rect uds_sink;
+	struct v4l2_rect uds_source;
+	struct v4l2_rect sru;
+	struct v4l2_rect wpf;
 };
 
 /*
@@ -106,7 +96,6 @@ struct vsp1_partition {
  * @configured: when false the @stream_config shall be written to the hardware
  * @interlaced: True when the pipeline is configured in interlaced mode
  * @partitions: The number of partitions used to process one frame
- * @partition: The current partition for configuration to process
  * @part_table: The pre-calculated partitions used by the pipeline
  */
 struct vsp1_pipeline {
@@ -130,6 +119,7 @@ struct vsp1_pipeline {
 	struct vsp1_entity *brx;
 	struct vsp1_entity *hgo;
 	struct vsp1_entity *hgt;
+	struct vsp1_entity *iif;
 	struct vsp1_entity *lif;
 	struct vsp1_entity *uds;
 	struct vsp1_entity *uds_input;
@@ -146,7 +136,6 @@ struct vsp1_pipeline {
 	bool interlaced;
 
 	unsigned int partitions;
-	struct vsp1_partition *partition;
 	struct vsp1_partition *part_table;
 
 	u32 underrun_count;
@@ -155,6 +144,24 @@ struct vsp1_pipeline {
 void vsp1_pipeline_reset(struct vsp1_pipeline *pipe);
 void vsp1_pipeline_init(struct vsp1_pipeline *pipe);
 
+void __vsp1_pipeline_dump(struct _ddebug *, struct vsp1_pipeline *pipe,
+			  const char *msg);
+
+#if defined(CONFIG_DYNAMIC_DEBUG) || \
+	(defined(CONFIG_DYNAMIC_DEBUG_CORE) && defined(DYNAMIC_DEBUG_MODULE))
+#define vsp1_pipeline_dump(pipe, msg)			\
+	_dynamic_func_call("vsp1_pipeline_dump()", __vsp1_pipeline_dump, pipe, msg)
+#elif defined(DEBUG)
+#define vsp1_pipeline_dump(pipe, msg)			\
+	__vsp1_pipeline_dump(NULL, pipe, msg)
+#else
+#define vsp1_pipeline_dump(pipe, msg)			\
+({							\
+	if (0)						\
+		__vsp1_pipeline_dump(NULL, pipe, msg);	\
+})
+#endif
+
 void vsp1_pipeline_run(struct vsp1_pipeline *pipe);
 bool vsp1_pipeline_stopped(struct vsp1_pipeline *pipe);
 int vsp1_pipeline_stop(struct vsp1_pipeline *pipe);
@@ -166,12 +173,17 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
 				   struct vsp1_dl_body *dlb,
 				   unsigned int alpha);
 
-void vsp1_pipeline_propagate_partition(struct vsp1_pipeline *pipe,
+void vsp1_pipeline_calculate_partition(struct vsp1_pipeline *pipe,
 				       struct vsp1_partition *partition,
-				       unsigned int index,
-				       struct vsp1_partition_window *window);
+				       unsigned int div_size,
+				       unsigned int index);
 
 const struct vsp1_format_info *vsp1_get_format_info(struct vsp1_device *vsp1,
 						    u32 fourcc);
+const struct vsp1_format_info *
+vsp1_get_format_info_by_index(struct vsp1_device *vsp1, unsigned int index,
+			      u32 code);
+void vsp1_adjust_color_space(u32 code, u32 *colorspace, u8 *xfer_func,
+			     u8 *encoding, u8 *quantization);
 
 #endif /* __VSP1_PIPE_H__ */
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_regs.h b/drivers/media/platform/renesas/vsp1/vsp1_regs.h
index 7eca82e0ba7e..86e47c2d991f 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_regs.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1_regs.h
@@ -253,6 +253,13 @@
 #define VI6_RPF_BRDITH_CTRL_CBRM	BIT(0)
 
 /* -----------------------------------------------------------------------------
+ * IIF Control Registers
+ */
+
+#define VI6_IIF_CTRL			0x0608
+#define VI6_IIF_CTRL_CTRL		0x13
+
+/* -----------------------------------------------------------------------------
  * WPF Control Registers
  */
 
@@ -388,6 +395,7 @@
 #define VI6_DPR_HST_ROUTE		0x2044
 #define VI6_DPR_HSI_ROUTE		0x2048
 #define VI6_DPR_BRU_ROUTE		0x204c
+#define VI6_DPR_ROUTE_IIFSEL		BIT(28)
 #define VI6_DPR_ILV_BRS_ROUTE		0x2050
 #define VI6_DPR_ROUTE_BRSSEL		BIT(28)
 #define VI6_DPR_ROUTE_FXA_MASK		(0xff << 16)
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_rpf.c b/drivers/media/platform/renesas/vsp1/vsp1_rpf.c
index c47579efc65f..811f2b7c5cc5 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_rpf.c
@@ -48,6 +48,7 @@ static inline void vsp1_rpf_write(struct vsp1_rwpf *rpf,
  */
 
 static void rpf_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -80,14 +81,10 @@ static void rpf_configure_stream(struct vsp1_entity *entity,
 	vsp1_rpf_write(rpf, dlb, VI6_RPF_SRCM_PSTRIDE, pstride);
 
 	/* Format */
-	sink_format = vsp1_entity_get_pad_format(&rpf->entity,
-						 rpf->entity.state,
-						 RWPF_PAD_SINK);
-	source_format = vsp1_entity_get_pad_format(&rpf->entity,
-						   rpf->entity.state,
-						   RWPF_PAD_SOURCE);
-
-	infmt = VI6_RPF_INFMT_CIPM
+	sink_format = v4l2_subdev_state_get_format(state, RWPF_PAD_SINK);
+	source_format = v4l2_subdev_state_get_format(state, RWPF_PAD_SOURCE);
+
+	infmt = (pipe->iif ? 0 : VI6_RPF_INFMT_CIPM)
 	      | (fmtinfo->hwfmt << VI6_RPF_INFMT_RDFMT_SHIFT);
 
 	if (fmtinfo->swap_yc)
@@ -95,12 +92,44 @@ static void rpf_configure_stream(struct vsp1_entity *entity,
 	if (fmtinfo->swap_uv)
 		infmt |= VI6_RPF_INFMT_SPUVS;
 
-	if (sink_format->code != source_format->code)
-		infmt |= VI6_RPF_INFMT_CSC;
+	if (sink_format->code != source_format->code) {
+		u16 ycbcr_enc;
+		u16 quantization;
+		u32 rdtm;
+
+		if (sink_format->code == MEDIA_BUS_FMT_AYUV8_1X32) {
+			ycbcr_enc = sink_format->ycbcr_enc;
+			quantization = sink_format->quantization;
+		} else {
+			ycbcr_enc = source_format->ycbcr_enc;
+			quantization = source_format->quantization;
+		}
+
+		if (ycbcr_enc == V4L2_YCBCR_ENC_601 &&
+		    quantization == V4L2_QUANTIZATION_LIM_RANGE)
+			rdtm = VI6_RPF_INFMT_RDTM_BT601;
+		else if (ycbcr_enc == V4L2_YCBCR_ENC_601 &&
+			 quantization == V4L2_QUANTIZATION_FULL_RANGE)
+			rdtm = VI6_RPF_INFMT_RDTM_BT601_EXT;
+		else if (ycbcr_enc == V4L2_YCBCR_ENC_709 &&
+			 quantization == V4L2_QUANTIZATION_LIM_RANGE)
+			rdtm = VI6_RPF_INFMT_RDTM_BT709;
+		else
+			rdtm = VI6_RPF_INFMT_RDTM_BT709_EXT;
+
+		infmt |= VI6_RPF_INFMT_CSC | rdtm;
+	}
 
 	vsp1_rpf_write(rpf, dlb, VI6_RPF_INFMT, infmt);
 	vsp1_rpf_write(rpf, dlb, VI6_RPF_DSWAP, fmtinfo->swap);
 
+	/* No further configuration for VSPX. */
+	if (pipe->iif) {
+		/* VSPX wants alpha_sel to be set to 0. */
+		vsp1_rpf_write(rpf, dlb, VI6_RPF_ALPH_SEL, 0);
+		return;
+	}
+
 	if (entity->vsp1->info->gen == 4) {
 		u32 ext_infmt0;
 		u32 ext_infmt1;
@@ -157,10 +186,8 @@ static void rpf_configure_stream(struct vsp1_entity *entity,
 	if (pipe->brx) {
 		const struct v4l2_rect *compose;
 
-		compose = vsp1_entity_get_pad_selection(pipe->brx,
-							pipe->brx->state,
-							rpf->brx_input,
-							V4L2_SEL_TGT_COMPOSE);
+		compose = v4l2_subdev_state_get_compose(pipe->brx->state,
+							rpf->brx_input);
 		left = compose->left;
 		top = compose->top;
 	}
@@ -284,6 +311,7 @@ static void rpf_configure_frame(struct vsp1_entity *entity,
 
 static void rpf_configure_partition(struct vsp1_entity *entity,
 				    struct vsp1_pipeline *pipe,
+				    const struct vsp1_partition *partition,
 				    struct vsp1_dl_list *dl,
 				    struct vsp1_dl_body *dlb)
 {
@@ -292,7 +320,7 @@ static void rpf_configure_partition(struct vsp1_entity *entity,
 	struct vsp1_device *vsp1 = rpf->entity.vsp1;
 	const struct vsp1_format_info *fmtinfo = rpf->fmtinfo;
 	const struct v4l2_pix_format_mplane *format = &rpf->format;
-	struct v4l2_rect crop;
+	struct v4l2_rect crop = partition->rpf[rpf->entity.index];
 
 	/*
 	 * Source size and crop offsets.
@@ -302,22 +330,6 @@ static void rpf_configure_partition(struct vsp1_entity *entity,
 	 * offsets are needed, as planes 2 and 3 always have identical
 	 * strides.
 	 */
-	crop = *vsp1_rwpf_get_crop(rpf, rpf->entity.state);
-
-	/*
-	 * Partition Algorithm Control
-	 *
-	 * The partition algorithm can split this frame into multiple
-	 * slices. We must scale our partition window based on the pipe
-	 * configuration to match the destination partition window.
-	 * To achieve this, we adjust our crop to provide a 'sub-crop'
-	 * matching the expected partition window. Only 'left' and
-	 * 'width' need to be adjusted.
-	 */
-	if (pipe->partitions > 1) {
-		crop.width = pipe->partition->rpf.width;
-		crop.left += pipe->partition->rpf.left;
-	}
 
 	if (pipe->interlaced) {
 		crop.height = round_down(crop.height / 2, fmtinfo->vsub);
@@ -366,12 +378,30 @@ static void rpf_configure_partition(struct vsp1_entity *entity,
 }
 
 static void rpf_partition(struct vsp1_entity *entity,
+			  struct v4l2_subdev_state *state,
 			  struct vsp1_pipeline *pipe,
 			  struct vsp1_partition *partition,
 			  unsigned int partition_idx,
-			  struct vsp1_partition_window *window)
+			  struct v4l2_rect *window)
 {
-	partition->rpf = *window;
+	struct vsp1_rwpf *rpf = to_rwpf(&entity->subdev);
+	struct v4l2_rect *rpf_rect = &partition->rpf[rpf->entity.index];
+
+	/*
+	 * Partition Algorithm Control
+	 *
+	 * The partition algorithm can split this frame into multiple slices. We
+	 * must adjust our partition window based on the pipe configuration to
+	 * match the destination partition window. To achieve this, we adjust
+	 * our crop to provide a 'sub-crop' matching the expected partition
+	 * window.
+	 */
+	*rpf_rect = *v4l2_subdev_state_get_crop(state, RWPF_PAD_SINK);
+
+	if (pipe->partitions > 1) {
+		rpf_rect->width = window->width;
+		rpf_rect->left += window->left;
+	}
 }
 
 static const struct vsp1_entity_operations rpf_entity_ops = {
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_rwpf.c b/drivers/media/platform/renesas/vsp1/vsp1_rwpf.c
index 09fb6ffa14e2..9c8085d5d306 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_rwpf.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_rwpf.c
@@ -10,18 +10,13 @@
 #include <media/v4l2-subdev.h>
 
 #include "vsp1.h"
+#include "vsp1_entity.h"
 #include "vsp1_rwpf.h"
 #include "vsp1_video.h"
 
 #define RWPF_MIN_WIDTH				1
 #define RWPF_MIN_HEIGHT				1
 
-struct v4l2_rect *vsp1_rwpf_get_crop(struct vsp1_rwpf *rwpf,
-				     struct v4l2_subdev_state *sd_state)
-{
-	return v4l2_subdev_state_get_crop(sd_state, RWPF_PAD_SINK);
-}
-
 /* -----------------------------------------------------------------------------
  * V4L2 Subdevice Operations
  */
@@ -41,6 +36,11 @@ static int vsp1_rwpf_enum_mbus_code(struct v4l2_subdev *subdev,
 
 	code->code = codes[code->index];
 
+	if (code->pad == RWPF_PAD_SOURCE &&
+	    code->code == MEDIA_BUS_FMT_AYUV8_1X32)
+		code->flags = V4L2_SUBDEV_MBUS_CODE_CSC_YCBCR_ENC
+			    | V4L2_SUBDEV_MBUS_CODE_CSC_QUANTIZATION;
+
 	return 0;
 }
 
@@ -79,15 +79,48 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 	    fmt->format.code != MEDIA_BUS_FMT_AYUV8_1X32)
 		fmt->format.code = MEDIA_BUS_FMT_AYUV8_1X32;
 
-	format = vsp1_entity_get_pad_format(&rwpf->entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 
 	if (fmt->pad == RWPF_PAD_SOURCE) {
+		const struct v4l2_mbus_framefmt *sink_format =
+			v4l2_subdev_state_get_format(state, RWPF_PAD_SINK);
+		u16 flags = fmt->format.flags & V4L2_MBUS_FRAMEFMT_SET_CSC;
+		bool csc;
+
 		/*
 		 * The RWPF performs format conversion but can't scale, only the
-		 * format code can be changed on the source pad.
+		 * format code, encoding and quantization can be changed on the
+		 * source pad when converting between RGB and YUV.
+		 */
+		if (sink_format->code != MEDIA_BUS_FMT_AHSV8888_1X32 &&
+		    fmt->format.code != MEDIA_BUS_FMT_AHSV8888_1X32)
+			format->code = fmt->format.code;
+		else
+			format->code = sink_format->code;
+
+		/*
+		 * Encoding and quantization can only be configured when YCbCr
+		 * <-> RGB is enabled. The V4L2 API requires userspace to set
+		 * the V4L2_MBUS_FRAMEFMT_SET_CSC flag. If either of these
+		 * conditions is not met, use the encoding and quantization
+		 * values from the sink pad.
 		 */
-		format->code = fmt->format.code;
+		csc = (format->code == MEDIA_BUS_FMT_AYUV8_1X32) !=
+		      (sink_format->code == MEDIA_BUS_FMT_AYUV8_1X32);
+
+		if (csc && (flags & V4L2_MBUS_FRAMEFMT_SET_CSC)) {
+			format->ycbcr_enc = fmt->format.ycbcr_enc;
+			format->quantization = fmt->format.quantization;
+		} else {
+			format->ycbcr_enc = sink_format->ycbcr_enc;
+			format->quantization = sink_format->quantization;
+		}
+
+		vsp1_entity_adjust_color_space(format);
+
 		fmt->format = *format;
+		fmt->format.flags = flags;
+
 		goto done;
 	}
 
@@ -97,7 +130,13 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 	format->height = clamp_t(unsigned int, fmt->format.height,
 				 RWPF_MIN_HEIGHT, rwpf->max_height);
 	format->field = V4L2_FIELD_NONE;
-	format->colorspace = V4L2_COLORSPACE_SRGB;
+
+	format->colorspace = fmt->format.colorspace;
+	format->xfer_func = fmt->format.xfer_func;
+	format->ycbcr_enc = fmt->format.ycbcr_enc;
+	format->quantization = fmt->format.quantization;
+
+	vsp1_entity_adjust_color_space(format);
 
 	fmt->format = *format;
 
@@ -105,7 +144,7 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 		struct v4l2_rect *crop;
 
 		/* Update the sink crop rectangle. */
-		crop = vsp1_rwpf_get_crop(rwpf, state);
+		crop = v4l2_subdev_state_get_crop(state, RWPF_PAD_SINK);
 		crop->left = 0;
 		crop->top = 0;
 		crop->width = fmt->format.width;
@@ -113,8 +152,7 @@ static int vsp1_rwpf_set_format(struct v4l2_subdev *subdev,
 	}
 
 	/* Propagate the format to the source pad. */
-	format = vsp1_entity_get_pad_format(&rwpf->entity, state,
-					    RWPF_PAD_SOURCE);
+	format = v4l2_subdev_state_get_format(state, RWPF_PAD_SOURCE);
 	*format = fmt->format;
 
 	if (rwpf->flip.rotate) {
@@ -153,12 +191,11 @@ static int vsp1_rwpf_get_selection(struct v4l2_subdev *subdev,
 
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP:
-		sel->r = *vsp1_rwpf_get_crop(rwpf, state);
+		sel->r = *v4l2_subdev_state_get_crop(state, RWPF_PAD_SINK);
 		break;
 
 	case V4L2_SEL_TGT_CROP_BOUNDS:
-		format = vsp1_entity_get_pad_format(&rwpf->entity, state,
-						    RWPF_PAD_SINK);
+		format = v4l2_subdev_state_get_format(state, RWPF_PAD_SINK);
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = format->width;
@@ -204,8 +241,7 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
 	}
 
 	/* Make sure the crop rectangle is entirely contained in the image. */
-	format = vsp1_entity_get_pad_format(&rwpf->entity, state,
-					    RWPF_PAD_SINK);
+	format = v4l2_subdev_state_get_format(state, RWPF_PAD_SINK);
 
 	/*
 	 * Restrict the crop rectangle coordinates to multiples of 2 to avoid
@@ -225,12 +261,11 @@ static int vsp1_rwpf_set_selection(struct v4l2_subdev *subdev,
 	sel->r.height = min_t(unsigned int, sel->r.height,
 			      format->height - sel->r.top);
 
-	crop = vsp1_rwpf_get_crop(rwpf, state);
+	crop = v4l2_subdev_state_get_crop(state, RWPF_PAD_SINK);
 	*crop = sel->r;
 
 	/* Propagate the format to the source pad. */
-	format = vsp1_entity_get_pad_format(&rwpf->entity, state,
-					    RWPF_PAD_SOURCE);
+	format = v4l2_subdev_state_get_format(state, RWPF_PAD_SOURCE);
 	format->width = crop->width;
 	format->height = crop->height;
 
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_rwpf.h b/drivers/media/platform/renesas/vsp1/vsp1_rwpf.h
index e0d212c70b2f..5ac9f0a6fafc 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/renesas/vsp1/vsp1_rwpf.h
@@ -85,7 +85,4 @@ int vsp1_rwpf_init_ctrls(struct vsp1_rwpf *rwpf, unsigned int ncontrols);
 
 extern const struct v4l2_subdev_ops vsp1_rwpf_subdev_ops;
 
-struct v4l2_rect *vsp1_rwpf_get_crop(struct vsp1_rwpf *rwpf,
-				     struct v4l2_subdev_state *sd_state);
-
 #endif /* __VSP1_RWPF_H__ */
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_sru.c b/drivers/media/platform/renesas/vsp1/vsp1_sru.c
index 11e008aa9f20..bba2872afaf2 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_sru.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_sru.c
@@ -14,6 +14,7 @@
 
 #include "vsp1.h"
 #include "vsp1_dl.h"
+#include "vsp1_entity.h"
 #include "vsp1_pipe.h"
 #include "vsp1_sru.h"
 
@@ -131,7 +132,7 @@ static int sru_enum_frame_size(struct v4l2_subdev *subdev,
 	if (!state)
 		return -EINVAL;
 
-	format = vsp1_entity_get_pad_format(&sru->entity, state, SRU_PAD_SINK);
+	format = v4l2_subdev_state_get_format(state, SRU_PAD_SINK);
 
 	mutex_lock(&sru->entity.lock);
 
@@ -178,16 +179,22 @@ static void sru_try_format(struct vsp1_sru *sru,
 		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
 			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
 
+		vsp1_entity_adjust_color_space(fmt);
+
 		fmt->width = clamp(fmt->width, SRU_MIN_SIZE, SRU_MAX_SIZE);
 		fmt->height = clamp(fmt->height, SRU_MIN_SIZE, SRU_MAX_SIZE);
 		break;
 
 	case SRU_PAD_SOURCE:
 		/* The SRU can't perform format conversion. */
-		format = vsp1_entity_get_pad_format(&sru->entity, sd_state,
-						    SRU_PAD_SINK);
+		format = v4l2_subdev_state_get_format(sd_state, SRU_PAD_SINK);
 		fmt->code = format->code;
 
+		fmt->colorspace = format->colorspace;
+		fmt->xfer_func = format->xfer_func;
+		fmt->ycbcr_enc = format->ycbcr_enc;
+		fmt->quantization = format->quantization;
+
 		/*
 		 * We can upscale by 2 in both direction, but not independently.
 		 * Compare the input and output rectangles areas (avoiding
@@ -212,7 +219,6 @@ static void sru_try_format(struct vsp1_sru *sru,
 	}
 
 	fmt->field = V4L2_FIELD_NONE;
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
 }
 
 static int sru_set_format(struct v4l2_subdev *subdev,
@@ -234,13 +240,12 @@ static int sru_set_format(struct v4l2_subdev *subdev,
 
 	sru_try_format(sru, state, fmt->pad, &fmt->format);
 
-	format = vsp1_entity_get_pad_format(&sru->entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 	*format = fmt->format;
 
 	if (fmt->pad == SRU_PAD_SINK) {
 		/* Propagate the format to the source pad. */
-		format = vsp1_entity_get_pad_format(&sru->entity, state,
-						    SRU_PAD_SOURCE);
+		format = v4l2_subdev_state_get_format(state, SRU_PAD_SOURCE);
 		*format = fmt->format;
 
 		sru_try_format(sru, state, SRU_PAD_SOURCE, format);
@@ -267,6 +272,7 @@ static const struct v4l2_subdev_ops sru_ops = {
  */
 
 static void sru_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -277,10 +283,8 @@ static void sru_configure_stream(struct vsp1_entity *entity,
 	struct v4l2_mbus_framefmt *output;
 	u32 ctrl0;
 
-	input = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					   SRU_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					    SRU_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, SRU_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, SRU_PAD_SOURCE);
 
 	if (input->code == MEDIA_BUS_FMT_ARGB8888_1X32)
 		ctrl0 = VI6_SRU_CTRL0_PARAM2 | VI6_SRU_CTRL0_PARAM3
@@ -301,16 +305,14 @@ static void sru_configure_stream(struct vsp1_entity *entity,
 }
 
 static unsigned int sru_max_width(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe)
 {
-	struct vsp1_sru *sru = to_sru(&entity->subdev);
 	struct v4l2_mbus_framefmt *input;
 	struct v4l2_mbus_framefmt *output;
 
-	input = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					   SRU_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					    SRU_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, SRU_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, SRU_PAD_SOURCE);
 
 	/*
 	 * The maximum input width of the SRU is 288 input pixels, but 32
@@ -324,24 +326,24 @@ static unsigned int sru_max_width(struct vsp1_entity *entity,
 }
 
 static void sru_partition(struct vsp1_entity *entity,
+			  struct v4l2_subdev_state *state,
 			  struct vsp1_pipeline *pipe,
 			  struct vsp1_partition *partition,
 			  unsigned int partition_idx,
-			  struct vsp1_partition_window *window)
+			  struct v4l2_rect *window)
 {
-	struct vsp1_sru *sru = to_sru(&entity->subdev);
 	struct v4l2_mbus_framefmt *input;
 	struct v4l2_mbus_framefmt *output;
 
-	input = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					   SRU_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&sru->entity, sru->entity.state,
-					    SRU_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, SRU_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, SRU_PAD_SOURCE);
 
 	/* Adapt if SRUx2 is enabled. */
 	if (input->width != output->width) {
 		window->width /= 2;
 		window->left /= 2;
+		window->height /= 2;
+		window->top /= 2;
 	}
 
 	partition->sru = *window;
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_uds.c b/drivers/media/platform/renesas/vsp1/vsp1_uds.c
index d89f1197b86c..2db473b6f83c 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_uds.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_uds.c
@@ -14,6 +14,7 @@
 
 #include "vsp1.h"
 #include "vsp1_dl.h"
+#include "vsp1_entity.h"
 #include "vsp1_pipe.h"
 #include "vsp1_uds.h"
 
@@ -136,7 +137,7 @@ static int uds_enum_frame_size(struct v4l2_subdev *subdev,
 	if (!state)
 		return -EINVAL;
 
-	format = vsp1_entity_get_pad_format(&uds->entity, state, UDS_PAD_SINK);
+	format = v4l2_subdev_state_get_format(state, UDS_PAD_SINK);
 
 	mutex_lock(&uds->entity.lock);
 
@@ -177,16 +178,22 @@ static void uds_try_format(struct vsp1_uds *uds,
 		    fmt->code != MEDIA_BUS_FMT_AYUV8_1X32)
 			fmt->code = MEDIA_BUS_FMT_AYUV8_1X32;
 
+		vsp1_entity_adjust_color_space(fmt);
+
 		fmt->width = clamp(fmt->width, UDS_MIN_SIZE, UDS_MAX_SIZE);
 		fmt->height = clamp(fmt->height, UDS_MIN_SIZE, UDS_MAX_SIZE);
 		break;
 
 	case UDS_PAD_SOURCE:
 		/* The UDS scales but can't perform format conversion. */
-		format = vsp1_entity_get_pad_format(&uds->entity, sd_state,
-						    UDS_PAD_SINK);
+		format = v4l2_subdev_state_get_format(sd_state, UDS_PAD_SINK);
 		fmt->code = format->code;
 
+		fmt->colorspace = format->colorspace;
+		fmt->xfer_func = format->xfer_func;
+		fmt->ycbcr_enc = format->ycbcr_enc;
+		fmt->quantization = format->quantization;
+
 		uds_output_limits(format->width, &minimum, &maximum);
 		fmt->width = clamp(fmt->width, minimum, maximum);
 		uds_output_limits(format->height, &minimum, &maximum);
@@ -195,7 +202,6 @@ static void uds_try_format(struct vsp1_uds *uds,
 	}
 
 	fmt->field = V4L2_FIELD_NONE;
-	fmt->colorspace = V4L2_COLORSPACE_SRGB;
 }
 
 static int uds_set_format(struct v4l2_subdev *subdev,
@@ -217,13 +223,12 @@ static int uds_set_format(struct v4l2_subdev *subdev,
 
 	uds_try_format(uds, state, fmt->pad, &fmt->format);
 
-	format = vsp1_entity_get_pad_format(&uds->entity, state, fmt->pad);
+	format = v4l2_subdev_state_get_format(state, fmt->pad);
 	*format = fmt->format;
 
 	if (fmt->pad == UDS_PAD_SINK) {
 		/* Propagate the format to the source pad. */
-		format = vsp1_entity_get_pad_format(&uds->entity, state,
-						    UDS_PAD_SOURCE);
+		format = v4l2_subdev_state_get_format(state, UDS_PAD_SOURCE);
 		*format = fmt->format;
 
 		uds_try_format(uds, state, UDS_PAD_SOURCE, format);
@@ -254,6 +259,7 @@ static const struct v4l2_subdev_ops uds_ops = {
  */
 
 static void uds_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -265,10 +271,8 @@ static void uds_configure_stream(struct vsp1_entity *entity,
 	unsigned int vscale;
 	bool multitap;
 
-	input = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					   UDS_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					    UDS_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, UDS_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, UDS_PAD_SOURCE);
 
 	hscale = uds_compute_ratio(input->width, output->width);
 	vscale = uds_compute_ratio(input->height, output->height);
@@ -303,15 +307,11 @@ static void uds_configure_stream(struct vsp1_entity *entity,
 
 static void uds_configure_partition(struct vsp1_entity *entity,
 				    struct vsp1_pipeline *pipe,
+				    const struct vsp1_partition *partition,
 				    struct vsp1_dl_list *dl,
 				    struct vsp1_dl_body *dlb)
 {
 	struct vsp1_uds *uds = to_uds(&entity->subdev);
-	struct vsp1_partition *partition = pipe->partition;
-	const struct v4l2_mbus_framefmt *output;
-
-	output = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					    UDS_PAD_SOURCE);
 
 	/* Input size clipping. */
 	vsp1_uds_write(uds, dlb, VI6_UDS_HSZCLIP, VI6_UDS_HSZCLIP_HCEN |
@@ -323,22 +323,20 @@ static void uds_configure_partition(struct vsp1_entity *entity,
 	vsp1_uds_write(uds, dlb, VI6_UDS_CLIP_SIZE,
 		       (partition->uds_source.width
 				<< VI6_UDS_CLIP_SIZE_HSIZE_SHIFT) |
-		       (output->height
+		       (partition->uds_source.height
 				<< VI6_UDS_CLIP_SIZE_VSIZE_SHIFT));
 }
 
 static unsigned int uds_max_width(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe)
 {
-	struct vsp1_uds *uds = to_uds(&entity->subdev);
 	const struct v4l2_mbus_framefmt *output;
 	const struct v4l2_mbus_framefmt *input;
 	unsigned int hscale;
 
-	input = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					   UDS_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					    UDS_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, UDS_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, UDS_PAD_SOURCE);
 	hscale = output->width / input->width;
 
 	/*
@@ -364,28 +362,26 @@ static unsigned int uds_max_width(struct vsp1_entity *entity,
  */
 
 static void uds_partition(struct vsp1_entity *entity,
+			  struct v4l2_subdev_state *state,
 			  struct vsp1_pipeline *pipe,
 			  struct vsp1_partition *partition,
 			  unsigned int partition_idx,
-			  struct vsp1_partition_window *window)
+			  struct v4l2_rect *window)
 {
-	struct vsp1_uds *uds = to_uds(&entity->subdev);
 	const struct v4l2_mbus_framefmt *output;
 	const struct v4l2_mbus_framefmt *input;
 
-	/* Initialise the partition state. */
-	partition->uds_sink = *window;
-	partition->uds_source = *window;
-
-	input = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					   UDS_PAD_SINK);
-	output = vsp1_entity_get_pad_format(&uds->entity, uds->entity.state,
-					    UDS_PAD_SOURCE);
+	input = v4l2_subdev_state_get_format(state, UDS_PAD_SINK);
+	output = v4l2_subdev_state_get_format(state, UDS_PAD_SOURCE);
 
 	partition->uds_sink.width = window->width * input->width
 				  / output->width;
 	partition->uds_sink.left = window->left * input->width
 				 / output->width;
+	partition->uds_sink.height = input->height;
+	partition->uds_sink.top = 0;
+
+	partition->uds_source = *window;
 
 	*window = partition->uds_sink;
 }
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_uif.c b/drivers/media/platform/renesas/vsp1/vsp1_uif.c
index f66936a28a2a..edaf28b544d2 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_uif.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_uif.c
@@ -104,8 +104,7 @@ static int uif_get_selection(struct v4l2_subdev *subdev,
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP_BOUNDS:
 	case V4L2_SEL_TGT_CROP_DEFAULT:
-		format = vsp1_entity_get_pad_format(&uif->entity, state,
-						    UIF_PAD_SINK);
+		format = v4l2_subdev_state_get_format(state, UIF_PAD_SINK);
 		sel->r.left = 0;
 		sel->r.top = 0;
 		sel->r.width = format->width;
@@ -113,8 +112,7 @@ static int uif_get_selection(struct v4l2_subdev *subdev,
 		break;
 
 	case V4L2_SEL_TGT_CROP:
-		sel->r = *vsp1_entity_get_pad_selection(&uif->entity, state,
-							sel->pad, sel->target);
+		sel->r = *v4l2_subdev_state_get_crop(state, sel->pad);
 		break;
 
 	default:
@@ -150,7 +148,7 @@ static int uif_set_selection(struct v4l2_subdev *subdev,
 	}
 
 	/* The crop rectangle must be inside the input frame. */
-	format = vsp1_entity_get_pad_format(&uif->entity, state, UIF_PAD_SINK);
+	format = v4l2_subdev_state_get_format(state, UIF_PAD_SINK);
 
 	sel->r.left = clamp_t(unsigned int, sel->r.left, 0, format->width - 1);
 	sel->r.top = clamp_t(unsigned int, sel->r.top, 0, format->height - 1);
@@ -160,8 +158,7 @@ static int uif_set_selection(struct v4l2_subdev *subdev,
 				format->height - sel->r.top);
 
 	/* Store the crop rectangle. */
-	selection = vsp1_entity_get_pad_selection(&uif->entity, state,
-						  sel->pad, V4L2_SEL_TGT_CROP);
+	selection = v4l2_subdev_state_get_crop(state, sel->pad);
 	*selection = sel->r;
 
 done:
@@ -191,6 +188,7 @@ static const struct v4l2_subdev_ops uif_ops = {
  */
 
 static void uif_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -203,8 +201,7 @@ static void uif_configure_stream(struct vsp1_entity *entity,
 	vsp1_uif_write(uif, dlb, VI6_UIF_DISCOM_DOCMPMR,
 		       VI6_UIF_DISCOM_DOCMPMR_SEL(9));
 
-	crop = vsp1_entity_get_pad_selection(entity, entity->state,
-					     UIF_PAD_SINK, V4L2_SEL_TGT_CROP);
+	crop = v4l2_subdev_state_get_crop(state, UIF_PAD_SINK);
 
 	left = crop->left;
 	width = crop->width;
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_video.c b/drivers/media/platform/renesas/vsp1/vsp1_video.c
index 5a9cb0e5640e..bc66fbdde3cc 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_video.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_video.c
@@ -78,8 +78,14 @@ static int vsp1_video_verify_format(struct vsp1_video *video)
 
 	if (video->rwpf->fmtinfo->mbus != fmt.format.code ||
 	    video->rwpf->format.height != fmt.format.height ||
-	    video->rwpf->format.width != fmt.format.width)
-		return -EINVAL;
+	    video->rwpf->format.width != fmt.format.width) {
+		dev_dbg(video->vsp1->dev,
+			"Format mismatch: 0x%04x/%ux%u != 0x%04x/%ux%u\n",
+			video->rwpf->fmtinfo->mbus, video->rwpf->format.width,
+			video->rwpf->format.height, fmt.format.code,
+			fmt.format.width, fmt.format.height);
+		return -EPIPE;
+	}
 
 	return 0;
 }
@@ -121,12 +127,24 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
 		info = vsp1_get_format_info(video->vsp1, VSP1_VIDEO_DEF_FORMAT);
 
 	pix->pixelformat = info->fourcc;
-	pix->colorspace = V4L2_COLORSPACE_SRGB;
 	pix->field = V4L2_FIELD_NONE;
 
-	if (info->fourcc == V4L2_PIX_FMT_HSV24 ||
-	    info->fourcc == V4L2_PIX_FMT_HSV32)
-		pix->hsv_enc = V4L2_HSV_ENC_256;
+	/*
+	 * Adjust the colour space fields. On capture devices, userspace needs
+	 * to set the V4L2_PIX_FMT_FLAG_SET_CSC to override the defaults. Reset
+	 * all fields to *_DEFAULT if the flag isn't set, to then handle
+	 * capture and output devices in the same way.
+	 */
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    !(pix->flags & V4L2_PIX_FMT_FLAG_SET_CSC)) {
+		pix->colorspace = V4L2_COLORSPACE_DEFAULT;
+		pix->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+		pix->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+		pix->quantization = V4L2_QUANTIZATION_DEFAULT;
+	}
+
+	vsp1_adjust_color_space(info->mbus, &pix->colorspace, &pix->xfer_func,
+				&pix->ycbcr_enc, &pix->quantization);
 
 	memset(pix->reserved, 0, sizeof(pix->reserved));
 
@@ -173,131 +191,6 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
 }
 
 /* -----------------------------------------------------------------------------
- * VSP1 Partition Algorithm support
- */
-
-/**
- * vsp1_video_calculate_partition - Calculate the active partition output window
- *
- * @pipe: the pipeline
- * @partition: partition that will hold the calculated values
- * @div_size: pre-determined maximum partition division size
- * @index: partition index
- */
-static void vsp1_video_calculate_partition(struct vsp1_pipeline *pipe,
-					   struct vsp1_partition *partition,
-					   unsigned int div_size,
-					   unsigned int index)
-{
-	const struct v4l2_mbus_framefmt *format;
-	struct vsp1_partition_window window;
-	unsigned int modulus;
-
-	/*
-	 * Partitions are computed on the size before rotation, use the format
-	 * at the WPF sink.
-	 */
-	format = vsp1_entity_get_pad_format(&pipe->output->entity,
-					    pipe->output->entity.state,
-					    RWPF_PAD_SINK);
-
-	/* A single partition simply processes the output size in full. */
-	if (pipe->partitions <= 1) {
-		window.left = 0;
-		window.width = format->width;
-
-		vsp1_pipeline_propagate_partition(pipe, partition, index,
-						  &window);
-		return;
-	}
-
-	/* Initialise the partition with sane starting conditions. */
-	window.left = index * div_size;
-	window.width = div_size;
-
-	modulus = format->width % div_size;
-
-	/*
-	 * We need to prevent the last partition from being smaller than the
-	 * *minimum* width of the hardware capabilities.
-	 *
-	 * If the modulus is less than half of the partition size,
-	 * the penultimate partition is reduced to half, which is added
-	 * to the final partition: |1234|1234|1234|12|341|
-	 * to prevent this:        |1234|1234|1234|1234|1|.
-	 */
-	if (modulus) {
-		/*
-		 * pipe->partitions is 1 based, whilst index is a 0 based index.
-		 * Normalise this locally.
-		 */
-		unsigned int partitions = pipe->partitions - 1;
-
-		if (modulus < div_size / 2) {
-			if (index == partitions - 1) {
-				/* Halve the penultimate partition. */
-				window.width = div_size / 2;
-			} else if (index == partitions) {
-				/* Increase the final partition. */
-				window.width = (div_size / 2) + modulus;
-				window.left -= div_size / 2;
-			}
-		} else if (index == partitions) {
-			window.width = modulus;
-		}
-	}
-
-	vsp1_pipeline_propagate_partition(pipe, partition, index, &window);
-}
-
-static int vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline *pipe)
-{
-	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
-	const struct v4l2_mbus_framefmt *format;
-	struct vsp1_entity *entity;
-	unsigned int div_size;
-	unsigned int i;
-
-	/*
-	 * Partitions are computed on the size before rotation, use the format
-	 * at the WPF sink.
-	 */
-	format = vsp1_entity_get_pad_format(&pipe->output->entity,
-					    pipe->output->entity.state,
-					    RWPF_PAD_SINK);
-	div_size = format->width;
-
-	/*
-	 * Only Gen3+ hardware requires image partitioning, Gen2 will operate
-	 * with a single partition that covers the whole output.
-	 */
-	if (vsp1->info->gen >= 3) {
-		list_for_each_entry(entity, &pipe->entities, list_pipe) {
-			unsigned int entity_max;
-
-			if (!entity->ops->max_width)
-				continue;
-
-			entity_max = entity->ops->max_width(entity, pipe);
-			if (entity_max)
-				div_size = min(div_size, entity_max);
-		}
-	}
-
-	pipe->partitions = DIV_ROUND_UP(format->width, div_size);
-	pipe->part_table = kcalloc(pipe->partitions, sizeof(*pipe->part_table),
-				   GFP_KERNEL);
-	if (!pipe->part_table)
-		return -ENOMEM;
-
-	for (i = 0; i < pipe->partitions; ++i)
-		vsp1_video_calculate_partition(pipe, &pipe->part_table[i],
-					       div_size, i);
-
-	return 0;
-}
-
-/* -----------------------------------------------------------------------------
  * Pipeline Management
  */
 
@@ -365,13 +258,12 @@ static void vsp1_video_pipeline_run_partition(struct vsp1_pipeline *pipe,
 					      struct vsp1_dl_list *dl,
 					      unsigned int partition)
 {
+	struct vsp1_partition *part = &pipe->part_table[partition];
 	struct vsp1_dl_body *dlb = vsp1_dl_list_get_body0(dl);
 	struct vsp1_entity *entity;
 
-	pipe->partition = &pipe->part_table[partition];
-
 	list_for_each_entry(entity, &pipe->entities, list_pipe)
-		vsp1_entity_configure_partition(entity, pipe, dl, dlb);
+		vsp1_entity_configure_partition(entity, pipe, part, dl, dlb);
 }
 
 static void vsp1_video_pipeline_run(struct vsp1_pipeline *pipe)
@@ -646,11 +538,19 @@ static int vsp1_video_pipeline_build(struct vsp1_pipeline *pipe,
 static int vsp1_video_pipeline_init(struct vsp1_pipeline *pipe,
 				    struct vsp1_video *video)
 {
+	int ret;
+
 	vsp1_pipeline_init(pipe);
 
 	pipe->frame_end = vsp1_video_pipeline_frame_end;
 
-	return vsp1_video_pipeline_build(pipe, video);
+	ret = vsp1_video_pipeline_build(pipe, video);
+	if (ret)
+		return ret;
+
+	vsp1_pipeline_dump(pipe, "video");
+
+	return 0;
 }
 
 static struct vsp1_pipeline *vsp1_video_pipeline_get(struct vsp1_video *video)
@@ -784,6 +684,54 @@ static void vsp1_video_buffer_queue(struct vb2_buffer *vb)
 	spin_unlock_irqrestore(&pipe->irqlock, flags);
 }
 
+static int vsp1_video_pipeline_setup_partitions(struct vsp1_pipeline *pipe)
+{
+	struct vsp1_device *vsp1 = pipe->output->entity.vsp1;
+	const struct v4l2_mbus_framefmt *format;
+	struct vsp1_entity *entity;
+	unsigned int div_size;
+	unsigned int i;
+
+	/*
+	 * Partitions are computed on the size before rotation, use the format
+	 * at the WPF sink.
+	 */
+	format = v4l2_subdev_state_get_format(pipe->output->entity.state,
+					      RWPF_PAD_SINK);
+	div_size = format->width;
+
+	/*
+	 * Only Gen3+ hardware requires image partitioning, Gen2 will operate
+	 * with a single partition that covers the whole output.
+	 */
+	if (vsp1->info->gen >= 3) {
+		list_for_each_entry(entity, &pipe->entities, list_pipe) {
+			unsigned int entity_max;
+
+			if (!entity->ops->max_width)
+				continue;
+
+			entity_max = entity->ops->max_width(entity,
+							    entity->state,
+							    pipe);
+			if (entity_max)
+				div_size = min(div_size, entity_max);
+		}
+	}
+
+	pipe->partitions = DIV_ROUND_UP(format->width, div_size);
+	pipe->part_table = kcalloc(pipe->partitions, sizeof(*pipe->part_table),
+				   GFP_KERNEL);
+	if (!pipe->part_table)
+		return -ENOMEM;
+
+	for (i = 0; i < pipe->partitions; ++i)
+		vsp1_pipeline_calculate_partition(pipe, &pipe->part_table[i],
+						  div_size, i);
+
+	return 0;
+}
+
 static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe)
 {
 	struct vsp1_entity *entity;
@@ -826,7 +774,7 @@ static int vsp1_video_setup_pipeline(struct vsp1_pipeline *pipe)
 
 	list_for_each_entry(entity, &pipe->entities, list_pipe) {
 		vsp1_entity_route_setup(entity, pipe, pipe->stream_config);
-		vsp1_entity_configure_stream(entity, pipe, NULL,
+		vsp1_entity_configure_stream(entity, entity->state, pipe, NULL,
 					     pipe->stream_config);
 	}
 
@@ -937,8 +885,6 @@ static const struct vb2_ops vsp1_video_queue_qops = {
 	.queue_setup = vsp1_video_queue_setup,
 	.buf_prepare = vsp1_video_buffer_prepare,
 	.buf_queue = vsp1_video_buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = vsp1_video_start_streaming,
 	.stop_streaming = vsp1_video_stop_streaming,
 };
@@ -954,16 +900,36 @@ vsp1_video_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
 
 	cap->capabilities = V4L2_CAP_DEVICE_CAPS | V4L2_CAP_STREAMING
-			  | V4L2_CAP_VIDEO_CAPTURE_MPLANE
+			  | V4L2_CAP_IO_MC | V4L2_CAP_VIDEO_CAPTURE_MPLANE
 			  | V4L2_CAP_VIDEO_OUTPUT_MPLANE;
 
-
 	strscpy(cap->driver, "vsp1", sizeof(cap->driver));
 	strscpy(cap->card, video->video.name, sizeof(cap->card));
 
 	return 0;
 }
 
+static int vsp1_video_enum_format(struct file *file, void *fh,
+				  struct v4l2_fmtdesc *f)
+{
+	struct v4l2_fh *vfh = file->private_data;
+	struct vsp1_video *video = to_vsp1_video(vfh->vdev);
+	const struct vsp1_format_info *info;
+
+	info = vsp1_get_format_info_by_index(video->vsp1, f->index, f->mbus_code);
+	if (!info)
+		return -EINVAL;
+
+	f->pixelformat = info->fourcc;
+
+	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
+	    info->mbus == MEDIA_BUS_FMT_AYUV8_1X32)
+		f->flags = V4L2_FMT_FLAG_CSC_YCBCR_ENC
+			 | V4L2_FMT_FLAG_CSC_QUANTIZATION;
+
+	return 0;
+}
+
 static int
 vsp1_video_get_format(struct file *file, void *fh, struct v4l2_format *format)
 {
@@ -1079,6 +1045,8 @@ err_pipe:
 
 static const struct v4l2_ioctl_ops vsp1_video_ioctl_ops = {
 	.vidioc_querycap		= vsp1_video_querycap,
+	.vidioc_enum_fmt_vid_cap	= vsp1_video_enum_format,
+	.vidioc_enum_fmt_vid_out	= vsp1_video_enum_format,
 	.vidioc_g_fmt_vid_cap_mplane	= vsp1_video_get_format,
 	.vidioc_s_fmt_vid_cap_mplane	= vsp1_video_set_format,
 	.vidioc_try_fmt_vid_cap_mplane	= vsp1_video_try_format,
@@ -1146,6 +1114,27 @@ static const struct v4l2_file_operations vsp1_video_fops = {
 };
 
 /* -----------------------------------------------------------------------------
+ * Media entity operations
+ */
+
+static int vsp1_video_link_validate(struct media_link *link)
+{
+	/*
+	 * Ideally, link validation should be implemented here instead of
+	 * calling vsp1_video_verify_format() in vsp1_video_streamon()
+	 * manually. That would however break userspace that start one video
+	 * device before configures formats on other video devices in the
+	 * pipeline. This operation is just a no-op to silence the warnings
+	 * from v4l2_subdev_link_validate().
+	 */
+	return 0;
+}
+
+static const struct media_entity_operations vsp1_video_media_ops = {
+	.link_validate = vsp1_video_link_validate,
+};
+
+/* -----------------------------------------------------------------------------
  * Suspend and Resume
  */
 
@@ -1252,14 +1241,14 @@ struct vsp1_video *vsp1_video_create(struct vsp1_device *vsp1,
 		video->pad.flags = MEDIA_PAD_FL_SOURCE;
 		video->video.vfl_dir = VFL_DIR_TX;
 		video->video.device_caps = V4L2_CAP_VIDEO_OUTPUT_MPLANE |
-					   V4L2_CAP_STREAMING;
+					   V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
 	} else {
 		direction = "output";
 		video->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 		video->pad.flags = MEDIA_PAD_FL_SINK;
 		video->video.vfl_dir = VFL_DIR_RX;
 		video->video.device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
-					   V4L2_CAP_STREAMING;
+					   V4L2_CAP_STREAMING | V4L2_CAP_IO_MC;
 	}
 
 	mutex_init(&video->lock);
@@ -1279,6 +1268,7 @@ struct vsp1_video *vsp1_video_create(struct vsp1_device *vsp1,
 
 	/* ... and the video node... */
 	video->video.v4l2_dev = &video->vsp1->v4l2_dev;
+	video->video.entity.ops = &vsp1_video_media_ops;
 	video->video.fops = &vsp1_video_fops;
 	snprintf(video->video.name, sizeof(video->video.name), "%s %s",
 		 rwpf->entity.subdev.name, direction);
diff --git a/drivers/media/platform/renesas/vsp1/vsp1_wpf.c b/drivers/media/platform/renesas/vsp1/vsp1_wpf.c
index 9693aeab1cac..30662cfdf837 100644
--- a/drivers/media/platform/renesas/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/renesas/vsp1/vsp1_wpf.c
@@ -65,12 +65,10 @@ static int vsp1_wpf_set_rotation(struct vsp1_rwpf *wpf, unsigned int rotation)
 		goto done;
 	}
 
-	sink_format = vsp1_entity_get_pad_format(&wpf->entity,
-						 wpf->entity.state,
-						 RWPF_PAD_SINK);
-	source_format = vsp1_entity_get_pad_format(&wpf->entity,
-						   wpf->entity.state,
-						   RWPF_PAD_SOURCE);
+	sink_format = v4l2_subdev_state_get_format(wpf->entity.state,
+						   RWPF_PAD_SINK);
+	source_format = v4l2_subdev_state_get_format(wpf->entity.state,
+						     RWPF_PAD_SOURCE);
 
 	mutex_lock(&wpf->entity.lock);
 
@@ -135,6 +133,7 @@ static int wpf_init_controls(struct vsp1_rwpf *wpf)
 {
 	struct vsp1_device *vsp1 = wpf->entity.vsp1;
 	unsigned int num_flip_ctrls;
+	int ret;
 
 	spin_lock_init(&wpf->flip.lock);
 
@@ -158,7 +157,9 @@ static int wpf_init_controls(struct vsp1_rwpf *wpf)
 		num_flip_ctrls = 0;
 	}
 
-	vsp1_rwpf_init_ctrls(wpf, num_flip_ctrls);
+	ret = vsp1_rwpf_init_ctrls(wpf, num_flip_ctrls);
+	if (ret < 0)
+		return ret;
 
 	if (num_flip_ctrls >= 1) {
 		wpf->flip.ctrls.vflip =
@@ -176,11 +177,8 @@ static int wpf_init_controls(struct vsp1_rwpf *wpf)
 		v4l2_ctrl_cluster(3, &wpf->flip.ctrls.vflip);
 	}
 
-	if (wpf->ctrls.error) {
-		dev_err(vsp1->dev, "wpf%u: failed to initialize controls\n",
-			wpf->entity.index);
+	if (wpf->ctrls.error)
 		return wpf->ctrls.error;
-	}
 
 	return 0;
 }
@@ -231,6 +229,7 @@ static int wpf_configure_writeback_chain(struct vsp1_rwpf *wpf,
 }
 
 static void wpf_configure_stream(struct vsp1_entity *entity,
+				 struct v4l2_subdev_state *state,
 				 struct vsp1_pipeline *pipe,
 				 struct vsp1_dl_list *dl,
 				 struct vsp1_dl_body *dlb)
@@ -245,15 +244,14 @@ static void wpf_configure_stream(struct vsp1_entity *entity,
 	u32 srcrpf = 0;
 	int ret;
 
-	sink_format = vsp1_entity_get_pad_format(&wpf->entity,
-						 wpf->entity.state,
-						 RWPF_PAD_SINK);
-	source_format = vsp1_entity_get_pad_format(&wpf->entity,
-						   wpf->entity.state,
-						   RWPF_PAD_SOURCE);
+	sink_format = v4l2_subdev_state_get_format(state, RWPF_PAD_SINK);
+	source_format = v4l2_subdev_state_get_format(state, RWPF_PAD_SOURCE);
 
-	/* Format */
-	if (!pipe->lif || wpf->writeback) {
+	/*
+	 * Format configuration. Skip for IIF (VSPX) or if the pipe doesn't
+	 * write to memory.
+	 */
+	if (!pipe->iif && (!pipe->lif || wpf->writeback)) {
 		const struct v4l2_pix_format_mplane *format = &wpf->format;
 		const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
 
@@ -284,8 +282,33 @@ static void wpf_configure_stream(struct vsp1_entity *entity,
 				       (256 << VI6_WPF_ROT_CTRL_LMEM_WD_SHIFT));
 	}
 
-	if (sink_format->code != source_format->code)
-		outfmt |= VI6_WPF_OUTFMT_CSC;
+	if (sink_format->code != source_format->code) {
+		u16 ycbcr_enc;
+		u16 quantization;
+		u32 wrtm;
+
+		if (sink_format->code == MEDIA_BUS_FMT_AYUV8_1X32) {
+			ycbcr_enc = sink_format->ycbcr_enc;
+			quantization = sink_format->quantization;
+		} else {
+			ycbcr_enc = source_format->ycbcr_enc;
+			quantization = source_format->quantization;
+		}
+
+		if (ycbcr_enc == V4L2_YCBCR_ENC_601 &&
+		    quantization == V4L2_QUANTIZATION_LIM_RANGE)
+			wrtm = VI6_WPF_OUTFMT_WRTM_BT601;
+		else if (ycbcr_enc == V4L2_YCBCR_ENC_601 &&
+			 quantization == V4L2_QUANTIZATION_FULL_RANGE)
+			wrtm = VI6_WPF_OUTFMT_WRTM_BT601_EXT;
+		else if (ycbcr_enc == V4L2_YCBCR_ENC_709 &&
+			 quantization == V4L2_QUANTIZATION_LIM_RANGE)
+			wrtm = VI6_WPF_OUTFMT_WRTM_BT709;
+		else
+			wrtm = VI6_WPF_OUTFMT_WRTM_BT709_EXT;
+
+		outfmt |= VI6_WPF_OUTFMT_CSC | wrtm;
+	}
 
 	wpf->outfmt = outfmt;
 
@@ -296,7 +319,7 @@ static void wpf_configure_stream(struct vsp1_entity *entity,
 	 * Sources. If the pipeline has a single input and BRx is not used,
 	 * configure it as the master layer. Otherwise configure all
 	 * inputs as sub-layers and select the virtual RPF as the master
-	 * layer.
+	 * layer. For VSPX configure the enabled sources as masters.
 	 */
 	for (i = 0; i < vsp1->info->rpf_count; ++i) {
 		struct vsp1_rwpf *input = pipe->inputs[i];
@@ -304,7 +327,7 @@ static void wpf_configure_stream(struct vsp1_entity *entity,
 		if (!input)
 			continue;
 
-		srcrpf |= (!pipe->brx && pipe->num_inputs == 1)
+		srcrpf |= (pipe->iif || (!pipe->brx && pipe->num_inputs == 1))
 			? VI6_WPF_SRCRPF_RPF_ACT_MST(input->entity.index)
 			: VI6_WPF_SRCRPF_RPF_ACT_SUB(input->entity.index);
 	}
@@ -321,6 +344,9 @@ static void wpf_configure_stream(struct vsp1_entity *entity,
 	vsp1_dl_body_write(dlb, VI6_WPF_IRQ_ENB(index),
 			   VI6_WPF_IRQ_ENB_DFEE);
 
+	if (pipe->iif)
+		return;
+
 	/*
 	 * Configure writeback for display pipelines (the wpf writeback flag is
 	 * never set for memory-to-memory pipelines). Start by adding a chained
@@ -367,13 +393,13 @@ static void wpf_configure_frame(struct vsp1_entity *entity,
 
 static void wpf_configure_partition(struct vsp1_entity *entity,
 				    struct vsp1_pipeline *pipe,
+				    const struct vsp1_partition *partition,
 				    struct vsp1_dl_list *dl,
 				    struct vsp1_dl_body *dlb)
 {
 	struct vsp1_rwpf *wpf = to_rwpf(&entity->subdev);
 	struct vsp1_device *vsp1 = wpf->entity.vsp1;
 	struct vsp1_rwpf_memory mem = wpf->mem;
-	const struct v4l2_mbus_framefmt *sink_format;
 	const struct v4l2_pix_format_mplane *format = &wpf->format;
 	const struct vsp1_format_info *fmtinfo = wpf->fmtinfo;
 	unsigned int width;
@@ -383,21 +409,13 @@ static void wpf_configure_partition(struct vsp1_entity *entity,
 	unsigned int flip;
 	unsigned int i;
 
-	sink_format = vsp1_entity_get_pad_format(&wpf->entity,
-						 wpf->entity.state,
-						 RWPF_PAD_SINK);
-	width = sink_format->width;
-	height = sink_format->height;
-	left = 0;
-
 	/*
-	 * Cropping. The partition algorithm can split the image into
-	 * multiple slices.
+	 * Cropping. The partition algorithm can split the image into multiple
+	 * slices.
 	 */
-	if (pipe->partitions > 1) {
-		width = pipe->partition->wpf.width;
-		left = pipe->partition->wpf.left;
-	}
+	width = partition->wpf.width;
+	left = partition->wpf.left;
+	height = partition->wpf.height;
 
 	vsp1_wpf_write(wpf, dlb, VI6_WPF_HSZCLIP, VI6_WPF_SZCLIP_EN |
 		       (0 << VI6_WPF_SZCLIP_OFST_SHIFT) |
@@ -508,6 +526,7 @@ static void wpf_configure_partition(struct vsp1_entity *entity,
 }
 
 static unsigned int wpf_max_width(struct vsp1_entity *entity,
+				  struct v4l2_subdev_state *state,
 				  struct vsp1_pipeline *pipe)
 {
 	struct vsp1_rwpf *wpf = to_rwpf(&entity->subdev);
@@ -516,10 +535,11 @@ static unsigned int wpf_max_width(struct vsp1_entity *entity,
 }
 
 static void wpf_partition(struct vsp1_entity *entity,
+			  struct v4l2_subdev_state *state,
 			  struct vsp1_pipeline *pipe,
 			  struct vsp1_partition *partition,
 			  unsigned int partition_idx,
-			  struct vsp1_partition_window *window)
+			  struct v4l2_rect *window)
 {
 	partition->wpf = *window;
 }
diff --git a/drivers/media/platform/rockchip/rga/rga-buf.c b/drivers/media/platform/rockchip/rga/rga-buf.c
index 662c81b6d0b5..730bdf98565a 100644
--- a/drivers/media/platform/rockchip/rga/rga-buf.c
+++ b/drivers/media/platform/rockchip/rga/rga-buf.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0-only
 /*
- * Copyright (C) 2017 Fuzhou Rockchip Electronics Co.Ltd
+ * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
  * Author: Jacob Chen <jacob-chen@iotwrt.com>
  */
 
@@ -119,6 +119,13 @@ static int rga_buf_prepare(struct vb2_buffer *vb)
 	if (IS_ERR(f))
 		return PTR_ERR(f);
 
+	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+		if (vbuf->field == V4L2_FIELD_ANY)
+			vbuf->field = V4L2_FIELD_NONE;
+		if (vbuf->field != V4L2_FIELD_NONE)
+			return -EINVAL;
+	}
+
 	for (i = 0; i < vb->num_planes; i++) {
 		vb2_set_plane_payload(vb, i, f->pix.plane_fmt[i].sizeimage);
 
@@ -195,6 +202,11 @@ static int rga_buf_start_streaming(struct vb2_queue *q, unsigned int count)
 		return ret;
 	}
 
+	if (V4L2_TYPE_IS_OUTPUT(q->type))
+		ctx->osequence = 0;
+	else
+		ctx->csequence = 0;
+
 	return 0;
 }
 
@@ -213,8 +225,6 @@ const struct vb2_ops rga_qops = {
 	.buf_prepare = rga_buf_prepare,
 	.buf_queue = rga_buf_queue,
 	.buf_cleanup = rga_buf_cleanup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = rga_buf_start_streaming,
 	.stop_streaming = rga_buf_stop_streaming,
 };
diff --git a/drivers/media/platform/rockchip/rga/rga-hw.c b/drivers/media/platform/rockchip/rga/rga-hw.c
index 11c3d7234757..43ed742a1649 100644
--- a/drivers/media/platform/rockchip/rga/rga-hw.c
+++ b/drivers/media/platform/rockchip/rga/rga-hw.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0-only
 /*
- * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Copyright (C) Rockchip Electronics Co., Ltd.
  * Author: Jacob Chen <jacob-chen@iotwrt.com>
  */
 
@@ -376,7 +376,7 @@ static void rga_cmd_set_dst_info(struct rga_ctx *ctx,
 	 * Configure the dest framebuffer base address with pixel offset.
 	 */
 	offsets = rga_get_addr_offset(&ctx->out, offset, dst_x, dst_y, dst_w, dst_h);
-	dst_offset = rga_lookup_draw_pos(&offsets, mir_mode, rot_mode);
+	dst_offset = rga_lookup_draw_pos(&offsets, rot_mode, mir_mode);
 
 	dest[(RGA_DST_Y_RGB_BASE_ADDR - RGA_MODE_BASE_REG) >> 2] =
 		dst_offset->y_off;
diff --git a/drivers/media/platform/rockchip/rga/rga-hw.h b/drivers/media/platform/rockchip/rga/rga-hw.h
index e8917e5630a4..cc6bd7f5b030 100644
--- a/drivers/media/platform/rockchip/rga/rga-hw.h
+++ b/drivers/media/platform/rockchip/rga/rga-hw.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
- * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Copyright (C) Rockchip Electronics Co., Ltd.
  * Author: Jacob Chen <jacob-chen@iotwrt.com>
  */
 #ifndef __RGA_HW_H__
diff --git a/drivers/media/platform/rockchip/rga/rga.c b/drivers/media/platform/rockchip/rga/rga.c
index 00fdfa9e10bc..3dccab5fa4a1 100644
--- a/drivers/media/platform/rockchip/rga/rga.c
+++ b/drivers/media/platform/rockchip/rga/rga.c
@@ -1,6 +1,6 @@
 // SPDX-License-Identifier: GPL-2.0-only
 /*
- * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Copyright (C) Rockchip Electronics Co., Ltd.
  * Author: Jacob Chen <jacob-chen@iotwrt.com>
  */
 
@@ -43,6 +43,8 @@ static void device_run(void *prv)
 	rga->curr = ctx;
 
 	src = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	src->sequence = ctx->osequence++;
+
 	dst = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 
 	rga_hw_start(rga, vb_to_rga(src), vb_to_rga(dst));
@@ -75,6 +77,8 @@ static irqreturn_t rga_isr(int irq, void *prv)
 
 		v4l2_m2m_buf_copy_metadata(src, dst, true);
 
+		dst->sequence = ctx->csequence++;
+
 		v4l2_m2m_buf_done(src, VB2_BUF_STATE_DONE);
 		v4l2_m2m_buf_done(dst, VB2_BUF_STATE_DONE);
 		v4l2_m2m_job_finish(rga->m2m_dev, ctx->fh.m2m_ctx);
@@ -98,7 +102,7 @@ queue_init(void *priv, struct vb2_queue *src_vq, struct vb2_queue *dst_vq)
 	src_vq->drv_priv = ctx;
 	src_vq->ops = &rga_qops;
 	src_vq->mem_ops = &vb2_dma_sg_memops;
-	dst_vq->gfp_flags = __GFP_DMA32;
+	src_vq->gfp_flags = __GFP_DMA32;
 	src_vq->buf_struct_size = sizeof(struct rga_vb_buffer);
 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	src_vq->lock = &ctx->rga->mutex;
@@ -968,7 +972,7 @@ MODULE_DEVICE_TABLE(of, rockchip_rga_match);
 
 static struct platform_driver rga_pdrv = {
 	.probe = rga_probe,
-	.remove_new = rga_remove,
+	.remove = rga_remove,
 	.driver = {
 		.name = RGA_NAME,
 		.pm = &rga_pm,
diff --git a/drivers/media/platform/rockchip/rga/rga.h b/drivers/media/platform/rockchip/rga/rga.h
index 3502dff6055c..530e12de73c4 100644
--- a/drivers/media/platform/rockchip/rga/rga.h
+++ b/drivers/media/platform/rockchip/rga/rga.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
- * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Copyright (C) Rockchip Electronics Co., Ltd.
  * Author: Jacob Chen <jacob-chen@iotwrt.com>
  */
 #ifndef __RGA_H__
@@ -57,6 +57,9 @@ struct rga_ctx {
 	struct rga_frame out;
 	struct v4l2_ctrl_handler ctrl_handler;
 
+	int osequence;
+	int csequence;
+
 	/* Control values */
 	u32 op;
 	u32 hflip;
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-capture.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-capture.c
index 2bddb4fa8a5c..6dcefd144d5a 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-capture.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-capture.c
@@ -35,8 +35,6 @@
 #define RKISP1_SP_DEV_NAME	RKISP1_DRIVER_NAME "_selfpath"
 #define RKISP1_MP_DEV_NAME	RKISP1_DRIVER_NAME "_mainpath"
 
-#define RKISP1_MIN_BUFFERS_NEEDED 3
-
 enum rkisp1_plane {
 	RKISP1_PLANE_Y	= 0,
 	RKISP1_PLANE_CB	= 1,
@@ -1203,8 +1201,6 @@ static const struct vb2_ops rkisp1_vb2_ops = {
 	.buf_init = rkisp1_vb2_buf_init,
 	.buf_queue = rkisp1_vb2_buf_queue,
 	.buf_prepare = rkisp1_vb2_buf_prepare,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.stop_streaming = rkisp1_vb2_stop_streaming,
 	.start_streaming = rkisp1_vb2_start_streaming,
 };
@@ -1563,7 +1559,7 @@ static int rkisp1_register_capture(struct rkisp1_capture *cap)
 	q->ops = &rkisp1_vb2_ops;
 	q->mem_ops = &vb2_dma_contig_memops;
 	q->buf_struct_size = sizeof(struct rkisp1_buffer);
-	q->min_queued_buffers = RKISP1_MIN_BUFFERS_NEEDED;
+	q->min_queued_buffers = 1;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->lock = &node->vlock;
 	q->dev = cap->rkisp1->dev;
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-common.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-common.c
index f956b90a407a..60c97bb7b18b 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-common.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-common.c
@@ -178,3 +178,17 @@ void rkisp1_sd_adjust_crop(struct v4l2_rect *crop,
 
 	rkisp1_sd_adjust_crop_rect(crop, &crop_bounds);
 }
+
+void rkisp1_bls_swap_regs(enum rkisp1_fmt_raw_pat_type pattern,
+			  const u32 input[4], u32 output[4])
+{
+	static const unsigned int swap[4][4] = {
+		[RKISP1_RAW_RGGB] = { 0, 1, 2, 3 },
+		[RKISP1_RAW_GRBG] = { 1, 0, 3, 2 },
+		[RKISP1_RAW_GBRG] = { 2, 3, 0, 1 },
+		[RKISP1_RAW_BGGR] = { 3, 2, 1, 0 },
+	};
+
+	for (unsigned int i = 0; i < 4; ++i)
+		output[i] = input[swap[pattern][i]];
+}
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-common.h b/drivers/media/platform/rockchip/rkisp1/rkisp1-common.h
index 26573f6ae575..ca952fd0829b 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-common.h
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-common.h
@@ -33,9 +33,10 @@ struct regmap;
 #define RKISP1_ISP_SD_SRC			BIT(0)
 #define RKISP1_ISP_SD_SINK			BIT(1)
 
-/* min and max values for the widths and heights of the entities */
-#define RKISP1_ISP_MAX_WIDTH			4032
-#define RKISP1_ISP_MAX_HEIGHT			3024
+/*
+ * Minimum values for the width and height of entities. The maximum values are
+ * model-specific and stored in the rkisp1_info structure.
+ */
 #define RKISP1_ISP_MIN_WIDTH			32
 #define RKISP1_ISP_MIN_HEIGHT			32
 
@@ -115,6 +116,8 @@ enum rkisp1_isp_pad {
  * @RKISP1_FEATURE_SELF_PATH: The ISP has a self path
  * @RKISP1_FEATURE_DUAL_CROP: The ISP has the dual crop block at the resizer input
  * @RKISP1_FEATURE_DMA_34BIT: The ISP uses 34-bit DMA addresses
+ * @RKISP1_FEATURE_BLS: The ISP has a dedicated BLS block
+ * @RKISP1_FEATURE_COMPAND: The ISP has a companding block
  *
  * The ISP features are stored in a bitmask in &rkisp1_info.features and allow
  * the driver to implement support for features present in some ISP versions
@@ -126,6 +129,8 @@ enum rkisp1_feature {
 	RKISP1_FEATURE_SELF_PATH = BIT(2),
 	RKISP1_FEATURE_DUAL_CROP = BIT(3),
 	RKISP1_FEATURE_DMA_34BIT = BIT(4),
+	RKISP1_FEATURE_BLS = BIT(5),
+	RKISP1_FEATURE_COMPAND = BIT(6),
 };
 
 #define rkisp1_has_feature(rkisp1, feature) \
@@ -140,6 +145,8 @@ enum rkisp1_feature {
  * @isr_size: number of entries in the @isrs array
  * @isp_ver: ISP version
  * @features: bitmask of rkisp1_feature features implemented by the ISP
+ * @max_width: maximum input frame width
+ * @max_height: maximum input frame height
  *
  * This structure contains information about the ISP specific to a particular
  * ISP model, version, or integration in a particular SoC.
@@ -151,6 +158,8 @@ struct rkisp1_info {
 	unsigned int isr_size;
 	enum rkisp1_cif_isp_version isp_ver;
 	unsigned int features;
+	unsigned int max_width;
+	unsigned int max_height;
 };
 
 /*
@@ -232,7 +241,7 @@ struct rkisp1_vdev_node {
 
 /*
  * struct rkisp1_buffer - A container for the vb2 buffers used by the video devices:
- *			  params, stats, mainpath, selfpath
+ *			  stats, mainpath, selfpath
  *
  * @vb:		vb2 buffer
  * @queue:	entry of the buffer in the queue
@@ -245,6 +254,26 @@ struct rkisp1_buffer {
 };
 
 /*
+ * struct rkisp1_params_buffer - A container for the vb2 buffers used by the
+ *				 params video device
+ *
+ * @vb:		vb2 buffer
+ * @queue:	entry of the buffer in the queue
+ * @cfg:	scratch buffer used for caching the ISP configuration parameters
+ */
+struct rkisp1_params_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head queue;
+	void *cfg;
+};
+
+static inline struct rkisp1_params_buffer *
+to_rkisp1_params_buffer(struct vb2_v4l2_buffer *vbuf)
+{
+	return container_of(vbuf, struct rkisp1_params_buffer, vb);
+}
+
+/*
  * struct rkisp1_dummy_buffer - A buffer to write the next frame to in case
  *				there are no vb2 buffers available.
  *
@@ -372,9 +401,11 @@ struct rkisp1_params_ops {
  * @ops:		pointer to the variant-specific operations
  * @config_lock:	locks the buffer list 'params'
  * @params:		queue of rkisp1_buffer
- * @vdev_fmt:		v4l2_format of the metadata format
+ * @metafmt		the currently enabled metadata format
  * @quantization:	the quantization configured on the isp's src pad
+ * @ycbcr_encoding	the YCbCr encoding
  * @raw_type:		the bayer pattern on the isp video sink pad
+ * @enabled_blocks:	bitmask of enabled ISP blocks
  */
 struct rkisp1_params {
 	struct rkisp1_vdev_node vnode;
@@ -383,11 +414,14 @@ struct rkisp1_params {
 
 	spinlock_t config_lock; /* locks the buffers list 'params' */
 	struct list_head params;
-	struct v4l2_format vdev_fmt;
+
+	const struct v4l2_meta_format *metafmt;
 
 	enum v4l2_quantization quantization;
 	enum v4l2_ycbcr_encoding ycbcr_encoding;
 	enum rkisp1_fmt_raw_pat_type raw_type;
+
+	u32 enabled_blocks;
 };
 
 /*
@@ -573,6 +607,9 @@ void rkisp1_sd_adjust_crop_rect(struct v4l2_rect *crop,
 void rkisp1_sd_adjust_crop(struct v4l2_rect *crop,
 			   const struct v4l2_mbus_framefmt *bounds);
 
+void rkisp1_bls_swap_regs(enum rkisp1_fmt_raw_pat_type pattern,
+			  const u32 input[4], u32 output[4]);
+
 /*
  * rkisp1_mbus_info_get_by_code - get the isp info of the media bus code
  *
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-csi.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-csi.c
index 4202642e0523..841e58c20f7f 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-csi.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-csi.c
@@ -307,6 +307,7 @@ static int rkisp1_csi_set_fmt(struct v4l2_subdev *sd,
 			      struct v4l2_subdev_state *sd_state,
 			      struct v4l2_subdev_format *fmt)
 {
+	struct rkisp1_csi *csi = to_rkisp1_csi(sd);
 	const struct rkisp1_mbus_info *mbus_info;
 	struct v4l2_mbus_framefmt *sink_fmt, *src_fmt;
 
@@ -326,10 +327,10 @@ static int rkisp1_csi_set_fmt(struct v4l2_subdev *sd,
 
 	sink_fmt->width = clamp_t(u32, fmt->format.width,
 				  RKISP1_ISP_MIN_WIDTH,
-				  RKISP1_ISP_MAX_WIDTH);
+				  csi->rkisp1->info->max_width);
 	sink_fmt->height = clamp_t(u32, fmt->format.height,
 				   RKISP1_ISP_MIN_HEIGHT,
-				   RKISP1_ISP_MAX_HEIGHT);
+				   csi->rkisp1->info->max_height);
 
 	fmt->format = *sink_fmt;
 
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-dev.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-dev.c
index bb0202386c70..dc65a7924f8a 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-dev.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-dev.c
@@ -228,6 +228,9 @@ static int rkisp1_subdev_notifier_register(struct rkisp1_device *rkisp1)
 			break;
 		}
 
+		if (ret)
+			break;
+
 		/* Parse the endpoint and validate the bus type. */
 		ret = v4l2_fwnode_endpoint_parse(ep, &vep);
 		if (ret) {
@@ -509,7 +512,10 @@ static const struct rkisp1_info px30_isp_info = {
 	.isp_ver = RKISP1_V12,
 	.features = RKISP1_FEATURE_MIPI_CSI2
 		  | RKISP1_FEATURE_SELF_PATH
-		  | RKISP1_FEATURE_DUAL_CROP,
+		  | RKISP1_FEATURE_DUAL_CROP
+		  | RKISP1_FEATURE_BLS,
+	.max_width = 3264,
+	.max_height = 2448,
 };
 
 static const char * const rk3399_isp_clks[] = {
@@ -530,7 +536,10 @@ static const struct rkisp1_info rk3399_isp_info = {
 	.isp_ver = RKISP1_V10,
 	.features = RKISP1_FEATURE_MIPI_CSI2
 		  | RKISP1_FEATURE_SELF_PATH
-		  | RKISP1_FEATURE_DUAL_CROP,
+		  | RKISP1_FEATURE_DUAL_CROP
+		  | RKISP1_FEATURE_BLS,
+	.max_width = 4416,
+	.max_height = 3312,
 };
 
 static const char * const imx8mp_isp_clks[] = {
@@ -550,7 +559,10 @@ static const struct rkisp1_info imx8mp_isp_info = {
 	.isr_size = ARRAY_SIZE(imx8mp_isp_isrs),
 	.isp_ver = RKISP1_V_IMX8MP,
 	.features = RKISP1_FEATURE_MAIN_STRIDE
-		  | RKISP1_FEATURE_DMA_34BIT,
+		  | RKISP1_FEATURE_DMA_34BIT
+		  | RKISP1_FEATURE_COMPAND,
+	.max_width = 4096,
+	.max_height = 3072,
 };
 
 static const struct of_device_id rkisp1_of_match[] = {
@@ -743,7 +755,7 @@ static struct platform_driver rkisp1_drv = {
 		.pm = &rkisp1_pm_ops,
 	},
 	.probe = rkisp1_probe,
-	.remove_new = rkisp1_remove,
+	.remove = rkisp1_remove,
 };
 
 module_platform_driver(rkisp1_drv);
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-isp.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-isp.c
index e45a213baf49..8c29a1c9309a 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-isp.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-isp.c
@@ -173,7 +173,7 @@ static void rkisp1_gasket_disable(struct rkisp1_device *rkisp1)
  * or at the frame end interrupt
  */
 static void rkisp1_config_ism(struct rkisp1_isp *isp,
-			      struct v4l2_subdev_state *sd_state)
+			      const struct v4l2_subdev_state *sd_state)
 {
 	const struct v4l2_rect *src_crop =
 		v4l2_subdev_state_get_crop(sd_state,
@@ -201,7 +201,7 @@ static void rkisp1_config_ism(struct rkisp1_isp *isp,
  * configure ISP blocks with input format, size......
  */
 static int rkisp1_config_isp(struct rkisp1_isp *isp,
-			     struct v4l2_subdev_state *sd_state,
+			     const struct v4l2_subdev_state *sd_state,
 			     enum v4l2_mbus_type mbus_type, u32 mbus_flags)
 {
 	struct rkisp1_device *rkisp1 = isp->rkisp1;
@@ -309,7 +309,7 @@ static int rkisp1_config_isp(struct rkisp1_isp *isp,
 	if (src_fmt->pixel_enc == V4L2_PIXEL_ENC_BAYER) {
 		rkisp1_params_disable(&rkisp1->params);
 	} else {
-		struct v4l2_mbus_framefmt *src_frm;
+		const struct v4l2_mbus_framefmt *src_frm;
 
 		src_frm = v4l2_subdev_state_get_format(sd_state,
 						       RKISP1_ISP_PAD_SOURCE_VIDEO);
@@ -429,7 +429,7 @@ static void rkisp1_config_clk(struct rkisp1_isp *isp)
 }
 
 static int rkisp1_isp_start(struct rkisp1_isp *isp,
-			    struct v4l2_subdev_state *sd_state,
+			    const struct v4l2_subdev_state *sd_state,
 			    struct media_pad *source)
 {
 	struct rkisp1_device *rkisp1 = isp->rkisp1;
@@ -517,6 +517,7 @@ static int rkisp1_isp_enum_frame_size(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_state *sd_state,
 				      struct v4l2_subdev_frame_size_enum *fse)
 {
+	struct rkisp1_isp *isp = to_rkisp1_isp(sd);
 	const struct rkisp1_mbus_info *mbus_info;
 
 	if (fse->pad == RKISP1_ISP_PAD_SINK_PARAMS ||
@@ -539,9 +540,9 @@ static int rkisp1_isp_enum_frame_size(struct v4l2_subdev *sd,
 		return -EINVAL;
 
 	fse->min_width = RKISP1_ISP_MIN_WIDTH;
-	fse->max_width = RKISP1_ISP_MAX_WIDTH;
+	fse->max_width = isp->rkisp1->info->max_width;
 	fse->min_height = RKISP1_ISP_MIN_HEIGHT;
-	fse->max_height = RKISP1_ISP_MAX_HEIGHT;
+	fse->max_height = isp->rkisp1->info->max_height;
 
 	return 0;
 }
@@ -772,10 +773,10 @@ static void rkisp1_isp_set_sink_fmt(struct rkisp1_isp *isp,
 
 	sink_fmt->width = clamp_t(u32, format->width,
 				  RKISP1_ISP_MIN_WIDTH,
-				  RKISP1_ISP_MAX_WIDTH);
+				  isp->rkisp1->info->max_width);
 	sink_fmt->height = clamp_t(u32, format->height,
 				   RKISP1_ISP_MIN_HEIGHT,
-				   RKISP1_ISP_MAX_HEIGHT);
+				   isp->rkisp1->info->max_height);
 
 	/*
 	 * Adjust the color space fields. Accept any color primaries and
@@ -880,7 +881,7 @@ static int rkisp1_isp_set_selection(struct v4l2_subdev *sd,
 	if (sel->target != V4L2_SEL_TGT_CROP)
 		return -EINVAL;
 
-	dev_dbg(isp->rkisp1->dev, "%s: pad: %d sel(%d,%d)/%dx%d\n", __func__,
+	dev_dbg(isp->rkisp1->dev, "%s: pad: %d sel(%d,%d)/%ux%u\n", __func__,
 		sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
 
 	if (sel->pad == RKISP1_ISP_PAD_SINK_VIDEO)
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-params.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-params.c
index 173d1ea41874..b28f4140c8a3 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-params.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-params.c
@@ -5,6 +5,9 @@
  * Copyright (C) 2017 Rockchip Electronics Co., Ltd.
  */
 
+#include <linux/math.h>
+#include <linux/string.h>
+
 #include <media/v4l2-common.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
@@ -33,6 +36,59 @@
 #define RKISP1_ISP_CC_COEFF(n) \
 			(RKISP1_CIF_ISP_CC_COEFF_0 + (n) * 4)
 
+#define RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS	BIT(0)
+#define RKISP1_EXT_PARAMS_BLOCK_GROUP_LSC	BIT(1)
+
+union rkisp1_ext_params_config {
+	struct rkisp1_ext_params_block_header header;
+	struct rkisp1_ext_params_bls_config bls;
+	struct rkisp1_ext_params_dpcc_config dpcc;
+	struct rkisp1_ext_params_sdg_config sdg;
+	struct rkisp1_ext_params_lsc_config lsc;
+	struct rkisp1_ext_params_awb_gain_config awbg;
+	struct rkisp1_ext_params_flt_config flt;
+	struct rkisp1_ext_params_bdm_config bdm;
+	struct rkisp1_ext_params_ctk_config ctk;
+	struct rkisp1_ext_params_goc_config goc;
+	struct rkisp1_ext_params_dpf_config dpf;
+	struct rkisp1_ext_params_dpf_strength_config dpfs;
+	struct rkisp1_ext_params_cproc_config cproc;
+	struct rkisp1_ext_params_ie_config ie;
+	struct rkisp1_ext_params_awb_meas_config awbm;
+	struct rkisp1_ext_params_hst_config hst;
+	struct rkisp1_ext_params_aec_config aec;
+	struct rkisp1_ext_params_afc_config afc;
+	struct rkisp1_ext_params_compand_bls_config compand_bls;
+	struct rkisp1_ext_params_compand_curve_config compand_curve;
+};
+
+enum rkisp1_params_formats {
+	RKISP1_PARAMS_FIXED,
+	RKISP1_PARAMS_EXTENSIBLE,
+};
+
+static const struct v4l2_meta_format rkisp1_params_formats[] = {
+	[RKISP1_PARAMS_FIXED] = {
+		.dataformat = V4L2_META_FMT_RK_ISP1_PARAMS,
+		.buffersize = sizeof(struct rkisp1_params_cfg),
+	},
+	[RKISP1_PARAMS_EXTENSIBLE] = {
+		.dataformat = V4L2_META_FMT_RK_ISP1_EXT_PARAMS,
+		.buffersize = sizeof(struct rkisp1_ext_params_cfg),
+	},
+};
+
+static const struct v4l2_meta_format *
+rkisp1_params_get_format_info(u32 dataformat)
+{
+	for (unsigned int i = 0; i < ARRAY_SIZE(rkisp1_params_formats); i++) {
+		if (rkisp1_params_formats[i].dataformat == dataformat)
+			return &rkisp1_params_formats[i];
+	}
+
+	return &rkisp1_params_formats[RKISP1_PARAMS_FIXED];
+}
+
 static inline void
 rkisp1_param_set_bits(struct rkisp1_params *params, u32 reg, u32 bit_mask)
 {
@@ -112,54 +168,20 @@ static void rkisp1_bls_config(struct rkisp1_params *params,
 	new_control &= RKISP1_CIF_ISP_BLS_ENA;
 	/* fixed subtraction values */
 	if (!arg->enable_auto) {
-		const struct rkisp1_cif_isp_bls_fixed_val *pval =
-								&arg->fixed_val;
-
-		switch (params->raw_type) {
-		case RKISP1_RAW_BGGR:
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_D_FIXED,
-				     pval->r);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_C_FIXED,
-				     pval->gr);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_B_FIXED,
-				     pval->gb);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_A_FIXED,
-				     pval->b);
-			break;
-		case RKISP1_RAW_GBRG:
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_C_FIXED,
-				     pval->r);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_D_FIXED,
-				     pval->gr);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_A_FIXED,
-				     pval->gb);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_B_FIXED,
-				     pval->b);
-			break;
-		case RKISP1_RAW_GRBG:
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_B_FIXED,
-				     pval->r);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_A_FIXED,
-				     pval->gr);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_D_FIXED,
-				     pval->gb);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_C_FIXED,
-				     pval->b);
-			break;
-		case RKISP1_RAW_RGGB:
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_A_FIXED,
-				     pval->r);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_B_FIXED,
-				     pval->gr);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_C_FIXED,
-				     pval->gb);
-			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_D_FIXED,
-				     pval->b);
-			break;
-		default:
-			break;
-		}
-
+		static const u32 regs[] = {
+			RKISP1_CIF_ISP_BLS_A_FIXED,
+			RKISP1_CIF_ISP_BLS_B_FIXED,
+			RKISP1_CIF_ISP_BLS_C_FIXED,
+			RKISP1_CIF_ISP_BLS_D_FIXED,
+		};
+		u32 swapped[4];
+
+		rkisp1_bls_swap_regs(params->raw_type, regs, swapped);
+
+		rkisp1_write(params->rkisp1, swapped[0], arg->fixed_val.r);
+		rkisp1_write(params->rkisp1, swapped[1], arg->fixed_val.gr);
+		rkisp1_write(params->rkisp1, swapped[2], arg->fixed_val.gb);
+		rkisp1_write(params->rkisp1, swapped[3], arg->fixed_val.b);
 	} else {
 		if (arg->en_windows & BIT(1)) {
 			rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_BLS_H2_START,
@@ -1239,6 +1261,93 @@ rkisp1_dpf_strength_config(struct rkisp1_params *params,
 	rkisp1_write(params->rkisp1, RKISP1_CIF_ISP_DPF_STRENGTH_R, arg->r);
 }
 
+static void rkisp1_compand_write_px_curve(struct rkisp1_params *params,
+					  unsigned int addr, const u8 *curve)
+{
+	const unsigned int points_per_reg = 6;
+	const unsigned int num_regs =
+		DIV_ROUND_UP(RKISP1_CIF_ISP_COMPAND_NUM_POINTS,
+			     points_per_reg);
+
+	/*
+	 * The compand curve is specified as a piecewise linear function with
+	 * 64 points. X coordinates are stored as a log2 of the displacement
+	 * from the previous point, in 5 bits, with 6 values per register. The
+	 * last register stores 4 values.
+	 */
+	for (unsigned int reg = 0; reg < num_regs; ++reg) {
+		unsigned int num_points =
+			min(RKISP1_CIF_ISP_COMPAND_NUM_POINTS -
+			    reg * points_per_reg, points_per_reg);
+		u32 val = 0;
+
+		for (unsigned int i = 0; i < num_points; i++)
+			val |= (*curve++ & 0x1f) << (i * 5);
+
+		rkisp1_write(params->rkisp1, addr, val);
+		addr += 4;
+	}
+}
+
+static void
+rkisp1_compand_write_curve_mem(struct rkisp1_params *params,
+			       unsigned int reg_addr, unsigned int reg_data,
+			       const u32 curve[RKISP1_CIF_ISP_COMPAND_NUM_POINTS])
+{
+	for (unsigned int i = 0; i < RKISP1_CIF_ISP_COMPAND_NUM_POINTS; i++) {
+		rkisp1_write(params->rkisp1, reg_addr, i);
+		rkisp1_write(params->rkisp1, reg_data, curve[i]);
+	}
+}
+
+static void
+rkisp1_compand_bls_config(struct rkisp1_params *params,
+			  const struct rkisp1_cif_isp_compand_bls_config *arg)
+{
+	static const u32 regs[] = {
+		RKISP1_CIF_ISP_COMPAND_BLS_A_FIXED,
+		RKISP1_CIF_ISP_COMPAND_BLS_B_FIXED,
+		RKISP1_CIF_ISP_COMPAND_BLS_C_FIXED,
+		RKISP1_CIF_ISP_COMPAND_BLS_D_FIXED,
+	};
+	u32 swapped[4];
+
+	rkisp1_bls_swap_regs(params->raw_type, regs, swapped);
+
+	rkisp1_write(params->rkisp1, swapped[0], arg->r);
+	rkisp1_write(params->rkisp1, swapped[1], arg->gr);
+	rkisp1_write(params->rkisp1, swapped[2], arg->gb);
+	rkisp1_write(params->rkisp1, swapped[3], arg->b);
+}
+
+static void
+rkisp1_compand_expand_config(struct rkisp1_params *params,
+			     const struct rkisp1_cif_isp_compand_curve_config *arg)
+{
+	rkisp1_compand_write_px_curve(params, RKISP1_CIF_ISP_COMPAND_EXPAND_PX_N(0),
+				      arg->px);
+	rkisp1_compand_write_curve_mem(params, RKISP1_CIF_ISP_COMPAND_EXPAND_Y_ADDR,
+				       RKISP1_CIF_ISP_COMPAND_EXPAND_Y_WRITE_DATA,
+				       arg->y);
+	rkisp1_compand_write_curve_mem(params, RKISP1_CIF_ISP_COMPAND_EXPAND_X_ADDR,
+				       RKISP1_CIF_ISP_COMPAND_EXPAND_X_WRITE_DATA,
+				       arg->x);
+}
+
+static void
+rkisp1_compand_compress_config(struct rkisp1_params *params,
+			       const struct rkisp1_cif_isp_compand_curve_config *arg)
+{
+	rkisp1_compand_write_px_curve(params, RKISP1_CIF_ISP_COMPAND_COMPRESS_PX_N(0),
+				      arg->px);
+	rkisp1_compand_write_curve_mem(params, RKISP1_CIF_ISP_COMPAND_COMPRESS_Y_ADDR,
+				       RKISP1_CIF_ISP_COMPAND_COMPRESS_Y_WRITE_DATA,
+				       arg->y);
+	rkisp1_compand_write_curve_mem(params, RKISP1_CIF_ISP_COMPAND_COMPRESS_X_ADDR,
+				       RKISP1_CIF_ISP_COMPAND_COMPRESS_X_WRITE_DATA,
+				       arg->x);
+}
+
 static void
 rkisp1_isp_isr_other_config(struct rkisp1_params *params,
 			    const struct rkisp1_params_cfg *new_params)
@@ -1249,6 +1358,12 @@ rkisp1_isp_isr_other_config(struct rkisp1_params *params,
 	module_cfg_update = new_params->module_cfg_update;
 	module_ens = new_params->module_ens;
 
+	if (!rkisp1_has_feature(params->rkisp1, BLS)) {
+		module_en_update &= ~RKISP1_CIF_ISP_MODULE_BLS;
+		module_cfg_update &= ~RKISP1_CIF_ISP_MODULE_BLS;
+		module_ens &= ~RKISP1_CIF_ISP_MODULE_BLS;
+	}
+
 	/* update dpc config */
 	if (module_cfg_update & RKISP1_CIF_ISP_MODULE_DPCC)
 		rkisp1_dpcc_config(params,
@@ -1501,21 +1616,551 @@ static void rkisp1_isp_isr_meas_config(struct rkisp1_params *params,
 	}
 }
 
-static bool rkisp1_params_get_buffer(struct rkisp1_params *params,
-				     struct rkisp1_buffer **buf,
-				     struct rkisp1_params_cfg **cfg)
+/*------------------------------------------------------------------------------
+ * Extensible parameters format handling
+ */
+
+static void
+rkisp1_ext_params_bls(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_bls_config *bls = &block->bls;
+
+	if (bls->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_BLS_CTRL,
+					RKISP1_CIF_ISP_BLS_ENA);
+		return;
+	}
+
+	rkisp1_bls_config(params, &bls->config);
+
+	if ((bls->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(bls->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_BLS_CTRL,
+				      RKISP1_CIF_ISP_BLS_ENA);
+}
+
+static void
+rkisp1_ext_params_dpcc(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
 {
-	if (list_empty(&params->params))
-		return false;
+	const struct rkisp1_ext_params_dpcc_config *dpcc = &block->dpcc;
+
+	if (dpcc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_DPCC_MODE,
+					RKISP1_CIF_ISP_DPCC_MODE_DPCC_ENABLE);
+		return;
+	}
 
-	*buf = list_first_entry(&params->params, struct rkisp1_buffer, queue);
-	*cfg = vb2_plane_vaddr(&(*buf)->vb.vb2_buf, 0);
+	rkisp1_dpcc_config(params, &dpcc->config);
 
-	return true;
+	if ((dpcc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(dpcc->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_DPCC_MODE,
+				      RKISP1_CIF_ISP_DPCC_MODE_DPCC_ENABLE);
+}
+
+static void
+rkisp1_ext_params_sdg(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_sdg_config *sdg = &block->sdg;
+
+	if (sdg->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_CTRL,
+					RKISP1_CIF_ISP_CTRL_ISP_GAMMA_IN_ENA);
+		return;
+	}
+
+	rkisp1_sdg_config(params, &sdg->config);
+
+	if ((sdg->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(sdg->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
+				      RKISP1_CIF_ISP_CTRL_ISP_GAMMA_IN_ENA);
+}
+
+static void
+rkisp1_ext_params_lsc(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_lsc_config *lsc = &block->lsc;
+
+	if (lsc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_LSC_CTRL,
+					RKISP1_CIF_ISP_LSC_CTRL_ENA);
+		return;
+	}
+
+	rkisp1_lsc_config(params, &lsc->config);
+
+	if ((lsc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(lsc->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_LSC_CTRL,
+				      RKISP1_CIF_ISP_LSC_CTRL_ENA);
+}
+
+static void
+rkisp1_ext_params_awbg(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_awb_gain_config *awbg = &block->awbg;
+
+	if (awbg->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_CTRL,
+					RKISP1_CIF_ISP_CTRL_ISP_AWB_ENA);
+		return;
+	}
+
+	params->ops->awb_gain_config(params, &awbg->config);
+
+	if ((awbg->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(awbg->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
+				      RKISP1_CIF_ISP_CTRL_ISP_AWB_ENA);
+}
+
+static void
+rkisp1_ext_params_flt(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_flt_config *flt = &block->flt;
+
+	if (flt->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_FILT_MODE,
+					RKISP1_CIF_ISP_FLT_ENA);
+		return;
+	}
+
+	rkisp1_flt_config(params, &flt->config);
+
+	if ((flt->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(flt->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_FILT_MODE,
+				      RKISP1_CIF_ISP_FLT_ENA);
+}
+
+static void
+rkisp1_ext_params_bdm(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_bdm_config *bdm = &block->bdm;
+
+	if (bdm->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_DEMOSAIC,
+					RKISP1_CIF_ISP_DEMOSAIC_BYPASS);
+		return;
+	}
+
+	rkisp1_bdm_config(params, &bdm->config);
+
+	if ((bdm->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(bdm->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_DEMOSAIC,
+				      RKISP1_CIF_ISP_DEMOSAIC_BYPASS);
+}
+
+static void
+rkisp1_ext_params_ctk(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_ctk_config *ctk = &block->ctk;
+
+	if (ctk->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_ctk_enable(params, false);
+		return;
+	}
+
+	rkisp1_ctk_config(params, &ctk->config);
+
+	if ((ctk->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(ctk->header.type)))
+		rkisp1_ctk_enable(params, true);
+}
+
+static void
+rkisp1_ext_params_goc(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_goc_config *goc = &block->goc;
+
+	if (goc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_CTRL,
+					RKISP1_CIF_ISP_CTRL_ISP_GAMMA_OUT_ENA);
+		return;
+	}
+
+	params->ops->goc_config(params, &goc->config);
+
+	/*
+	 * Unconditionally re-enable the GOC module which gets disabled by
+	 * goc_config().
+	 */
+	rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
+			      RKISP1_CIF_ISP_CTRL_ISP_GAMMA_OUT_ENA);
+}
+
+static void
+rkisp1_ext_params_dpf(struct rkisp1_params *params,
+		      const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_dpf_config *dpf = &block->dpf;
+
+	if (dpf->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_DPF_MODE,
+					RKISP1_CIF_ISP_DPF_MODE_EN);
+		return;
+	}
+
+	rkisp1_dpf_config(params, &dpf->config);
+
+	if ((dpf->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(dpf->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_DPF_MODE,
+				      RKISP1_CIF_ISP_DPF_MODE_EN);
+}
+
+static void
+rkisp1_ext_params_dpfs(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_dpf_strength_config *dpfs = &block->dpfs;
+
+	rkisp1_dpf_strength_config(params, &dpfs->config);
+}
+
+static void
+rkisp1_ext_params_cproc(struct rkisp1_params *params,
+			const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_cproc_config *cproc = &block->cproc;
+
+	if (cproc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_C_PROC_CTRL,
+					RKISP1_CIF_C_PROC_CTR_ENABLE);
+		return;
+	}
+
+	rkisp1_cproc_config(params, &cproc->config);
+
+	if ((cproc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(cproc->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_C_PROC_CTRL,
+				      RKISP1_CIF_C_PROC_CTR_ENABLE);
+}
+
+static void
+rkisp1_ext_params_ie(struct rkisp1_params *params,
+		     const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_ie_config *ie = &block->ie;
+
+	if (ie->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_ie_enable(params, false);
+		return;
+	}
+
+	rkisp1_ie_config(params, &ie->config);
+
+	if ((ie->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(ie->header.type)))
+		rkisp1_ie_enable(params, true);
+}
+
+static void
+rkisp1_ext_params_awbm(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_awb_meas_config *awbm = &block->awbm;
+
+	if (awbm->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		params->ops->awb_meas_enable(params, &awbm->config,
+					     false);
+		return;
+	}
+
+	params->ops->awb_meas_config(params, &awbm->config);
+
+	if ((awbm->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(awbm->header.type)))
+		params->ops->awb_meas_enable(params, &awbm->config,
+					     true);
+}
+
+static void
+rkisp1_ext_params_hstm(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_hst_config *hst = &block->hst;
+
+	if (hst->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		params->ops->hst_enable(params, &hst->config, false);
+		return;
+	}
+
+	params->ops->hst_config(params, &hst->config);
+
+	if ((hst->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(hst->header.type)))
+		params->ops->hst_enable(params, &hst->config, true);
+}
+
+static void
+rkisp1_ext_params_aecm(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_aec_config *aec = &block->aec;
+
+	if (aec->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_EXP_CTRL,
+					RKISP1_CIF_ISP_EXP_ENA);
+		return;
+	}
+
+	params->ops->aec_config(params, &aec->config);
+
+	if ((aec->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(aec->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_EXP_CTRL,
+				      RKISP1_CIF_ISP_EXP_ENA);
+}
+
+static void
+rkisp1_ext_params_afcm(struct rkisp1_params *params,
+		       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_afc_config *afc = &block->afc;
+
+	if (afc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_AFM_CTRL,
+					RKISP1_CIF_ISP_AFM_ENA);
+		return;
+	}
+
+	params->ops->afm_config(params, &afc->config);
+
+	if ((afc->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(afc->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_AFM_CTRL,
+				      RKISP1_CIF_ISP_AFM_ENA);
+}
+
+static void rkisp1_ext_params_compand_bls(struct rkisp1_params *params,
+					  const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_compand_bls_config *bls =
+		&block->compand_bls;
+
+	if (bls->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+					RKISP1_CIF_ISP_COMPAND_CTRL_BLS_ENABLE);
+		return;
+	}
+
+	rkisp1_compand_bls_config(params, &bls->config);
+
+	if ((bls->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(bls->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+				      RKISP1_CIF_ISP_COMPAND_CTRL_BLS_ENABLE);
+}
+
+static void rkisp1_ext_params_compand_expand(struct rkisp1_params *params,
+					     const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_compand_curve_config *curve =
+		&block->compand_curve;
+
+	if (curve->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+					RKISP1_CIF_ISP_COMPAND_CTRL_EXPAND_ENABLE);
+		return;
+	}
+
+	rkisp1_compand_expand_config(params, &curve->config);
+
+	if ((curve->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(curve->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+				      RKISP1_CIF_ISP_COMPAND_CTRL_EXPAND_ENABLE);
+}
+
+static void rkisp1_ext_params_compand_compress(struct rkisp1_params *params,
+					       const union rkisp1_ext_params_config *block)
+{
+	const struct rkisp1_ext_params_compand_curve_config *curve =
+		&block->compand_curve;
+
+	if (curve->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE) {
+		rkisp1_param_clear_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+					RKISP1_CIF_ISP_COMPAND_CTRL_COMPRESS_ENABLE);
+		return;
+	}
+
+	rkisp1_compand_compress_config(params, &curve->config);
+
+	if ((curve->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE) &&
+	    !(params->enabled_blocks & BIT(curve->header.type)))
+		rkisp1_param_set_bits(params, RKISP1_CIF_ISP_COMPAND_CTRL,
+				      RKISP1_CIF_ISP_COMPAND_CTRL_COMPRESS_ENABLE);
+}
+
+typedef void (*rkisp1_block_handler)(struct rkisp1_params *params,
+			     const union rkisp1_ext_params_config *config);
+
+static const struct rkisp1_ext_params_handler {
+	size_t size;
+	rkisp1_block_handler handler;
+	unsigned int group;
+	unsigned int features;
+} rkisp1_ext_params_handlers[] = {
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_BLS] = {
+		.size		= sizeof(struct rkisp1_ext_params_bls_config),
+		.handler	= rkisp1_ext_params_bls,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+		.features       = RKISP1_FEATURE_BLS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_DPCC] = {
+		.size		= sizeof(struct rkisp1_ext_params_dpcc_config),
+		.handler	= rkisp1_ext_params_dpcc,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_SDG] = {
+		.size		= sizeof(struct rkisp1_ext_params_sdg_config),
+		.handler	= rkisp1_ext_params_sdg,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_AWB_GAIN] = {
+		.size		= sizeof(struct rkisp1_ext_params_awb_gain_config),
+		.handler	= rkisp1_ext_params_awbg,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_FLT] = {
+		.size		= sizeof(struct rkisp1_ext_params_flt_config),
+		.handler	= rkisp1_ext_params_flt,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_BDM] = {
+		.size		= sizeof(struct rkisp1_ext_params_bdm_config),
+		.handler	= rkisp1_ext_params_bdm,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_CTK] = {
+		.size		= sizeof(struct rkisp1_ext_params_ctk_config),
+		.handler	= rkisp1_ext_params_ctk,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_GOC] = {
+		.size		= sizeof(struct rkisp1_ext_params_goc_config),
+		.handler	= rkisp1_ext_params_goc,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_DPF] = {
+		.size		= sizeof(struct rkisp1_ext_params_dpf_config),
+		.handler	= rkisp1_ext_params_dpf,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_DPF_STRENGTH] = {
+		.size		= sizeof(struct rkisp1_ext_params_dpf_strength_config),
+		.handler	= rkisp1_ext_params_dpfs,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_CPROC] = {
+		.size		= sizeof(struct rkisp1_ext_params_cproc_config),
+		.handler	= rkisp1_ext_params_cproc,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_IE] = {
+		.size		= sizeof(struct rkisp1_ext_params_ie_config),
+		.handler	= rkisp1_ext_params_ie,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_LSC] = {
+		.size		= sizeof(struct rkisp1_ext_params_lsc_config),
+		.handler	= rkisp1_ext_params_lsc,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_LSC,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_AWB_MEAS] = {
+		.size		= sizeof(struct rkisp1_ext_params_awb_meas_config),
+		.handler	= rkisp1_ext_params_awbm,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_HST_MEAS] = {
+		.size		= sizeof(struct rkisp1_ext_params_hst_config),
+		.handler	= rkisp1_ext_params_hstm,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_AEC_MEAS] = {
+		.size		= sizeof(struct rkisp1_ext_params_aec_config),
+		.handler	= rkisp1_ext_params_aecm,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_AFC_MEAS] = {
+		.size		= sizeof(struct rkisp1_ext_params_afc_config),
+		.handler	= rkisp1_ext_params_afcm,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_COMPAND_BLS] = {
+		.size		= sizeof(struct rkisp1_ext_params_compand_bls_config),
+		.handler	= rkisp1_ext_params_compand_bls,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+		.features	= RKISP1_FEATURE_COMPAND,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_COMPAND_EXPAND] = {
+		.size		= sizeof(struct rkisp1_ext_params_compand_curve_config),
+		.handler	= rkisp1_ext_params_compand_expand,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+		.features	= RKISP1_FEATURE_COMPAND,
+	},
+	[RKISP1_EXT_PARAMS_BLOCK_TYPE_COMPAND_COMPRESS] = {
+		.size		= sizeof(struct rkisp1_ext_params_compand_curve_config),
+		.handler	= rkisp1_ext_params_compand_compress,
+		.group		= RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS,
+		.features	= RKISP1_FEATURE_COMPAND,
+	},
+};
+
+static void rkisp1_ext_params_config(struct rkisp1_params *params,
+				     struct rkisp1_ext_params_cfg *cfg,
+				     u32 block_group_mask)
+{
+	size_t block_offset = 0;
+
+	if (WARN_ON(!cfg))
+		return;
+
+	/* Walk the list of parameter blocks and process them. */
+	while (block_offset < cfg->data_size) {
+		const struct rkisp1_ext_params_handler *block_handler;
+		const union rkisp1_ext_params_config *block;
+
+		block = (const union rkisp1_ext_params_config *)
+			&cfg->data[block_offset];
+		block_offset += block->header.size;
+
+		/*
+		 * Make sure the block is supported by the platform and in the
+		 * list of groups to configure.
+		 */
+		block_handler = &rkisp1_ext_params_handlers[block->header.type];
+		if (!(block_handler->group & block_group_mask))
+			continue;
+
+		if ((params->rkisp1->info->features & block_handler->features) !=
+		    block_handler->features)
+			continue;
+
+		block_handler->handler(params, block);
+
+		if (block->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE)
+			params->enabled_blocks &= ~BIT(block->header.type);
+		else if (block->header.flags & RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE)
+			params->enabled_blocks |= BIT(block->header.type);
+	}
 }
 
 static void rkisp1_params_complete_buffer(struct rkisp1_params *params,
-					  struct rkisp1_buffer *buf,
+					  struct rkisp1_params_buffer *buf,
 					  unsigned int frame_sequence)
 {
 	list_del(&buf->queue);
@@ -1527,17 +2172,24 @@ static void rkisp1_params_complete_buffer(struct rkisp1_params *params,
 void rkisp1_params_isr(struct rkisp1_device *rkisp1)
 {
 	struct rkisp1_params *params = &rkisp1->params;
-	struct rkisp1_params_cfg *new_params;
-	struct rkisp1_buffer *cur_buf;
+	struct rkisp1_params_buffer *cur_buf;
 
 	spin_lock(&params->config_lock);
 
-	if (!rkisp1_params_get_buffer(params, &cur_buf, &new_params))
+	cur_buf = list_first_entry_or_null(&params->params,
+					   struct rkisp1_params_buffer, queue);
+	if (!cur_buf)
 		goto unlock;
 
-	rkisp1_isp_isr_other_config(params, new_params);
-	rkisp1_isp_isr_lsc_config(params, new_params);
-	rkisp1_isp_isr_meas_config(params, new_params);
+	if (params->metafmt->dataformat == V4L2_META_FMT_RK_ISP1_PARAMS) {
+		rkisp1_isp_isr_other_config(params, cur_buf->cfg);
+		rkisp1_isp_isr_lsc_config(params, cur_buf->cfg);
+		rkisp1_isp_isr_meas_config(params, cur_buf->cfg);
+	} else {
+		rkisp1_ext_params_config(params, cur_buf->cfg,
+					 RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS |
+					 RKISP1_EXT_PARAMS_BLOCK_GROUP_LSC);
+	}
 
 	/* update shadow register immediately */
 	rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
@@ -1603,8 +2255,7 @@ void rkisp1_params_pre_configure(struct rkisp1_params *params,
 				 enum v4l2_ycbcr_encoding ycbcr_encoding)
 {
 	struct rkisp1_cif_isp_hst_config hst = rkisp1_hst_params_default_config;
-	struct rkisp1_params_cfg *new_params;
-	struct rkisp1_buffer *cur_buf;
+	struct rkisp1_params_buffer *cur_buf;
 
 	params->quantization = quantization;
 	params->ycbcr_encoding = ycbcr_encoding;
@@ -1633,11 +2284,18 @@ void rkisp1_params_pre_configure(struct rkisp1_params *params,
 
 	/* apply the first buffer if there is one already */
 
-	if (!rkisp1_params_get_buffer(params, &cur_buf, &new_params))
+	cur_buf = list_first_entry_or_null(&params->params,
+					   struct rkisp1_params_buffer, queue);
+	if (!cur_buf)
 		goto unlock;
 
-	rkisp1_isp_isr_other_config(params, new_params);
-	rkisp1_isp_isr_meas_config(params, new_params);
+	if (params->metafmt->dataformat == V4L2_META_FMT_RK_ISP1_PARAMS) {
+		rkisp1_isp_isr_other_config(params, cur_buf->cfg);
+		rkisp1_isp_isr_meas_config(params, cur_buf->cfg);
+	} else {
+		rkisp1_ext_params_config(params, cur_buf->cfg,
+					 RKISP1_EXT_PARAMS_BLOCK_GROUP_OTHERS);
+	}
 
 	/* update shadow register immediately */
 	rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
@@ -1649,8 +2307,7 @@ unlock:
 
 void rkisp1_params_post_configure(struct rkisp1_params *params)
 {
-	struct rkisp1_params_cfg *new_params;
-	struct rkisp1_buffer *cur_buf;
+	struct rkisp1_params_buffer *cur_buf;
 
 	spin_lock_irq(&params->config_lock);
 
@@ -1662,11 +2319,16 @@ void rkisp1_params_post_configure(struct rkisp1_params *params)
 	 * ordering doesn't affect other ISP versions negatively, do so
 	 * unconditionally.
 	 */
-
-	if (!rkisp1_params_get_buffer(params, &cur_buf, &new_params))
+	cur_buf = list_first_entry_or_null(&params->params,
+					   struct rkisp1_params_buffer, queue);
+	if (!cur_buf)
 		goto unlock;
 
-	rkisp1_isp_isr_lsc_config(params, new_params);
+	if (params->metafmt->dataformat == V4L2_META_FMT_RK_ISP1_PARAMS)
+		rkisp1_isp_isr_lsc_config(params, cur_buf->cfg);
+	else
+		rkisp1_ext_params_config(params, cur_buf->cfg,
+					 RKISP1_EXT_PARAMS_BLOCK_GROUP_LSC);
 
 	/* update shadow register immediately */
 	rkisp1_param_set_bits(params, RKISP1_CIF_ISP_CTRL,
@@ -1742,12 +2404,12 @@ static int rkisp1_params_enum_fmt_meta_out(struct file *file, void *priv,
 					   struct v4l2_fmtdesc *f)
 {
 	struct video_device *video = video_devdata(file);
-	struct rkisp1_params *params = video_get_drvdata(video);
 
-	if (f->index > 0 || f->type != video->queue->type)
+	if (f->index >= ARRAY_SIZE(rkisp1_params_formats) ||
+	    f->type != video->queue->type)
 		return -EINVAL;
 
-	f->pixelformat = params->vdev_fmt.fmt.meta.dataformat;
+	f->pixelformat = rkisp1_params_formats[f->index].dataformat;
 
 	return 0;
 }
@@ -1762,9 +2424,40 @@ static int rkisp1_params_g_fmt_meta_out(struct file *file, void *fh,
 	if (f->type != video->queue->type)
 		return -EINVAL;
 
-	memset(meta, 0, sizeof(*meta));
-	meta->dataformat = params->vdev_fmt.fmt.meta.dataformat;
-	meta->buffersize = params->vdev_fmt.fmt.meta.buffersize;
+	*meta = *params->metafmt;
+
+	return 0;
+}
+
+static int rkisp1_params_try_fmt_meta_out(struct file *file, void *fh,
+					  struct v4l2_format *f)
+{
+	struct video_device *video = video_devdata(file);
+	struct v4l2_meta_format *meta = &f->fmt.meta;
+
+	if (f->type != video->queue->type)
+		return -EINVAL;
+
+	*meta = *rkisp1_params_get_format_info(meta->dataformat);
+
+	return 0;
+}
+
+static int rkisp1_params_s_fmt_meta_out(struct file *file, void *fh,
+					struct v4l2_format *f)
+{
+	struct video_device *video = video_devdata(file);
+	struct rkisp1_params *params = video_get_drvdata(video);
+	struct v4l2_meta_format *meta = &f->fmt.meta;
+
+	if (f->type != video->queue->type)
+		return -EINVAL;
+
+	if (vb2_is_busy(video->queue))
+		return -EBUSY;
+
+	params->metafmt = rkisp1_params_get_format_info(meta->dataformat);
+	*meta = *params->metafmt;
 
 	return 0;
 }
@@ -1794,8 +2487,8 @@ static const struct v4l2_ioctl_ops rkisp1_params_ioctl = {
 	.vidioc_streamoff = vb2_ioctl_streamoff,
 	.vidioc_enum_fmt_meta_out = rkisp1_params_enum_fmt_meta_out,
 	.vidioc_g_fmt_meta_out = rkisp1_params_g_fmt_meta_out,
-	.vidioc_s_fmt_meta_out = rkisp1_params_g_fmt_meta_out,
-	.vidioc_try_fmt_meta_out = rkisp1_params_g_fmt_meta_out,
+	.vidioc_s_fmt_meta_out = rkisp1_params_s_fmt_meta_out,
+	.vidioc_try_fmt_meta_out = rkisp1_params_try_fmt_meta_out,
 	.vidioc_querycap = rkisp1_params_querycap,
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
@@ -1807,22 +2500,46 @@ static int rkisp1_params_vb2_queue_setup(struct vb2_queue *vq,
 					 unsigned int sizes[],
 					 struct device *alloc_devs[])
 {
+	struct rkisp1_params *params = vq->drv_priv;
+
 	*num_buffers = clamp_t(u32, *num_buffers,
 			       RKISP1_ISP_PARAMS_REQ_BUFS_MIN,
 			       RKISP1_ISP_PARAMS_REQ_BUFS_MAX);
 
 	*num_planes = 1;
 
-	sizes[0] = sizeof(struct rkisp1_params_cfg);
+	sizes[0] = params->metafmt->buffersize;
 
 	return 0;
 }
 
+static int rkisp1_params_vb2_buf_init(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct rkisp1_params_buffer *params_buf = to_rkisp1_params_buffer(vbuf);
+	struct rkisp1_params *params = vb->vb2_queue->drv_priv;
+
+	params_buf->cfg = kvmalloc(params->metafmt->buffersize,
+				   GFP_KERNEL);
+	if (!params_buf->cfg)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void rkisp1_params_vb2_buf_cleanup(struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct rkisp1_params_buffer *params_buf = to_rkisp1_params_buffer(vbuf);
+
+	kvfree(params_buf->cfg);
+	params_buf->cfg = NULL;
+}
+
 static void rkisp1_params_vb2_buf_queue(struct vb2_buffer *vb)
 {
 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
-	struct rkisp1_buffer *params_buf =
-		container_of(vbuf, struct rkisp1_buffer, vb);
+	struct rkisp1_params_buffer *params_buf = to_rkisp1_params_buffer(vbuf);
 	struct vb2_queue *vq = vb->vb2_queue;
 	struct rkisp1_params *params = vq->drv_priv;
 
@@ -1831,12 +2548,133 @@ static void rkisp1_params_vb2_buf_queue(struct vb2_buffer *vb)
 	spin_unlock_irq(&params->config_lock);
 }
 
+static int rkisp1_params_prepare_ext_params(struct rkisp1_params *params,
+					    struct vb2_buffer *vb)
+{
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct rkisp1_params_buffer *params_buf = to_rkisp1_params_buffer(vbuf);
+	size_t header_size = offsetof(struct rkisp1_ext_params_cfg, data);
+	struct rkisp1_ext_params_cfg *cfg = params_buf->cfg;
+	size_t payload_size = vb2_get_plane_payload(vb, 0);
+	struct rkisp1_ext_params_cfg *usr_cfg =
+		vb2_plane_vaddr(&vbuf->vb2_buf, 0);
+	size_t block_offset = 0;
+	size_t cfg_size;
+
+	/*
+	 * Validate the buffer payload size before copying the parameters. The
+	 * payload has to be smaller than the destination buffer size and larger
+	 * than the header size.
+	 */
+	if (payload_size > params->metafmt->buffersize) {
+		dev_dbg(params->rkisp1->dev,
+			"Too large buffer payload size %zu\n", payload_size);
+		return -EINVAL;
+	}
+
+	if (payload_size < header_size) {
+		dev_dbg(params->rkisp1->dev,
+			"Buffer payload %zu smaller than header size %zu\n",
+			payload_size, header_size);
+		return -EINVAL;
+	}
+
+	/*
+	 * Copy the parameters buffer to the internal scratch buffer to avoid
+	 * userspace modifying the buffer content while the driver processes it.
+	 */
+	memcpy(cfg, usr_cfg, payload_size);
+
+	/* Only v1 is supported at the moment. */
+	if (cfg->version != RKISP1_EXT_PARAM_BUFFER_V1) {
+		dev_dbg(params->rkisp1->dev,
+			"Unsupported extensible format version: %u\n",
+			cfg->version);
+		return -EINVAL;
+	}
+
+	/* Validate the size reported in the parameters buffer header. */
+	cfg_size = header_size + cfg->data_size;
+	if (cfg_size != payload_size) {
+		dev_dbg(params->rkisp1->dev,
+			"Data size %zu different than buffer payload size %zu\n",
+			cfg_size, payload_size);
+		return -EINVAL;
+	}
+
+	/* Walk the list of parameter blocks and validate them. */
+	cfg_size = cfg->data_size;
+	while (cfg_size >= sizeof(struct rkisp1_ext_params_block_header)) {
+		const struct rkisp1_ext_params_block_header *block;
+		const struct rkisp1_ext_params_handler *handler;
+
+		block = (const struct rkisp1_ext_params_block_header *)
+			&cfg->data[block_offset];
+
+		if (block->type >= ARRAY_SIZE(rkisp1_ext_params_handlers)) {
+			dev_dbg(params->rkisp1->dev,
+				"Invalid parameters block type\n");
+			return -EINVAL;
+		}
+
+		if (block->size > cfg_size) {
+			dev_dbg(params->rkisp1->dev,
+				"Premature end of parameters data\n");
+			return -EINVAL;
+		}
+
+		if ((block->flags & (RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE |
+				     RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE)) ==
+		   (RKISP1_EXT_PARAMS_FL_BLOCK_ENABLE |
+		    RKISP1_EXT_PARAMS_FL_BLOCK_DISABLE)) {
+			dev_dbg(params->rkisp1->dev,
+				"Invalid parameters block flags\n");
+			return -EINVAL;
+		}
+
+		handler = &rkisp1_ext_params_handlers[block->type];
+		if (block->size != handler->size) {
+			dev_dbg(params->rkisp1->dev,
+				"Invalid parameters block size\n");
+			return -EINVAL;
+		}
+
+		block_offset += block->size;
+		cfg_size -= block->size;
+	}
+
+	if (cfg_size) {
+		dev_dbg(params->rkisp1->dev,
+			"Unexpected data after the parameters buffer end\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int rkisp1_params_vb2_buf_prepare(struct vb2_buffer *vb)
 {
-	if (vb2_plane_size(vb, 0) < sizeof(struct rkisp1_params_cfg))
+	struct rkisp1_params *params = vb->vb2_queue->drv_priv;
+	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+	struct rkisp1_params_buffer *params_buf = to_rkisp1_params_buffer(vbuf);
+	struct rkisp1_params_cfg *cfg = vb2_plane_vaddr(&vbuf->vb2_buf, 0);
+	size_t payload = vb2_get_plane_payload(vb, 0);
+
+	if (params->metafmt->dataformat == V4L2_META_FMT_RK_ISP1_EXT_PARAMS)
+		return rkisp1_params_prepare_ext_params(params, vb);
+
+	/*
+	 * For the fixed parameters format the payload size must be exactly the
+	 * size of the parameters structure.
+	 */
+	if (payload != sizeof(*cfg))
 		return -EINVAL;
 
-	vb2_set_plane_payload(vb, 0, sizeof(struct rkisp1_params_cfg));
+	/*
+	 * Copy the parameters buffer to the internal scratch buffer to avoid
+	 * userspace modifying the buffer content while the driver processes it.
+	 */
+	memcpy(params_buf->cfg, cfg, payload);
 
 	return 0;
 }
@@ -1844,7 +2682,7 @@ static int rkisp1_params_vb2_buf_prepare(struct vb2_buffer *vb)
 static void rkisp1_params_vb2_stop_streaming(struct vb2_queue *vq)
 {
 	struct rkisp1_params *params = vq->drv_priv;
-	struct rkisp1_buffer *buf;
+	struct rkisp1_params_buffer *buf;
 	LIST_HEAD(tmp_list);
 
 	/*
@@ -1858,16 +2696,17 @@ static void rkisp1_params_vb2_stop_streaming(struct vb2_queue *vq)
 
 	list_for_each_entry(buf, &tmp_list, queue)
 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+
+	params->enabled_blocks = 0;
 }
 
 static const struct vb2_ops rkisp1_params_vb2_ops = {
 	.queue_setup = rkisp1_params_vb2_queue_setup,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
+	.buf_init = rkisp1_params_vb2_buf_init,
+	.buf_cleanup = rkisp1_params_vb2_buf_cleanup,
 	.buf_queue = rkisp1_params_vb2_buf_queue,
 	.buf_prepare = rkisp1_params_vb2_buf_prepare,
 	.stop_streaming = rkisp1_params_vb2_stop_streaming,
-
 };
 
 static const struct v4l2_file_operations rkisp1_params_fops = {
@@ -1890,26 +2729,13 @@ static int rkisp1_params_init_vb2_queue(struct vb2_queue *q,
 	q->drv_priv = params;
 	q->ops = &rkisp1_params_vb2_ops;
 	q->mem_ops = &vb2_vmalloc_memops;
-	q->buf_struct_size = sizeof(struct rkisp1_buffer);
+	q->buf_struct_size = sizeof(struct rkisp1_params_buffer);
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	q->lock = &node->vlock;
 
 	return vb2_queue_init(q);
 }
 
-static void rkisp1_init_params(struct rkisp1_params *params)
-{
-	params->vdev_fmt.fmt.meta.dataformat =
-		V4L2_META_FMT_RK_ISP1_PARAMS;
-	params->vdev_fmt.fmt.meta.buffersize =
-		sizeof(struct rkisp1_params_cfg);
-
-	if (params->rkisp1->info->isp_ver == RKISP1_V12)
-		params->ops = &rkisp1_v12_params_ops;
-	else
-		params->ops = &rkisp1_v10_params_ops;
-}
-
 int rkisp1_params_register(struct rkisp1_device *rkisp1)
 {
 	struct rkisp1_params *params = &rkisp1->params;
@@ -1938,7 +2764,14 @@ int rkisp1_params_register(struct rkisp1_device *rkisp1)
 	vdev->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_META_OUTPUT;
 	vdev->vfl_dir = VFL_DIR_TX;
 	rkisp1_params_init_vb2_queue(vdev->queue, params);
-	rkisp1_init_params(params);
+
+	params->metafmt = &rkisp1_params_formats[RKISP1_PARAMS_FIXED];
+
+	if (params->rkisp1->info->isp_ver == RKISP1_V12)
+		params->ops = &rkisp1_v12_params_ops;
+	else
+		params->ops = &rkisp1_v10_params_ops;
+
 	video_set_drvdata(vdev, params);
 
 	node->pad.flags = MEDIA_PAD_FL_SOURCE;
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-regs.h b/drivers/media/platform/rockchip/rkisp1/rkisp1-regs.h
index fccf4c17ee8d..139177db9c6d 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-regs.h
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-regs.h
@@ -327,13 +327,6 @@
 #define RKISP1_CIF_IMG_EFF_CTRL_CFG_UPD			BIT(4)
 #define RKISP1_CIF_IMG_EFF_CTRL_YCBCR_FULL		BIT(5)
 
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_BLACKWHITE_SHIFT	0
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_NEGATIVE_SHIFT	1
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_SEPIA_SHIFT	2
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_COLOR_SEL_SHIFT	3
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_EMBOSS_SHIFT	4
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_SKETCH_SHIFT	5
-#define RKISP1_CIF_IMG_EFF_CTRL_MODE_SHARPEN_SHIFT	6
 #define RKISP1_CIF_IMG_EFF_CTRL_MODE_MASK		0xe
 
 /* IMG_EFF_COLOR_SEL */
@@ -704,6 +697,12 @@
 #define RKISP1_CIF_ISP_DPF_SPATIAL_COEFF_MAX		0x1f
 #define RKISP1_CIF_ISP_DPF_NLL_COEFF_N_MAX		0x3ff
 
+/* COMPAND */
+#define RKISP1_CIF_ISP_COMPAND_CTRL_EXPAND_ENABLE	BIT(0)
+#define RKISP1_CIF_ISP_COMPAND_CTRL_COMPRESS_ENABLE	BIT(1)
+#define RKISP1_CIF_ISP_COMPAND_CTRL_SOFT_RESET_FLAG	BIT(2)
+#define RKISP1_CIF_ISP_COMPAND_CTRL_BLS_ENABLE		BIT(3)
+
 /* =================================================================== */
 /*                            CIF Registers                            */
 /* =================================================================== */
@@ -1394,6 +1393,23 @@
 #define RKISP1_CIF_ISP_VSM_DELTA_H		(RKISP1_CIF_ISP_VSM_BASE + 0x0000001c)
 #define RKISP1_CIF_ISP_VSM_DELTA_V		(RKISP1_CIF_ISP_VSM_BASE + 0x00000020)
 
+#define RKISP1_CIF_ISP_COMPAND_BASE			0x00003200
+#define RKISP1_CIF_ISP_COMPAND_CTRL			(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000000)
+#define RKISP1_CIF_ISP_COMPAND_BLS_A_FIXED		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000004)
+#define RKISP1_CIF_ISP_COMPAND_BLS_B_FIXED		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000008)
+#define RKISP1_CIF_ISP_COMPAND_BLS_C_FIXED		(RKISP1_CIF_ISP_COMPAND_BASE + 0x0000000c)
+#define RKISP1_CIF_ISP_COMPAND_BLS_D_FIXED		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000010)
+#define RKISP1_CIF_ISP_COMPAND_EXPAND_PX_N(n)		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000014 + (n) * 4)
+#define RKISP1_CIF_ISP_COMPAND_COMPRESS_PX_N(n)		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000040 + (n) * 4)
+#define RKISP1_CIF_ISP_COMPAND_EXPAND_Y_ADDR		(RKISP1_CIF_ISP_COMPAND_BASE + 0x0000006c)
+#define RKISP1_CIF_ISP_COMPAND_EXPAND_Y_WRITE_DATA	(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000070)
+#define RKISP1_CIF_ISP_COMPAND_COMPRESS_Y_ADDR		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000074)
+#define RKISP1_CIF_ISP_COMPAND_COMPRESS_Y_WRITE_DATA	(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000078)
+#define RKISP1_CIF_ISP_COMPAND_EXPAND_X_ADDR		(RKISP1_CIF_ISP_COMPAND_BASE + 0x0000007c)
+#define RKISP1_CIF_ISP_COMPAND_EXPAND_X_WRITE_DATA	(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000080)
+#define RKISP1_CIF_ISP_COMPAND_COMPRESS_X_ADDR		(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000084)
+#define RKISP1_CIF_ISP_COMPAND_COMPRESS_X_WRITE_DATA	(RKISP1_CIF_ISP_COMPAND_BASE + 0x00000088)
+
 #define RKISP1_CIF_ISP_CSI0_BASE		0x00007000
 #define RKISP1_CIF_ISP_CSI0_CTRL0		(RKISP1_CIF_ISP_CSI0_BASE + 0x00000000)
 
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-resizer.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-resizer.c
index 6f3931ca5b51..8e6b753d3081 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-resizer.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-resizer.c
@@ -135,11 +135,11 @@ static void rkisp1_dcrop_disable(struct rkisp1_resizer *rsz,
 
 /* configure dual-crop unit */
 static void rkisp1_dcrop_config(struct rkisp1_resizer *rsz,
-				struct v4l2_subdev_state *sd_state)
+				const struct v4l2_subdev_state *sd_state)
 {
 	struct rkisp1_device *rkisp1 = rsz->rkisp1;
-	struct v4l2_mbus_framefmt *sink_fmt;
-	struct v4l2_rect *sink_crop;
+	const struct v4l2_mbus_framefmt *sink_fmt;
+	const struct v4l2_rect *sink_crop;
 	u32 dc_ctrl;
 
 	sink_crop = v4l2_subdev_state_get_crop(sd_state, RKISP1_RSZ_PAD_SINK);
@@ -264,7 +264,7 @@ static void rkisp1_rsz_config_regs(struct rkisp1_resizer *rsz,
 }
 
 static void rkisp1_rsz_config(struct rkisp1_resizer *rsz,
-			      struct v4l2_subdev_state *sd_state,
+			      const struct v4l2_subdev_state *sd_state,
 			      enum rkisp1_shadow_regs_when when)
 {
 	const struct rkisp1_rsz_yuv_mbus_info *sink_yuv_info, *src_yuv_info;
@@ -494,10 +494,10 @@ static void rkisp1_rsz_set_sink_fmt(struct rkisp1_resizer *rsz,
 
 	sink_fmt->width = clamp_t(u32, format->width,
 				  RKISP1_ISP_MIN_WIDTH,
-				  RKISP1_ISP_MAX_WIDTH);
+				  rsz->rkisp1->info->max_width);
 	sink_fmt->height = clamp_t(u32, format->height,
 				   RKISP1_ISP_MIN_HEIGHT,
-				   RKISP1_ISP_MAX_HEIGHT);
+				   rsz->rkisp1->info->max_height);
 
 	/*
 	 * Adjust the color space fields. Accept any color primaries and
@@ -600,7 +600,7 @@ static int rkisp1_rsz_set_selection(struct v4l2_subdev *sd,
 	if (sel->target != V4L2_SEL_TGT_CROP || sel->pad == RKISP1_RSZ_PAD_SRC)
 		return -EINVAL;
 
-	dev_dbg(rsz->rkisp1->dev, "%s: pad: %d sel(%d,%d)/%dx%d\n", __func__,
+	dev_dbg(rsz->rkisp1->dev, "%s: pad: %d sel(%d,%d)/%ux%u\n", __func__,
 		sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
 
 	rkisp1_rsz_set_sink_crop(rsz, sd_state, &sel->r);
diff --git a/drivers/media/platform/rockchip/rkisp1/rkisp1-stats.c b/drivers/media/platform/rockchip/rkisp1/rkisp1-stats.c
index 2795eef91bdd..d5fdb8f82dc7 100644
--- a/drivers/media/platform/rockchip/rkisp1/rkisp1-stats.c
+++ b/drivers/media/platform/rockchip/rkisp1/rkisp1-stats.c
@@ -150,8 +150,6 @@ static const struct vb2_ops rkisp1_stats_vb2_ops = {
 	.queue_setup = rkisp1_stats_vb2_queue_setup,
 	.buf_queue = rkisp1_stats_vb2_buf_queue,
 	.buf_prepare = rkisp1_stats_vb2_buf_prepare,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.stop_streaming = rkisp1_stats_vb2_stop_streaming,
 };
 
@@ -304,48 +302,25 @@ static void rkisp1_stats_get_hst_meas_v12(struct rkisp1_stats *stats,
 static void rkisp1_stats_get_bls_meas(struct rkisp1_stats *stats,
 				      struct rkisp1_stat_buffer *pbuf)
 {
+	static const u32 regs[] = {
+		RKISP1_CIF_ISP_BLS_A_MEASURED,
+		RKISP1_CIF_ISP_BLS_B_MEASURED,
+		RKISP1_CIF_ISP_BLS_C_MEASURED,
+		RKISP1_CIF_ISP_BLS_D_MEASURED,
+	};
 	struct rkisp1_device *rkisp1 = stats->rkisp1;
 	const struct rkisp1_mbus_info *in_fmt = rkisp1->isp.sink_fmt;
 	struct rkisp1_cif_isp_bls_meas_val *bls_val;
+	u32 swapped[4];
+
+	rkisp1_bls_swap_regs(in_fmt->bayer_pat, regs, swapped);
 
 	bls_val = &pbuf->params.ae.bls_val;
-	if (in_fmt->bayer_pat == RKISP1_RAW_BGGR) {
-		bls_val->meas_b =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED);
-		bls_val->meas_gb =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED);
-		bls_val->meas_gr =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED);
-		bls_val->meas_r =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED);
-	} else if (in_fmt->bayer_pat == RKISP1_RAW_GBRG) {
-		bls_val->meas_gb =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED);
-		bls_val->meas_b =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED);
-		bls_val->meas_r =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED);
-		bls_val->meas_gr =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED);
-	} else if (in_fmt->bayer_pat == RKISP1_RAW_GRBG) {
-		bls_val->meas_gr =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED);
-		bls_val->meas_r =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED);
-		bls_val->meas_b =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED);
-		bls_val->meas_gb =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED);
-	} else if (in_fmt->bayer_pat == RKISP1_RAW_RGGB) {
-		bls_val->meas_r =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED);
-		bls_val->meas_gr =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED);
-		bls_val->meas_gb =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED);
-		bls_val->meas_b =
-			rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED);
-	}
+
+	bls_val->meas_r = rkisp1_read(rkisp1, swapped[0]);
+	bls_val->meas_gr = rkisp1_read(rkisp1, swapped[1]);
+	bls_val->meas_gb = rkisp1_read(rkisp1, swapped[2]);
+	bls_val->meas_b = rkisp1_read(rkisp1, swapped[3]);
 }
 
 static const struct rkisp1_stats_ops rkisp1_v10_stats_ops = {
diff --git a/drivers/media/platform/samsung/exynos-gsc/gsc-core.c b/drivers/media/platform/samsung/exynos-gsc/gsc-core.c
index 618ae55fe396..a06d7cace92f 100644
--- a/drivers/media/platform/samsung/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/samsung/exynos-gsc/gsc-core.c
@@ -1225,7 +1225,7 @@ static void gsc_remove(struct platform_device *pdev)
 static int gsc_m2m_suspend(struct gsc_dev *gsc)
 {
 	unsigned long flags;
-	int timeout;
+	long time_left;
 
 	spin_lock_irqsave(&gsc->slock, flags);
 	if (!gsc_m2m_pending(gsc)) {
@@ -1236,12 +1236,12 @@ static int gsc_m2m_suspend(struct gsc_dev *gsc)
 	set_bit(ST_M2M_SUSPENDING, &gsc->state);
 	spin_unlock_irqrestore(&gsc->slock, flags);
 
-	timeout = wait_event_timeout(gsc->irq_queue,
-			     test_bit(ST_M2M_SUSPENDED, &gsc->state),
-			     GSC_SHUTDOWN_TIMEOUT);
+	time_left = wait_event_timeout(gsc->irq_queue,
+				       test_bit(ST_M2M_SUSPENDED, &gsc->state),
+				       GSC_SHUTDOWN_TIMEOUT);
 
 	clear_bit(ST_M2M_SUSPENDING, &gsc->state);
-	return timeout == 0 ? -EAGAIN : 0;
+	return time_left == 0 ? -EAGAIN : 0;
 }
 
 static void gsc_m2m_resume(struct gsc_dev *gsc)
@@ -1309,7 +1309,7 @@ static const struct dev_pm_ops gsc_pm_ops = {
 
 static struct platform_driver gsc_driver = {
 	.probe		= gsc_probe,
-	.remove_new	= gsc_remove,
+	.remove		= gsc_remove,
 	.driver = {
 		.name	= GSC_MODULE_NAME,
 		.pm	= &gsc_pm_ops,
diff --git a/drivers/media/platform/samsung/exynos-gsc/gsc-m2m.c b/drivers/media/platform/samsung/exynos-gsc/gsc-m2m.c
index b7854ce5fb8e..4bda1c369c44 100644
--- a/drivers/media/platform/samsung/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/samsung/exynos-gsc/gsc-m2m.c
@@ -276,8 +276,6 @@ static const struct vb2_ops gsc_m2m_qops = {
 	.queue_setup	 = gsc_m2m_queue_setup,
 	.buf_prepare	 = gsc_m2m_buf_prepare,
 	.buf_queue	 = gsc_m2m_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.stop_streaming	 = gsc_m2m_stop_streaming,
 	.start_streaming = gsc_m2m_start_streaming,
 };
diff --git a/drivers/media/platform/samsung/exynos4-is/common.c b/drivers/media/platform/samsung/exynos4-is/common.c
index e41333535eac..77007f1a909b 100644
--- a/drivers/media/platform/samsung/exynos4-is/common.c
+++ b/drivers/media/platform/samsung/exynos4-is/common.c
@@ -44,4 +44,5 @@ void __fimc_vidioc_querycap(struct device *dev, struct v4l2_capability *cap)
 }
 EXPORT_SYMBOL(__fimc_vidioc_querycap);
 
+MODULE_DESCRIPTION("Samsung S5P/EXYNOS4 SoC Camera Subsystem driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-capture.c b/drivers/media/platform/samsung/exynos4-is/fimc-capture.c
index ffa4ea21387d..5b412afd7d60 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-capture.c
@@ -455,8 +455,6 @@ static const struct vb2_ops fimc_capture_qops = {
 	.queue_setup		= queue_setup,
 	.buf_prepare		= buffer_prepare,
 	.buf_queue		= buffer_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
 };
@@ -702,7 +700,7 @@ static void fimc_capture_try_selection(struct fimc_ctx *ctx,
 	r->top  = clamp_t(u32, r->top, 0, sink->f_height - r->height);
 	r->left = round_down(r->left, var->hor_offs_align);
 
-	dbg("target %#x: (%d,%d)/%dx%d, sink fmt: %dx%d",
+	dbg("target %#x: (%d,%d)/%ux%u, sink fmt: %dx%d",
 	    target, r->left, r->top, r->width, r->height,
 	    sink->f_width, sink->f_height);
 }
@@ -1624,7 +1622,7 @@ static int fimc_subdev_get_selection(struct v4l2_subdev *sd,
 		r->height = f->height;
 	}
 
-	dbg("target %#x: l:%d, t:%d, %dx%d, f_w: %d, f_h: %d",
+	dbg("target %#x: (%d,%d)/%ux%u, f_w: %d, f_h: %d",
 	    sel->pad, r->left, r->top, r->width, r->height,
 	    f->f_width, f->f_height);
 
@@ -1673,7 +1671,7 @@ static int fimc_subdev_set_selection(struct v4l2_subdev *sd,
 		spin_unlock_irqrestore(&fimc->slock, flags);
 	}
 
-	dbg("target %#x: (%d,%d)/%dx%d", sel->target, r->left, r->top,
+	dbg("target %#x: (%d,%d)/%ux%u", sel->target, r->left, r->top,
 	    r->width, r->height);
 
 	mutex_unlock(&fimc->lock);
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-core.c b/drivers/media/platform/samsung/exynos4-is/fimc-core.c
index aae74b501a42..2c9edd0a559b 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-core.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-core.c
@@ -822,7 +822,7 @@ err:
 static int fimc_m2m_suspend(struct fimc_dev *fimc)
 {
 	unsigned long flags;
-	int timeout;
+	long time_left;
 
 	spin_lock_irqsave(&fimc->slock, flags);
 	if (!fimc_m2m_pending(fimc)) {
@@ -833,12 +833,12 @@ static int fimc_m2m_suspend(struct fimc_dev *fimc)
 	set_bit(ST_M2M_SUSPENDING, &fimc->state);
 	spin_unlock_irqrestore(&fimc->slock, flags);
 
-	timeout = wait_event_timeout(fimc->irq_queue,
-			     test_bit(ST_M2M_SUSPENDED, &fimc->state),
-			     FIMC_SHUTDOWN_TIMEOUT);
+	time_left = wait_event_timeout(fimc->irq_queue,
+				       test_bit(ST_M2M_SUSPENDED, &fimc->state),
+				       FIMC_SHUTDOWN_TIMEOUT);
 
 	clear_bit(ST_M2M_SUSPENDING, &fimc->state);
-	return timeout == 0 ? -EAGAIN : 0;
+	return time_left == 0 ? -EAGAIN : 0;
 }
 
 static int fimc_m2m_resume(struct fimc_dev *fimc)
@@ -1157,7 +1157,7 @@ static const struct dev_pm_ops fimc_pm_ops = {
 
 static struct platform_driver fimc_driver = {
 	.probe		= fimc_probe,
-	.remove_new	= fimc_remove,
+	.remove		= fimc_remove,
 	.driver = {
 		.of_match_table = fimc_of_match,
 		.name		= FIMC_DRIVER_NAME,
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.c b/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.c
index 7a48fad1df16..ac67a04e5eeb 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.c
@@ -12,137 +12,6 @@
 
 #include "fimc-is-errno.h"
 
-const char *fimc_is_param_strerr(unsigned int error)
-{
-	switch (error) {
-	case ERROR_COMMON_CMD:
-		return "ERROR_COMMON_CMD: Invalid Command";
-	case ERROR_COMMON_PARAMETER:
-		return "ERROR_COMMON_PARAMETER: Invalid Parameter";
-	case ERROR_COMMON_SETFILE_LOAD:
-		return "ERROR_COMMON_SETFILE_LOAD: Illegal Setfile Loading";
-	case ERROR_COMMON_SETFILE_ADJUST:
-		return "ERROR_COMMON_SETFILE_ADJUST: Setfile isn't adjusted";
-	case ERROR_COMMON_SETFILE_INDEX:
-		return "ERROR_COMMON_SETFILE_INDEX: Invalid setfile index";
-	case ERROR_COMMON_INPUT_PATH:
-		return "ERROR_COMMON_INPUT_PATH: Input path can be changed in ready state";
-	case ERROR_COMMON_INPUT_INIT:
-		return "ERROR_COMMON_INPUT_INIT: IP can not start if input path is not set";
-	case ERROR_COMMON_OUTPUT_PATH:
-		return "ERROR_COMMON_OUTPUT_PATH: Output path can be changed in ready state (stop)";
-	case ERROR_COMMON_OUTPUT_INIT:
-		return "ERROR_COMMON_OUTPUT_INIT: IP can not start if output path is not set";
-	case ERROR_CONTROL_BYPASS:
-		return "ERROR_CONTROL_BYPASS";
-	case ERROR_OTF_INPUT_FORMAT:
-		return "ERROR_OTF_INPUT_FORMAT: Invalid format  (DRC: YUV444, FD: YUV444, 422, 420)";
-	case ERROR_OTF_INPUT_WIDTH:
-		return "ERROR_OTF_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
-	case ERROR_OTF_INPUT_HEIGHT:
-		return "ERROR_OTF_INPUT_HEIGHT: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
-	case ERROR_OTF_INPUT_BIT_WIDTH:
-		return "ERROR_OTF_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
-	case ERROR_DMA_INPUT_WIDTH:
-		return "ERROR_DMA_INPUT_WIDTH: Invalid width (DRC: 128~8192, FD: 32~8190)";
-	case ERROR_DMA_INPUT_HEIGHT:
-		return "ERROR_DMA_INPUT_HEIGHT: Invalid height (DRC: 64~8192, FD: 16~8190)";
-	case ERROR_DMA_INPUT_FORMAT:
-		return "ERROR_DMA_INPUT_FORMAT: Invalid format (DRC: YUV444 or YUV422, FD: YUV444,422,420)";
-	case ERROR_DMA_INPUT_BIT_WIDTH:
-		return "ERROR_DMA_INPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
-	case ERROR_DMA_INPUT_ORDER:
-		return "ERROR_DMA_INPUT_ORDER: Invalid order(DRC: YYCbCr,YCbYCr,FD:NO,YYCbCr,YCbYCr,CbCr,CrCb)";
-	case ERROR_DMA_INPUT_PLANE:
-		return "ERROR_DMA_INPUT_PLANE: Invalid plane (DRC: 3, FD: 1, 2, 3)";
-	case ERROR_OTF_OUTPUT_WIDTH:
-		return "ERROR_OTF_OUTPUT_WIDTH: Invalid width (DRC: 128~8192)";
-	case ERROR_OTF_OUTPUT_HEIGHT:
-		return "ERROR_OTF_OUTPUT_HEIGHT: Invalid height (DRC: 64~8192)";
-	case ERROR_OTF_OUTPUT_FORMAT:
-		return "ERROR_OTF_OUTPUT_FORMAT: Invalid format (DRC: YUV444)";
-	case ERROR_OTF_OUTPUT_BIT_WIDTH:
-		return "ERROR_OTF_OUTPUT_BIT_WIDTH: Invalid bit-width (DRC: 8~12bits, FD: 8bit)";
-	case ERROR_DMA_OUTPUT_WIDTH:
-		return "ERROR_DMA_OUTPUT_WIDTH";
-	case ERROR_DMA_OUTPUT_HEIGHT:
-		return "ERROR_DMA_OUTPUT_HEIGHT";
-	case ERROR_DMA_OUTPUT_FORMAT:
-		return "ERROR_DMA_OUTPUT_FORMAT";
-	case ERROR_DMA_OUTPUT_BIT_WIDTH:
-		return "ERROR_DMA_OUTPUT_BIT_WIDTH";
-	case ERROR_DMA_OUTPUT_PLANE:
-		return "ERROR_DMA_OUTPUT_PLANE";
-	case ERROR_DMA_OUTPUT_ORDER:
-		return "ERROR_DMA_OUTPUT_ORDER";
-
-	/* Sensor Error(100~199) */
-	case ERROR_SENSOR_I2C_FAIL:
-		return "ERROR_SENSOR_I2C_FAIL";
-	case ERROR_SENSOR_INVALID_FRAMERATE:
-		return "ERROR_SENSOR_INVALID_FRAMERATE";
-	case ERROR_SENSOR_INVALID_EXPOSURETIME:
-		return "ERROR_SENSOR_INVALID_EXPOSURETIME";
-	case ERROR_SENSOR_INVALID_SIZE:
-		return "ERROR_SENSOR_INVALID_SIZE";
-	case ERROR_SENSOR_INVALID_SETTING:
-		return "ERROR_SENSOR_INVALID_SETTING";
-	case ERROR_SENSOR_ACTUATOR_INIT_FAIL:
-		return "ERROR_SENSOR_ACTUATOR_INIT_FAIL";
-	case ERROR_SENSOR_INVALID_AF_POS:
-		return "ERROR_SENSOR_INVALID_AF_POS";
-	case ERROR_SENSOR_UNSUPPORT_FUNC:
-		return "ERROR_SENSOR_UNSUPPORT_FUNC";
-	case ERROR_SENSOR_UNSUPPORT_PERI:
-		return "ERROR_SENSOR_UNSUPPORT_PERI";
-	case ERROR_SENSOR_UNSUPPORT_AF:
-		return "ERROR_SENSOR_UNSUPPORT_AF";
-
-	/* ISP Error (200~299) */
-	case ERROR_ISP_AF_BUSY:
-		return "ERROR_ISP_AF_BUSY";
-	case ERROR_ISP_AF_INVALID_COMMAND:
-		return "ERROR_ISP_AF_INVALID_COMMAND";
-	case ERROR_ISP_AF_INVALID_MODE:
-		return "ERROR_ISP_AF_INVALID_MODE";
-
-	/* DRC Error (300~399) */
-	/* FD Error  (400~499) */
-	case ERROR_FD_CONFIG_MAX_NUMBER_STATE:
-		return "ERROR_FD_CONFIG_MAX_NUMBER_STATE";
-	case ERROR_FD_CONFIG_MAX_NUMBER_INVALID:
-		return "ERROR_FD_CONFIG_MAX_NUMBER_INVALID";
-	case ERROR_FD_CONFIG_YAW_ANGLE_STATE:
-		return "ERROR_FD_CONFIG_YAW_ANGLE_STATE";
-	case ERROR_FD_CONFIG_YAW_ANGLE_INVALID:
-		return "ERROR_FD_CONFIG_YAW_ANGLE_INVALID\n";
-	case ERROR_FD_CONFIG_ROLL_ANGLE_STATE:
-		return "ERROR_FD_CONFIG_ROLL_ANGLE_STATE";
-	case ERROR_FD_CONFIG_ROLL_ANGLE_INVALID:
-		return "ERROR_FD_CONFIG_ROLL_ANGLE_INVALID";
-	case ERROR_FD_CONFIG_SMILE_MODE_INVALID:
-		return "ERROR_FD_CONFIG_SMILE_MODE_INVALID";
-	case ERROR_FD_CONFIG_BLINK_MODE_INVALID:
-		return "ERROR_FD_CONFIG_BLINK_MODE_INVALID";
-	case ERROR_FD_CONFIG_EYES_DETECT_INVALID:
-		return "ERROR_FD_CONFIG_EYES_DETECT_INVALID";
-	case ERROR_FD_CONFIG_MOUTH_DETECT_INVALID:
-		return "ERROR_FD_CONFIG_MOUTH_DETECT_INVALID";
-	case ERROR_FD_CONFIG_ORIENTATION_STATE:
-		return "ERROR_FD_CONFIG_ORIENTATION_STATE";
-	case ERROR_FD_CONFIG_ORIENTATION_INVALID:
-		return "ERROR_FD_CONFIG_ORIENTATION_INVALID";
-	case ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID:
-		return "ERROR_FD_CONFIG_ORIENTATION_VALUE_INVALID";
-	case ERROR_FD_RESULT:
-		return "ERROR_FD_RESULT";
-	case ERROR_FD_MODE:
-		return "ERROR_FD_MODE";
-	default:
-		return "Unknown";
-	}
-}
-
 const char *fimc_is_strerr(unsigned int error)
 {
 	error &= ~IS_ERROR_TIME_OUT_FLAG;
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.h b/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.h
index 809e117331c0..fa8204ffec7b 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.h
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-errno.h
@@ -240,6 +240,5 @@ enum fimc_is_error {
 };
 
 const char *fimc_is_strerr(unsigned int error);
-const char *fimc_is_param_strerr(unsigned int error);
 
 #endif /* FIMC_IS_ERR_H_ */
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-i2c.c b/drivers/media/platform/samsung/exynos4-is/fimc-is-i2c.c
index 44363c4241d5..b243cbb1d010 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-i2c.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-i2c.c
@@ -137,7 +137,7 @@ static const struct of_device_id fimc_is_i2c_of_match[] = {
 
 static struct platform_driver fimc_is_i2c_driver = {
 	.probe		= fimc_is_i2c_probe,
-	.remove_new	= fimc_is_i2c_remove,
+	.remove		= fimc_is_i2c_remove,
 	.driver = {
 		.of_match_table = fimc_is_i2c_of_match,
 		.name		= "fimc-isp-i2c",
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-param.c b/drivers/media/platform/samsung/exynos4-is/fimc-is-param.c
index 9c816ae3b3e5..443362da8cc8 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-param.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-param.c
@@ -204,15 +204,6 @@ int __is_hw_update_params(struct fimc_is *is)
 	return ret;
 }
 
-void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
-{
-	struct isp_param *isp;
-
-	isp = &is->config[is->config_index].isp;
-	mf->width = isp->otf_input.width;
-	mf->height = isp->otf_input.height;
-}
-
 void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf)
 {
 	unsigned int index = is->config_index;
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-param.h b/drivers/media/platform/samsung/exynos4-is/fimc-is-param.h
index 206904674927..10ad02f36fed 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-param.h
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-param.h
@@ -994,7 +994,6 @@ void fimc_is_set_initial_params(struct fimc_is *is);
 unsigned int __get_pending_param_count(struct fimc_is *is);
 
 int  __is_hw_update_params(struct fimc_is *is);
-void __is_get_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
 void __is_set_frame_size(struct fimc_is *is, struct v4l2_mbus_framefmt *mf);
 void __is_set_sensor(struct fimc_is *is, int fps);
 void __is_set_isp_aa_ae(struct fimc_is *is);
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is-regs.c b/drivers/media/platform/samsung/exynos4-is/fimc-is-regs.c
index 366e6393817d..5f9c44e825a5 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is-regs.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is-regs.c
@@ -164,6 +164,7 @@ int fimc_is_hw_change_mode(struct fimc_is *is)
 	if (WARN_ON(is->config_index >= ARRAY_SIZE(cmd)))
 		return -EINVAL;
 
+	fimc_is_hw_wait_intmsr0_intmsd0(is);
 	mcuctl_write(cmd[is->config_index], is, MCUCTL_REG_ISSR(0));
 	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
 	mcuctl_write(is->setfile.sub_index, is, MCUCTL_REG_ISSR(2));
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-is.c b/drivers/media/platform/samsung/exynos4-is/fimc-is.c
index 39aab667910d..2e8fe9e49735 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-is.c
@@ -963,7 +963,7 @@ static const struct dev_pm_ops fimc_is_pm_ops = {
 
 static struct platform_driver fimc_is_driver = {
 	.probe		= fimc_is_probe,
-	.remove_new	= fimc_is_remove,
+	.remove		= fimc_is_remove,
 	.driver = {
 		.of_match_table	= fimc_is_of_match,
 		.name		= FIMC_IS_DRV_NAME,
@@ -999,4 +999,5 @@ module_exit(fimc_is_module_exit);
 MODULE_ALIAS("platform:" FIMC_IS_DRV_NAME);
 MODULE_AUTHOR("Younghwan Joo <yhwan.joo@samsung.com>");
 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
+MODULE_DESCRIPTION("Samsung EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-isp-video.c b/drivers/media/platform/samsung/exynos4-is/fimc-isp-video.c
index 06c4352562b3..ad219ac1b951 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-isp-video.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-isp-video.c
@@ -255,8 +255,6 @@ static const struct vb2_ops isp_video_capture_qops = {
 	.queue_setup	 = isp_video_capture_queue_setup,
 	.buf_prepare	 = isp_video_capture_buffer_prepare,
 	.buf_queue	 = isp_video_capture_buffer_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = isp_video_capture_start_streaming,
 	.stop_streaming	 = isp_video_capture_stop_streaming,
 };
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-lite.c b/drivers/media/platform/samsung/exynos4-is/fimc-lite.c
index d1d860fa3454..0ce293b0718b 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-lite.c
@@ -441,8 +441,6 @@ static const struct vb2_ops fimc_lite_qops = {
 	.queue_setup	 = queue_setup,
 	.buf_prepare	 = buffer_prepare,
 	.buf_queue	 = buffer_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming	 = stop_streaming,
 };
@@ -613,7 +611,7 @@ static void fimc_lite_try_crop(struct fimc_lite *fimc, struct v4l2_rect *r)
 	r->left = round_down(r->left, fimc->dd->win_hor_offs_align);
 	r->top  = clamp_t(u32, r->top, 0, frame->f_height - r->height);
 
-	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, sink fmt: %dx%d\n",
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%ux%u, sink fmt: %dx%d\n",
 		 r->left, r->top, r->width, r->height,
 		 frame->f_width, frame->f_height);
 }
@@ -633,7 +631,7 @@ static void fimc_lite_try_compose(struct fimc_lite *fimc, struct v4l2_rect *r)
 	r->left = round_down(r->left, fimc->dd->out_hor_offs_align);
 	r->top  = clamp_t(u32, r->top, 0, fimc->out_frame.f_height - r->height);
 
-	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%dx%d, source fmt: %dx%d\n",
+	v4l2_dbg(1, debug, &fimc->subdev, "(%d,%d)/%ux%u, source fmt: %dx%d\n",
 		 r->left, r->top, r->width, r->height,
 		 frame->f_width, frame->f_height);
 }
@@ -1142,7 +1140,7 @@ static int fimc_lite_subdev_get_selection(struct v4l2_subdev *sd,
 	}
 	mutex_unlock(&fimc->lock);
 
-	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d)/%ux%u, f_w: %d, f_h: %d\n",
 		 __func__, f->rect.left, f->rect.top, f->rect.width,
 		 f->rect.height, f->f_width, f->f_height);
 
@@ -1176,7 +1174,7 @@ static int fimc_lite_subdev_set_selection(struct v4l2_subdev *sd,
 	}
 	mutex_unlock(&fimc->lock);
 
-	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %d, f_h: %d\n",
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d)/%ux%u, f_w: %d, f_h: %d\n",
 		 __func__, f->rect.left, f->rect.top, f->rect.width,
 		 f->rect.height, f->f_width, f->f_height);
 
@@ -1654,7 +1652,7 @@ MODULE_DEVICE_TABLE(of, flite_of_match);
 
 static struct platform_driver fimc_lite_driver = {
 	.probe		= fimc_lite_probe,
-	.remove_new	= fimc_lite_remove,
+	.remove		= fimc_lite_remove,
 	.driver = {
 		.of_match_table = flite_of_match,
 		.name		= FIMC_LITE_DRV_NAME,
@@ -1662,5 +1660,6 @@ static struct platform_driver fimc_lite_driver = {
 	}
 };
 module_platform_driver(fimc_lite_driver);
+MODULE_DESCRIPTION("Samsung EXYNOS FIMC-LITE (camera host interface) driver");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS("platform:" FIMC_LITE_DRV_NAME);
diff --git a/drivers/media/platform/samsung/exynos4-is/fimc-m2m.c b/drivers/media/platform/samsung/exynos4-is/fimc-m2m.c
index 199997eec1cc..951433c8e92a 100644
--- a/drivers/media/platform/samsung/exynos4-is/fimc-m2m.c
+++ b/drivers/media/platform/samsung/exynos4-is/fimc-m2m.c
@@ -216,8 +216,6 @@ static const struct vb2_ops fimc_qops = {
 	.queue_setup	 = fimc_queue_setup,
 	.buf_prepare	 = fimc_buf_prepare,
 	.buf_queue	 = fimc_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.stop_streaming	 = stop_streaming,
 	.start_streaming = start_streaming,
 };
diff --git a/drivers/media/platform/samsung/exynos4-is/media-dev.c b/drivers/media/platform/samsung/exynos4-is/media-dev.c
index 5f10bb4eb4f7..b5ee3c547789 100644
--- a/drivers/media/platform/samsung/exynos4-is/media-dev.c
+++ b/drivers/media/platform/samsung/exynos4-is/media-dev.c
@@ -1564,7 +1564,7 @@ MODULE_DEVICE_TABLE(of, fimc_md_of_match);
 
 static struct platform_driver fimc_md_driver = {
 	.probe		= fimc_md_probe,
-	.remove_new	= fimc_md_remove,
+	.remove		= fimc_md_remove,
 	.driver = {
 		.of_match_table = of_match_ptr(fimc_md_of_match),
 		.name		= "s5p-fimc-md",
diff --git a/drivers/media/platform/samsung/exynos4-is/media-dev.h b/drivers/media/platform/samsung/exynos4-is/media-dev.h
index 786264cf79dc..ea496670d4b5 100644
--- a/drivers/media/platform/samsung/exynos4-is/media-dev.h
+++ b/drivers/media/platform/samsung/exynos4-is/media-dev.h
@@ -178,8 +178,9 @@ int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
 #ifdef CONFIG_OF
 static inline bool fimc_md_is_isp_available(struct device_node *node)
 {
-	node = of_get_child_by_name(node, FIMC_IS_OF_NODE_NAME);
-	return node ? of_device_is_available(node) : false;
+	struct device_node *child __free(device_node) =
+		of_get_available_child_by_name(node, FIMC_IS_OF_NODE_NAME);
+	return child;
 }
 #else
 #define fimc_md_is_isp_available(node) (false)
diff --git a/drivers/media/platform/samsung/exynos4-is/mipi-csis.c b/drivers/media/platform/samsung/exynos4-is/mipi-csis.c
index 4b9b20ba3504..452880b5350c 100644
--- a/drivers/media/platform/samsung/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/samsung/exynos4-is/mipi-csis.c
@@ -940,13 +940,19 @@ static int s5pcsis_pm_resume(struct device *dev, bool runtime)
 					       state->supplies);
 			goto unlock;
 		}
-		clk_enable(state->clock[CSIS_CLK_GATE]);
+		ret = clk_enable(state->clock[CSIS_CLK_GATE]);
+		if (ret) {
+			phy_power_off(state->phy);
+			regulator_bulk_disable(CSIS_NUM_SUPPLIES,
+					       state->supplies);
+			goto unlock;
+		}
 	}
 	if (state->flags & ST_STREAMING)
 		s5pcsis_start_stream(state);
 
 	state->flags &= ~ST_SUSPENDED;
- unlock:
+unlock:
 	mutex_unlock(&state->lock);
 	return ret ? -EAGAIN : 0;
 }
@@ -1020,7 +1026,7 @@ MODULE_DEVICE_TABLE(of, s5pcsis_of_match);
 
 static struct platform_driver s5pcsis_driver = {
 	.probe		= s5pcsis_probe,
-	.remove_new	= s5pcsis_remove,
+	.remove		= s5pcsis_remove,
 	.driver		= {
 		.of_match_table = s5pcsis_of_match,
 		.name		= CSIS_DRIVER_NAME,
diff --git a/drivers/media/platform/samsung/s3c-camif/camif-capture.c b/drivers/media/platform/samsung/s3c-camif/camif-capture.c
index be58260ea67e..3e566b65f417 100644
--- a/drivers/media/platform/samsung/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/samsung/s3c-camif/camif-capture.c
@@ -525,8 +525,6 @@ static const struct vb2_ops s3c_camif_qops = {
 	.queue_setup	 = queue_setup,
 	.buf_prepare	 = buffer_prepare,
 	.buf_queue	 = buffer_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = start_streaming,
 	.stop_streaming	 = stop_streaming,
 };
@@ -1032,9 +1030,9 @@ static int s3c_camif_s_selection(struct file *file, void *priv,
 	vp->state |= ST_VP_CONFIG;
 	spin_unlock_irqrestore(&camif->slock, flags);
 
-	pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%dx%d\n",
-		sel->type, sel->target, sel->flags,
-		sel->r.left, sel->r.top, sel->r.width, sel->r.height);
+	pr_debug("type: %#x, target: %#x, flags: %#x, (%d,%d)/%ux%u\n",
+		 sel->type, sel->target, sel->flags,
+		 sel->r.left, sel->r.top, sel->r.width, sel->r.height);
 
 	return 0;
 }
@@ -1374,7 +1372,7 @@ static int s3c_camif_subdev_get_selection(struct v4l2_subdev *sd,
 
 	mutex_unlock(&camif->lock);
 
-	v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d) %dx%d, size: %ux%u\n",
+	v4l2_dbg(1, debug, sd, "%s: crop: (%d,%d)/%ux%u, size: %ux%u\n",
 		 __func__, crop->left, crop->top, crop->width,
 		 crop->height, mf->width, mf->height);
 
@@ -1426,7 +1424,7 @@ static void __camif_try_crop(struct camif_dev *camif, struct v4l2_rect *r)
 		}
 	}
 
-	v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%dx%d, fmt: %ux%u\n",
+	v4l2_dbg(1, debug, &camif->v4l2_dev, "crop: (%d,%d)/%ux%u, fmt: %ux%u\n",
 		 r->left, r->top, r->width, r->height, mf->width, mf->height);
 }
 
@@ -1466,7 +1464,7 @@ static int s3c_camif_subdev_set_selection(struct v4l2_subdev *sd,
 	}
 	mutex_unlock(&camif->lock);
 
-	v4l2_dbg(1, debug, sd, "%s: (%d,%d) %dx%d, f_w: %u, f_h: %u\n",
+	v4l2_dbg(1, debug, sd, "%s: (%d,%d)/%ux%u, f_w: %u, f_h: %u\n",
 		 __func__, crop->left, crop->top, crop->width, crop->height,
 		 camif->mbus_fmt.width, camif->mbus_fmt.height);
 
diff --git a/drivers/media/platform/samsung/s3c-camif/camif-core.c b/drivers/media/platform/samsung/s3c-camif/camif-core.c
index e4529f666e20..221e3c447f36 100644
--- a/drivers/media/platform/samsung/s3c-camif/camif-core.c
+++ b/drivers/media/platform/samsung/s3c-camif/camif-core.c
@@ -527,10 +527,19 @@ static void s3c_camif_remove(struct platform_device *pdev)
 static int s3c_camif_runtime_resume(struct device *dev)
 {
 	struct camif_dev *camif = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_enable(camif->clock[CLK_GATE]);
+	if (ret)
+		return ret;
 
-	clk_enable(camif->clock[CLK_GATE]);
 	/* null op on s3c244x */
-	clk_enable(camif->clock[CLK_CAM]);
+	ret = clk_enable(camif->clock[CLK_CAM]);
+	if (ret) {
+		clk_disable(camif->clock[CLK_GATE]);
+		return ret;
+	}
+
 	return 0;
 }
 
@@ -622,7 +631,7 @@ static const struct dev_pm_ops s3c_camif_pm_ops = {
 
 static struct platform_driver s3c_camif_driver = {
 	.probe		= s3c_camif_probe,
-	.remove_new	= s3c_camif_remove,
+	.remove		= s3c_camif_remove,
 	.id_table	= s3c_camif_driver_ids,
 	.driver = {
 		.name	= S3C_CAMIF_DRIVER_NAME,
diff --git a/drivers/media/platform/samsung/s5p-g2d/g2d.c b/drivers/media/platform/samsung/s5p-g2d/g2d.c
index 89aeba47ed07..ffed16a34493 100644
--- a/drivers/media/platform/samsung/s5p-g2d/g2d.c
+++ b/drivers/media/platform/samsung/s5p-g2d/g2d.c
@@ -133,8 +133,6 @@ static const struct vb2_ops g2d_qops = {
 	.queue_setup	= g2d_queue_setup,
 	.buf_prepare	= g2d_buf_prepare,
 	.buf_queue	= g2d_buf_queue,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -777,7 +775,7 @@ MODULE_DEVICE_TABLE(of, exynos_g2d_match);
 
 static struct platform_driver g2d_pdrv = {
 	.probe		= g2d_probe,
-	.remove_new	= g2d_remove,
+	.remove		= g2d_remove,
 	.driver		= {
 		.name = G2D_NAME,
 		.of_match_table = exynos_g2d_match,
diff --git a/drivers/media/platform/samsung/s5p-jpeg/jpeg-core.c b/drivers/media/platform/samsung/s5p-jpeg/jpeg-core.c
index d2c4a0178b3c..ac4cf269456a 100644
--- a/drivers/media/platform/samsung/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/samsung/s5p-jpeg/jpeg-core.c
@@ -775,11 +775,14 @@ static void exynos4_jpeg_parse_decode_h_tbl(struct s5p_jpeg_ctx *ctx)
 		(unsigned long)vb2_plane_vaddr(&vb->vb2_buf, 0) + ctx->out_q.sos + 2;
 	jpeg_buffer.curr = 0;
 
-	word = 0;
-
 	if (get_word_be(&jpeg_buffer, &word))
 		return;
-	jpeg_buffer.size = (long)word - 2;
+
+	if (word < 2)
+		jpeg_buffer.size = 0;
+	else
+		jpeg_buffer.size = (long)word - 2;
+
 	jpeg_buffer.data += 2;
 	jpeg_buffer.curr = 0;
 
@@ -1058,6 +1061,7 @@ static int get_word_be(struct s5p_jpeg_buffer *buf, unsigned int *word)
 	if (byte == -1)
 		return -1;
 	*word = (unsigned int)byte | temp;
+
 	return 0;
 }
 
@@ -1145,7 +1149,7 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 			if (get_word_be(&jpeg_buffer, &word))
 				break;
 			length = (long)word - 2;
-			if (!length)
+			if (length <= 0)
 				return false;
 			sof = jpeg_buffer.curr; /* after 0xffc0 */
 			sof_len = length;
@@ -1176,7 +1180,7 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 			if (get_word_be(&jpeg_buffer, &word))
 				break;
 			length = (long)word - 2;
-			if (!length)
+			if (length <= 0)
 				return false;
 			if (n_dqt >= S5P_JPEG_MAX_MARKER)
 				return false;
@@ -1189,7 +1193,7 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 			if (get_word_be(&jpeg_buffer, &word))
 				break;
 			length = (long)word - 2;
-			if (!length)
+			if (length <= 0)
 				return false;
 			if (n_dht >= S5P_JPEG_MAX_MARKER)
 				return false;
@@ -1214,6 +1218,7 @@ static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 			if (get_word_be(&jpeg_buffer, &word))
 				break;
 			length = (long)word - 2;
+			/* No need to check underflows as skip() does it  */
 			skip(&jpeg_buffer, length);
 			break;
 		}
@@ -2590,8 +2595,6 @@ static const struct vb2_ops s5p_jpeg_qops = {
 	.queue_setup		= s5p_jpeg_queue_setup,
 	.buf_prepare		= s5p_jpeg_buf_prepare,
 	.buf_queue		= s5p_jpeg_buf_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.start_streaming	= s5p_jpeg_start_streaming,
 	.stop_streaming		= s5p_jpeg_stop_streaming,
 };
@@ -3160,7 +3163,7 @@ static void *jpeg_get_drv_data(struct device *dev)
 
 static struct platform_driver s5p_jpeg_driver = {
 	.probe = s5p_jpeg_probe,
-	.remove_new = s5p_jpeg_remove,
+	.remove = s5p_jpeg_remove,
 	.driver = {
 		.of_match_table	= samsung_jpeg_match,
 		.name		= S5P_JPEG_M2M_NAME,
diff --git a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.c b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.c
index 637a5104d948..6657d294c10a 100644
--- a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.c
+++ b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.c
@@ -427,11 +427,6 @@ void exynos3250_jpeg_clear_int_status(void __iomem *regs,
 	writel(value, regs + EXYNOS3250_JPGINTST);
 }
 
-unsigned int exynos3250_jpeg_operating(void __iomem *regs)
-{
-	return readl(regs + S5P_JPGOPR) & EXYNOS3250_JPGOPR_MASK;
-}
-
 unsigned int exynos3250_jpeg_compressed_size(void __iomem *regs)
 {
 	return readl(regs + EXYNOS3250_JPGCNT) & EXYNOS3250_JPGCNT_MASK;
diff --git a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.h b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.h
index 15af928fad76..709c61ae322c 100644
--- a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.h
+++ b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos3250.h
@@ -45,7 +45,6 @@ void exynos3250_jpeg_rstart(void __iomem *regs);
 unsigned int exynos3250_jpeg_get_int_status(void __iomem *regs);
 void exynos3250_jpeg_clear_int_status(void __iomem *regs,
 						unsigned int value);
-unsigned int exynos3250_jpeg_operating(void __iomem *regs);
 unsigned int exynos3250_jpeg_compressed_size(void __iomem *regs);
 void exynos3250_jpeg_dec_stream_size(void __iomem *regs, unsigned int size);
 void exynos3250_jpeg_dec_scaling_ratio(void __iomem *regs, unsigned int sratio);
diff --git a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.c b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.c
index 0828cfa783fe..479288fc8c77 100644
--- a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.c
+++ b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.c
@@ -185,11 +185,6 @@ unsigned int exynos4_jpeg_get_int_status(void __iomem *base)
 	return readl(base + EXYNOS4_INT_STATUS_REG);
 }
 
-unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base)
-{
-	return readl(base + EXYNOS4_FIFO_STATUS_REG);
-}
-
 void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value)
 {
 	unsigned int	reg;
@@ -300,22 +295,8 @@ void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size)
 	writel(size, base + EXYNOS4_BITSTREAM_SIZE_REG);
 }
 
-void exynos4_jpeg_get_frame_size(void __iomem *base,
-			unsigned int *width, unsigned int *height)
-{
-	*width = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) &
-				EXYNOS4_DECODED_SIZE_MASK);
-	*height = (readl(base + EXYNOS4_DECODE_XY_SIZE_REG) >> 16) &
-				EXYNOS4_DECODED_SIZE_MASK;
-}
-
 unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base)
 {
 	return readl(base + EXYNOS4_DECODE_IMG_FMT_REG) &
 				EXYNOS4_JPEG_DECODED_IMG_FMT_MASK;
 }
-
-void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size)
-{
-	writel(size, base + EXYNOS4_INT_TIMER_COUNT_REG);
-}
diff --git a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.h b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.h
index 3e2887526960..b941cc89e4ba 100644
--- a/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.h
+++ b/drivers/media/platform/samsung/s5p-jpeg/jpeg-hw-exynos4.h
@@ -35,10 +35,6 @@ void exynos4_jpeg_select_dec_h_tbl(void __iomem *base, char c, char x);
 void exynos4_jpeg_set_encode_hoff_cnt(void __iomem *base, unsigned int fmt);
 void exynos4_jpeg_set_dec_bitstream_size(void __iomem *base, unsigned int size);
 unsigned int exynos4_jpeg_get_stream_size(void __iomem *base);
-void exynos4_jpeg_get_frame_size(void __iomem *base,
-			unsigned int *width, unsigned int *height);
 unsigned int exynos4_jpeg_get_frame_fmt(void __iomem *base);
-unsigned int exynos4_jpeg_get_fifo_status(void __iomem *base);
-void exynos4_jpeg_set_timer_count(void __iomem *base, unsigned int size);
 
 #endif /* JPEG_HW_EXYNOS4_H_ */
diff --git a/drivers/media/platform/samsung/s5p-mfc/regs-mfc-v6.h b/drivers/media/platform/samsung/s5p-mfc/regs-mfc-v6.h
index fa49fe580e1a..075a58b50b8c 100644
--- a/drivers/media/platform/samsung/s5p-mfc/regs-mfc-v6.h
+++ b/drivers/media/platform/samsung/s5p-mfc/regs-mfc-v6.h
@@ -45,6 +45,7 @@
 #define S5P_FIMV_H2R_CMD_WAKEUP_V6		8
 #define S5P_FIMV_CH_LAST_FRAME_V6		9
 #define S5P_FIMV_H2R_CMD_FLUSH_V6		10
+#define S5P_FIMV_H2R_CMD_NAL_ABORT_V6		11
 /* RMVME: REALLOC used? */
 #define S5P_FIMV_CH_FRAME_START_REALLOC_V6	5
 
diff --git a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc.c b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc.c
index 50451984d59f..73fdcd362265 100644
--- a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc.c
@@ -143,7 +143,7 @@ void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
 
 static void s5p_mfc_watchdog(struct timer_list *t)
 {
-	struct s5p_mfc_dev *dev = from_timer(dev, t, watchdog_timer);
+	struct s5p_mfc_dev *dev = timer_container_of(dev, t, watchdog_timer);
 
 	if (test_bit(0, &dev->hw_lock))
 		atomic_inc(&dev->watchdog_cnt);
@@ -739,6 +739,20 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 		ctx->state = MFCINST_RUNNING;
 		goto irq_cleanup_hw;
 
+	case S5P_MFC_R2H_CMD_ENC_BUFFER_FUL_RET:
+		ctx->state = MFCINST_NAL_ABORT;
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+		set_work_bit(ctx);
+		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		break;
+
+	case S5P_MFC_R2H_CMD_NAL_ABORT_RET:
+		ctx->state = MFCINST_ERROR;
+		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
+		goto irq_cleanup_hw;
+
 	default:
 		mfc_debug(2, "Unknown int reason\n");
 		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
@@ -774,8 +788,10 @@ static int s5p_mfc_open(struct file *file)
 	int ret = 0;
 
 	mfc_debug_enter();
-	if (mutex_lock_interruptible(&dev->mfc_mutex))
-		return -ERESTARTSYS;
+	if (mutex_lock_interruptible(&dev->mfc_mutex)) {
+		ret = -ERESTARTSYS;
+		goto err_enter;
+	}
 	dev->num_inst++;	/* It is guarded by mfc_mutex in vfd */
 	/* Allocate memory for context */
 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
@@ -933,7 +949,7 @@ err_pwr_enable:
 	if (dev->num_inst == 1) {
 		if (s5p_mfc_power_off(dev) < 0)
 			mfc_err("power off failed\n");
-		del_timer_sync(&dev->watchdog_timer);
+		timer_delete_sync(&dev->watchdog_timer);
 	}
 err_ctrls_setup:
 	s5p_mfc_dec_ctrls_delete(ctx);
@@ -946,6 +962,7 @@ err_no_ctx:
 err_alloc:
 	dev->num_inst--;
 	mutex_unlock(&dev->mfc_mutex);
+err_enter:
 	mfc_debug_leave();
 	return ret;
 }
@@ -982,7 +999,7 @@ static int s5p_mfc_release(struct file *file)
 		if (dev->num_inst == 0) {
 			mfc_debug(2, "Last instance\n");
 			s5p_mfc_deinit_hw(dev);
-			del_timer_sync(&dev->watchdog_timer);
+			timer_delete_sync(&dev->watchdog_timer);
 			s5p_mfc_clock_off(dev);
 			if (s5p_mfc_power_off(dev) < 0)
 				mfc_err("Power off failed\n");
@@ -1458,7 +1475,7 @@ static void s5p_mfc_remove(struct platform_device *pdev)
 	}
 	mutex_unlock(&dev->mfc_mutex);
 
-	del_timer_sync(&dev->watchdog_timer);
+	timer_delete_sync(&dev->watchdog_timer);
 	flush_work(&dev->watchdog_work);
 
 	video_unregister_device(dev->vfd_enc);
@@ -1721,7 +1738,7 @@ MODULE_DEVICE_TABLE(of, exynos_mfc_match);
 
 static struct platform_driver s5p_mfc_driver = {
 	.probe		= s5p_mfc_probe,
-	.remove_new	= s5p_mfc_remove,
+	.remove		= s5p_mfc_remove,
 	.driver	= {
 		.name	= S5P_MFC_NAME,
 		.pm	= &s5p_mfc_pm_ops,
diff --git a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_common.h
index 3cc2a4f5c40a..86c316c1ff8f 100644
--- a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_common.h
@@ -141,6 +141,7 @@ enum s5p_mfc_inst_state {
 	MFCINST_RES_CHANGE_INIT,
 	MFCINST_RES_CHANGE_FLUSH,
 	MFCINST_RES_CHANGE_END,
+	MFCINST_NAL_ABORT,
 };
 
 /*
diff --git a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_dec.c
index 91e102d4ec4e..3efbc3367906 100644
--- a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_dec.c
@@ -1161,8 +1161,6 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops s5p_mfc_dec_qops = {
 	.queue_setup		= s5p_mfc_queue_setup,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.buf_init		= s5p_mfc_buf_init,
 	.start_streaming	= s5p_mfc_start_streaming,
 	.stop_streaming		= s5p_mfc_stop_streaming,
diff --git a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_enc.c
index 81cbb36fb382..6c603dcd5664 100644
--- a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_enc.c
@@ -2652,8 +2652,6 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 
 static const struct vb2_ops s5p_mfc_enc_qops = {
 	.queue_setup		= s5p_mfc_queue_setup,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.buf_init		= s5p_mfc_buf_init,
 	.buf_prepare		= s5p_mfc_buf_prepare,
 	.start_streaming	= s5p_mfc_start_streaming,
diff --git a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_opr_v6.c
index 73f7af674c01..98f8292b3173 100644
--- a/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/samsung/s5p-mfc/s5p_mfc_opr_v6.c
@@ -549,8 +549,9 @@ static void s5p_mfc_enc_calc_src_size_v6(struct s5p_mfc_ctx *ctx)
 		case V4L2_PIX_FMT_NV21M:
 			ctx->stride[0] = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
 			ctx->stride[1] = ALIGN(ctx->img_width, S5P_FIMV_NV12M_HALIGN_V6);
-			ctx->luma_size = ctx->stride[0] * ALIGN(ctx->img_height, 16);
-			ctx->chroma_size =  ctx->stride[0] * ALIGN(ctx->img_height / 2, 16);
+			ctx->luma_size = ALIGN(ctx->stride[0] * ALIGN(ctx->img_height, 16), 256);
+			ctx->chroma_size = ALIGN(ctx->stride[0] * ALIGN(ctx->img_height / 2, 16),
+					256);
 			break;
 		case V4L2_PIX_FMT_YUV420M:
 		case V4L2_PIX_FMT_YVU420M:
@@ -2228,6 +2229,11 @@ static void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 		case MFCINST_HEAD_PRODUCED:
 			ret = s5p_mfc_run_init_enc_buffers(ctx);
 			break;
+		case MFCINST_NAL_ABORT:
+			mfc_write(dev, ctx->inst_no, S5P_FIMV_INSTANCE_ID_V6);
+			s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc,
+					dev, S5P_FIMV_H2R_CMD_NAL_ABORT_V6, NULL);
+			break;
 		default:
 			ret = -EAGAIN;
 		}
diff --git a/drivers/media/platform/st/sti/bdisp/bdisp-debug.c b/drivers/media/platform/st/sti/bdisp/bdisp-debug.c
index a27f638df11c..f9348aeacc11 100644
--- a/drivers/media/platform/st/sti/bdisp/bdisp-debug.c
+++ b/drivers/media/platform/st/sti/bdisp/bdisp-debug.c
@@ -455,11 +455,11 @@ static int last_request_show(struct seq_file *s, void *data)
 
 	seq_printf(s, "Format:    %s\t\t\t%s\n",
 		   bdisp_fmt_to_str(src), bdisp_fmt_to_str(dst));
-	seq_printf(s, "Crop area: %dx%d @ %d,%d  ==>\t%dx%d @ %d,%d\n",
-		   src.crop.width, src.crop.height,
+	seq_printf(s, "Crop area: (%d,%d)/%ux%u  ==>\t(%d,%d)/%ux%u\n",
 		   src.crop.left, src.crop.top,
-		   dst.crop.width, dst.crop.height,
-		   dst.crop.left, dst.crop.top);
+		   src.crop.width, src.crop.height,
+		   dst.crop.left, dst.crop.top,
+		   dst.crop.width, dst.crop.height);
 	seq_printf(s, "Buff size: %dx%d\t\t%dx%d\n\n",
 		   src.width, src.height, dst.width, dst.height);
 
diff --git a/drivers/media/platform/st/sti/bdisp/bdisp-v4l2.c b/drivers/media/platform/st/sti/bdisp/bdisp-v4l2.c
index 1328b4eb6b9f..1eb934490c0b 100644
--- a/drivers/media/platform/st/sti/bdisp/bdisp-v4l2.c
+++ b/drivers/media/platform/st/sti/bdisp/bdisp-v4l2.c
@@ -531,8 +531,6 @@ static const struct vb2_ops bdisp_qops = {
 	.queue_setup     = bdisp_queue_setup,
 	.buf_prepare     = bdisp_buf_prepare,
 	.buf_queue       = bdisp_buf_queue,
-	.wait_prepare    = vb2_ops_wait_prepare,
-	.wait_finish     = vb2_ops_wait_finish,
 	.stop_streaming  = bdisp_stop_streaming,
 	.start_streaming = bdisp_start_streaming,
 };
@@ -955,8 +953,8 @@ static int bdisp_s_selection(struct file *file, void *fh,
 	if ((out.left < 0) || (out.left >= frame->width) ||
 	    (out.top < 0) || (out.top >= frame->height)) {
 		dev_err(ctx->bdisp_dev->dev,
-			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
-			out.width, out.height, out.left, out.top,
+			"Invalid crop: (%d,%d)/%ux%u vs frame: %dx%d\n",
+			out.left, out.top, out.width, out.height,
 			frame->width, frame->height);
 		return -EINVAL;
 	}
@@ -968,8 +966,8 @@ static int bdisp_s_selection(struct file *file, void *fh,
 	if (((out.left + out.width) > frame->width) ||
 	    ((out.top + out.height) > frame->height)) {
 		dev_err(ctx->bdisp_dev->dev,
-			"Invalid crop: %dx%d@(%d,%d) vs frame: %dx%d\n",
-			out.width, out.height, out.left, out.top,
+			"Invalid crop: (%d,%d)/%ux%u vs frame: %dx%d\n",
+			out.left, out.top, out.width, out.height,
 			frame->width, frame->height);
 		return -EINVAL;
 	}
@@ -984,9 +982,9 @@ static int bdisp_s_selection(struct file *file, void *fh,
 	if ((out.left != in->left) || (out.top != in->top) ||
 	    (out.width != in->width) || (out.height != in->height)) {
 		dev_dbg(ctx->bdisp_dev->dev,
-			"%s crop updated: %dx%d@(%d,%d) -> %dx%d@(%d,%d)\n",
-			__func__, in->width, in->height, in->left, in->top,
-			out.width, out.height, out.left, out.top);
+			"%s crop updated: (%d,%d)/%ux%u -> (%d,%d)/%ux%u\n",
+			__func__, in->left, in->top, in->width, in->height,
+			out.left, out.top, out.width, out.height);
 		*in = out;
 	}
 
@@ -1160,7 +1158,7 @@ static void bdisp_irq_timeout(struct work_struct *ptr)
 static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
 {
 	unsigned long flags;
-	int timeout;
+	long time_left;
 
 	spin_lock_irqsave(&bdisp->slock, flags);
 	if (!test_bit(ST_M2M_RUNNING, &bdisp->state)) {
@@ -1171,13 +1169,13 @@ static int bdisp_m2m_suspend(struct bdisp_dev *bdisp)
 	set_bit(ST_M2M_SUSPENDING, &bdisp->state);
 	spin_unlock_irqrestore(&bdisp->slock, flags);
 
-	timeout = wait_event_timeout(bdisp->irq_queue,
-				     test_bit(ST_M2M_SUSPENDED, &bdisp->state),
-				     BDISP_WORK_TIMEOUT);
+	time_left = wait_event_timeout(bdisp->irq_queue,
+				       test_bit(ST_M2M_SUSPENDED, &bdisp->state),
+				       BDISP_WORK_TIMEOUT);
 
 	clear_bit(ST_M2M_SUSPENDING, &bdisp->state);
 
-	if (!timeout) {
+	if (!time_left) {
 		dev_err(bdisp->dev, "%s IRQ timeout\n", __func__);
 		return -EAGAIN;
 	}
@@ -1411,7 +1409,7 @@ MODULE_DEVICE_TABLE(of, bdisp_match_types);
 
 static struct platform_driver bdisp_driver = {
 	.probe          = bdisp_probe,
-	.remove_new     = bdisp_remove,
+	.remove         = bdisp_remove,
 	.driver         = {
 		.name           = BDISP_NAME,
 		.of_match_table = bdisp_match_types,
diff --git a/drivers/media/platform/st/sti/c8sectpfe/Kconfig b/drivers/media/platform/st/sti/c8sectpfe/Kconfig
index 702b910509c9..01c33d9c9ec3 100644
--- a/drivers/media/platform/st/sti/c8sectpfe/Kconfig
+++ b/drivers/media/platform/st/sti/c8sectpfe/Kconfig
@@ -5,7 +5,6 @@ config DVB_C8SECTPFE
 	depends on PINCTRL && DVB_CORE && I2C
 	depends on ARCH_STI || ARCH_MULTIPLATFORM || COMPILE_TEST
 	select FW_LOADER
-	select DEBUG_FS
 	select DVB_LNBP21 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_STV090x if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_STB6100 if MEDIA_SUBDRV_AUTOSELECT
diff --git a/drivers/media/platform/st/sti/c8sectpfe/Makefile b/drivers/media/platform/st/sti/c8sectpfe/Makefile
index aedfc725cc19..99425137ee0a 100644
--- a/drivers/media/platform/st/sti/c8sectpfe/Makefile
+++ b/drivers/media/platform/st/sti/c8sectpfe/Makefile
@@ -1,6 +1,9 @@
 # SPDX-License-Identifier: GPL-2.0
-c8sectpfe-y += c8sectpfe-core.o c8sectpfe-common.o c8sectpfe-dvb.o \
-		c8sectpfe-debugfs.o
+c8sectpfe-y += c8sectpfe-core.o c8sectpfe-common.o c8sectpfe-dvb.o
+
+ifneq ($(CONFIG_DEBUG_FS),)
+c8sectpfe-y += c8sectpfe-debugfs.o
+endif
 
 obj-$(CONFIG_DVB_C8SECTPFE) += c8sectpfe.o
 
diff --git a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.c b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.c
index e4cf27b5a072..602c37cbe177 100644
--- a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.c
+++ b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.c
@@ -24,7 +24,6 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/module.h>
-#include <linux/of_gpio.h>
 #include <linux/of_platform.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/pinctrl/pinctrl.h>
@@ -63,7 +62,7 @@ static int load_c8sectpfe_fw(struct c8sectpfei *fei);
 
 static void c8sectpfe_timer_interrupt(struct timer_list *t)
 {
-	struct c8sectpfei *fei = from_timer(fei, t, timer);
+	struct c8sectpfei *fei = timer_container_of(fei, t, timer);
 	struct channel_info *channel;
 	int chan_num;
 
@@ -73,16 +72,16 @@ static void c8sectpfe_timer_interrupt(struct timer_list *t)
 
 		/* is this descriptor initialised and TP enabled */
 		if (channel->irec && readl(channel->irec + DMA_PRDS_TPENABLE))
-			tasklet_schedule(&channel->tsklet);
+			queue_work(system_bh_wq, &channel->bh_work);
 	}
 
 	fei->timer.expires = jiffies +	msecs_to_jiffies(POLL_MSECS);
 	add_timer(&fei->timer);
 }
 
-static void channel_swdemux_tsklet(struct tasklet_struct *t)
+static void channel_swdemux_bh_work(struct work_struct *t)
 {
-	struct channel_info *channel = from_tasklet(channel, t, tsklet);
+	struct channel_info *channel = from_work(channel, t, bh_work);
 	struct c8sectpfei *fei;
 	unsigned long wp, rp;
 	int pos, num_packets, n, size;
@@ -211,7 +210,7 @@ static int c8sectpfe_start_feed(struct dvb_demux_feed *dvbdmxfeed)
 
 		dev_dbg(fei->dev, "Starting channel=%p\n", channel);
 
-		tasklet_setup(&channel->tsklet, channel_swdemux_tsklet);
+		INIT_WORK(&channel->bh_work, channel_swdemux_bh_work);
 
 		/* Reset the internal inputblock sram pointers */
 		writel(channel->fifo,
@@ -304,7 +303,7 @@ static int c8sectpfe_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 		/* disable this channels descriptor */
 		writel(0,  channel->irec + DMA_PRDS_TPENABLE);
 
-		tasklet_disable(&channel->tsklet);
+		disable_work_sync(&channel->bh_work);
 
 		/* now request memdma channel goes idle */
 		idlereq = (1 << channel->tsin_id) | IDLEREQ;
@@ -352,7 +351,7 @@ static int c8sectpfe_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 		dev_dbg(fei->dev, "%s:%d global_feed_count=%d\n"
 			, __func__, __LINE__, fei->global_feed_count);
 
-		del_timer(&fei->timer);
+		timer_delete(&fei->timer);
 	}
 
 	mutex_unlock(&fei->lock);
@@ -631,8 +630,8 @@ static int configure_memdma_and_inputblock(struct c8sectpfei *fei,
 	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSWP_TP(0));
 	writel(tsin->back_buffer_busaddr, tsin->irec + DMA_PRDS_BUSRP_TP(0));
 
-	/* initialize tasklet */
-	tasklet_setup(&tsin->tsklet, channel_swdemux_tsklet);
+	/* initialize bh work */
+	INIT_WORK(&tsin->bh_work, channel_swdemux_bh_work);
 
 	return 0;
 
@@ -798,13 +797,12 @@ static int c8sectpfe_probe(struct platform_device *pdev)
 		}
 		tsin->i2c_adapter =
 			of_find_i2c_adapter_by_node(i2c_bus);
+		of_node_put(i2c_bus);
 		if (!tsin->i2c_adapter) {
 			dev_err(&pdev->dev, "No i2c adapter found\n");
-			of_node_put(i2c_bus);
 			ret = -ENODEV;
 			goto err_node_put;
 		}
-		of_node_put(i2c_bus);
 
 		/* Acquire reset GPIO and activate it */
 		tsin->rst_gpio = devm_fwnode_gpiod_get(dev,
@@ -1097,7 +1095,6 @@ static int load_slim_core_fw(const struct firmware *fw, struct c8sectpfei *fei)
 		}
 	}
 
-	release_firmware(fw);
 	return err;
 }
 
@@ -1121,6 +1118,7 @@ static int load_c8sectpfe_fw(struct c8sectpfei *fei)
 	}
 
 	err = load_slim_core_fw(fw, fei);
+	release_firmware(fw);
 	if (err) {
 		dev_err(fei->dev, "load_slim_core_fw failed err=(%d)\n", err);
 		return err;
@@ -1159,7 +1157,7 @@ static struct platform_driver c8sectpfe_driver = {
 		.of_match_table = c8sectpfe_match,
 	},
 	.probe	= c8sectpfe_probe,
-	.remove_new = c8sectpfe_remove,
+	.remove = c8sectpfe_remove,
 };
 
 module_platform_driver(c8sectpfe_driver);
diff --git a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.h b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.h
index bf377cc82225..c1b124c6ef12 100644
--- a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.h
+++ b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-core.h
@@ -51,7 +51,7 @@ struct channel_info {
 	unsigned long  fifo;
 
 	struct completion idle_completion;
-	struct tasklet_struct tsklet;
+	struct work_struct bh_work;
 
 	struct c8sectpfei *fei;
 	void __iomem *irec;
diff --git a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-debugfs.h b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-debugfs.h
index d2c35fb32d7e..3fe177b59b16 100644
--- a/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-debugfs.h
+++ b/drivers/media/platform/st/sti/c8sectpfe/c8sectpfe-debugfs.h
@@ -12,7 +12,12 @@
 
 #include "c8sectpfe-core.h"
 
+#if defined(CONFIG_DEBUG_FS)
 void c8sectpfe_debugfs_init(struct c8sectpfei *);
 void c8sectpfe_debugfs_exit(struct c8sectpfei *);
+#else
+static inline void c8sectpfe_debugfs_init(struct c8sectpfei *fei) {};
+static inline void c8sectpfe_debugfs_exit(struct c8sectpfei *fei) {};
+#endif
 
 #endif /* __C8SECTPFE_DEBUG_H */
diff --git a/drivers/media/platform/st/sti/delta/delta-debug.c b/drivers/media/platform/st/sti/delta/delta-debug.c
index 4b2eb6b63aa2..6acf46913cda 100644
--- a/drivers/media/platform/st/sti/delta/delta-debug.c
+++ b/drivers/media/platform/st/sti/delta/delta-debug.c
@@ -16,14 +16,14 @@ char *delta_streaminfo_str(struct delta_streaminfo *s, char *str,
 		return NULL;
 
 	snprintf(str, len,
-		 "%4.4s %dx%d %s %s dpb=%d %s %s %s%dx%d@(%d,%d) %s%d/%d",
+		 "%4.4s %dx%d %s %s dpb=%d %s %s %s(%d,%d)/%ux%u %s%d/%d",
 		 (char *)&s->streamformat, s->width, s->height,
 		 s->profile, s->level, s->dpb,
 		 (s->field == V4L2_FIELD_NONE) ? "progressive" : "interlaced",
 		 s->other,
 		 s->flags & DELTA_STREAMINFO_FLAG_CROP ? "crop=" : "",
-		 s->crop.width, s->crop.height,
 		 s->crop.left, s->crop.top,
+		 s->crop.width, s->crop.height,
 		 s->flags & DELTA_STREAMINFO_FLAG_PIXELASPECT ? "par=" : "",
 		 s->pixelaspect.numerator,
 		 s->pixelaspect.denominator);
@@ -38,13 +38,13 @@ char *delta_frameinfo_str(struct delta_frameinfo *f, char *str,
 		return NULL;
 
 	snprintf(str, len,
-		 "%4.4s %dx%d aligned %dx%d %s %s%dx%d@(%d,%d) %s%d/%d",
+		 "%4.4s %dx%d aligned %dx%d %s %s(%d,%d)/%ux%u %s%d/%d",
 		 (char *)&f->pixelformat, f->width, f->height,
 		 f->aligned_width, f->aligned_height,
 		 (f->field == V4L2_FIELD_NONE) ? "progressive" : "interlaced",
 		 f->flags & DELTA_STREAMINFO_FLAG_CROP ? "crop=" : "",
-		 f->crop.width, f->crop.height,
 		 f->crop.left, f->crop.top,
+		 f->crop.width, f->crop.height,
 		 f->flags & DELTA_STREAMINFO_FLAG_PIXELASPECT ? "par=" : "",
 		 f->pixelaspect.numerator,
 		 f->pixelaspect.denominator);
diff --git a/drivers/media/platform/st/sti/delta/delta-v4l2.c b/drivers/media/platform/st/sti/delta/delta-v4l2.c
index da402d1e9171..196e6a40335d 100644
--- a/drivers/media/platform/st/sti/delta/delta-v4l2.c
+++ b/drivers/media/platform/st/sti/delta/delta-v4l2.c
@@ -1559,8 +1559,6 @@ static const struct vb2_ops delta_vb2_au_ops = {
 	.queue_setup = delta_vb2_au_queue_setup,
 	.buf_prepare = delta_vb2_au_prepare,
 	.buf_queue = delta_vb2_au_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = delta_vb2_au_start_streaming,
 	.stop_streaming = delta_vb2_au_stop_streaming,
 };
@@ -1570,8 +1568,6 @@ static const struct vb2_ops delta_vb2_frame_ops = {
 	.buf_prepare = delta_vb2_frame_prepare,
 	.buf_finish = delta_vb2_frame_finish,
 	.buf_queue = delta_vb2_frame_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.stop_streaming = delta_vb2_frame_stop_streaming,
 };
 
@@ -1954,7 +1950,7 @@ MODULE_DEVICE_TABLE(of, delta_match_types);
 
 static struct platform_driver delta_driver = {
 	.probe = delta_probe,
-	.remove_new = delta_remove,
+	.remove = delta_remove,
 	.driver = {
 		   .name = DELTA_NAME,
 		   .of_match_table = delta_match_types,
diff --git a/drivers/media/platform/st/sti/hva/hva-hw.c b/drivers/media/platform/st/sti/hva/hva-hw.c
index fe4ea2e7f37e..fcb18fb52fdd 100644
--- a/drivers/media/platform/st/sti/hva/hva-hw.c
+++ b/drivers/media/platform/st/sti/hva/hva-hw.c
@@ -406,8 +406,7 @@ err_pm:
 err_disable:
 	pm_runtime_disable(dev);
 err_clk:
-	if (hva->clk)
-		clk_unprepare(hva->clk);
+	clk_unprepare(hva->clk);
 
 	return ret;
 }
diff --git a/drivers/media/platform/st/sti/hva/hva-v4l2.c b/drivers/media/platform/st/sti/hva/hva-v4l2.c
index 161a5c0fbc4e..5366c0f92549 100644
--- a/drivers/media/platform/st/sti/hva/hva-v4l2.c
+++ b/drivers/media/platform/st/sti/hva/hva-v4l2.c
@@ -1114,8 +1114,6 @@ static const struct vb2_ops hva_qops = {
 	.buf_queue		= hva_buf_queue,
 	.start_streaming	= hva_start_streaming,
 	.stop_streaming		= hva_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /*
@@ -1456,7 +1454,7 @@ MODULE_DEVICE_TABLE(of, hva_match_types);
 
 static struct platform_driver hva_driver = {
 	.probe  = hva_probe,
-	.remove_new = hva_remove,
+	.remove = hva_remove,
 	.driver = {
 		.name		= HVA_NAME,
 		.of_match_table	= hva_match_types,
diff --git a/drivers/media/platform/st/stm32/Kconfig b/drivers/media/platform/st/stm32/Kconfig
index 9df9a2a17728..f12e67bcc9bc 100644
--- a/drivers/media/platform/st/stm32/Kconfig
+++ b/drivers/media/platform/st/stm32/Kconfig
@@ -1,6 +1,20 @@
 # SPDX-License-Identifier: GPL-2.0-only
 
 # V4L drivers
+config VIDEO_STM32_CSI
+	tristate "STM32 Camera Serial Interface (CSI) support"
+	depends on V4L_PLATFORM_DRIVERS
+	depends on VIDEO_DEV && OF
+	depends on ARCH_STM32 || COMPILE_TEST
+	select MEDIA_CONTROLLER
+	select V4L2_FWNODE
+	help
+	  This module makes the STM32 Camera Serial Interface (CSI)
+	  available as a v4l2 device.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called stm32-csi.
+
 config VIDEO_STM32_DCMI
 	tristate "STM32 Digital Camera Memory Interface (DCMI) support"
 	depends on V4L_PLATFORM_DRIVERS
diff --git a/drivers/media/platform/st/stm32/Makefile b/drivers/media/platform/st/stm32/Makefile
index 7ed8297b9b19..9ae57897f030 100644
--- a/drivers/media/platform/st/stm32/Makefile
+++ b/drivers/media/platform/st/stm32/Makefile
@@ -1,4 +1,5 @@
 # SPDX-License-Identifier: GPL-2.0-only
+obj-$(CONFIG_VIDEO_STM32_CSI) += stm32-csi.o
 obj-$(CONFIG_VIDEO_STM32_DCMI) += stm32-dcmi.o
 obj-$(CONFIG_VIDEO_STM32_DCMIPP) += stm32-dcmipp/
 stm32-dma2d-objs := dma2d/dma2d.o dma2d/dma2d-hw.o
diff --git a/drivers/media/platform/st/stm32/dma2d/dma2d.c b/drivers/media/platform/st/stm32/dma2d/dma2d.c
index 92f1edee58f8..48fa781aab06 100644
--- a/drivers/media/platform/st/stm32/dma2d/dma2d.c
+++ b/drivers/media/platform/st/stm32/dma2d/dma2d.c
@@ -186,8 +186,6 @@ static const struct vb2_ops dma2d_qops = {
 	.buf_queue	= dma2d_buf_queue,
 	.start_streaming = dma2d_start_streaming,
 	.stop_streaming  = dma2d_stop_streaming,
-	.wait_prepare	= vb2_ops_wait_prepare,
-	.wait_finish	= vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -492,7 +490,8 @@ static void device_run(void *prv)
 	dst->sequence = frm_cap->sequence++;
 	v4l2_m2m_buf_copy_metadata(src, dst, true);
 
-	clk_enable(dev->gate);
+	if (clk_enable(dev->gate))
+		goto end;
 
 	dma2d_config_fg(dev, frm_out,
 			vb2_dma_contig_plane_dma_addr(&src->vb2_buf, 0));
@@ -717,7 +716,7 @@ MODULE_DEVICE_TABLE(of, stm32_dma2d_match);
 
 static struct platform_driver dma2d_pdrv = {
 	.probe		= dma2d_probe,
-	.remove_new	= dma2d_remove,
+	.remove		= dma2d_remove,
 	.driver		= {
 		.name = DMA2D_NAME,
 		.of_match_table = stm32_dma2d_match,
diff --git a/drivers/media/platform/st/stm32/stm32-csi.c b/drivers/media/platform/st/stm32/stm32-csi.c
new file mode 100644
index 000000000000..b69048144cc1
--- /dev/null
+++ b/drivers/media/platform/st/stm32/stm32-csi.c
@@ -0,0 +1,1141 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for STM32 Camera Serial Interface
+ *
+ * Copyright (C) STMicroelectronics SA 2024
+ * Author: Alain Volmat <alain.volmat@foss.st.com>
+ * for STMicroelectronics.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+
+#include <media/mipi-csi2.h>
+#include <media/v4l2-fwnode.h>
+#include <media/v4l2-subdev.h>
+
+#define STM32_CSI_CR				0x0000
+#define STM32_CSI_CR_CSIEN			BIT(0)
+#define STM32_CSI_CR_VCXSTART(x)		BIT(2 + ((x) * 4))
+#define STM32_CSI_CR_VCXSTOP(x)			BIT(3 + ((x) * 4))
+#define STM32_CSI_PCR				0x0004
+#define STM32_CSI_PCR_DL1EN			BIT(3)
+#define STM32_CSI_PCR_DL0EN			BIT(2)
+#define STM32_CSI_PCR_CLEN			BIT(1)
+#define STM32_CSI_PCR_PWRDOWN			BIT(0)
+#define STM32_CSI_VCXCFGR1(x)			((((x) + 1) * 0x0010) + 0x0)
+#define STM32_CSI_VCXCFGR1_ALLDT		BIT(0)
+#define STM32_CSI_VCXCFGR1_DT0EN		BIT(1)
+#define STM32_CSI_VCXCFGR1_DT1EN		BIT(2)
+#define STM32_CSI_VCXCFGR1_CDTFT_SHIFT		8
+#define STM32_CSI_VCXCFGR1_DT0_SHIFT		16
+#define STM32_CSI_VCXCFGR1_DT0FT_SHIFT		24
+#define STM32_CSI_VCXCFGR2(x)			((((x) + 1) * 0x0010) + 0x4)
+#define STM32_CSI_VCXCFGR2_DT1_SHIFT		0
+#define STM32_CSI_VCXCFGR2_DT1FT_SHIFT		8
+#define STM32_CSI_INPUT_BPP8			2
+#define STM32_CSI_INPUT_BPP10			3
+#define STM32_CSI_INPUT_BPP12			4
+#define STM32_CSI_INPUT_BPP14			5
+#define STM32_CSI_LMCFGR			0x0070
+#define STM32_CSI_LMCFGR_LANENB_SHIFT		8
+#define STM32_CSI_LMCFGR_DLMAP_SHIFT		16
+#define STM32_CSI_IER0				0x0080
+#define STM32_CSI_IER1				0x0084
+#define STM32_CSI_SR0				0x0090
+#define STM32_CSI_SR0_SYNCERRF			BIT(30)
+#define STM32_CSI_SR0_SPKTERRF			BIT(28)
+#define STM32_CSI_SR0_IDERRF			BIT(27)
+#define STM32_CSI_SR0_CECCERRF			BIT(26)
+#define STM32_CSI_SR0_ECCERRF			BIT(25)
+#define STM32_CSI_SR0_CRCERRF			BIT(24)
+#define STM32_CSI_SR0_CCFIFOFF			BIT(21)
+#define STM32_CSI_SR0_VCXSTATEF(x)		BIT(17 + (x))
+#define STM32_CSI_SR1				0x0094
+#define STM32_CSI_SR1_ECTRLDL1F			BIT(12)
+#define STM32_CSI_SR1_ESYNCESCDL1F		BIT(11)
+#define STM32_CSI_SR1_EESCDL1F			BIT(10)
+#define STM32_CSI_SR1_ESOTSYNCDL1F		BIT(9)
+#define STM32_CSI_SR1_ESOTDL1F			BIT(8)
+#define STM32_CSI_SR1_ECTRLDL0F			BIT(4)
+#define STM32_CSI_SR1_ESYNCESCDL0F		BIT(3)
+#define STM32_CSI_SR1_EESCDL0F			BIT(2)
+#define STM32_CSI_SR1_ESOTSYNCDL0F		BIT(1)
+#define STM32_CSI_SR1_ESOTDL0F			BIT(0)
+#define STM32_CSI_FCR0				0x0100
+#define STM32_CSI_FCR1				0x0104
+#define STM32_CSI_SPDFR				0x0110
+#define STM32_CSI_DT_MASK			0x3f
+#define STM32_CSI_VC_MASK			0x03
+#define STM32_CSI_ERR1				0x0114
+#define STM32_CSI_ERR1_IDVCERR_SHIFT		22
+#define STM32_CSI_ERR1_IDDTERR_SHIFT		16
+#define STM32_CSI_ERR1_CECCVCERR_SHIFT		14
+#define STM32_CSI_ERR1_CECCDTERR_SHIFT		8
+#define STM32_CSI_ERR1_CRCVCERR_SHIFT		6
+#define STM32_CSI_ERR1_CRCDTERR_SHIFT		0
+#define STM32_CSI_ERR2				0x0118
+#define STM32_CSI_ERR2_SYNCVCERR_SHIFT		18
+#define STM32_CSI_ERR2_SPKTVCERR_SHIFT		6
+#define STM32_CSI_ERR2_SPKTDTERR_SHIFT		0
+#define STM32_CSI_PRCR				0x1000
+#define STM32_CSI_PRCR_PEN			BIT(1)
+#define STM32_CSI_PMCR				0x1004
+#define STM32_CSI_PFCR				0x1008
+#define STM32_CSI_PFCR_CCFR_MASK		GENMASK(5, 0)
+#define STM32_CSI_PFCR_CCFR_SHIFT		0
+#define STM32_CSI_PFCR_HSFR_MASK		GENMASK(14, 8)
+#define STM32_CSI_PFCR_HSFR_SHIFT		8
+#define STM32_CSI_PFCR_DLD			BIT(16)
+#define STM32_CSI_PTCR0				0x1010
+#define STM32_CSI_PTCR0_TCKEN			BIT(0)
+#define STM32_CSI_PTCR1				0x1014
+#define STM32_CSI_PTCR1_TWM			BIT(16)
+#define STM32_CSI_PTCR1_TDI_MASK		GENMASK(7, 0)
+#define STM32_CSI_PTCR1_TDI_SHIFT		0
+#define STM32_CSI_PTSR				0x1018
+
+#define STM32_CSI_LANES_MAX	2
+
+#define STM32_CSI_SR0_ERRORS	(STM32_CSI_SR0_SYNCERRF | STM32_CSI_SR0_SPKTERRF |\
+				 STM32_CSI_SR0_IDERRF | STM32_CSI_SR0_CECCERRF |\
+				 STM32_CSI_SR0_ECCERRF | STM32_CSI_SR0_CRCERRF |\
+				 STM32_CSI_SR0_CCFIFOFF)
+#define STM32_CSI_SR1_DL0_ERRORS	(STM32_CSI_SR1_ECTRLDL0F | STM32_CSI_SR1_ESYNCESCDL0F |\
+				 STM32_CSI_SR1_EESCDL0F | STM32_CSI_SR1_ESOTSYNCDL0F |\
+				 STM32_CSI_SR1_ESOTDL0F)
+#define STM32_CSI_SR1_DL1_ERRORS	(STM32_CSI_SR1_ECTRLDL1F | STM32_CSI_SR1_ESYNCESCDL1F |\
+				 STM32_CSI_SR1_EESCDL1F | STM32_CSI_SR1_ESOTSYNCDL1F |\
+				 STM32_CSI_SR1_ESOTDL1F)
+#define STM32_CSI_SR1_ERRORS	(STM32_CSI_SR1_DL0_ERRORS | STM32_CSI_SR1_DL1_ERRORS)
+
+enum stm32_csi_pads {
+	STM32_CSI_PAD_SINK,
+	STM32_CSI_PAD_SOURCE,
+	STM32_CSI_PAD_MAX,
+};
+
+struct stm32_csi_event {
+	u32 mask;
+	const char * const name;
+};
+
+static const struct stm32_csi_event stm32_csi_events_sr0[] = {
+	{STM32_CSI_SR0_SYNCERRF,	"Synchronization error"},
+	{STM32_CSI_SR0_SPKTERRF,	"Short packet error"},
+	{STM32_CSI_SR0_IDERRF,		"Data type ID error"},
+	{STM32_CSI_SR0_CECCERRF,	"Corrected ECC error"},
+	{STM32_CSI_SR0_ECCERRF,		"ECC error"},
+	{STM32_CSI_SR0_CRCERRF,		"CRC error"},
+	{STM32_CSI_SR0_CCFIFOFF,	"Clk changer FIFO full error"},
+};
+
+#define STM32_CSI_NUM_SR0_EVENTS ARRAY_SIZE(stm32_csi_events_sr0)
+
+static const struct stm32_csi_event stm32_csi_events_sr1[] = {
+	{STM32_CSI_SR1_ECTRLDL1F,	"L1: D-PHY control error"},
+	{STM32_CSI_SR1_ESYNCESCDL1F,
+		"L1: D-PHY low power data transmission synchro error"},
+	{STM32_CSI_SR1_EESCDL1F,	"L1: D-PHY escape entry error"},
+	{STM32_CSI_SR1_ESOTSYNCDL1F,
+		"L1: Start of transmission synchro error"},
+	{STM32_CSI_SR1_ESOTDL1F,	"L1: Start of transmission error"},
+	{STM32_CSI_SR1_ECTRLDL0F,	"L0: D-PHY control error"},
+	{STM32_CSI_SR1_ESYNCESCDL0F,
+		"L0: D-PHY low power data transmission synchro error"},
+	{STM32_CSI_SR1_EESCDL0F,	"L0: D-PHY escape entry error"},
+	{STM32_CSI_SR1_ESOTSYNCDL0F,
+		"L0: Start of transmission synchro error"},
+	{STM32_CSI_SR1_ESOTDL0F,	"L0: Start of transmission error"},
+};
+
+#define STM32_CSI_NUM_SR1_EVENTS ARRAY_SIZE(stm32_csi_events_sr1)
+
+enum stm32_csi_clk {
+	STM32_CSI_CLK_PCLK,
+	STM32_CSI_CLK_TXESC,
+	STM32_CSI_CLK_CSI2PHY,
+	STM32_CSI_CLK_NB,
+};
+
+static const char * const stm32_csi_clks_id[] = {
+	"pclk",
+	"txesc",
+	"csi2phy",
+};
+
+struct stm32_csi_dev {
+	struct device			*dev;
+
+	void __iomem			*base;
+
+	struct clk_bulk_data		clks[STM32_CSI_CLK_NB];
+	struct regulator_bulk_data	supplies[2];
+
+	u8				lanes[STM32_CSI_LANES_MAX];
+	u8				num_lanes;
+
+	/*
+	 * spinlock slock is used to protect to srX_counters tables being
+	 * accessed from log_status and interrupt context
+	 */
+	spinlock_t			slock;
+
+	u32				sr0_counters[STM32_CSI_NUM_SR0_EVENTS];
+	u32				sr1_counters[STM32_CSI_NUM_SR1_EVENTS];
+
+	struct v4l2_subdev		sd;
+	struct v4l2_async_notifier	notifier;
+	struct media_pad		pads[STM32_CSI_PAD_MAX];
+
+	/* Remote source */
+	struct v4l2_subdev		*s_subdev;
+	u32				s_subdev_pad_nb;
+};
+
+struct stm32_csi_fmts {
+	u32 code;
+	u32 datatype;
+	u32 input_fmt;
+	u8 bpp;
+};
+
+#define FMT_MBUS_DT_DTFMT_BPP(mbus, dt, input, byteperpixel)		\
+	{								\
+		.code = MEDIA_BUS_FMT_##mbus,				\
+		.datatype = MIPI_CSI2_DT_##dt,				\
+		.input_fmt = STM32_CSI_INPUT_##input,	\
+		.bpp = byteperpixel,					\
+	}
+static const struct stm32_csi_fmts stm32_csi_formats[] = {
+	/* YUV 422 8 bit */
+	FMT_MBUS_DT_DTFMT_BPP(UYVY8_1X16, YUV422_8B, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(YUYV8_1X16, YUV422_8B, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(YVYU8_1X16, YUV422_8B, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(VYUY8_1X16, YUV422_8B, BPP8, 8),
+
+	/* Raw Bayer */
+	/* 8 bit */
+	FMT_MBUS_DT_DTFMT_BPP(SBGGR8_1X8, RAW8, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(SGBRG8_1X8, RAW8, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(SGRBG8_1X8, RAW8, BPP8, 8),
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB8_1X8, RAW8, BPP8, 8),
+	/* 10 bit */
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
+	FMT_MBUS_DT_DTFMT_BPP(SGBRG10_1X10, RAW10, BPP10, 10),
+	FMT_MBUS_DT_DTFMT_BPP(SGRBG10_1X10, RAW10, BPP10, 10),
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB10_1X10, RAW10, BPP10, 10),
+	/* 12 bit */
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
+	FMT_MBUS_DT_DTFMT_BPP(SGBRG12_1X12, RAW12, BPP12, 12),
+	FMT_MBUS_DT_DTFMT_BPP(SGRBG12_1X12, RAW12, BPP12, 12),
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB12_1X12, RAW12, BPP12, 12),
+	/* 14 bit */
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
+	FMT_MBUS_DT_DTFMT_BPP(SGBRG14_1X14, RAW14, BPP14, 14),
+	FMT_MBUS_DT_DTFMT_BPP(SGRBG14_1X14, RAW14, BPP14, 14),
+	FMT_MBUS_DT_DTFMT_BPP(SRGGB14_1X14, RAW14, BPP14, 14),
+
+	/* RGB 565 */
+	FMT_MBUS_DT_DTFMT_BPP(RGB565_1X16, RGB565, BPP8, 8),
+
+	/* JPEG (datatype isn't used) */
+	FMT_MBUS_DT_DTFMT_BPP(JPEG_1X8, NULL, BPP8, 8),
+};
+
+struct stm32_csi_mbps_phy_reg {
+	unsigned int mbps;
+	unsigned int hsfreqrange;
+	unsigned int osc_freq_target;
+};
+
+/*
+ * Table describing configuration of the PHY depending on the
+ * intended Bit Rate. From table 5-8 Frequency Ranges and Defaults
+ * of the Synopsis DWC MIPI PHY databook
+ */
+static const struct stm32_csi_mbps_phy_reg snps_stm32mp25[] = {
+	{ .mbps =   80,	.hsfreqrange = 0x00,	.osc_freq_target = 460 },
+	{ .mbps =   90, .hsfreqrange = 0x10,	.osc_freq_target = 460 },
+	{ .mbps =  100, .hsfreqrange = 0x20,	.osc_freq_target = 460 },
+	{ .mbps =  110, .hsfreqrange = 0x30,	.osc_freq_target = 460 },
+	{ .mbps =  120, .hsfreqrange = 0x01,	.osc_freq_target = 460 },
+	{ .mbps =  130, .hsfreqrange = 0x11,	.osc_freq_target = 460 },
+	{ .mbps =  140, .hsfreqrange = 0x21,	.osc_freq_target = 460 },
+	{ .mbps =  150, .hsfreqrange = 0x31,	.osc_freq_target = 460 },
+	{ .mbps =  160, .hsfreqrange = 0x02,	.osc_freq_target = 460 },
+	{ .mbps =  170, .hsfreqrange = 0x12,	.osc_freq_target = 460 },
+	{ .mbps =  180, .hsfreqrange = 0x22,	.osc_freq_target = 460 },
+	{ .mbps =  190, .hsfreqrange = 0x32,	.osc_freq_target = 460 },
+	{ .mbps =  205, .hsfreqrange = 0x03,	.osc_freq_target = 460 },
+	{ .mbps =  220, .hsfreqrange = 0x13,	.osc_freq_target = 460 },
+	{ .mbps =  235, .hsfreqrange = 0x23,	.osc_freq_target = 460 },
+	{ .mbps =  250, .hsfreqrange = 0x33,	.osc_freq_target = 460 },
+	{ .mbps =  275, .hsfreqrange = 0x04,	.osc_freq_target = 460 },
+	{ .mbps =  300, .hsfreqrange = 0x14,	.osc_freq_target = 460 },
+	{ .mbps =  325, .hsfreqrange = 0x25,	.osc_freq_target = 460 },
+	{ .mbps =  350, .hsfreqrange = 0x35,	.osc_freq_target = 460 },
+	{ .mbps =  400, .hsfreqrange = 0x05,	.osc_freq_target = 460 },
+	{ .mbps =  450, .hsfreqrange = 0x16,	.osc_freq_target = 460 },
+	{ .mbps =  500, .hsfreqrange = 0x26,	.osc_freq_target = 460 },
+	{ .mbps =  550, .hsfreqrange = 0x37,	.osc_freq_target = 460 },
+	{ .mbps =  600, .hsfreqrange = 0x07,	.osc_freq_target = 460 },
+	{ .mbps =  650, .hsfreqrange = 0x18,	.osc_freq_target = 460 },
+	{ .mbps =  700, .hsfreqrange = 0x28,	.osc_freq_target = 460 },
+	{ .mbps =  750, .hsfreqrange = 0x39,	.osc_freq_target = 460 },
+	{ .mbps =  800, .hsfreqrange = 0x09,	.osc_freq_target = 460 },
+	{ .mbps =  850, .hsfreqrange = 0x19,	.osc_freq_target = 460 },
+	{ .mbps =  900, .hsfreqrange = 0x29,	.osc_freq_target = 460 },
+	{ .mbps =  950, .hsfreqrange = 0x3a,	.osc_freq_target = 460 },
+	{ .mbps = 1000, .hsfreqrange = 0x0a,	.osc_freq_target = 460 },
+	{ .mbps = 1050, .hsfreqrange = 0x1a,	.osc_freq_target = 460 },
+	{ .mbps = 1100, .hsfreqrange = 0x2a,	.osc_freq_target = 460 },
+	{ .mbps = 1150, .hsfreqrange = 0x3b,	.osc_freq_target = 460 },
+	{ .mbps = 1200, .hsfreqrange = 0x0b,	.osc_freq_target = 460 },
+	{ .mbps = 1250, .hsfreqrange = 0x1b,	.osc_freq_target = 460 },
+	{ .mbps = 1300, .hsfreqrange = 0x2b,	.osc_freq_target = 460 },
+	{ .mbps = 1350, .hsfreqrange = 0x3c,	.osc_freq_target = 460 },
+	{ .mbps = 1400, .hsfreqrange = 0x0c,	.osc_freq_target = 460 },
+	{ .mbps = 1450, .hsfreqrange = 0x1c,	.osc_freq_target = 460 },
+	{ .mbps = 1500, .hsfreqrange = 0x2c,	.osc_freq_target = 460 },
+	{ .mbps = 1550, .hsfreqrange = 0x3d,	.osc_freq_target = 285 },
+	{ .mbps = 1600, .hsfreqrange = 0x0d,	.osc_freq_target = 295 },
+	{ .mbps = 1650, .hsfreqrange = 0x1d,	.osc_freq_target = 304 },
+	{ .mbps = 1700, .hsfreqrange = 0x2e,	.osc_freq_target = 313 },
+	{ .mbps = 1750, .hsfreqrange = 0x3e,	.osc_freq_target = 322 },
+	{ .mbps = 1800, .hsfreqrange = 0x0e,	.osc_freq_target = 331 },
+	{ .mbps = 1850, .hsfreqrange = 0x1e,	.osc_freq_target = 341 },
+	{ .mbps = 1900, .hsfreqrange = 0x2f,	.osc_freq_target = 350 },
+	{ .mbps = 1950, .hsfreqrange = 0x3f,	.osc_freq_target = 359 },
+	{ .mbps = 2000, .hsfreqrange = 0x0f,	.osc_freq_target = 368 },
+	{ .mbps = 2050, .hsfreqrange = 0x40,	.osc_freq_target = 377 },
+	{ .mbps = 2100, .hsfreqrange = 0x41,	.osc_freq_target = 387 },
+	{ .mbps = 2150, .hsfreqrange = 0x42,	.osc_freq_target = 396 },
+	{ .mbps = 2200, .hsfreqrange = 0x43,	.osc_freq_target = 405 },
+	{ .mbps = 2250, .hsfreqrange = 0x44,	.osc_freq_target = 414 },
+	{ .mbps = 2300, .hsfreqrange = 0x45,	.osc_freq_target = 423 },
+	{ .mbps = 2350, .hsfreqrange = 0x46,	.osc_freq_target = 432 },
+	{ .mbps = 2400, .hsfreqrange = 0x47,	.osc_freq_target = 442 },
+	{ .mbps = 2450, .hsfreqrange = 0x48,	.osc_freq_target = 451 },
+	{ .mbps = 2500, .hsfreqrange = 0x49,	.osc_freq_target = 460 },
+};
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = 640,
+	.height = 480,
+	.code = MEDIA_BUS_FMT_RGB565_1X16,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = V4L2_COLORSPACE_REC709,
+	.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT,
+	.quantization = V4L2_QUANTIZATION_DEFAULT,
+	.xfer_func = V4L2_XFER_FUNC_DEFAULT,
+};
+
+static const struct stm32_csi_fmts *stm32_csi_code_to_fmt(unsigned int code)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(stm32_csi_formats); i++)
+		if (stm32_csi_formats[i].code == code)
+			return &stm32_csi_formats[i];
+
+	return NULL;
+}
+
+static inline struct stm32_csi_dev *to_csidev(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct stm32_csi_dev, sd);
+}
+
+static int stm32_csi_setup_lane_merger(struct stm32_csi_dev *csidev)
+{
+	u32 lmcfgr = 0;
+	unsigned int i;
+
+	for (i = 0; i < csidev->num_lanes; i++) {
+		if (!csidev->lanes[i] || csidev->lanes[i] > STM32_CSI_LANES_MAX) {
+			dev_err(csidev->dev, "Invalid lane id (%d)\n", csidev->lanes[i]);
+			return -EINVAL;
+		}
+		lmcfgr |= (csidev->lanes[i] << ((i * 4) + STM32_CSI_LMCFGR_DLMAP_SHIFT));
+	}
+
+	lmcfgr |= (csidev->num_lanes << STM32_CSI_LMCFGR_LANENB_SHIFT);
+
+	writel_relaxed(lmcfgr, csidev->base + STM32_CSI_LMCFGR);
+
+	return 0;
+}
+
+static void stm32_csi_phy_reg_write(struct stm32_csi_dev *csidev,
+				    u32 addr, u32 val)
+{
+	/* Based on sequence described at section 5.2.3.2 of DesignWave document */
+	/* For writing the 4-bit testcode MSBs */
+	/* Set testen to high */
+	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
+
+	/* Set testclk to high */
+	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
+
+	/* Place 0x00 in testdin */
+	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
+
+	/*
+	 * Set testclk to low (with the falling edge on testclk, the testdin
+	 * signal content is latched internally)
+	 */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
+
+	/* Set testen to low */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
+
+	/* Place the 8-bit word corresponding to the testcode MSBs in testdin */
+	writel_relaxed(((addr >> 8) & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
+		       csidev->base + STM32_CSI_PTCR1);
+
+	/* Set testclk to high */
+	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
+
+	/* For writing the 8-bit testcode LSBs */
+	/* Set testclk to low */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
+
+	/* Set testen to high */
+	writel_relaxed(STM32_CSI_PTCR1_TWM, csidev->base + STM32_CSI_PTCR1);
+
+	/* Set testclk to high */
+	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
+
+	/* Place the 8-bit word test data in testdin */
+	writel_relaxed((addr & STM32_CSI_PTCR1_TDI_MASK) <<
+		       STM32_CSI_PTCR1_TDI_SHIFT | STM32_CSI_PTCR1_TWM,
+		       csidev->base + STM32_CSI_PTCR1);
+
+	/*
+	 * Set testclk to low (with the falling edge on testclk, the testdin
+	 * signal content is latched internally)
+	 */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
+
+	/* Set testen to low */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR1);
+
+	/* For writing the data */
+	/* Place the 8-bit word corresponding to the page offset in testdin */
+	writel_relaxed((val & STM32_CSI_PTCR1_TDI_MASK) << STM32_CSI_PTCR1_TDI_SHIFT,
+		       csidev->base + STM32_CSI_PTCR1);
+
+	/* Set testclk to high (test data is programmed internally */
+	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
+
+	/* Finish by setting testclk to low */
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
+}
+
+static int stm32_csi_start(struct stm32_csi_dev *csidev,
+			   struct v4l2_subdev_state *state)
+{
+	struct media_pad *src_pad =
+		&csidev->s_subdev->entity.pads[csidev->s_subdev_pad_nb];
+	const struct stm32_csi_mbps_phy_reg *phy_regs = NULL;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	const struct stm32_csi_fmts *fmt;
+	unsigned long phy_clk_frate;
+	u32 lanes_ie, lanes_en;
+	unsigned int mbps;
+	unsigned int i;
+	s64 link_freq;
+	int ret;
+	u32 ccfr;
+
+	dev_dbg(csidev->dev, "Starting the CSI2\n");
+
+	/* Get the bpp value on pad0 (input of CSI) */
+	sink_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
+	fmt = stm32_csi_code_to_fmt(sink_fmt->code);
+
+	/* Get the remote sensor link frequency */
+	if (!csidev->s_subdev)
+		return -EIO;
+
+	link_freq = v4l2_get_link_freq(src_pad,
+				       fmt->bpp, 2 * csidev->num_lanes);
+	if (link_freq < 0)
+		return link_freq;
+
+	/* MBPS is expressed in Mbps, hence link_freq / 100000 * 2 */
+	mbps = div_s64(link_freq, 500000);
+	dev_dbg(csidev->dev, "Computed Mbps: %u\n", mbps);
+
+	for (i = 0; i < ARRAY_SIZE(snps_stm32mp25); i++) {
+		if (snps_stm32mp25[i].mbps >= mbps) {
+			phy_regs = &snps_stm32mp25[i];
+			break;
+		}
+	}
+
+	if (!phy_regs) {
+		dev_err(csidev->dev, "Unsupported PHY speed (%u Mbps)", mbps);
+		return -ERANGE;
+	}
+
+	dev_dbg(csidev->dev, "PHY settings: (%u Mbps, %u HS FRange, %u OSC Freq)\n",
+		phy_regs->mbps, phy_regs->hsfreqrange,
+		phy_regs->osc_freq_target);
+
+	/* Prepare lanes related configuration bits */
+	lanes_ie = STM32_CSI_SR1_DL0_ERRORS;
+	lanes_en = STM32_CSI_PCR_DL0EN;
+	if (csidev->num_lanes == 2) {
+		lanes_ie |= STM32_CSI_SR1_DL1_ERRORS;
+		lanes_en |= STM32_CSI_PCR_DL1EN;
+	}
+
+	ret = pm_runtime_get_sync(csidev->dev);
+	if (ret < 0)
+		goto error_put;
+
+	/* Retrieve CSI2PHY clock rate to compute CCFR value */
+	phy_clk_frate = clk_get_rate(csidev->clks[STM32_CSI_CLK_CSI2PHY].clk);
+	if (!phy_clk_frate) {
+		dev_err(csidev->dev, "CSI2PHY clock rate invalid (0)\n");
+		ret = -EINVAL;
+		goto error_put;
+	}
+
+	ret = stm32_csi_setup_lane_merger(csidev);
+	if (ret)
+		goto error_put;
+
+	/* Enable the CSI */
+	writel_relaxed(STM32_CSI_CR_CSIEN, csidev->base + STM32_CSI_CR);
+
+	/* Enable some global CSI related interrupts - bits are same as SR0 */
+	writel_relaxed(STM32_CSI_SR0_ERRORS, csidev->base + STM32_CSI_IER0);
+
+	/* Enable lanes related error interrupts */
+	writel_relaxed(lanes_ie, csidev->base + STM32_CSI_IER1);
+
+	/* Initialization of the D-PHY */
+	/* Stop the D-PHY */
+	writel_relaxed(0, csidev->base + STM32_CSI_PRCR);
+
+	/* Keep the D-PHY in power down state */
+	writel_relaxed(0, csidev->base + STM32_CSI_PCR);
+
+	/* Enable testclr clock during 15ns */
+	writel_relaxed(STM32_CSI_PTCR0_TCKEN, csidev->base + STM32_CSI_PTCR0);
+	udelay(1);
+	writel_relaxed(0, csidev->base + STM32_CSI_PTCR0);
+
+	/* Set hsfreqrange */
+	phy_clk_frate /= 1000000;
+	ccfr = (phy_clk_frate - 17) * 4;
+	writel_relaxed((ccfr << STM32_CSI_PFCR_CCFR_SHIFT) |
+		       (phy_regs->hsfreqrange << STM32_CSI_PFCR_HSFR_SHIFT),
+		       csidev->base + STM32_CSI_PFCR);
+
+	/* set reg @08 deskew_polarity_rw 1'b1 */
+	stm32_csi_phy_reg_write(csidev, 0x08, 0x38);
+
+	/* set reg @0xE4 counter_for_des_en_config_if_rx 0x10 + DLL prog EN */
+	/* This is because 13<= cfgclkfreqrange[5:0]<=38 */
+	stm32_csi_phy_reg_write(csidev, 0xe4, 0x11);
+
+	/* set reg @0xe2 & reg @0xe3 value DLL target oscilation freq */
+	/* Based on the table page 77, osc_freq_target */
+	stm32_csi_phy_reg_write(csidev, 0xe2, phy_regs->osc_freq_target & 0xFF);
+	stm32_csi_phy_reg_write(csidev, 0xe3, (phy_regs->osc_freq_target >> 8) & 0x0F);
+
+	writel_relaxed(STM32_CSI_PFCR_DLD | readl_relaxed(csidev->base + STM32_CSI_PFCR),
+		       csidev->base + STM32_CSI_PFCR);
+
+	/* Enable Lanes */
+	writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN, csidev->base + STM32_CSI_PCR);
+	writel_relaxed(lanes_en | STM32_CSI_PCR_CLEN | STM32_CSI_PCR_PWRDOWN,
+		       csidev->base + STM32_CSI_PCR);
+
+	writel_relaxed(STM32_CSI_PRCR_PEN, csidev->base + STM32_CSI_PRCR);
+
+	/* Remove the force */
+	writel_relaxed(0, csidev->base + STM32_CSI_PMCR);
+
+	return ret;
+
+error_put:
+	pm_runtime_put(csidev->dev);
+	return ret;
+}
+
+static void stm32_csi_stop(struct stm32_csi_dev *csidev)
+{
+	dev_dbg(csidev->dev, "Stopping the CSI2\n");
+
+	/* Disable the D-PHY */
+	writel_relaxed(0, csidev->base + STM32_CSI_PCR);
+
+	/* Disable ITs */
+	writel_relaxed(0, csidev->base + STM32_CSI_IER0);
+	writel_relaxed(0, csidev->base + STM32_CSI_IER1);
+
+	/* Disable the CSI */
+	writel_relaxed(0, csidev->base + STM32_CSI_CR);
+
+	pm_runtime_put(csidev->dev);
+}
+
+static int stm32_csi_start_vc(struct stm32_csi_dev *csidev,
+			      struct v4l2_subdev_state *state, u32 vc)
+{
+	struct v4l2_mbus_framefmt *mbus_fmt;
+	const struct stm32_csi_fmts *fmt;
+	u32 status;
+	u32 cfgr1;
+	int ret;
+
+	mbus_fmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
+	fmt = stm32_csi_code_to_fmt(mbus_fmt->code);
+
+	/* If the mbus code is JPEG, don't enable filtering */
+	if (mbus_fmt->code == MEDIA_BUS_FMT_JPEG_1X8) {
+		cfgr1 = STM32_CSI_VCXCFGR1_ALLDT;
+		cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_CDTFT_SHIFT;
+		dev_dbg(csidev->dev, "VC%d: enable AllDT mode\n", vc);
+	} else {
+		cfgr1 = fmt->datatype << STM32_CSI_VCXCFGR1_DT0_SHIFT;
+		cfgr1 |= fmt->input_fmt << STM32_CSI_VCXCFGR1_DT0FT_SHIFT;
+		cfgr1 |= STM32_CSI_VCXCFGR1_DT0EN;
+		dev_dbg(csidev->dev, "VC%d: enable DT0(0x%x)/DT0FT(0x%x)\n",
+			vc, fmt->datatype, fmt->input_fmt);
+	}
+	writel_relaxed(cfgr1, csidev->base + STM32_CSI_VCXCFGR1(vc));
+
+	/* Enable processing of the virtual-channel and wait for its status */
+	writel_relaxed(STM32_CSI_CR_VCXSTART(vc) | STM32_CSI_CR_CSIEN,
+		       csidev->base + STM32_CSI_CR);
+
+	ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
+					 status,
+					 status & STM32_CSI_SR0_VCXSTATEF(vc),
+					 1000, 1000000);
+	if (ret) {
+		dev_err(csidev->dev, "failed to start VC(%d)\n", vc);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int stm32_csi_stop_vc(struct stm32_csi_dev *csidev, u32 vc)
+{
+	u32 status;
+	int ret;
+
+	/* Stop the Virtual Channel */
+	writel_relaxed(STM32_CSI_CR_VCXSTOP(vc) | STM32_CSI_CR_CSIEN,
+		       csidev->base + STM32_CSI_CR);
+
+	ret = readl_relaxed_poll_timeout(csidev->base + STM32_CSI_SR0,
+					 status,
+					 !(status & STM32_CSI_SR0_VCXSTATEF(vc)),
+					 1000, 1000000);
+	if (ret) {
+		dev_err(csidev->dev, "failed to stop VC(%d)\n", vc);
+		return ret;
+	}
+
+	/* Disable all DTs */
+	writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR1(vc));
+	writel_relaxed(0, csidev->base + STM32_CSI_VCXCFGR2(vc));
+
+	return 0;
+}
+
+static int stm32_csi_disable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state, u32 pad,
+				     u64 streams_mask)
+{
+	struct stm32_csi_dev *csidev = to_csidev(sd);
+	int ret;
+
+	ret = v4l2_subdev_disable_streams(csidev->s_subdev,
+					  csidev->s_subdev_pad_nb, BIT_ULL(0));
+	if (ret)
+		return ret;
+
+	/* Stop the VC0 */
+	ret = stm32_csi_stop_vc(csidev, 0);
+	if (ret)
+		dev_err(csidev->dev, "Failed to stop VC0\n");
+
+	stm32_csi_stop(csidev);
+
+	return 0;
+}
+
+static int stm32_csi_enable_streams(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state, u32 pad,
+				    u64 streams_mask)
+{
+	struct stm32_csi_dev *csidev = to_csidev(sd);
+	int ret;
+
+	ret = stm32_csi_start(csidev, state);
+	if (ret)
+		return ret;
+
+	/* Configure & start the VC0 */
+	ret = stm32_csi_start_vc(csidev, state, 0);
+	if (ret) {
+		dev_err(csidev->dev, "Failed to start VC0\n");
+		goto failed_start_vc;
+	}
+
+	ret = v4l2_subdev_enable_streams(csidev->s_subdev,
+					 csidev->s_subdev_pad_nb, BIT_ULL(0));
+	if (ret)
+		goto failed_enable_streams;
+
+	return 0;
+
+failed_enable_streams:
+	stm32_csi_stop_vc(csidev, 0);
+failed_start_vc:
+	stm32_csi_stop(csidev);
+	return ret;
+}
+
+static int stm32_csi_init_state(struct v4l2_subdev *sd,
+				struct v4l2_subdev_state *state)
+{
+	unsigned int i;
+
+	for (i = 0; i < sd->entity.num_pads; i++)
+		*v4l2_subdev_state_get_format(state, i) = fmt_default;
+
+	return 0;
+}
+
+static int stm32_csi_enum_mbus_code(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_mbus_code_enum *code)
+{
+	if (code->index >= ARRAY_SIZE(stm32_csi_formats))
+		return -EINVAL;
+
+	code->code = stm32_csi_formats[code->index].code;
+	return 0;
+}
+
+static int stm32_csi_set_pad_format(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_format *format)
+{
+	struct stm32_csi_dev *csidev = to_csidev(sd);
+	struct v4l2_mbus_framefmt *framefmt;
+	const struct stm32_csi_fmts *fmt;
+
+	fmt = stm32_csi_code_to_fmt(format->format.code);
+	if (!fmt) {
+		dev_dbg(csidev->dev, "Unsupported code %d, use default\n",
+			format->format.code);
+		format->format.code = fmt_default.code;
+	}
+
+	framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SINK);
+
+	if (format->pad == STM32_CSI_PAD_SOURCE)
+		format->format = *framefmt;
+	else
+		*framefmt = format->format;
+
+	framefmt = v4l2_subdev_state_get_format(state, STM32_CSI_PAD_SOURCE);
+	*framefmt = format->format;
+
+	return 0;
+}
+
+static int stm32_csi_log_status(struct v4l2_subdev *sd)
+{
+	struct stm32_csi_dev *csidev = to_csidev(sd);
+	unsigned long flags;
+	unsigned int i;
+
+	spin_lock_irqsave(&csidev->slock, flags);
+
+	for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++) {
+		if (csidev->sr0_counters[i])
+			dev_info(csidev->dev, "%s events: %d\n",
+				 stm32_csi_events_sr0[i].name,
+				 csidev->sr0_counters[i]);
+	}
+
+	for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++) {
+		if (csidev->sr1_counters[i])
+			dev_info(csidev->dev, "%s events: %d\n",
+				 stm32_csi_events_sr1[i].name,
+				 csidev->sr1_counters[i]);
+	}
+
+	spin_unlock_irqrestore(&csidev->slock, flags);
+
+	return 0;
+}
+
+static const struct v4l2_subdev_core_ops stm32_csi_core_ops = {
+	.log_status = stm32_csi_log_status,
+};
+
+static const struct v4l2_subdev_video_ops stm32_csi_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_pad_ops stm32_csi_pad_ops = {
+	.enum_mbus_code = stm32_csi_enum_mbus_code,
+	.set_fmt = stm32_csi_set_pad_format,
+	.get_fmt = v4l2_subdev_get_fmt,
+	.enable_streams = stm32_csi_enable_streams,
+	.disable_streams = stm32_csi_disable_streams,
+};
+
+static const struct v4l2_subdev_ops stm32_csi_subdev_ops = {
+	.core		= &stm32_csi_core_ops,
+	.pad		= &stm32_csi_pad_ops,
+	.video		= &stm32_csi_video_ops,
+};
+
+static const struct v4l2_subdev_internal_ops stm32_csi_subdev_internal_ops = {
+	.init_state = stm32_csi_init_state,
+};
+
+static int stm32_csi_async_bound(struct v4l2_async_notifier *notifier,
+				 struct v4l2_subdev *s_subdev,
+				 struct v4l2_async_connection *asd)
+{
+	struct v4l2_subdev *sd = notifier->sd;
+	struct stm32_csi_dev *csidev = to_csidev(sd);
+	int remote_pad;
+
+	remote_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
+						 s_subdev->fwnode,
+						 MEDIA_PAD_FL_SOURCE);
+	if (remote_pad < 0) {
+		dev_err(csidev->dev, "Couldn't find output pad for subdev %s\n",
+			s_subdev->name);
+		return remote_pad;
+	}
+
+	csidev->s_subdev = s_subdev;
+	csidev->s_subdev_pad_nb = remote_pad;
+
+	return media_create_pad_link(&csidev->s_subdev->entity,
+				     remote_pad, &csidev->sd.entity,
+				     STM32_CSI_PAD_SINK,
+				     MEDIA_LNK_FL_ENABLED |
+				     MEDIA_LNK_FL_IMMUTABLE);
+}
+
+static const struct v4l2_async_notifier_operations stm32_csi_notifier_ops = {
+	.bound		= stm32_csi_async_bound,
+};
+
+static irqreturn_t stm32_csi_irq_thread(int irq, void *arg)
+{
+	struct stm32_csi_dev *csidev = arg;
+	unsigned long flags;
+	u32 sr0, sr1;
+	int i;
+
+	sr0 = readl_relaxed(csidev->base + STM32_CSI_SR0);
+	sr1 = readl_relaxed(csidev->base + STM32_CSI_SR1);
+
+	/* Clear interrupt */
+	writel_relaxed(sr0 & STM32_CSI_SR0_ERRORS,
+		       csidev->base + STM32_CSI_FCR0);
+	writel_relaxed(sr1 & STM32_CSI_SR1_ERRORS,
+		       csidev->base + STM32_CSI_FCR1);
+
+	spin_lock_irqsave(&csidev->slock, flags);
+
+	for (i = 0; i < STM32_CSI_NUM_SR0_EVENTS; i++)
+		if (sr0 & stm32_csi_events_sr0[i].mask)
+			csidev->sr0_counters[i]++;
+
+	for (i = 0; i < STM32_CSI_NUM_SR1_EVENTS; i++)
+		if (sr1 & stm32_csi_events_sr1[i].mask)
+			csidev->sr1_counters[i]++;
+
+	spin_unlock_irqrestore(&csidev->slock, flags);
+
+	return IRQ_HANDLED;
+}
+
+static int stm32_csi_get_resources(struct stm32_csi_dev *csidev,
+				   struct platform_device *pdev)
+{
+	unsigned int i;
+	int irq, ret;
+
+	csidev->base = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
+	if (IS_ERR(csidev->base))
+		return dev_err_probe(&pdev->dev, PTR_ERR(csidev->base),
+				     "Failed to ioremap resource\n");
+
+	for (i = 0; i < STM32_CSI_CLK_NB; i++)
+		csidev->clks[i].id = stm32_csi_clks_id[i];
+
+	ret = devm_clk_bulk_get(&pdev->dev, STM32_CSI_CLK_NB,
+				csidev->clks);
+	if (ret < 0)
+		return dev_err_probe(&pdev->dev, ret, "Couldn't get clks\n");
+
+	csidev->supplies[0].supply = "vdd";
+	csidev->supplies[1].supply = "vdda18";
+	ret = devm_regulator_bulk_get(&pdev->dev, ARRAY_SIZE(csidev->supplies),
+				      csidev->supplies);
+	if (ret)
+		return dev_err_probe(&pdev->dev, ret,
+				     "Failed to request regulator vdd\n");
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
+					stm32_csi_irq_thread, IRQF_ONESHOT,
+					dev_name(&pdev->dev), csidev);
+	if (ret)
+		return dev_err_probe(&pdev->dev, ret,
+				     "Unable to request irq");
+
+	return 0;
+}
+
+static int stm32_csi_parse_dt(struct stm32_csi_dev *csidev)
+{
+	struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
+	struct v4l2_async_connection *asd;
+	struct fwnode_handle *ep;
+	int ret;
+
+	/* Get bus characteristics from devicetree */
+	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csidev->dev), 0, 0,
+					     FWNODE_GRAPH_ENDPOINT_NEXT);
+	if (!ep) {
+		dev_err(csidev->dev, "Could not find the endpoint\n");
+		return -ENODEV;
+	}
+
+	ret = v4l2_fwnode_endpoint_parse(ep, &v4l2_ep);
+	if (ret) {
+		dev_err(csidev->dev, "Could not parse v4l2 endpoint\n");
+		goto out;
+	}
+
+	csidev->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes;
+	if (csidev->num_lanes > STM32_CSI_LANES_MAX) {
+		dev_err(csidev->dev, "Unsupported number of data-lanes: %d\n",
+			csidev->num_lanes);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	memcpy(csidev->lanes, v4l2_ep.bus.mipi_csi2.data_lanes,
+	       sizeof(csidev->lanes));
+
+	v4l2_async_subdev_nf_init(&csidev->notifier, &csidev->sd);
+
+	asd = v4l2_async_nf_add_fwnode_remote(&csidev->notifier, ep,
+					      struct v4l2_async_connection);
+
+
+	if (IS_ERR(asd)) {
+		dev_err(csidev->dev, "Failed to add fwnode remote subdev\n");
+		ret = PTR_ERR(asd);
+		goto out;
+	}
+
+	csidev->notifier.ops = &stm32_csi_notifier_ops;
+
+	ret = v4l2_async_nf_register(&csidev->notifier);
+	if (ret) {
+		dev_err(csidev->dev, "Failed to register notifier\n");
+		v4l2_async_nf_cleanup(&csidev->notifier);
+		goto out;
+	}
+
+out:
+	fwnode_handle_put(ep);
+	return ret;
+}
+
+static int stm32_csi_probe(struct platform_device *pdev)
+{
+	struct stm32_csi_dev *csidev;
+	struct reset_control *rstc;
+	int ret;
+
+	csidev = devm_kzalloc(&pdev->dev, sizeof(*csidev), GFP_KERNEL);
+	if (!csidev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, csidev);
+	csidev->dev = &pdev->dev;
+
+	spin_lock_init(&csidev->slock);
+
+	ret = stm32_csi_get_resources(csidev, pdev);
+	if (ret)
+		return ret;
+
+	ret = stm32_csi_parse_dt(csidev);
+	if (ret)
+		return ret;
+
+	csidev->sd.owner = THIS_MODULE;
+	csidev->sd.dev = &pdev->dev;
+	csidev->sd.internal_ops = &stm32_csi_subdev_internal_ops;
+	v4l2_subdev_init(&csidev->sd, &stm32_csi_subdev_ops);
+	v4l2_set_subdevdata(&csidev->sd, &pdev->dev);
+	snprintf(csidev->sd.name, sizeof(csidev->sd.name), "%s",
+		 dev_name(&pdev->dev));
+
+	/* Create our media pads */
+	csidev->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	csidev->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+	csidev->pads[STM32_CSI_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
+	csidev->pads[STM32_CSI_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&csidev->sd.entity, STM32_CSI_PAD_MAX,
+				     csidev->pads);
+	if (ret)
+		goto err_cleanup;
+
+	ret = v4l2_subdev_init_finalize(&csidev->sd);
+	if (ret < 0)
+		goto err_cleanup;
+
+	/* Reset device */
+	rstc = devm_reset_control_get_exclusive(&pdev->dev, NULL);
+	if (IS_ERR(rstc)) {
+		ret = dev_err_probe(&pdev->dev, PTR_ERR(rstc),
+				    "Couldn't get reset control\n");
+		goto err_cleanup;
+	}
+
+	ret = reset_control_assert(rstc);
+	if (ret) {
+		ret = dev_err_probe(&pdev->dev, ret,
+				    "Failed to assert the reset line\n");
+		goto err_cleanup;
+	}
+
+	usleep_range(3000, 5000);
+
+	ret = reset_control_deassert(rstc);
+	if (ret) {
+		ret = dev_err_probe(&pdev->dev, ret,
+				    "Failed to deassert the reset line\n");
+		goto err_cleanup;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	ret = v4l2_async_register_subdev(&csidev->sd);
+	if (ret < 0)
+		goto err_cleanup;
+
+	dev_info(&pdev->dev,
+		 "Probed CSI with %u lanes\n", csidev->num_lanes);
+
+	return 0;
+
+err_cleanup:
+	v4l2_async_nf_cleanup(&csidev->notifier);
+	return ret;
+}
+
+static void stm32_csi_remove(struct platform_device *pdev)
+{
+	struct stm32_csi_dev *csidev = platform_get_drvdata(pdev);
+
+	v4l2_async_unregister_subdev(&csidev->sd);
+
+	pm_runtime_disable(&pdev->dev);
+}
+
+static int stm32_csi_runtime_suspend(struct device *dev)
+{
+	struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
+	int ret;
+
+	clk_bulk_disable_unprepare(STM32_CSI_CLK_NB, csidev->clks);
+
+	ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies),
+				     csidev->supplies);
+	if (ret < 0)
+		dev_err(dev, "cannot disable regulators %d\n", ret);
+
+	return 0;
+}
+
+static int stm32_csi_runtime_resume(struct device *dev)
+{
+	struct stm32_csi_dev *csidev = dev_get_drvdata(dev);
+	int ret;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(csidev->supplies),
+				    csidev->supplies);
+	if (ret)
+		goto error_out;
+
+	ret = clk_bulk_prepare_enable(STM32_CSI_CLK_NB, csidev->clks);
+	if (ret)
+		goto error_disable_supplies;
+
+	return 0;
+
+error_disable_supplies:
+	ret = regulator_bulk_disable(ARRAY_SIZE(csidev->supplies), csidev->supplies);
+	if (ret < 0)
+		dev_err(dev, "cannot disable regulators %d\n", ret);
+error_out:
+	dev_err(csidev->dev, "Failed to resume: %d\n", ret);
+
+	return ret;
+}
+
+static const struct of_device_id stm32_csi_of_table[] = {
+	{ .compatible = "st,stm32mp25-csi", },
+	{ /* end node */ },
+};
+MODULE_DEVICE_TABLE(of, stm32_csi_of_table);
+
+static const struct dev_pm_ops stm32_csi_pm_ops = {
+	RUNTIME_PM_OPS(stm32_csi_runtime_suspend,
+		       stm32_csi_runtime_resume, NULL)
+};
+
+static struct platform_driver stm32_csi_driver = {
+	.driver	= {
+		.name = "stm32-csi",
+		.of_match_table = stm32_csi_of_table,
+		.pm = pm_ptr(&stm32_csi_pm_ops),
+	},
+	.probe	= stm32_csi_probe,
+	.remove = stm32_csi_remove,
+};
+
+module_platform_driver(stm32_csi_driver);
+
+MODULE_AUTHOR("Alain Volmat <alain.volmat@foss.st.com>");
+MODULE_DESCRIPTION("STM32 CSI controller");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/st/stm32/stm32-dcmi.c b/drivers/media/platform/st/stm32/stm32-dcmi.c
index ff3331af9406..d94c61b8569d 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmi.c
+++ b/drivers/media/platform/st/stm32/stm32-dcmi.c
@@ -388,9 +388,9 @@ static void dcmi_set_crop(struct stm32_dcmi *dcmi)
 		 ((dcmi->crop.left << 1));
 	reg_write(dcmi->regs, DCMI_CWSTRT, start);
 
-	dev_dbg(dcmi->dev, "Cropping to %ux%u@%u:%u\n",
-		dcmi->crop.width, dcmi->crop.height,
-		dcmi->crop.left, dcmi->crop.top);
+	dev_dbg(dcmi->dev, "Cropping to (%d,%d)/%ux%u\n",
+		dcmi->crop.left, dcmi->crop.top,
+		dcmi->crop.width, dcmi->crop.height);
 
 	/* Enable crop */
 	reg_set(dcmi->regs, DCMI_CR, CR_CROP);
@@ -898,8 +898,6 @@ static const struct vb2_ops dcmi_video_qops = {
 	.buf_queue		= dcmi_buf_queue,
 	.start_streaming	= dcmi_start_streaming,
 	.stop_streaming		= dcmi_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int dcmi_g_fmt_vid_cap(struct file *file, void *priv,
@@ -1294,8 +1292,8 @@ static int dcmi_s_selection(struct file *file, void *priv,
 		/* Crop if request is different than sensor resolution */
 		dcmi->do_crop = true;
 		dcmi->crop = r;
-		dev_dbg(dcmi->dev, "s_selection: crop %ux%u@(%u,%u) from %ux%u\n",
-			r.width, r.height, r.left, r.top,
+		dev_dbg(dcmi->dev, "s_selection: crop (%d,%d)/%ux%u from %ux%u\n",
+			r.left, r.top, r.width, r.height,
 			pix.width, pix.height);
 	} else {
 		/* Disable crop */
@@ -1684,18 +1682,14 @@ static int dcmi_formats_init(struct stm32_dcmi *dcmi)
 		return -ENXIO;
 
 	dcmi->num_of_sd_formats = num_fmts;
-	dcmi->sd_formats = devm_kcalloc(dcmi->dev,
-					num_fmts, sizeof(struct dcmi_format *),
-					GFP_KERNEL);
+	dcmi->sd_formats = devm_kmemdup_array(dcmi->dev, sd_fmts, num_fmts,
+					      sizeof(*sd_fmts), GFP_KERNEL);
 	if (!dcmi->sd_formats) {
 		dev_err(dcmi->dev, "Could not allocate memory\n");
 		return -ENOMEM;
 	}
 
-	memcpy(dcmi->sd_formats, sd_fmts,
-	       num_fmts * sizeof(struct dcmi_format *));
 	dcmi->sd_format = dcmi->sd_formats[0];
-
 	return 0;
 }
 
@@ -2149,7 +2143,7 @@ static const struct dev_pm_ops dcmi_pm_ops = {
 
 static struct platform_driver stm32_dcmi_driver = {
 	.probe		= dcmi_probe,
-	.remove_new	= dcmi_remove,
+	.remove		= dcmi_remove,
 	.driver		= {
 		.name = DRV_NAME,
 		.of_match_table = of_match_ptr(stm32_dcmi_of_match),
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile b/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
index 8920d9388a21..159105fb40b8 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/Makefile
@@ -1,4 +1,4 @@
 # SPDX-License-Identifier: GPL-2.0
-stm32-dcmipp-y := dcmipp-core.o dcmipp-common.o dcmipp-parallel.o dcmipp-byteproc.o dcmipp-bytecap.o
+stm32-dcmipp-y := dcmipp-core.o dcmipp-common.o dcmipp-input.o dcmipp-byteproc.o dcmipp-bytecap.o
 
 obj-$(CONFIG_VIDEO_STM32_DCMIPP) += stm32-dcmipp.o
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-bytecap.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-bytecap.c
index 9f768f011fa2..1c1b6b48918e 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-bytecap.c
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-bytecap.c
@@ -56,15 +56,32 @@ struct dcmipp_bytecap_pix_map {
 
 static const struct dcmipp_bytecap_pix_map dcmipp_bytecap_pix_map_list[] = {
 	PIXMAP_MBUS_PFMT(RGB565_2X8_LE, RGB565),
+	PIXMAP_MBUS_PFMT(RGB565_1X16, RGB565),
 	PIXMAP_MBUS_PFMT(YUYV8_2X8, YUYV),
+	PIXMAP_MBUS_PFMT(YUYV8_1X16, YUYV),
 	PIXMAP_MBUS_PFMT(YVYU8_2X8, YVYU),
+	PIXMAP_MBUS_PFMT(YVYU8_1X16, YVYU),
 	PIXMAP_MBUS_PFMT(UYVY8_2X8, UYVY),
+	PIXMAP_MBUS_PFMT(UYVY8_1X16, UYVY),
 	PIXMAP_MBUS_PFMT(VYUY8_2X8, VYUY),
+	PIXMAP_MBUS_PFMT(VYUY8_1X16, VYUY),
 	PIXMAP_MBUS_PFMT(Y8_1X8, GREY),
 	PIXMAP_MBUS_PFMT(SBGGR8_1X8, SBGGR8),
 	PIXMAP_MBUS_PFMT(SGBRG8_1X8, SGBRG8),
 	PIXMAP_MBUS_PFMT(SGRBG8_1X8, SGRBG8),
 	PIXMAP_MBUS_PFMT(SRGGB8_1X8, SRGGB8),
+	PIXMAP_MBUS_PFMT(SBGGR10_1X10, SBGGR10),
+	PIXMAP_MBUS_PFMT(SGBRG10_1X10, SGBRG10),
+	PIXMAP_MBUS_PFMT(SGRBG10_1X10, SGRBG10),
+	PIXMAP_MBUS_PFMT(SRGGB10_1X10, SRGGB10),
+	PIXMAP_MBUS_PFMT(SBGGR12_1X12, SBGGR12),
+	PIXMAP_MBUS_PFMT(SGBRG12_1X12, SGBRG12),
+	PIXMAP_MBUS_PFMT(SGRBG12_1X12, SGRBG12),
+	PIXMAP_MBUS_PFMT(SRGGB12_1X12, SRGGB12),
+	PIXMAP_MBUS_PFMT(SBGGR14_1X14, SBGGR14),
+	PIXMAP_MBUS_PFMT(SGBRG14_1X14, SGBRG14),
+	PIXMAP_MBUS_PFMT(SGRBG14_1X14, SGRBG14),
+	PIXMAP_MBUS_PFMT(SRGGB14_1X14, SRGGB14),
 	PIXMAP_MBUS_PFMT(JPEG_1X8, JPEG),
 };
 
@@ -112,6 +129,7 @@ struct dcmipp_bytecap_device {
 	u32 sequence;
 	struct media_pipeline pipe;
 	struct v4l2_subdev *s_subdev;
+	u32 s_subdev_pad_nb;
 
 	enum dcmipp_state state;
 
@@ -250,34 +268,34 @@ static int dcmipp_bytecap_enum_fmt_vid_cap(struct file *file, void *priv,
 {
 	const struct dcmipp_bytecap_pix_map *vpix;
 	unsigned int index = f->index;
-	unsigned int i;
+	unsigned int i, prev_pixelformat = 0;
 
-	if (f->mbus_code) {
-		/*
-		 * If a media bus code is specified, only enumerate formats
-		 * compatible with it.
-		 */
-		for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
-			vpix = &dcmipp_bytecap_pix_map_list[i];
-			if (vpix->code != f->mbus_code)
-				continue;
+	/*
+	 * List up all formats (or only ones matching f->mbus_code), taking
+	 * care of removing duplicated entries (due to support of both
+	 * parallel & csi 16 bits formats
+	 */
+	for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
+		vpix = &dcmipp_bytecap_pix_map_list[i];
+		/* Skip formats not matching requested mbus code */
+		if (f->mbus_code && vpix->code != f->mbus_code)
+			continue;
 
-			if (index == 0)
-				break;
+		/* Skip duplicated pixelformat */
+		if (vpix->pixelformat == prev_pixelformat)
+			continue;
 
-			index--;
-		}
+		prev_pixelformat = vpix->pixelformat;
 
-		if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
-			return -EINVAL;
-	} else {
-		/* Otherwise, enumerate all formats. */
-		if (f->index >= ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
-			return -EINVAL;
+		if (index == 0)
+			break;
 
-		vpix = &dcmipp_bytecap_pix_map_list[f->index];
+		index--;
 	}
 
+	if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
+		return -EINVAL;
+
 	f->pixelformat = vpix->pixelformat;
 
 	return 0;
@@ -337,33 +355,6 @@ static const struct v4l2_ioctl_ops dcmipp_bytecap_ioctl_ops = {
 	.vidioc_streamoff = vb2_ioctl_streamoff,
 };
 
-static int dcmipp_pipeline_s_stream(struct dcmipp_bytecap_device *vcap,
-				    int state)
-{
-	struct media_pad *pad;
-	int ret;
-
-	/*
-	 * Get source subdev - since link is IMMUTABLE, pointer is cached
-	 * within the dcmipp_bytecap_device structure
-	 */
-	if (!vcap->s_subdev) {
-		pad = media_pad_remote_pad_first(&vcap->vdev.entity.pads[0]);
-		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
-			return -EINVAL;
-		vcap->s_subdev = media_entity_to_v4l2_subdev(pad->entity);
-	}
-
-	ret = v4l2_subdev_call(vcap->s_subdev, video, s_stream, state);
-	if (ret < 0) {
-		dev_err(vcap->dev, "failed to %s streaming (%d)\n",
-			state ? "start" : "stop", ret);
-		return ret;
-	}
-
-	return 0;
-}
-
 static void dcmipp_start_capture(struct dcmipp_bytecap_device *vcap,
 				 struct dcmipp_buf *buf)
 {
@@ -395,11 +386,24 @@ static int dcmipp_bytecap_start_streaming(struct vb2_queue *vq,
 	struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
 	struct media_entity *entity = &vcap->vdev.entity;
 	struct dcmipp_buf *buf;
+	struct media_pad *pad;
 	int ret;
 
 	vcap->sequence = 0;
 	memset(&vcap->count, 0, sizeof(vcap->count));
 
+	/*
+	 * Get source subdev - since link is IMMUTABLE, pointer is cached
+	 * within the dcmipp_bytecap_device structure
+	 */
+	if (!vcap->s_subdev) {
+		pad = media_pad_remote_pad_first(&vcap->vdev.entity.pads[0]);
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+			return -EINVAL;
+		vcap->s_subdev = media_entity_to_v4l2_subdev(pad->entity);
+		vcap->s_subdev_pad_nb = pad->index;
+	}
+
 	ret = pm_runtime_resume_and_get(vcap->dev);
 	if (ret < 0) {
 		dev_err(vcap->dev, "%s: Failed to start streaming, cannot get sync (%d)\n",
@@ -414,7 +418,8 @@ static int dcmipp_bytecap_start_streaming(struct vb2_queue *vq,
 		goto err_pm_put;
 	}
 
-	ret = dcmipp_pipeline_s_stream(vcap, 1);
+	ret = v4l2_subdev_enable_streams(vcap->s_subdev,
+					 vcap->s_subdev_pad_nb, BIT_ULL(0));
 	if (ret)
 		goto err_media_pipeline_stop;
 
@@ -482,7 +487,10 @@ static void dcmipp_bytecap_stop_streaming(struct vb2_queue *vq)
 	int ret;
 	u32 status;
 
-	dcmipp_pipeline_s_stream(vcap, 0);
+	ret = v4l2_subdev_disable_streams(vcap->s_subdev,
+					  vcap->s_subdev_pad_nb, BIT_ULL(0));
+	if (ret)
+		dev_warn(vcap->dev, "Failed to disable stream\n");
 
 	/* Stop the media pipeline */
 	media_pipeline_stop(vcap->vdev.entity.pads);
@@ -625,12 +633,6 @@ static const struct vb2_ops dcmipp_bytecap_qops = {
 	.buf_prepare		= dcmipp_bytecap_buf_prepare,
 	.buf_queue		= dcmipp_bytecap_buf_queue,
 	.queue_setup		= dcmipp_bytecap_queue_setup,
-	/*
-	 * Since q->lock is set we can use the standard
-	 * vb2_ops_wait_prepare/finish helper functions.
-	 */
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static void dcmipp_bytecap_release(struct video_device *vdev)
@@ -816,8 +818,7 @@ static int dcmipp_bytecap_link_validate(struct media_link *link)
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		.pad = link->source->index,
 	};
-	const struct dcmipp_bytecap_pix_map *vpix;
-	int ret;
+	int ret, i;
 
 	ret = v4l2_subdev_call(source_sd, pad, get_fmt, NULL, &source_fmt);
 	if (ret < 0)
@@ -831,10 +832,17 @@ static int dcmipp_bytecap_link_validate(struct media_link *link)
 		return -EINVAL;
 	}
 
-	vpix = dcmipp_bytecap_pix_map_by_pixelformat(vcap->format.pixelformat);
-	if (source_fmt.format.code != vpix->code) {
-		dev_err(vcap->dev, "Wrong mbus_code 0x%x, (0x%x expected)\n",
-			vpix->code, source_fmt.format.code);
+	for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
+		if (dcmipp_bytecap_pix_map_list[i].pixelformat ==
+			vcap->format.pixelformat &&
+		    dcmipp_bytecap_pix_map_list[i].code ==
+			source_fmt.format.code)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list)) {
+		dev_err(vcap->dev, "mbus code 0x%x do not match capture device format (0x%x)\n",
+			vcap->format.pixelformat, source_fmt.format.code);
 		return -EINVAL;
 	}
 
@@ -893,7 +901,7 @@ struct dcmipp_ent_device *dcmipp_bytecap_ent_init(struct device *dev,
 	q->dev = dev;
 
 	/* DCMIPP requires 16 bytes aligned buffers */
-	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32) & ~0x0f);
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
 	if (ret) {
 		dev_err(dev, "Failed to set DMA mask\n");
 		goto err_mutex_destroy;
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-byteproc.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-byteproc.c
index 5a361ad6b023..db76a02a1848 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-byteproc.c
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-byteproc.c
@@ -48,15 +48,32 @@ struct dcmipp_byteproc_pix_map {
 	}
 static const struct dcmipp_byteproc_pix_map dcmipp_byteproc_pix_map_list[] = {
 	PIXMAP_MBUS_BPP(RGB565_2X8_LE, 2),
+	PIXMAP_MBUS_BPP(RGB565_1X16, 2),
 	PIXMAP_MBUS_BPP(YUYV8_2X8, 2),
+	PIXMAP_MBUS_BPP(YUYV8_1X16, 2),
 	PIXMAP_MBUS_BPP(YVYU8_2X8, 2),
+	PIXMAP_MBUS_BPP(YVYU8_1X16, 2),
 	PIXMAP_MBUS_BPP(UYVY8_2X8, 2),
+	PIXMAP_MBUS_BPP(UYVY8_1X16, 2),
 	PIXMAP_MBUS_BPP(VYUY8_2X8, 2),
+	PIXMAP_MBUS_BPP(VYUY8_1X16, 2),
 	PIXMAP_MBUS_BPP(Y8_1X8, 1),
 	PIXMAP_MBUS_BPP(SBGGR8_1X8, 1),
 	PIXMAP_MBUS_BPP(SGBRG8_1X8, 1),
 	PIXMAP_MBUS_BPP(SGRBG8_1X8, 1),
 	PIXMAP_MBUS_BPP(SRGGB8_1X8, 1),
+	PIXMAP_MBUS_BPP(SBGGR10_1X10, 2),
+	PIXMAP_MBUS_BPP(SGBRG10_1X10, 2),
+	PIXMAP_MBUS_BPP(SGRBG10_1X10, 2),
+	PIXMAP_MBUS_BPP(SRGGB10_1X10, 2),
+	PIXMAP_MBUS_BPP(SBGGR12_1X12, 2),
+	PIXMAP_MBUS_BPP(SGBRG12_1X12, 2),
+	PIXMAP_MBUS_BPP(SGRBG12_1X12, 2),
+	PIXMAP_MBUS_BPP(SRGGB12_1X12, 2),
+	PIXMAP_MBUS_BPP(SBGGR14_1X14, 2),
+	PIXMAP_MBUS_BPP(SGBRG14_1X14, 2),
+	PIXMAP_MBUS_BPP(SGRBG14_1X14, 2),
+	PIXMAP_MBUS_BPP(SRGGB14_1X14, 2),
 	PIXMAP_MBUS_BPP(JPEG_1X8, 1),
 };
 
@@ -78,7 +95,6 @@ struct dcmipp_byteproc_device {
 	struct v4l2_subdev sd;
 	struct device *dev;
 	void __iomem *regs;
-	bool streaming;
 };
 
 static const struct v4l2_mbus_framefmt fmt_default = {
@@ -239,11 +255,10 @@ static int dcmipp_byteproc_set_fmt(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state,
 				   struct v4l2_subdev_format *fmt)
 {
-	struct dcmipp_byteproc_device *byteproc = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf;
 	struct v4l2_rect *crop, *compose;
 
-	if (byteproc->streaming)
+	if (v4l2_subdev_is_streaming(sd))
 		return -EBUSY;
 
 	mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
@@ -358,8 +373,8 @@ static int dcmipp_byteproc_set_selection(struct v4l2_subdev *sd,
 		mf->width = s->r.width;
 		mf->height = s->r.height;
 
-		dev_dbg(byteproc->dev, "s_selection: crop %ux%u@(%u,%u)\n",
-			crop->width, crop->height, crop->left, crop->top);
+		dev_dbg(byteproc->dev, "s_selection: crop (%d,%d)/%ux%u\n",
+			crop->left, crop->top, crop->width, crop->height);
 		break;
 	case V4L2_SEL_TGT_COMPOSE:
 		mf = v4l2_subdev_state_get_format(sd_state, 0);
@@ -371,9 +386,9 @@ static int dcmipp_byteproc_set_selection(struct v4l2_subdev *sd,
 		mf->width = s->r.width;
 		mf->height = s->r.height;
 
-		dev_dbg(byteproc->dev, "s_selection: compose %ux%u@(%u,%u)\n",
-			compose->width, compose->height,
-			compose->left, compose->top);
+		dev_dbg(byteproc->dev, "s_selection: compose (%d,%d)/%ux%u\n",
+			compose->left, compose->top,
+			compose->width, compose->height);
 		break;
 	default:
 		return -EINVAL;
@@ -382,30 +397,19 @@ static int dcmipp_byteproc_set_selection(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static const struct v4l2_subdev_pad_ops dcmipp_byteproc_pad_ops = {
-	.enum_mbus_code		= dcmipp_byteproc_enum_mbus_code,
-	.enum_frame_size	= dcmipp_byteproc_enum_frame_size,
-	.get_fmt		= v4l2_subdev_get_fmt,
-	.set_fmt		= dcmipp_byteproc_set_fmt,
-	.get_selection		= dcmipp_byteproc_get_selection,
-	.set_selection		= dcmipp_byteproc_set_selection,
-};
-
 static int dcmipp_byteproc_configure_scale_crop
-			(struct dcmipp_byteproc_device *byteproc)
+			(struct dcmipp_byteproc_device *byteproc,
+			 struct v4l2_subdev_state *state)
 {
 	const struct dcmipp_byteproc_pix_map *vpix;
-	struct v4l2_subdev_state *state;
 	struct v4l2_mbus_framefmt *sink_fmt;
 	u32 hprediv, vprediv;
 	struct v4l2_rect *compose, *crop;
 	u32 val = 0;
 
-	state = v4l2_subdev_lock_and_get_active_state(&byteproc->sd);
 	sink_fmt = v4l2_subdev_state_get_format(state, 0);
 	compose = v4l2_subdev_state_get_compose(state, 0);
 	crop = v4l2_subdev_state_get_crop(state, 1);
-	v4l2_subdev_unlock_state(state);
 
 	/* find output format bpp */
 	vpix = dcmipp_byteproc_pix_map_by_code(sink_fmt->code);
@@ -460,48 +464,73 @@ static int dcmipp_byteproc_configure_scale_crop
 	return 0;
 }
 
-static int dcmipp_byteproc_s_stream(struct v4l2_subdev *sd, int enable)
+static int dcmipp_byteproc_enable_streams(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_state *state,
+					  u32 pad, u64 streams_mask)
 {
 	struct dcmipp_byteproc_device *byteproc = v4l2_get_subdevdata(sd);
 	struct v4l2_subdev *s_subdev;
-	struct media_pad *pad;
-	int ret = 0;
+	struct media_pad *s_pad;
+	int ret;
 
 	/* Get source subdev */
-	pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
-	if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
 		return -EINVAL;
-	s_subdev = media_entity_to_v4l2_subdev(pad->entity);
-
-	if (enable) {
-		ret = dcmipp_byteproc_configure_scale_crop(byteproc);
-		if (ret)
-			return ret;
-
-		ret = v4l2_subdev_call(s_subdev, video, s_stream, enable);
-		if (ret < 0) {
-			dev_err(byteproc->dev,
-				"failed to start source subdev streaming (%d)\n",
-				ret);
-			return ret;
-		}
-	} else {
-		ret = v4l2_subdev_call(s_subdev, video, s_stream, enable);
-		if (ret < 0) {
-			dev_err(byteproc->dev,
-				"failed to stop source subdev streaming (%d)\n",
-				ret);
-			return ret;
-		}
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	ret = dcmipp_byteproc_configure_scale_crop(byteproc, state);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_enable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(byteproc->dev,
+			"failed to start source subdev streaming (%d)\n", ret);
+		return ret;
 	}
 
-	byteproc->streaming = enable;
+	return 0;
+}
+
+static int dcmipp_byteproc_disable_streams(struct v4l2_subdev *sd,
+					   struct v4l2_subdev_state *state,
+					   u32 pad, u64 streams_mask)
+{
+	struct dcmipp_byteproc_device *byteproc = v4l2_get_subdevdata(sd);
+	struct v4l2_subdev *s_subdev;
+	struct media_pad *s_pad;
+	int ret;
+
+	/* Get source subdev */
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
+		return -EINVAL;
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	ret = v4l2_subdev_disable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(byteproc->dev,
+			"failed to start source subdev streaming (%d)\n", ret);
+		return ret;
+	}
 
 	return 0;
 }
 
+static const struct v4l2_subdev_pad_ops dcmipp_byteproc_pad_ops = {
+	.enum_mbus_code		= dcmipp_byteproc_enum_mbus_code,
+	.enum_frame_size	= dcmipp_byteproc_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= dcmipp_byteproc_set_fmt,
+	.get_selection		= dcmipp_byteproc_get_selection,
+	.set_selection		= dcmipp_byteproc_set_selection,
+	.enable_streams		= dcmipp_byteproc_enable_streams,
+	.disable_streams	= dcmipp_byteproc_disable_streams,
+};
+
 static const struct v4l2_subdev_video_ops dcmipp_byteproc_video_ops = {
-	.s_stream = dcmipp_byteproc_s_stream,
+	.s_stream = v4l2_subdev_s_stream_helper,
 };
 
 static const struct v4l2_subdev_ops dcmipp_byteproc_ops = {
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
index 7a7cf43baf24..fe5f97233f5e 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-common.h
@@ -199,11 +199,11 @@ static inline void __reg_clear(struct device *dev, void __iomem *base, u32 reg,
 }
 
 /* DCMIPP subdev init / release entry points */
-struct dcmipp_ent_device *dcmipp_par_ent_init(struct device *dev,
+struct dcmipp_ent_device *dcmipp_inp_ent_init(struct device *dev,
 					      const char *entity_name,
 					      struct v4l2_device *v4l2_dev,
 					      void __iomem *regs);
-void dcmipp_par_ent_release(struct dcmipp_ent_device *ved);
+void dcmipp_inp_ent_release(struct dcmipp_ent_device *ved);
 struct dcmipp_ent_device *
 dcmipp_byteproc_ent_init(struct device *dev, const char *entity_name,
 			 struct v4l2_device *v4l2_dev, void __iomem *regs);
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-core.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-core.c
index bce821eb71ce..1b7bae3266c8 100644
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-core.c
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-core.c
@@ -40,6 +40,7 @@ struct dcmipp_device {
 
 	/* Hardware resources */
 	void __iomem			*regs;
+	struct clk			*mclk;
 	struct clk			*kclk;
 
 	/* The pipeline configuration */
@@ -87,6 +88,9 @@ struct dcmipp_pipeline_config {
 	size_t num_ents;
 	const struct dcmipp_ent_link *links;
 	size_t num_links;
+	u32 hw_revision;
+	bool has_csi2;
+	bool needs_mclk;
 };
 
 /* --------------------------------------------------------------------------
@@ -95,9 +99,9 @@ struct dcmipp_pipeline_config {
 
 static const struct dcmipp_ent_config stm32mp13_ent_config[] = {
 	{
-		.name = "dcmipp_parallel",
-		.init = dcmipp_par_ent_init,
-		.release = dcmipp_par_ent_release,
+		.name = "dcmipp_input",
+		.init = dcmipp_inp_ent_init,
+		.release = dcmipp_inp_ent_release,
 	},
 	{
 		.name = "dcmipp_dump_postproc",
@@ -111,22 +115,60 @@ static const struct dcmipp_ent_config stm32mp13_ent_config[] = {
 	},
 };
 
-#define ID_PARALLEL 0
+#define ID_INPUT 0
 #define ID_DUMP_BYTEPROC 1
 #define ID_DUMP_CAPTURE 2
 
 static const struct dcmipp_ent_link stm32mp13_ent_links[] = {
-	DCMIPP_ENT_LINK(ID_PARALLEL,      1, ID_DUMP_BYTEPROC, 0,
+	DCMIPP_ENT_LINK(ID_INPUT, 1, ID_DUMP_BYTEPROC, 0,
 			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
 	DCMIPP_ENT_LINK(ID_DUMP_BYTEPROC, 1, ID_DUMP_CAPTURE,  0,
 			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
 };
 
+#define DCMIPP_STM32MP13_VERR	0x10
 static const struct dcmipp_pipeline_config stm32mp13_pipe_cfg = {
 	.ents		= stm32mp13_ent_config,
 	.num_ents	= ARRAY_SIZE(stm32mp13_ent_config),
 	.links		= stm32mp13_ent_links,
-	.num_links	= ARRAY_SIZE(stm32mp13_ent_links)
+	.num_links	= ARRAY_SIZE(stm32mp13_ent_links),
+	.hw_revision	= DCMIPP_STM32MP13_VERR
+};
+
+static const struct dcmipp_ent_config stm32mp25_ent_config[] = {
+	{
+		.name = "dcmipp_input",
+		.init = dcmipp_inp_ent_init,
+		.release = dcmipp_inp_ent_release,
+	},
+	{
+		.name = "dcmipp_dump_postproc",
+		.init = dcmipp_byteproc_ent_init,
+		.release = dcmipp_byteproc_ent_release,
+	},
+	{
+		.name = "dcmipp_dump_capture",
+		.init = dcmipp_bytecap_ent_init,
+		.release = dcmipp_bytecap_ent_release,
+	},
+};
+
+static const struct dcmipp_ent_link stm32mp25_ent_links[] = {
+	DCMIPP_ENT_LINK(ID_INPUT, 1, ID_DUMP_BYTEPROC, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+	DCMIPP_ENT_LINK(ID_DUMP_BYTEPROC, 1, ID_DUMP_CAPTURE,  0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+};
+
+#define DCMIPP_STM32MP25_VERR  0x30
+static const struct dcmipp_pipeline_config stm32mp25_pipe_cfg = {
+	.ents		= stm32mp25_ent_config,
+	.num_ents	= ARRAY_SIZE(stm32mp25_ent_config),
+	.links		= stm32mp25_ent_links,
+	.num_links	= ARRAY_SIZE(stm32mp25_ent_links),
+	.hw_revision    = DCMIPP_STM32MP25_VERR,
+	.has_csi2	= true,
+	.needs_mclk	= true
 };
 
 #define LINK_FLAG_TO_STR(f) ((f) == 0 ? "" :\
@@ -202,13 +244,14 @@ static int dcmipp_create_subdevs(struct dcmipp_device *dcmipp)
 	return 0;
 
 err_init_entity:
-	while (i > 0)
-		dcmipp->pipe_cfg->ents[i - 1].release(dcmipp->entity[i - 1]);
+	while (i-- > 0)
+		dcmipp->pipe_cfg->ents[i].release(dcmipp->entity[i]);
 	return ret;
 }
 
 static const struct of_device_id dcmipp_of_match[] = {
 	{ .compatible = "st,stm32mp13-dcmipp", .data = &stm32mp13_pipe_cfg },
+	{ .compatible = "st,stm32mp25-dcmipp", .data = &stm32mp25_pipe_cfg },
 	{ /* end node */ },
 };
 MODULE_DEVICE_TABLE(of, dcmipp_of_match);
@@ -257,11 +300,14 @@ static int dcmipp_graph_notify_bound(struct v4l2_async_notifier *notifier,
 				     struct v4l2_async_connection *asd)
 {
 	struct dcmipp_device *dcmipp = notifier_to_dcmipp(notifier);
-	unsigned int ret;
-	int src_pad;
+	int ret = -EINVAL;
+	int src_pad, i;
 	struct dcmipp_ent_device *sink;
-	struct v4l2_fwnode_endpoint vep = { .bus_type = V4L2_MBUS_PARALLEL };
+	struct v4l2_fwnode_endpoint vep = { 0 };
 	struct fwnode_handle *ep;
+	enum v4l2_mbus_type supported_types[] = {
+		V4L2_MBUS_PARALLEL, V4L2_MBUS_BT656, V4L2_MBUS_CSI2_DPHY
+	};
 
 	dev_dbg(dcmipp->dev, "Subdev \"%s\" bound\n", subdev->name);
 
@@ -281,21 +327,28 @@ static int dcmipp_graph_notify_bound(struct v4l2_async_notifier *notifier,
 		return -ENODEV;
 	}
 
-	/* Check for parallel bus-type first, then bt656 */
-	ret = v4l2_fwnode_endpoint_parse(ep, &vep);
-	if (ret) {
-		vep.bus_type = V4L2_MBUS_BT656;
+	/* Check for supported MBUS type */
+	for (i = 0; i < ARRAY_SIZE(supported_types); i++) {
+		/* Only MP25 supports CSI input */
+		if (supported_types[i] == V4L2_MBUS_CSI2_DPHY &&
+		    !dcmipp->pipe_cfg->has_csi2)
+			continue;
+
+		vep.bus_type = supported_types[i];
 		ret = v4l2_fwnode_endpoint_parse(ep, &vep);
-		if (ret) {
-			dev_err(dcmipp->dev, "Could not parse the endpoint\n");
-			fwnode_handle_put(ep);
-			return ret;
-		}
+		if (!ret)
+			break;
 	}
 
 	fwnode_handle_put(ep);
 
-	if (vep.bus.parallel.bus_width == 0) {
+	if (ret) {
+		dev_err(dcmipp->dev, "Could not parse the endpoint\n");
+		return ret;
+	}
+
+	if (vep.bus_type != V4L2_MBUS_CSI2_DPHY &&
+	    vep.bus.parallel.bus_width == 0) {
 		dev_err(dcmipp->dev, "Invalid parallel interface bus-width\n");
 		return -ENODEV;
 	}
@@ -308,11 +361,13 @@ static int dcmipp_graph_notify_bound(struct v4l2_async_notifier *notifier,
 		return -ENODEV;
 	}
 
-	/* Parallel input device detected, connect it to parallel subdev */
-	sink = dcmipp->entity[ID_PARALLEL];
-	sink->bus.flags = vep.bus.parallel.flags;
-	sink->bus.bus_width = vep.bus.parallel.bus_width;
-	sink->bus.data_shift = vep.bus.parallel.data_shift;
+	/* Connect input device to the dcmipp_input subdev */
+	sink = dcmipp->entity[ID_INPUT];
+	if (vep.bus_type != V4L2_MBUS_CSI2_DPHY) {
+		sink->bus.flags = vep.bus.parallel.flags;
+		sink->bus.bus_width = vep.bus.parallel.bus_width;
+		sink->bus.data_shift = vep.bus.parallel.data_shift;
+	}
 	sink->bus_type = vep.bus_type;
 	ret = media_create_pad_link(&subdev->entity, src_pad, sink->ent, 0,
 				    MEDIA_LNK_FL_IMMUTABLE |
@@ -411,7 +466,7 @@ static int dcmipp_graph_init(struct dcmipp_device *dcmipp)
 static int dcmipp_probe(struct platform_device *pdev)
 {
 	struct dcmipp_device *dcmipp;
-	struct clk *kclk;
+	struct clk *kclk, *mclk;
 	const struct dcmipp_pipeline_config *pipe_cfg;
 	struct reset_control *rstc;
 	int irq;
@@ -439,11 +494,8 @@ static int dcmipp_probe(struct platform_device *pdev)
 				     "Could not get reset control\n");
 
 	irq = platform_get_irq(pdev, 0);
-	if (irq <= 0) {
-		if (irq != -EPROBE_DEFER)
-			dev_err(&pdev->dev, "Could not get irq\n");
-		return irq ? irq : -ENXIO;
-	}
+	if (irq < 0)
+		return irq;
 
 	dcmipp->regs = devm_platform_get_and_ioremap_resource(pdev, 0, NULL);
 	if (IS_ERR(dcmipp->regs)) {
@@ -474,12 +526,20 @@ static int dcmipp_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	kclk = devm_clk_get(&pdev->dev, NULL);
+	kclk = devm_clk_get(&pdev->dev, "kclk");
 	if (IS_ERR(kclk))
 		return dev_err_probe(&pdev->dev, PTR_ERR(kclk),
 				     "Unable to get kclk\n");
 	dcmipp->kclk = kclk;
 
+	if (dcmipp->pipe_cfg->needs_mclk) {
+		mclk = devm_clk_get(&pdev->dev, "mclk");
+		if (IS_ERR(mclk))
+			return dev_err_probe(&pdev->dev, PTR_ERR(mclk),
+					     "Unable to get mclk\n");
+		dcmipp->mclk = mclk;
+	}
+
 	dcmipp->entity = devm_kcalloc(&pdev->dev, dcmipp->pipe_cfg->num_ents,
 				      sizeof(*dcmipp->entity), GFP_KERNEL);
 	if (!dcmipp->entity)
@@ -499,6 +559,7 @@ static int dcmipp_probe(struct platform_device *pdev)
 	/* Initialize media device */
 	strscpy(dcmipp->mdev.model, DCMIPP_MDEV_MODEL_NAME,
 		sizeof(dcmipp->mdev.model));
+	dcmipp->mdev.hw_revision = pipe_cfg->hw_revision;
 	dcmipp->mdev.dev = &pdev->dev;
 	media_device_init(&dcmipp->mdev);
 
@@ -541,6 +602,7 @@ static int dcmipp_runtime_suspend(struct device *dev)
 	struct dcmipp_device *dcmipp = dev_get_drvdata(dev);
 
 	clk_disable_unprepare(dcmipp->kclk);
+	clk_disable_unprepare(dcmipp->mclk);
 
 	return 0;
 }
@@ -550,9 +612,17 @@ static int dcmipp_runtime_resume(struct device *dev)
 	struct dcmipp_device *dcmipp = dev_get_drvdata(dev);
 	int ret;
 
+	ret = clk_prepare_enable(dcmipp->mclk);
+	if (ret) {
+		dev_err(dev, "%s: Failed to prepare_enable mclk\n", __func__);
+		return ret;
+	}
+
 	ret = clk_prepare_enable(dcmipp->kclk);
-	if (ret)
+	if (ret) {
+		clk_disable_unprepare(dcmipp->mclk);
 		dev_err(dev, "%s: Failed to prepare_enable kclk\n", __func__);
+	}
 
 	return ret;
 }
@@ -586,7 +656,7 @@ static const struct dev_pm_ops dcmipp_pm_ops = {
 
 static struct platform_driver dcmipp_pdrv = {
 	.probe		= dcmipp_probe,
-	.remove_new	= dcmipp_remove,
+	.remove		= dcmipp_remove,
 	.driver		= {
 		.name	= DCMIPP_PDEV_NAME,
 		.of_match_table = dcmipp_of_match,
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-input.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-input.c
new file mode 100644
index 000000000000..7e5311b67d7e
--- /dev/null
+++ b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-input.c
@@ -0,0 +1,540 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Driver for STM32 Digital Camera Memory Interface Pixel Processor
+ *
+ * Copyright (C) STMicroelectronics SA 2023
+ * Authors: Hugues Fruchet <hugues.fruchet@foss.st.com>
+ *          Alain Volmat <alain.volmat@foss.st.com>
+ *          for STMicroelectronics.
+ */
+
+#include <linux/v4l2-mediabus.h>
+#include <media/mipi-csi2.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+#include "dcmipp-common.h"
+
+#define DCMIPP_PRCR	0x104
+#define DCMIPP_PRCR_FORMAT_SHIFT	16
+#define DCMIPP_PRCR_FORMAT_YUV422	0x1e
+#define DCMIPP_PRCR_FORMAT_RGB565	0x22
+#define DCMIPP_PRCR_FORMAT_RAW8		0x2a
+#define DCMIPP_PRCR_FORMAT_RAW10	0x2b
+#define DCMIPP_PRCR_FORMAT_RAW12	0x2c
+#define DCMIPP_PRCR_FORMAT_RAW14	0x2d
+#define DCMIPP_PRCR_FORMAT_G8		0x4a
+#define DCMIPP_PRCR_FORMAT_BYTE_STREAM	0x5a
+#define DCMIPP_PRCR_ESS			BIT(4)
+#define DCMIPP_PRCR_PCKPOL		BIT(5)
+#define DCMIPP_PRCR_HSPOL		BIT(6)
+#define DCMIPP_PRCR_VSPOL		BIT(7)
+#define DCMIPP_PRCR_ENABLE		BIT(14)
+#define DCMIPP_PRCR_SWAPCYCLES		BIT(25)
+
+#define DCMIPP_PRESCR	0x108
+#define DCMIPP_PRESUR	0x10c
+
+#define DCMIPP_CMCR	0x204
+#define DCMIPP_CMCR_INSEL	BIT(0)
+
+#define DCMIPP_P0FSCR	0x404
+#define DCMIPP_P0FSCR_DTMODE_MASK	GENMASK(17, 16)
+#define DCMIPP_P0FSCR_DTMODE_SHIFT	16
+#define DCMIPP_P0FSCR_DTMODE_DTIDA	0x00
+#define DCMIPP_P0FSCR_DTMODE_ALLDT	0x03
+#define DCMIPP_P0FSCR_DTIDA_MASK	GENMASK(5, 0)
+#define DCMIPP_P0FSCR_DTIDA_SHIFT	0
+
+#define IS_SINK(pad) (!(pad))
+#define IS_SRC(pad)  ((pad))
+
+struct dcmipp_inp_pix_map {
+	unsigned int code_sink;
+	unsigned int code_src;
+	/* Parallel related information */
+	u8 prcr_format;
+	u8 prcr_swapcycles;
+	/* CSI related information */
+	unsigned int dt;
+};
+
+#define PIXMAP_SINK_SRC_PRCR_SWAP(sink, src, prcr, swap, data_type)	\
+	{							\
+		.code_sink = MEDIA_BUS_FMT_##sink,		\
+		.code_src = MEDIA_BUS_FMT_##src,		\
+		.prcr_format = DCMIPP_PRCR_FORMAT_##prcr,	\
+		.prcr_swapcycles = swap,			\
+		.dt = data_type,				\
+	}
+static const struct dcmipp_inp_pix_map dcmipp_inp_pix_map_list[] = {
+	/* RGB565 */
+	PIXMAP_SINK_SRC_PRCR_SWAP(RGB565_2X8_LE, RGB565_2X8_LE, RGB565, 1, MIPI_CSI2_DT_RGB565),
+	PIXMAP_SINK_SRC_PRCR_SWAP(RGB565_2X8_BE, RGB565_2X8_LE, RGB565, 0, MIPI_CSI2_DT_RGB565),
+	PIXMAP_SINK_SRC_PRCR_SWAP(RGB565_1X16, RGB565_1X16, RGB565, 0, MIPI_CSI2_DT_RGB565),
+	/* YUV422 */
+	PIXMAP_SINK_SRC_PRCR_SWAP(YUYV8_2X8, YUYV8_2X8, YUV422, 1, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(YUYV8_1X16, YUYV8_1X16, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(YUYV8_2X8, UYVY8_2X8, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(UYVY8_2X8, UYVY8_2X8, YUV422, 1, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(UYVY8_1X16, UYVY8_1X16, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(UYVY8_2X8, YUYV8_2X8, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(YVYU8_2X8, YVYU8_2X8, YUV422, 1, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(YVYU8_1X16, YVYU8_1X16, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(VYUY8_2X8, VYUY8_2X8, YUV422, 1, MIPI_CSI2_DT_YUV422_8B),
+	PIXMAP_SINK_SRC_PRCR_SWAP(VYUY8_1X16, VYUY8_1X16, YUV422, 0, MIPI_CSI2_DT_YUV422_8B),
+	/* GREY */
+	PIXMAP_SINK_SRC_PRCR_SWAP(Y8_1X8, Y8_1X8, G8, 0, MIPI_CSI2_DT_RAW8),
+	/* Raw Bayer */
+	PIXMAP_SINK_SRC_PRCR_SWAP(SBGGR8_1X8, SBGGR8_1X8, RAW8, 0, MIPI_CSI2_DT_RAW8),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGBRG8_1X8, SGBRG8_1X8, RAW8, 0, MIPI_CSI2_DT_RAW8),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGRBG8_1X8, SGRBG8_1X8, RAW8, 0, MIPI_CSI2_DT_RAW8),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SRGGB8_1X8, SRGGB8_1X8, RAW8, 0, MIPI_CSI2_DT_RAW8),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SBGGR10_1X10, SBGGR10_1X10, RAW10, 0, MIPI_CSI2_DT_RAW10),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGBRG10_1X10, SGBRG10_1X10, RAW10, 0, MIPI_CSI2_DT_RAW10),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGRBG10_1X10, SGRBG10_1X10, RAW10, 0, MIPI_CSI2_DT_RAW10),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SRGGB10_1X10, SRGGB10_1X10, RAW10, 0, MIPI_CSI2_DT_RAW10),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SBGGR12_1X12, SBGGR12_1X12, RAW12, 0, MIPI_CSI2_DT_RAW12),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGBRG12_1X12, SGBRG12_1X12, RAW12, 0, MIPI_CSI2_DT_RAW12),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGRBG12_1X12, SGRBG12_1X12, RAW12, 0, MIPI_CSI2_DT_RAW12),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SRGGB12_1X12, SRGGB12_1X12, RAW12, 0, MIPI_CSI2_DT_RAW12),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SBGGR14_1X14, SBGGR14_1X14, RAW14, 0, MIPI_CSI2_DT_RAW14),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGBRG14_1X14, SGBRG14_1X14, RAW14, 0, MIPI_CSI2_DT_RAW14),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SGRBG14_1X14, SGRBG14_1X14, RAW14, 0, MIPI_CSI2_DT_RAW14),
+	PIXMAP_SINK_SRC_PRCR_SWAP(SRGGB14_1X14, SRGGB14_1X14, RAW14, 0, MIPI_CSI2_DT_RAW14),
+	/* JPEG */
+	PIXMAP_SINK_SRC_PRCR_SWAP(JPEG_1X8, JPEG_1X8, BYTE_STREAM, 0, 0),
+};
+
+/*
+ * Search through the pix_map table, skipping two consecutive entry with the
+ * same code
+ */
+static inline const struct dcmipp_inp_pix_map *dcmipp_inp_pix_map_by_index
+						(unsigned int index,
+						 unsigned int pad)
+{
+	unsigned int i = 0;
+	u32 prev_code = 0, cur_code;
+
+	while (i < ARRAY_SIZE(dcmipp_inp_pix_map_list)) {
+		if (IS_SRC(pad))
+			cur_code = dcmipp_inp_pix_map_list[i].code_src;
+		else
+			cur_code = dcmipp_inp_pix_map_list[i].code_sink;
+
+		if (cur_code == prev_code) {
+			i++;
+			continue;
+		}
+		prev_code = cur_code;
+
+		if (index == 0)
+			break;
+		i++;
+		index--;
+	}
+
+	if (i >= ARRAY_SIZE(dcmipp_inp_pix_map_list))
+		return NULL;
+
+	return &dcmipp_inp_pix_map_list[i];
+}
+
+static inline const struct dcmipp_inp_pix_map *dcmipp_inp_pix_map_by_code
+					(u32 code_sink, u32 code_src)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(dcmipp_inp_pix_map_list); i++) {
+		if ((dcmipp_inp_pix_map_list[i].code_sink == code_sink &&
+		     dcmipp_inp_pix_map_list[i].code_src == code_src) ||
+		    (dcmipp_inp_pix_map_list[i].code_sink == code_src &&
+		     dcmipp_inp_pix_map_list[i].code_src == code_sink) ||
+		    (dcmipp_inp_pix_map_list[i].code_sink == code_sink &&
+		     code_src == 0) ||
+		    (code_sink == 0 &&
+		     dcmipp_inp_pix_map_list[i].code_src == code_src))
+			return &dcmipp_inp_pix_map_list[i];
+	}
+	return NULL;
+}
+
+struct dcmipp_inp_device {
+	struct dcmipp_ent_device ved;
+	struct v4l2_subdev sd;
+	struct device *dev;
+	void __iomem *regs;
+};
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+	.width = DCMIPP_FMT_WIDTH_DEFAULT,
+	.height = DCMIPP_FMT_HEIGHT_DEFAULT,
+	.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
+	.field = V4L2_FIELD_NONE,
+	.colorspace = DCMIPP_COLORSPACE_DEFAULT,
+	.ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
+	.quantization = DCMIPP_QUANTIZATION_DEFAULT,
+	.xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
+};
+
+static int dcmipp_inp_init_state(struct v4l2_subdev *sd,
+				 struct v4l2_subdev_state *sd_state)
+{
+	unsigned int i;
+
+	for (i = 0; i < sd->entity.num_pads; i++) {
+		struct v4l2_mbus_framefmt *mf;
+
+		mf = v4l2_subdev_state_get_format(sd_state, i);
+		*mf = fmt_default;
+	}
+
+	return 0;
+}
+
+static int dcmipp_inp_enum_mbus_code(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *sd_state,
+				     struct v4l2_subdev_mbus_code_enum *code)
+{
+	const struct dcmipp_inp_pix_map *vpix =
+		dcmipp_inp_pix_map_by_index(code->index, code->pad);
+
+	if (!vpix)
+		return -EINVAL;
+
+	code->code = IS_SRC(code->pad) ? vpix->code_src : vpix->code_sink;
+
+	return 0;
+}
+
+static int dcmipp_inp_enum_frame_size(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *sd_state,
+				      struct v4l2_subdev_frame_size_enum *fse)
+{
+	const struct dcmipp_inp_pix_map *vpix;
+
+	if (fse->index)
+		return -EINVAL;
+
+	/* Only accept code in the pix map table */
+	vpix = dcmipp_inp_pix_map_by_code(IS_SINK(fse->pad) ? fse->code : 0,
+					  IS_SRC(fse->pad) ? fse->code : 0);
+	if (!vpix)
+		return -EINVAL;
+
+	fse->min_width = DCMIPP_FRAME_MIN_WIDTH;
+	fse->max_width = DCMIPP_FRAME_MAX_WIDTH;
+	fse->min_height = DCMIPP_FRAME_MIN_HEIGHT;
+	fse->max_height = DCMIPP_FRAME_MAX_HEIGHT;
+
+	return 0;
+}
+
+static void dcmipp_inp_adjust_fmt(struct dcmipp_inp_device *inp,
+				  struct v4l2_mbus_framefmt *fmt, __u32 pad)
+{
+	const struct dcmipp_inp_pix_map *vpix;
+
+	/* Only accept code in the pix map table */
+	vpix = dcmipp_inp_pix_map_by_code(IS_SINK(pad) ? fmt->code : 0,
+					  IS_SRC(pad) ? fmt->code : 0);
+	if (!vpix)
+		fmt->code = fmt_default.code;
+
+	/* Exclude JPEG if BT656 bus is selected */
+	if (vpix && vpix->code_sink == MEDIA_BUS_FMT_JPEG_1X8 &&
+	    inp->ved.bus_type == V4L2_MBUS_BT656)
+		fmt->code = fmt_default.code;
+
+	fmt->width = clamp_t(u32, fmt->width, DCMIPP_FRAME_MIN_WIDTH,
+			     DCMIPP_FRAME_MAX_WIDTH) & ~1;
+	fmt->height = clamp_t(u32, fmt->height, DCMIPP_FRAME_MIN_HEIGHT,
+			      DCMIPP_FRAME_MAX_HEIGHT) & ~1;
+
+	if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
+		fmt->field = fmt_default.field;
+
+	dcmipp_colorimetry_clamp(fmt);
+}
+
+static int dcmipp_inp_set_fmt(struct v4l2_subdev *sd,
+			      struct v4l2_subdev_state *sd_state,
+			      struct v4l2_subdev_format *fmt)
+{
+	struct dcmipp_inp_device *inp = v4l2_get_subdevdata(sd);
+	struct v4l2_mbus_framefmt *mf;
+
+	if (v4l2_subdev_is_streaming(sd))
+		return -EBUSY;
+
+	mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
+
+	/* Set the new format */
+	dcmipp_inp_adjust_fmt(inp, &fmt->format, fmt->pad);
+
+	dev_dbg(inp->dev, "%s: format update: old:%dx%d (0x%x, %d, %d, %d, %d) new:%dx%d (0x%x, %d, %d, %d, %d)\n",
+		inp->sd.name,
+		/* old */
+		mf->width, mf->height, mf->code,
+		mf->colorspace,	mf->quantization,
+		mf->xfer_func, mf->ycbcr_enc,
+		/* new */
+		fmt->format.width, fmt->format.height, fmt->format.code,
+		fmt->format.colorspace, fmt->format.quantization,
+		fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+	*mf = fmt->format;
+
+	/* When setting the sink format, report that format on the src pad */
+	if (IS_SINK(fmt->pad)) {
+		mf = v4l2_subdev_state_get_format(sd_state, 1);
+		*mf = fmt->format;
+		dcmipp_inp_adjust_fmt(inp, mf, 1);
+	}
+
+	return 0;
+}
+
+static int dcmipp_inp_configure_parallel(struct dcmipp_inp_device *inp,
+					 struct v4l2_subdev_state *state)
+{
+	u32 val = 0;
+	const struct dcmipp_inp_pix_map *vpix;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+
+	/* Set vertical synchronization polarity */
+	if (inp->ved.bus.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
+		val |= DCMIPP_PRCR_VSPOL;
+
+	/* Set horizontal synchronization polarity */
+	if (inp->ved.bus.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
+		val |= DCMIPP_PRCR_HSPOL;
+
+	/* Set pixel clock polarity */
+	if (inp->ved.bus.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
+		val |= DCMIPP_PRCR_PCKPOL;
+
+	/*
+	 * BT656 embedded synchronisation bus mode.
+	 *
+	 * Default SAV/EAV mode is supported here with default codes
+	 * SAV=0xff000080 & EAV=0xff00009d.
+	 * With DCMIPP this means LSC=SAV=0x80 & LEC=EAV=0x9d.
+	 */
+	if (inp->ved.bus_type == V4L2_MBUS_BT656) {
+		val |= DCMIPP_PRCR_ESS;
+
+		/* Unmask all codes */
+		reg_write(inp, DCMIPP_PRESUR, 0xffffffff);/* FEC:LEC:LSC:FSC */
+
+		/* Trig on LSC=0x80 & LEC=0x9d codes, ignore FSC and FEC */
+		reg_write(inp, DCMIPP_PRESCR, 0xff9d80ff);/* FEC:LEC:LSC:FSC */
+	}
+
+	/* Set format */
+	sink_fmt = v4l2_subdev_state_get_format(state, 0);
+	src_fmt = v4l2_subdev_state_get_format(state, 1);
+
+	vpix = dcmipp_inp_pix_map_by_code(sink_fmt->code, src_fmt->code);
+	if (!vpix) {
+		dev_err(inp->dev, "Invalid sink/src format configuration\n");
+		return -EINVAL;
+	}
+
+	val |= vpix->prcr_format << DCMIPP_PRCR_FORMAT_SHIFT;
+
+	/* swap cycles */
+	if (vpix->prcr_swapcycles)
+		val |= DCMIPP_PRCR_SWAPCYCLES;
+
+	reg_write(inp, DCMIPP_PRCR, val);
+
+	/* Select the DCMIPP parallel interface */
+	reg_write(inp, DCMIPP_CMCR, 0);
+
+	/* Enable parallel interface */
+	reg_set(inp, DCMIPP_PRCR, DCMIPP_PRCR_ENABLE);
+
+	return 0;
+}
+
+static int dcmipp_inp_configure_csi(struct dcmipp_inp_device *inp,
+				    struct v4l2_subdev_state *state)
+{
+	const struct dcmipp_inp_pix_map *vpix;
+	struct v4l2_mbus_framefmt *sink_fmt;
+	struct v4l2_mbus_framefmt *src_fmt;
+
+	/* Get format information */
+	sink_fmt = v4l2_subdev_state_get_format(state, 0);
+	src_fmt = v4l2_subdev_state_get_format(state, 1);
+
+	vpix = dcmipp_inp_pix_map_by_code(sink_fmt->code, src_fmt->code);
+	if (!vpix) {
+		dev_err(inp->dev, "Invalid sink/src format configuration\n");
+		return -EINVAL;
+	}
+
+	/* Apply configuration on each input pipe */
+	reg_clear(inp, DCMIPP_P0FSCR,
+		  DCMIPP_P0FSCR_DTMODE_MASK | DCMIPP_P0FSCR_DTIDA_MASK);
+
+	/* In case of JPEG we don't know the DT so we allow all data */
+	/*
+	 * TODO - check instead dt == 0 for the time being to allow other
+	 * unknown data-type
+	 */
+	if (!vpix->dt)
+		reg_set(inp, DCMIPP_P0FSCR,
+			DCMIPP_P0FSCR_DTMODE_ALLDT << DCMIPP_P0FSCR_DTMODE_SHIFT);
+	else
+		reg_set(inp, DCMIPP_P0FSCR,
+			vpix->dt << DCMIPP_P0FSCR_DTIDA_SHIFT |
+			DCMIPP_P0FSCR_DTMODE_DTIDA);
+
+	/* Select the DCMIPP CSI interface */
+	reg_write(inp, DCMIPP_CMCR, DCMIPP_CMCR_INSEL);
+
+	return 0;
+}
+
+static int dcmipp_inp_enable_streams(struct v4l2_subdev *sd,
+				     struct v4l2_subdev_state *state,
+				     u32 pad, u64 streams_mask)
+{
+	struct dcmipp_inp_device *inp =
+				container_of(sd, struct dcmipp_inp_device, sd);
+	struct v4l2_subdev *s_subdev;
+	struct media_pad *s_pad;
+	int ret = 0;
+
+	/* Get source subdev */
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
+		return -EINVAL;
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	if (inp->ved.bus_type == V4L2_MBUS_PARALLEL ||
+	    inp->ved.bus_type == V4L2_MBUS_BT656)
+		ret = dcmipp_inp_configure_parallel(inp, state);
+	else if (inp->ved.bus_type == V4L2_MBUS_CSI2_DPHY)
+		ret = dcmipp_inp_configure_csi(inp, state);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_enable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(inp->dev,
+			"failed to start source subdev streaming (%d)\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dcmipp_inp_disable_streams(struct v4l2_subdev *sd,
+				      struct v4l2_subdev_state *state,
+				      u32 pad, u64 streams_mask)
+{
+	struct dcmipp_inp_device *inp =
+				container_of(sd, struct dcmipp_inp_device, sd);
+	struct v4l2_subdev *s_subdev;
+	struct media_pad *s_pad;
+	int ret;
+
+	/* Get source subdev */
+	s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
+	if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
+		return -EINVAL;
+	s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
+
+	ret = v4l2_subdev_disable_streams(s_subdev, s_pad->index, BIT_ULL(0));
+	if (ret < 0) {
+		dev_err(inp->dev,
+			"failed to stop source subdev streaming (%d)\n", ret);
+		return ret;
+	}
+
+	if (inp->ved.bus_type == V4L2_MBUS_PARALLEL ||
+	    inp->ved.bus_type == V4L2_MBUS_BT656) {
+		/* Disable parallel interface */
+		reg_clear(inp, DCMIPP_PRCR, DCMIPP_PRCR_ENABLE);
+	}
+
+	return 0;
+}
+
+static const struct v4l2_subdev_pad_ops dcmipp_inp_pad_ops = {
+	.enum_mbus_code		= dcmipp_inp_enum_mbus_code,
+	.enum_frame_size	= dcmipp_inp_enum_frame_size,
+	.get_fmt		= v4l2_subdev_get_fmt,
+	.set_fmt		= dcmipp_inp_set_fmt,
+	.enable_streams		= dcmipp_inp_enable_streams,
+	.disable_streams	= dcmipp_inp_disable_streams,
+};
+
+static const struct v4l2_subdev_video_ops dcmipp_inp_video_ops = {
+	.s_stream = v4l2_subdev_s_stream_helper,
+};
+
+static const struct v4l2_subdev_ops dcmipp_inp_ops = {
+	.pad = &dcmipp_inp_pad_ops,
+	.video = &dcmipp_inp_video_ops,
+};
+
+static void dcmipp_inp_release(struct v4l2_subdev *sd)
+{
+	struct dcmipp_inp_device *inp =
+				container_of(sd, struct dcmipp_inp_device, sd);
+
+	kfree(inp);
+}
+
+static const struct v4l2_subdev_internal_ops dcmipp_inp_int_ops = {
+	.init_state = dcmipp_inp_init_state,
+	.release = dcmipp_inp_release,
+};
+
+void dcmipp_inp_ent_release(struct dcmipp_ent_device *ved)
+{
+	struct dcmipp_inp_device *inp =
+			container_of(ved, struct dcmipp_inp_device, ved);
+
+	dcmipp_ent_sd_unregister(ved, &inp->sd);
+}
+
+struct dcmipp_ent_device *dcmipp_inp_ent_init(struct device *dev,
+					      const char *entity_name,
+					      struct v4l2_device *v4l2_dev,
+					      void __iomem *regs)
+{
+	struct dcmipp_inp_device *inp;
+	const unsigned long pads_flag[] = {
+		MEDIA_PAD_FL_SINK, MEDIA_PAD_FL_SOURCE,
+	};
+	int ret;
+
+	/* Allocate the inp struct */
+	inp = kzalloc(sizeof(*inp), GFP_KERNEL);
+	if (!inp)
+		return ERR_PTR(-ENOMEM);
+
+	inp->regs = regs;
+
+	/* Initialize ved and sd */
+	ret = dcmipp_ent_sd_register(&inp->ved, &inp->sd, v4l2_dev,
+				     entity_name, MEDIA_ENT_F_VID_IF_BRIDGE,
+				     ARRAY_SIZE(pads_flag), pads_flag,
+				     &dcmipp_inp_int_ops, &dcmipp_inp_ops,
+				     NULL, NULL);
+	if (ret) {
+		kfree(inp);
+		return ERR_PTR(ret);
+	}
+
+	inp->dev = dev;
+
+	return &inp->ved;
+}
diff --git a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-parallel.c b/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-parallel.c
deleted file mode 100644
index 62c5c3331cfe..000000000000
--- a/drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-parallel.c
+++ /dev/null
@@ -1,440 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * Driver for STM32 Digital Camera Memory Interface Pixel Processor
- *
- * Copyright (C) STMicroelectronics SA 2023
- * Authors: Hugues Fruchet <hugues.fruchet@foss.st.com>
- *          Alain Volmat <alain.volmat@foss.st.com>
- *          for STMicroelectronics.
- */
-
-#include <linux/v4l2-mediabus.h>
-#include <media/v4l2-event.h>
-#include <media/v4l2-subdev.h>
-
-#include "dcmipp-common.h"
-
-#define DCMIPP_PRCR	0x104
-#define DCMIPP_PRCR_FORMAT_SHIFT	16
-#define DCMIPP_PRCR_FORMAT_YUV422	0x1e
-#define DCMIPP_PRCR_FORMAT_RGB565	0x22
-#define DCMIPP_PRCR_FORMAT_RAW8		0x2a
-#define DCMIPP_PRCR_FORMAT_G8		0x4a
-#define DCMIPP_PRCR_FORMAT_BYTE_STREAM	0x5a
-#define DCMIPP_PRCR_ESS			BIT(4)
-#define DCMIPP_PRCR_PCKPOL		BIT(5)
-#define DCMIPP_PRCR_HSPOL		BIT(6)
-#define DCMIPP_PRCR_VSPOL		BIT(7)
-#define DCMIPP_PRCR_ENABLE		BIT(14)
-#define DCMIPP_PRCR_SWAPCYCLES		BIT(25)
-
-#define DCMIPP_PRESCR	0x108
-#define DCMIPP_PRESUR	0x10c
-
-#define IS_SINK(pad) (!(pad))
-#define IS_SRC(pad)  ((pad))
-
-struct dcmipp_par_pix_map {
-	unsigned int code_sink;
-	unsigned int code_src;
-	u8 prcr_format;
-	u8 prcr_swapcycles;
-};
-
-#define PIXMAP_SINK_SRC_PRCR_SWAP(sink, src, prcr, swap)	\
-	{							\
-		.code_sink = MEDIA_BUS_FMT_##sink,		\
-		.code_src = MEDIA_BUS_FMT_##src,		\
-		.prcr_format = DCMIPP_PRCR_FORMAT_##prcr,	\
-		.prcr_swapcycles = swap,			\
-	}
-static const struct dcmipp_par_pix_map dcmipp_par_pix_map_list[] = {
-	/* RGB565 */
-	PIXMAP_SINK_SRC_PRCR_SWAP(RGB565_2X8_LE, RGB565_2X8_LE, RGB565, 1),
-	PIXMAP_SINK_SRC_PRCR_SWAP(RGB565_2X8_BE, RGB565_2X8_LE, RGB565, 0),
-	/* YUV422 */
-	PIXMAP_SINK_SRC_PRCR_SWAP(YUYV8_2X8, YUYV8_2X8, YUV422, 1),
-	PIXMAP_SINK_SRC_PRCR_SWAP(YUYV8_2X8, UYVY8_2X8, YUV422, 0),
-	PIXMAP_SINK_SRC_PRCR_SWAP(UYVY8_2X8, UYVY8_2X8, YUV422, 1),
-	PIXMAP_SINK_SRC_PRCR_SWAP(UYVY8_2X8, YUYV8_2X8, YUV422, 0),
-	PIXMAP_SINK_SRC_PRCR_SWAP(YVYU8_2X8, YVYU8_2X8, YUV422, 1),
-	PIXMAP_SINK_SRC_PRCR_SWAP(VYUY8_2X8, VYUY8_2X8, YUV422, 1),
-	/* GREY */
-	PIXMAP_SINK_SRC_PRCR_SWAP(Y8_1X8, Y8_1X8, G8, 0),
-	/* Raw Bayer */
-	PIXMAP_SINK_SRC_PRCR_SWAP(SBGGR8_1X8, SBGGR8_1X8, RAW8, 0),
-	PIXMAP_SINK_SRC_PRCR_SWAP(SGBRG8_1X8, SGBRG8_1X8, RAW8, 0),
-	PIXMAP_SINK_SRC_PRCR_SWAP(SGRBG8_1X8, SGRBG8_1X8, RAW8, 0),
-	PIXMAP_SINK_SRC_PRCR_SWAP(SRGGB8_1X8, SRGGB8_1X8, RAW8, 0),
-	/* JPEG */
-	PIXMAP_SINK_SRC_PRCR_SWAP(JPEG_1X8, JPEG_1X8, BYTE_STREAM, 0),
-};
-
-/*
- * Search through the pix_map table, skipping two consecutive entry with the
- * same code
- */
-static inline const struct dcmipp_par_pix_map *dcmipp_par_pix_map_by_index
-						(unsigned int index,
-						 unsigned int pad)
-{
-	unsigned int i = 0;
-	u32 prev_code = 0, cur_code;
-
-	while (i < ARRAY_SIZE(dcmipp_par_pix_map_list)) {
-		if (IS_SRC(pad))
-			cur_code = dcmipp_par_pix_map_list[i].code_src;
-		else
-			cur_code = dcmipp_par_pix_map_list[i].code_sink;
-
-		if (cur_code == prev_code) {
-			i++;
-			continue;
-		}
-		prev_code = cur_code;
-
-		if (index == 0)
-			break;
-		i++;
-		index--;
-	}
-
-	if (i >= ARRAY_SIZE(dcmipp_par_pix_map_list))
-		return NULL;
-
-	return &dcmipp_par_pix_map_list[i];
-}
-
-static inline const struct dcmipp_par_pix_map *dcmipp_par_pix_map_by_code
-					(u32 code_sink, u32 code_src)
-{
-	unsigned int i;
-
-	for (i = 0; i < ARRAY_SIZE(dcmipp_par_pix_map_list); i++) {
-		if ((dcmipp_par_pix_map_list[i].code_sink == code_sink &&
-		     dcmipp_par_pix_map_list[i].code_src == code_src) ||
-		    (dcmipp_par_pix_map_list[i].code_sink == code_src &&
-		     dcmipp_par_pix_map_list[i].code_src == code_sink) ||
-		    (dcmipp_par_pix_map_list[i].code_sink == code_sink &&
-		     code_src == 0) ||
-		    (code_sink == 0 &&
-		     dcmipp_par_pix_map_list[i].code_src == code_src))
-			return &dcmipp_par_pix_map_list[i];
-	}
-	return NULL;
-}
-
-struct dcmipp_par_device {
-	struct dcmipp_ent_device ved;
-	struct v4l2_subdev sd;
-	struct device *dev;
-	void __iomem *regs;
-	bool streaming;
-};
-
-static const struct v4l2_mbus_framefmt fmt_default = {
-	.width = DCMIPP_FMT_WIDTH_DEFAULT,
-	.height = DCMIPP_FMT_HEIGHT_DEFAULT,
-	.code = MEDIA_BUS_FMT_RGB565_2X8_LE,
-	.field = V4L2_FIELD_NONE,
-	.colorspace = DCMIPP_COLORSPACE_DEFAULT,
-	.ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
-	.quantization = DCMIPP_QUANTIZATION_DEFAULT,
-	.xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
-};
-
-static int dcmipp_par_init_state(struct v4l2_subdev *sd,
-				 struct v4l2_subdev_state *sd_state)
-{
-	unsigned int i;
-
-	for (i = 0; i < sd->entity.num_pads; i++) {
-		struct v4l2_mbus_framefmt *mf;
-
-		mf = v4l2_subdev_state_get_format(sd_state, i);
-		*mf = fmt_default;
-	}
-
-	return 0;
-}
-
-static int dcmipp_par_enum_mbus_code(struct v4l2_subdev *sd,
-				     struct v4l2_subdev_state *sd_state,
-				     struct v4l2_subdev_mbus_code_enum *code)
-{
-	const struct dcmipp_par_pix_map *vpix =
-		dcmipp_par_pix_map_by_index(code->index, code->pad);
-
-	if (!vpix)
-		return -EINVAL;
-
-	code->code = IS_SRC(code->pad) ? vpix->code_src : vpix->code_sink;
-
-	return 0;
-}
-
-static int dcmipp_par_enum_frame_size(struct v4l2_subdev *sd,
-				      struct v4l2_subdev_state *sd_state,
-				      struct v4l2_subdev_frame_size_enum *fse)
-{
-	const struct dcmipp_par_pix_map *vpix;
-
-	if (fse->index)
-		return -EINVAL;
-
-	/* Only accept code in the pix map table */
-	vpix = dcmipp_par_pix_map_by_code(IS_SINK(fse->pad) ? fse->code : 0,
-					  IS_SRC(fse->pad) ? fse->code : 0);
-	if (!vpix)
-		return -EINVAL;
-
-	fse->min_width = DCMIPP_FRAME_MIN_WIDTH;
-	fse->max_width = DCMIPP_FRAME_MAX_WIDTH;
-	fse->min_height = DCMIPP_FRAME_MIN_HEIGHT;
-	fse->max_height = DCMIPP_FRAME_MAX_HEIGHT;
-
-	return 0;
-}
-
-static void dcmipp_par_adjust_fmt(struct dcmipp_par_device *par,
-				  struct v4l2_mbus_framefmt *fmt, __u32 pad)
-{
-	const struct dcmipp_par_pix_map *vpix;
-
-	/* Only accept code in the pix map table */
-	vpix = dcmipp_par_pix_map_by_code(IS_SINK(pad) ? fmt->code : 0,
-					  IS_SRC(pad) ? fmt->code : 0);
-	if (!vpix)
-		fmt->code = fmt_default.code;
-
-	/* Exclude JPEG if BT656 bus is selected */
-	if (vpix && vpix->code_sink == MEDIA_BUS_FMT_JPEG_1X8 &&
-	    par->ved.bus_type == V4L2_MBUS_BT656)
-		fmt->code = fmt_default.code;
-
-	fmt->width = clamp_t(u32, fmt->width, DCMIPP_FRAME_MIN_WIDTH,
-			     DCMIPP_FRAME_MAX_WIDTH) & ~1;
-	fmt->height = clamp_t(u32, fmt->height, DCMIPP_FRAME_MIN_HEIGHT,
-			      DCMIPP_FRAME_MAX_HEIGHT) & ~1;
-
-	if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
-		fmt->field = fmt_default.field;
-
-	dcmipp_colorimetry_clamp(fmt);
-}
-
-static int dcmipp_par_set_fmt(struct v4l2_subdev *sd,
-			      struct v4l2_subdev_state *sd_state,
-			      struct v4l2_subdev_format *fmt)
-{
-	struct dcmipp_par_device *par = v4l2_get_subdevdata(sd);
-	struct v4l2_mbus_framefmt *mf;
-
-	if (par->streaming)
-		return -EBUSY;
-
-	mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
-
-	/* Set the new format */
-	dcmipp_par_adjust_fmt(par, &fmt->format, fmt->pad);
-
-	dev_dbg(par->dev, "%s: format update: old:%dx%d (0x%x, %d, %d, %d, %d) new:%dx%d (0x%x, %d, %d, %d, %d)\n",
-		par->sd.name,
-		/* old */
-		mf->width, mf->height, mf->code,
-		mf->colorspace,	mf->quantization,
-		mf->xfer_func, mf->ycbcr_enc,
-		/* new */
-		fmt->format.width, fmt->format.height, fmt->format.code,
-		fmt->format.colorspace, fmt->format.quantization,
-		fmt->format.xfer_func, fmt->format.ycbcr_enc);
-
-	*mf = fmt->format;
-
-	/* When setting the sink format, report that format on the src pad */
-	if (IS_SINK(fmt->pad)) {
-		mf = v4l2_subdev_state_get_format(sd_state, 1);
-		*mf = fmt->format;
-		dcmipp_par_adjust_fmt(par, mf, 1);
-	}
-
-	return 0;
-}
-
-static const struct v4l2_subdev_pad_ops dcmipp_par_pad_ops = {
-	.enum_mbus_code		= dcmipp_par_enum_mbus_code,
-	.enum_frame_size	= dcmipp_par_enum_frame_size,
-	.get_fmt		= v4l2_subdev_get_fmt,
-	.set_fmt		= dcmipp_par_set_fmt,
-};
-
-static int dcmipp_par_configure(struct dcmipp_par_device *par)
-{
-	u32 val = 0;
-	const struct dcmipp_par_pix_map *vpix;
-	struct v4l2_subdev_state *state;
-	struct v4l2_mbus_framefmt *sink_fmt;
-	struct v4l2_mbus_framefmt *src_fmt;
-
-	/* Set vertical synchronization polarity */
-	if (par->ved.bus.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
-		val |= DCMIPP_PRCR_VSPOL;
-
-	/* Set horizontal synchronization polarity */
-	if (par->ved.bus.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
-		val |= DCMIPP_PRCR_HSPOL;
-
-	/* Set pixel clock polarity */
-	if (par->ved.bus.flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
-		val |= DCMIPP_PRCR_PCKPOL;
-
-	/*
-	 * BT656 embedded synchronisation bus mode.
-	 *
-	 * Default SAV/EAV mode is supported here with default codes
-	 * SAV=0xff000080 & EAV=0xff00009d.
-	 * With DCMIPP this means LSC=SAV=0x80 & LEC=EAV=0x9d.
-	 */
-	if (par->ved.bus_type == V4L2_MBUS_BT656) {
-		val |= DCMIPP_PRCR_ESS;
-
-		/* Unmask all codes */
-		reg_write(par, DCMIPP_PRESUR, 0xffffffff);/* FEC:LEC:LSC:FSC */
-
-		/* Trig on LSC=0x80 & LEC=0x9d codes, ignore FSC and FEC */
-		reg_write(par, DCMIPP_PRESCR, 0xff9d80ff);/* FEC:LEC:LSC:FSC */
-	}
-
-	/* Set format */
-	state = v4l2_subdev_lock_and_get_active_state(&par->sd);
-	sink_fmt = v4l2_subdev_state_get_format(state, 0);
-	src_fmt = v4l2_subdev_state_get_format(state, 1);
-	v4l2_subdev_unlock_state(state);
-
-	vpix = dcmipp_par_pix_map_by_code(sink_fmt->code, src_fmt->code);
-	if (!vpix) {
-		dev_err(par->dev, "Invalid sink/src format configuration\n");
-		return -EINVAL;
-	}
-
-	val |= vpix->prcr_format << DCMIPP_PRCR_FORMAT_SHIFT;
-
-	/* swap cycles */
-	if (vpix->prcr_swapcycles)
-		val |= DCMIPP_PRCR_SWAPCYCLES;
-
-	reg_write(par, DCMIPP_PRCR, val);
-
-	return 0;
-}
-
-static int dcmipp_par_s_stream(struct v4l2_subdev *sd, int enable)
-{
-	struct dcmipp_par_device *par =
-				container_of(sd, struct dcmipp_par_device, sd);
-	struct v4l2_subdev *s_subdev;
-	struct media_pad *pad;
-	int ret = 0;
-
-	/* Get source subdev */
-	pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
-	if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
-		return -EINVAL;
-	s_subdev = media_entity_to_v4l2_subdev(pad->entity);
-
-	if (enable) {
-		ret = dcmipp_par_configure(par);
-		if (ret)
-			return ret;
-
-		/* Enable parallel interface */
-		reg_set(par, DCMIPP_PRCR, DCMIPP_PRCR_ENABLE);
-
-		ret = v4l2_subdev_call(s_subdev, video, s_stream, enable);
-		if (ret < 0) {
-			dev_err(par->dev,
-				"failed to start source subdev streaming (%d)\n",
-				ret);
-			return ret;
-		}
-	} else {
-		ret = v4l2_subdev_call(s_subdev, video, s_stream, enable);
-		if (ret < 0) {
-			dev_err(par->dev,
-				"failed to stop source subdev streaming (%d)\n",
-				ret);
-			return ret;
-		}
-
-		/* Disable parallel interface */
-		reg_clear(par, DCMIPP_PRCR, DCMIPP_PRCR_ENABLE);
-	}
-
-	par->streaming = enable;
-
-	return ret;
-}
-
-static const struct v4l2_subdev_video_ops dcmipp_par_video_ops = {
-	.s_stream = dcmipp_par_s_stream,
-};
-
-static const struct v4l2_subdev_ops dcmipp_par_ops = {
-	.pad = &dcmipp_par_pad_ops,
-	.video = &dcmipp_par_video_ops,
-};
-
-static void dcmipp_par_release(struct v4l2_subdev *sd)
-{
-	struct dcmipp_par_device *par =
-				container_of(sd, struct dcmipp_par_device, sd);
-
-	kfree(par);
-}
-
-static const struct v4l2_subdev_internal_ops dcmipp_par_int_ops = {
-	.init_state = dcmipp_par_init_state,
-	.release = dcmipp_par_release,
-};
-
-void dcmipp_par_ent_release(struct dcmipp_ent_device *ved)
-{
-	struct dcmipp_par_device *par =
-			container_of(ved, struct dcmipp_par_device, ved);
-
-	dcmipp_ent_sd_unregister(ved, &par->sd);
-}
-
-struct dcmipp_ent_device *dcmipp_par_ent_init(struct device *dev,
-					      const char *entity_name,
-					      struct v4l2_device *v4l2_dev,
-					      void __iomem *regs)
-{
-	struct dcmipp_par_device *par;
-	const unsigned long pads_flag[] = {
-		MEDIA_PAD_FL_SINK, MEDIA_PAD_FL_SOURCE,
-	};
-	int ret;
-
-	/* Allocate the par struct */
-	par = kzalloc(sizeof(*par), GFP_KERNEL);
-	if (!par)
-		return ERR_PTR(-ENOMEM);
-
-	par->regs = regs;
-
-	/* Initialize ved and sd */
-	ret = dcmipp_ent_sd_register(&par->ved, &par->sd, v4l2_dev,
-				     entity_name, MEDIA_ENT_F_VID_IF_BRIDGE,
-				     ARRAY_SIZE(pads_flag), pads_flag,
-				     &dcmipp_par_int_ops, &dcmipp_par_ops,
-				     NULL, NULL);
-	if (ret) {
-		kfree(par);
-		return ERR_PTR(ret);
-	}
-
-	par->dev = dev;
-
-	return &par->ved;
-}
diff --git a/drivers/media/platform/sunxi/sun4i-csi/sun4i_csi.c b/drivers/media/platform/sunxi/sun4i-csi/sun4i_csi.c
index 097a3a08ef7d..e53a07b770b7 100644
--- a/drivers/media/platform/sunxi/sun4i-csi/sun4i_csi.c
+++ b/drivers/media/platform/sunxi/sun4i-csi/sun4i_csi.c
@@ -35,7 +35,18 @@ struct sun4i_csi_traits {
 	bool has_isp;
 };
 
+static int sun4i_csi_video_link_validate(struct media_link *link)
+{
+	dev_warn_once(link->graph_obj.mdev->dev,
+		      "Driver bug: link validation not implemented\n");
+	return 0;
+}
+
 static const struct media_entity_operations sun4i_csi_video_entity_ops = {
+	.link_validate = sun4i_csi_video_link_validate,
+};
+
+static const struct media_entity_operations sun4i_csi_subdev_entity_ops = {
 	.link_validate = v4l2_subdev_link_validate,
 };
 
@@ -214,6 +225,7 @@ static int sun4i_csi_probe(struct platform_device *pdev)
 	subdev->internal_ops = &sun4i_csi_subdev_internal_ops;
 	subdev->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
 	subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
+	subdev->entity.ops = &sun4i_csi_subdev_entity_ops;
 	subdev->owner = THIS_MODULE;
 	snprintf(subdev->name, sizeof(subdev->name), "sun4i-csi-0");
 	v4l2_set_subdevdata(subdev, csi);
@@ -328,7 +340,7 @@ static const struct dev_pm_ops sun4i_csi_pm_ops = {
 
 static struct platform_driver sun4i_csi_driver = {
 	.probe	= sun4i_csi_probe,
-	.remove_new = sun4i_csi_remove,
+	.remove = sun4i_csi_remove,
 	.driver	= {
 		.name		= "sun4i-csi",
 		.of_match_table	= sun4i_csi_of_match,
diff --git a/drivers/media/platform/sunxi/sun4i-csi/sun4i_dma.c b/drivers/media/platform/sunxi/sun4i-csi/sun4i_dma.c
index d1371e130113..e911c7f7acc5 100644
--- a/drivers/media/platform/sunxi/sun4i-csi/sun4i_dma.c
+++ b/drivers/media/platform/sunxi/sun4i-csi/sun4i_dma.c
@@ -371,8 +371,6 @@ static const struct vb2_ops sun4i_csi_qops = {
 	.buf_queue		= sun4i_csi_buffer_queue,
 	.start_streaming	= sun4i_csi_start_streaming,
 	.stop_streaming		= sun4i_csi_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static irqreturn_t sun4i_csi_irq(int irq, void *data)
diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
index c6ba385c0c86..af2a32c226a5 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi.c
@@ -423,7 +423,7 @@ MODULE_DEVICE_TABLE(of, sun6i_csi_of_match);
 
 static struct platform_driver sun6i_csi_platform_driver = {
 	.probe	= sun6i_csi_probe,
-	.remove_new = sun6i_csi_remove,
+	.remove = sun6i_csi_remove,
 	.driver	= {
 		.name		= SUN6I_CSI_NAME,
 		.of_match_table	= sun6i_csi_of_match,
diff --git a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
index 14c0dc827c52..76356bc7f10e 100644
--- a/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
+++ b/drivers/media/platform/sunxi/sun6i-csi/sun6i_csi_capture.c
@@ -657,8 +657,6 @@ static const struct vb2_ops sun6i_csi_capture_queue_ops = {
 	.buf_queue		= sun6i_csi_capture_buffer_queue,
 	.start_streaming	= sun6i_csi_capture_start_streaming,
 	.stop_streaming		= sun6i_csi_capture_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* V4L2 Device */
diff --git a/drivers/media/platform/sunxi/sun6i-mipi-csi2/sun6i_mipi_csi2.c b/drivers/media/platform/sunxi/sun6i-mipi-csi2/sun6i_mipi_csi2.c
index f9d4dc45b490..b06cb73015cd 100644
--- a/drivers/media/platform/sunxi/sun6i-mipi-csi2/sun6i_mipi_csi2.c
+++ b/drivers/media/platform/sunxi/sun6i-mipi-csi2/sun6i_mipi_csi2.c
@@ -757,7 +757,7 @@ MODULE_DEVICE_TABLE(of, sun6i_mipi_csi2_of_match);
 
 static struct platform_driver sun6i_mipi_csi2_platform_driver = {
 	.probe	= sun6i_mipi_csi2_probe,
-	.remove_new = sun6i_mipi_csi2_remove,
+	.remove = sun6i_mipi_csi2_remove,
 	.driver	= {
 		.name		= SUN6I_MIPI_CSI2_NAME,
 		.of_match_table	= sun6i_mipi_csi2_of_match,
diff --git a/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/Kconfig b/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/Kconfig
index 47a8c0fb7eb9..99c401e653bc 100644
--- a/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/Kconfig
+++ b/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/Kconfig
@@ -8,6 +8,7 @@ config VIDEO_SUN8I_A83T_MIPI_CSI2
 	select VIDEO_V4L2_SUBDEV_API
 	select V4L2_FWNODE
 	select REGMAP_MMIO
+	select GENERIC_PHY
 	select GENERIC_PHY_MIPI_DPHY
 	help
 	   Support for the Allwinner A83T MIPI CSI-2 controller and D-PHY.
diff --git a/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/sun8i_a83t_mipi_csi2.c b/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/sun8i_a83t_mipi_csi2.c
index 4a5698eb12b7..dbc51daa4fe3 100644
--- a/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/sun8i_a83t_mipi_csi2.c
+++ b/drivers/media/platform/sunxi/sun8i-a83t-mipi-csi2/sun8i_a83t_mipi_csi2.c
@@ -824,7 +824,7 @@ MODULE_DEVICE_TABLE(of, sun8i_a83t_mipi_csi2_of_match);
 
 static struct platform_driver sun8i_a83t_mipi_csi2_platform_driver = {
 	.probe	= sun8i_a83t_mipi_csi2_probe,
-	.remove_new = sun8i_a83t_mipi_csi2_remove,
+	.remove = sun8i_a83t_mipi_csi2_remove,
 	.driver	= {
 		.name		= SUN8I_A83T_MIPI_CSI2_NAME,
 		.of_match_table	= sun8i_a83t_mipi_csi2_of_match,
diff --git a/drivers/media/platform/sunxi/sun8i-di/sun8i-di.c b/drivers/media/platform/sunxi/sun8i-di/sun8i-di.c
index a1c35a2b68ed..3e7f2df70408 100644
--- a/drivers/media/platform/sunxi/sun8i-di/sun8i-di.c
+++ b/drivers/media/platform/sunxi/sun8i-di/sun8i-di.c
@@ -659,8 +659,6 @@ static const struct vb2_ops deinterlace_qops = {
 	.buf_queue		= deinterlace_buf_queue,
 	.start_streaming	= deinterlace_start_streaming,
 	.stop_streaming		= deinterlace_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int deinterlace_queue_init(void *priv, struct vb2_queue *src_vq,
@@ -1001,7 +999,7 @@ static const struct dev_pm_ops deinterlace_pm_ops = {
 
 static struct platform_driver deinterlace_driver = {
 	.probe		= deinterlace_probe,
-	.remove_new	= deinterlace_remove,
+	.remove		= deinterlace_remove,
 	.driver		= {
 		.name		= DEINTERLACE_NAME,
 		.of_match_table	= deinterlace_dt_match,
diff --git a/drivers/media/platform/sunxi/sun8i-rotate/sun8i_rotate.c b/drivers/media/platform/sunxi/sun8i-rotate/sun8i_rotate.c
index a12323ca89fa..abd10b218aa1 100644
--- a/drivers/media/platform/sunxi/sun8i-rotate/sun8i_rotate.c
+++ b/drivers/media/platform/sunxi/sun8i-rotate/sun8i_rotate.c
@@ -522,8 +522,6 @@ static const struct vb2_ops rotate_qops = {
 	.buf_queue		= rotate_buf_queue,
 	.start_streaming	= rotate_start_streaming,
 	.stop_streaming		= rotate_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int rotate_queue_init(void *priv, struct vb2_queue *src_vq,
@@ -905,7 +903,7 @@ static const struct dev_pm_ops rotate_pm_ops = {
 
 static struct platform_driver rotate_driver = {
 	.probe		= rotate_probe,
-	.remove_new	= rotate_remove,
+	.remove		= rotate_remove,
 	.driver		= {
 		.name		= ROTATE_NAME,
 		.of_match_table	= rotate_dt_match,
diff --git a/drivers/media/platform/synopsys/Kconfig b/drivers/media/platform/synopsys/Kconfig
new file mode 100644
index 000000000000..4fd521f78425
--- /dev/null
+++ b/drivers/media/platform/synopsys/Kconfig
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+source "drivers/media/platform/synopsys/hdmirx/Kconfig"
diff --git a/drivers/media/platform/synopsys/Makefile b/drivers/media/platform/synopsys/Makefile
new file mode 100644
index 000000000000..3b12c574dd67
--- /dev/null
+++ b/drivers/media/platform/synopsys/Makefile
@@ -0,0 +1,2 @@
+# SPDX-License-Identifier: GPL-2.0-only
+obj-y += hdmirx/
diff --git a/drivers/media/platform/synopsys/hdmirx/Kconfig b/drivers/media/platform/synopsys/hdmirx/Kconfig
new file mode 100644
index 000000000000..4321f985f632
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/Kconfig
@@ -0,0 +1,36 @@
+# SPDX-License-Identifier: GPL-2.0
+
+config VIDEO_SYNOPSYS_HDMIRX
+	tristate "Synopsys DesignWare HDMI Receiver driver"
+	depends on ARCH_ROCKCHIP || COMPILE_TEST
+	depends on VIDEO_DEV
+	select MEDIA_CONTROLLER
+	select VIDEO_V4L2_SUBDEV_API
+	select VIDEOBUF2_DMA_CONTIG
+	select CEC_CORE
+	select HDMI
+	help
+	  Support for Synopsys HDMI HDMI RX Controller.
+	  This driver supports HDMI 2.0 version.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called synopsys_hdmirx.
+
+config VIDEO_SYNOPSYS_HDMIRX_LOAD_DEFAULT_EDID
+	bool "Load default EDID"
+	depends on VIDEO_SYNOPSYS_HDMIRX
+	help
+	  Preload default EDID (Extended Display Identification Data)
+	  branded by Linux Foundation that exposes display modes up
+	  to 4k@30Hz, which have best compatibility with HDMI transmitters.
+
+	  Enabling this option is recommended for a non-production use-cases.
+	  It will make driver usable out-of-the-box.
+
+	  For a higher display modes you will need to load customized EDID
+	  from userspace using v4l2-ctl tool or by other means.
+
+	  Without enabling this option driver will be practically
+	  non-functional until EDID will be loaded from userspace.
+	  Which is a wanted behavior when using this driver in a
+	  commercial product that should utilize own branded EDID.
diff --git a/drivers/media/platform/synopsys/hdmirx/Makefile b/drivers/media/platform/synopsys/hdmirx/Makefile
new file mode 100644
index 000000000000..2fa2d9e25300
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+synopsys-hdmirx-objs := snps_hdmirx.o snps_hdmirx_cec.o
+
+obj-$(CONFIG_VIDEO_SYNOPSYS_HDMIRX) += synopsys-hdmirx.o
diff --git a/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.c b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.c
new file mode 100644
index 000000000000..7af6765532e3
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.c
@@ -0,0 +1,2746 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2025 Collabora, Ltd.
+ * Author: Shreeya Patel <shreeya.patel@collabora.com>
+ * Author: Dmitry Osipenko <dmitry.osipenko@collabora.com>
+ *
+ * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
+ * Author: Dingxian Wen <shawn.wen@rock-chips.com>
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/gpio/consumer.h>
+#include <linux/hdmi.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/mfd/syscon.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_reserved_mem.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/property.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/workqueue.h>
+
+#include <media/cec.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-v4l2.h>
+
+#include "snps_hdmirx.h"
+#include "snps_hdmirx_cec.h"
+
+#define EDID_NUM_BLOCKS_MAX				4
+#define EDID_BLOCK_SIZE					128
+#define HDMIRX_PLANE_Y					0
+#define HDMIRX_PLANE_CBCR				1
+#define FILTER_FRAME_CNT				6
+
+static int debug;
+module_param(debug, int, 0644);
+MODULE_PARM_DESC(debug, "debug level (0-3)");
+
+enum hdmirx_pix_fmt {
+	HDMIRX_RGB888 = 0,
+	HDMIRX_YUV422 = 1,
+	HDMIRX_YUV444 = 2,
+	HDMIRX_YUV420 = 3,
+};
+
+enum ddr_store_fmt {
+	STORE_RGB888 = 0,
+	STORE_RGBA_ARGB,
+	STORE_YUV420_8BIT,
+	STORE_YUV420_10BIT,
+	STORE_YUV422_8BIT,
+	STORE_YUV422_10BIT,
+	STORE_YUV444_8BIT,
+	STORE_YUV420_16BIT = 8,
+	STORE_YUV422_16BIT = 9,
+};
+
+enum hdmirx_reg_attr {
+	HDMIRX_ATTR_RW = 0,
+	HDMIRX_ATTR_RO = 1,
+	HDMIRX_ATTR_WO = 2,
+	HDMIRX_ATTR_RE = 3,
+};
+
+enum {
+	HDMIRX_RST_A,
+	HDMIRX_RST_P,
+	HDMIRX_RST_REF,
+	HDMIRX_RST_BIU,
+	HDMIRX_NUM_RST,
+};
+
+static const char *const pix_fmt_str[] = {
+	"RGB888",
+	"YUV422",
+	"YUV444",
+	"YUV420",
+};
+
+struct hdmirx_buffer {
+	struct vb2_v4l2_buffer vb;
+	struct list_head queue;
+	u32 buff_addr[VIDEO_MAX_PLANES];
+};
+
+struct hdmirx_stream {
+	struct snps_hdmirx_dev *hdmirx_dev;
+	struct video_device vdev;
+	struct vb2_queue buf_queue;
+	struct list_head buf_head;
+	struct hdmirx_buffer *curr_buf;
+	struct hdmirx_buffer *next_buf;
+	struct v4l2_pix_format_mplane pixm;
+	const struct v4l2_format_info *out_finfo;
+	struct mutex vlock; /* to lock resources associated with video buffer and video device */
+	spinlock_t vbq_lock; /* to lock video buffer queue */
+	bool stopping;
+	wait_queue_head_t wq_stopped;
+	u32 sequence;
+	u32 line_flag_int_cnt;
+	u32 irq_stat;
+};
+
+struct snps_hdmirx_dev {
+	struct device *dev;
+	struct hdmirx_stream stream;
+	struct v4l2_device v4l2_dev;
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *detect_tx_5v_ctrl;
+	struct v4l2_ctrl *rgb_range;
+	struct v4l2_ctrl *content_type;
+	struct v4l2_dv_timings timings;
+	struct gpio_desc *detect_5v_gpio;
+	struct delayed_work delayed_work_hotplug;
+	struct delayed_work delayed_work_res_change;
+	struct hdmirx_cec *cec;
+	struct mutex stream_lock; /* to lock video stream capture */
+	struct mutex work_lock; /* to lock the critical section of hotplug event */
+	struct reset_control_bulk_data resets[HDMIRX_NUM_RST];
+	struct clk_bulk_data *clks;
+	struct regmap *grf;
+	struct regmap *vo1_grf;
+	struct completion cr_write_done;
+	struct completion timer_base_lock;
+	struct completion avi_pkt_rcv;
+	struct dentry *debugfs_dir;
+	struct v4l2_debugfs_if *infoframes;
+	enum hdmirx_pix_fmt pix_fmt;
+	void __iomem *regs;
+	int hdmi_irq;
+	int dma_irq;
+	int det_irq;
+	bool hpd_trigger_level_high;
+	bool tmds_clk_ratio;
+	bool plugged;
+	int num_clks;
+	u32 edid_blocks_written;
+	u32 cur_fmt_fourcc;
+	u32 color_depth;
+	spinlock_t rst_lock; /* to lock register access */
+	u8 edid[EDID_NUM_BLOCKS_MAX * EDID_BLOCK_SIZE];
+};
+
+static const struct v4l2_dv_timings cea640x480 = V4L2_DV_BT_CEA_640X480P59_94;
+
+static const struct v4l2_dv_timings_cap hdmirx_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(640, 4096,			/* min/max width */
+			     480, 2160,			/* min/max height */
+			     20000000, 600000000,	/* min/max pixelclock */
+			     /* standards */
+			     V4L2_DV_BT_STD_CEA861,
+			     /* capabilities */
+			     V4L2_DV_BT_CAP_PROGRESSIVE |
+			     V4L2_DV_BT_CAP_INTERLACED)
+};
+
+static void hdmirx_writel(struct snps_hdmirx_dev *hdmirx_dev, int reg, u32 val)
+{
+	guard(spinlock_irqsave)(&hdmirx_dev->rst_lock);
+
+	writel(val, hdmirx_dev->regs + reg);
+}
+
+static u32 hdmirx_readl(struct snps_hdmirx_dev *hdmirx_dev, int reg)
+{
+	guard(spinlock_irqsave)(&hdmirx_dev->rst_lock);
+
+	return readl(hdmirx_dev->regs + reg);
+}
+
+static void hdmirx_reset_dma(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	guard(spinlock_irqsave)(&hdmirx_dev->rst_lock);
+
+	reset_control_reset(hdmirx_dev->resets[0].rstc);
+}
+
+static void hdmirx_update_bits(struct snps_hdmirx_dev *hdmirx_dev, int reg,
+			       u32 mask, u32 data)
+{
+	u32 val;
+
+	guard(spinlock_irqsave)(&hdmirx_dev->rst_lock);
+
+	val = readl(hdmirx_dev->regs + reg) & ~mask;
+	val |= (data & mask);
+	writel(val, hdmirx_dev->regs + reg);
+}
+
+static int hdmirx_subscribe_event(struct v4l2_fh *fh,
+				  const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_SOURCE_CHANGE:
+		return v4l2_src_change_event_subscribe(fh, sub);
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	default:
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static bool tx_5v_power_present(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	const unsigned int detection_threshold = 7;
+	int val, i, cnt = 0;
+	bool ret;
+
+	for (i = 0; i < 10; i++) {
+		usleep_range(1000, 1100);
+		val = gpiod_get_value(hdmirx_dev->detect_5v_gpio);
+		if (val > 0)
+			cnt++;
+		if (cnt >= detection_threshold)
+			break;
+	}
+
+	ret = (cnt >= detection_threshold) ? true : false;
+	v4l2_dbg(3, debug, &hdmirx_dev->v4l2_dev, "%s: %d\n", __func__, ret);
+
+	return ret;
+}
+
+static bool signal_not_lock(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	u32 mu_status, dma_st10, cmu_st;
+
+	mu_status = hdmirx_readl(hdmirx_dev, MAINUNIT_STATUS);
+	dma_st10 = hdmirx_readl(hdmirx_dev, DMA_STATUS10);
+	cmu_st = hdmirx_readl(hdmirx_dev, CMU_STATUS);
+
+	if ((mu_status & TMDSVALID_STABLE_ST) &&
+	    (dma_st10 & HDMIRX_LOCK) &&
+	    (cmu_st & TMDSQPCLK_LOCKED_ST))
+		return false;
+
+	return true;
+}
+
+static void hdmirx_get_timings(struct snps_hdmirx_dev *hdmirx_dev,
+			       struct v4l2_bt_timings *bt)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 hact, vact, htotal, vtotal, fps;
+	u32 hfp, hs, hbp, vfp, vs, vbp;
+	u32 val;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS2);
+	hact = (val >> 16) & 0xffff;
+	vact = val & 0xffff;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS3);
+	htotal = (val >> 16) & 0xffff;
+	vtotal = val & 0xffff;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS4);
+	hs = (val >> 16) & 0xffff;
+	vs = val & 0xffff;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS5);
+	hbp = (val >> 16) & 0xffff;
+	vbp = val & 0xffff;
+
+	if (hdmirx_dev->pix_fmt == HDMIRX_YUV420) {
+		htotal *= 2;
+		hbp *= 2;
+		hs *= 2;
+	}
+
+	hfp = htotal - hact - hs - hbp;
+	vfp = vtotal - vact - vs - vbp;
+
+	fps = div_u64(bt->pixelclock + (htotal * vtotal) / 2, htotal * vtotal);
+	bt->width = hact;
+	bt->height = vact;
+	bt->hfrontporch = hfp;
+	bt->hsync = hs;
+	bt->hbackporch = hbp;
+	bt->vfrontporch = vfp;
+	bt->vsync = vs;
+	bt->vbackporch = vbp;
+
+	v4l2_dbg(1, debug, v4l2_dev, "get timings from dma\n");
+	v4l2_dbg(1, debug, v4l2_dev,
+		 "act:%ux%u%s, total:%ux%u, fps:%u, pixclk:%llu\n",
+		 bt->width, bt->height, bt->interlaced ? "i" : "p",
+		 htotal, vtotal, fps, bt->pixelclock);
+
+	v4l2_dbg(2, debug, v4l2_dev,
+		 "hfp:%u, hact:%u, hs:%u, hbp:%u, vfp:%u, vact:%u, vs:%u, vbp:%u\n",
+		 bt->hfrontporch, hact, bt->hsync, bt->hbackporch,
+		 bt->vfrontporch, vact, bt->vsync, bt->vbackporch);
+
+	if (bt->interlaced == V4L2_DV_INTERLACED) {
+		bt->height *= 2;
+		bt->il_vfrontporch = bt->vfrontporch;
+		bt->il_vsync = bt->vsync + 1;
+		bt->il_vbackporch = bt->vbackporch;
+	}
+}
+
+static bool hdmirx_check_timing_valid(struct v4l2_bt_timings *bt)
+{
+	/*
+	 * Sanity-check timing values. Some of the values will be outside
+	 * of a valid range till hardware becomes ready to perform capture.
+	 */
+	if (bt->width < 100 || bt->width > 5000 ||
+	    bt->height < 100 || bt->height > 5000)
+		return false;
+
+	if (!bt->hsync || bt->hsync > 200 ||
+	    !bt->vsync || bt->vsync > 100)
+		return false;
+
+	/*
+	 * According to the CEA-861, 1280x720p25 Hblank timing is up to 2680,
+	 * and all standard video format timings are less than 3000.
+	 */
+	if (!bt->hbackporch || bt->hbackporch > 3000 ||
+	    !bt->vbackporch || bt->vbackporch > 3000)
+		return false;
+
+	if (!bt->hfrontporch || bt->hfrontporch > 3000 ||
+	    !bt->vfrontporch || bt->vfrontporch > 3000)
+		return false;
+
+	return true;
+}
+
+static void hdmirx_toggle_polarity(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	u32 val = hdmirx_readl(hdmirx_dev, DMA_CONFIG6);
+
+	if (!(val & (VSYNC_TOGGLE_EN | HSYNC_TOGGLE_EN))) {
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6,
+				   VSYNC_TOGGLE_EN | HSYNC_TOGGLE_EN,
+				   VSYNC_TOGGLE_EN | HSYNC_TOGGLE_EN);
+		hdmirx_update_bits(hdmirx_dev, VIDEO_CONFIG2,
+				   VPROC_VSYNC_POL_OVR_VALUE |
+				   VPROC_VSYNC_POL_OVR_EN |
+				   VPROC_HSYNC_POL_OVR_VALUE |
+				   VPROC_HSYNC_POL_OVR_EN,
+				   VPROC_VSYNC_POL_OVR_EN |
+				   VPROC_HSYNC_POL_OVR_EN);
+		return;
+	}
+
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6,
+			   VSYNC_TOGGLE_EN | HSYNC_TOGGLE_EN, 0);
+
+	hdmirx_update_bits(hdmirx_dev, VIDEO_CONFIG2,
+			   VPROC_VSYNC_POL_OVR_VALUE |
+			   VPROC_VSYNC_POL_OVR_EN |
+			   VPROC_HSYNC_POL_OVR_VALUE |
+			   VPROC_HSYNC_POL_OVR_EN, 0);
+}
+
+/*
+ * When querying DV timings during preview, if the DMA's timing is stable,
+ * we retrieve the timings directly from the DMA. However, if the current
+ * resolution is negative, obtaining the timing from CTRL may require a
+ * change in the sync polarity, potentially leading to DMA errors.
+ */
+static int hdmirx_get_detected_timings(struct snps_hdmirx_dev *hdmirx_dev,
+				       struct v4l2_dv_timings *timings)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct v4l2_bt_timings *bt = &timings->bt;
+	u32 val, tmdsqpclk_freq, pix_clk;
+	unsigned int num_retries = 0;
+	u32 field_type, deframer_st;
+	u64 tmp_data, tmds_clk;
+	bool is_dvi_mode;
+	int ret;
+
+	mutex_lock(&hdmirx_dev->work_lock);
+retry:
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+	timings->type = V4L2_DV_BT_656_1120;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS11);
+	field_type = (val & HDMIRX_TYPE_MASK) >> 7;
+
+	if (field_type & BIT(0))
+		bt->interlaced = V4L2_DV_INTERLACED;
+	else
+		bt->interlaced = V4L2_DV_PROGRESSIVE;
+
+	deframer_st = hdmirx_readl(hdmirx_dev, DEFRAMER_STATUS);
+	is_dvi_mode = !(deframer_st & OPMODE_STS_MASK);
+
+	tmdsqpclk_freq = hdmirx_readl(hdmirx_dev, CMU_TMDSQPCLK_FREQ);
+	tmds_clk = tmdsqpclk_freq * 4 * 1000;
+	tmp_data = tmds_clk * 24;
+	do_div(tmp_data, hdmirx_dev->color_depth);
+	pix_clk = tmp_data;
+	bt->pixelclock = pix_clk;
+
+	if (hdmirx_dev->pix_fmt == HDMIRX_YUV420)
+		bt->pixelclock *= 2;
+
+	hdmirx_get_timings(hdmirx_dev, bt);
+
+	v4l2_dbg(2, debug, v4l2_dev, "tmds_clk:%llu, pix_clk:%d\n", tmds_clk, pix_clk);
+	v4l2_dbg(1, debug, v4l2_dev, "interlace:%d, fmt:%d, color:%d, mode:%s\n",
+		 bt->interlaced, hdmirx_dev->pix_fmt,
+		 hdmirx_dev->color_depth,
+		 is_dvi_mode ? "dvi" : "hdmi");
+	v4l2_dbg(2, debug, v4l2_dev, "deframer_st:%#x\n", deframer_st);
+
+	/*
+	 * Timing will be invalid until it's latched by HW or if signal's
+	 * polarity doesn't match.
+	 */
+	if (!hdmirx_check_timing_valid(bt)) {
+		if (num_retries++ < 20) {
+			if (num_retries == 10)
+				hdmirx_toggle_polarity(hdmirx_dev);
+
+			usleep_range(10 * 1000, 10 * 1100);
+			goto retry;
+		}
+
+		ret = -ERANGE;
+	} else {
+		ret = 0;
+	}
+
+	mutex_unlock(&hdmirx_dev->work_lock);
+
+	return ret;
+}
+
+static bool port_no_link(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	return !tx_5v_power_present(hdmirx_dev);
+}
+
+static int hdmirx_query_dv_timings(struct file *file, void *_fh,
+				   struct v4l2_dv_timings *timings)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	int ret;
+
+	if (port_no_link(hdmirx_dev)) {
+		v4l2_err(v4l2_dev, "%s: port has no link\n", __func__);
+		return -ENOLINK;
+	}
+
+	if (signal_not_lock(hdmirx_dev)) {
+		v4l2_err(v4l2_dev, "%s: signal is not locked\n", __func__);
+		return -ENOLCK;
+	}
+
+	ret = hdmirx_get_detected_timings(hdmirx_dev, timings);
+	if (ret)
+		return ret;
+
+	if (debug)
+		v4l2_print_dv_timings(hdmirx_dev->v4l2_dev.name,
+				      "query_dv_timings: ", timings, false);
+
+	if (!v4l2_valid_dv_timings(timings, &hdmirx_timings_cap, NULL, NULL)) {
+		v4l2_dbg(1, debug, v4l2_dev, "%s: timings out of range\n", __func__);
+		return -ERANGE;
+	}
+
+	return 0;
+}
+
+static void hdmirx_hpd_ctrl(struct snps_hdmirx_dev *hdmirx_dev, bool en)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: %sable, hpd_trigger_level_high:%d\n",
+		 __func__, en ? "en" : "dis", hdmirx_dev->hpd_trigger_level_high);
+
+	hdmirx_update_bits(hdmirx_dev, SCDC_CONFIG, HPDLOW, en ? 0 : HPDLOW);
+	hdmirx_writel(hdmirx_dev, CORE_CONFIG,
+		      hdmirx_dev->hpd_trigger_level_high ? en : !en);
+
+	/* 100ms delay as per HDMI spec */
+	if (!en)
+		msleep(100);
+}
+
+static void hdmirx_write_edid_data(struct snps_hdmirx_dev *hdmirx_dev,
+				   u8 *edid, unsigned int num_blocks)
+{
+	static u8 data[EDID_NUM_BLOCKS_MAX * EDID_BLOCK_SIZE];
+	unsigned int edid_len = num_blocks * EDID_BLOCK_SIZE;
+	unsigned int i;
+
+	cec_s_phys_addr_from_edid(hdmirx_dev->cec->adap,
+				  (const struct edid *)edid);
+
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG11,
+			   EDID_READ_EN_MASK |
+			   EDID_WRITE_EN_MASK |
+			   EDID_SLAVE_ADDR_MASK,
+			   EDID_READ_EN(0) |
+			   EDID_WRITE_EN(1) |
+			   EDID_SLAVE_ADDR(0x50));
+	for (i = 0; i < edid_len; i++)
+		hdmirx_writel(hdmirx_dev, DMA_CONFIG10, edid[i]);
+
+	/* read out for debug */
+	if (debug >= 2) {
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG11,
+				   EDID_READ_EN_MASK |
+				   EDID_WRITE_EN_MASK,
+				   EDID_READ_EN(1) |
+				   EDID_WRITE_EN(0));
+
+		for (i = 0; i < edid_len; i++)
+			data[i] = hdmirx_readl(hdmirx_dev, DMA_STATUS14);
+
+		print_hex_dump(KERN_INFO, "", DUMP_PREFIX_NONE, 16, 1, data,
+			       edid_len, false);
+	}
+
+	/*
+	 * Must set EDID_READ_EN & EDID_WRITE_EN bit to 0,
+	 * when the read/write edid operation is completed. Otherwise, it
+	 * will affect the reading and writing of other registers.
+	 */
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG11,
+			   EDID_READ_EN_MASK | EDID_WRITE_EN_MASK,
+			   EDID_READ_EN(0) | EDID_WRITE_EN(0));
+}
+
+static void hdmirx_write_edid(struct snps_hdmirx_dev *hdmirx_dev,
+			      struct v4l2_edid *edid)
+{
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	memset(hdmirx_dev->edid, 0, sizeof(hdmirx_dev->edid));
+
+	hdmirx_write_edid_data(hdmirx_dev, edid->edid, edid->blocks);
+
+	hdmirx_dev->edid_blocks_written = edid->blocks;
+	memcpy(hdmirx_dev->edid, edid->edid, edid->blocks * EDID_BLOCK_SIZE);
+}
+
+/*
+ * Before clearing interrupt, we need to read the interrupt status.
+ */
+static inline void hdmirx_clear_interrupt(struct snps_hdmirx_dev *hdmirx_dev,
+					  u32 reg, u32 val)
+{
+	/* (interrupt status register) = (interrupt clear register) - 0x8 */
+	hdmirx_readl(hdmirx_dev, reg - 0x8);
+	hdmirx_writel(hdmirx_dev, reg, val);
+}
+
+static void hdmirx_interrupts_setup(struct snps_hdmirx_dev *hdmirx_dev, bool en)
+{
+	v4l2_dbg(1, debug, &hdmirx_dev->v4l2_dev, "%s: %sable\n",
+		 __func__, en ? "en" : "dis");
+
+	disable_irq(hdmirx_dev->hdmi_irq);
+
+	/* Note: In DVI mode, it needs to be written twice to take effect. */
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, AVPUNIT_0_INT_CLEAR, 0xffffffff);
+
+	if (en) {
+		hdmirx_update_bits(hdmirx_dev, MAINUNIT_0_INT_MASK_N,
+				   TMDSQPCLK_OFF_CHG | TMDSQPCLK_LOCKED_CHG,
+				   TMDSQPCLK_OFF_CHG | TMDSQPCLK_LOCKED_CHG);
+		hdmirx_update_bits(hdmirx_dev, MAINUNIT_2_INT_MASK_N,
+				   TMDSVALID_STABLE_CHG, TMDSVALID_STABLE_CHG);
+		hdmirx_update_bits(hdmirx_dev, AVPUNIT_0_INT_MASK_N,
+				   CED_DYN_CNT_CH2_IRQ |
+				   CED_DYN_CNT_CH1_IRQ |
+				   CED_DYN_CNT_CH0_IRQ,
+				   CED_DYN_CNT_CH2_IRQ |
+				   CED_DYN_CNT_CH1_IRQ |
+				   CED_DYN_CNT_CH0_IRQ);
+	} else {
+		hdmirx_writel(hdmirx_dev, MAINUNIT_0_INT_MASK_N, 0);
+		hdmirx_writel(hdmirx_dev, MAINUNIT_2_INT_MASK_N, 0);
+		hdmirx_writel(hdmirx_dev, AVPUNIT_0_INT_MASK_N, 0);
+	}
+
+	enable_irq(hdmirx_dev->hdmi_irq);
+}
+
+static void hdmirx_plugout(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	if (!hdmirx_dev->plugged)
+		return;
+
+	hdmirx_update_bits(hdmirx_dev, SCDC_CONFIG, POWERPROVIDED, 0);
+	hdmirx_interrupts_setup(hdmirx_dev, false);
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, HDMIRX_DMA_EN, 0);
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG4,
+			   LINE_FLAG_INT_EN |
+			   HDMIRX_DMA_IDLE_INT |
+			   HDMIRX_LOCK_DISABLE_INT |
+			   LAST_FRAME_AXI_UNFINISH_INT_EN |
+			   FIFO_OVERFLOW_INT_EN |
+			   FIFO_UNDERFLOW_INT_EN |
+			   HDMIRX_AXI_ERROR_INT_EN, 0);
+	hdmirx_reset_dma(hdmirx_dev);
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, HDMI_DISABLE | PHY_RESET |
+			   PHY_PDDQ, HDMI_DISABLE);
+	hdmirx_writel(hdmirx_dev, PHYCREG_CONFIG0, 0x0);
+	cancel_delayed_work(&hdmirx_dev->delayed_work_res_change);
+
+	/* will be NULL on driver removal */
+	if (hdmirx_dev->rgb_range)
+		v4l2_ctrl_s_ctrl(hdmirx_dev->rgb_range, V4L2_DV_RGB_RANGE_AUTO);
+
+	if (hdmirx_dev->content_type)
+		v4l2_ctrl_s_ctrl(hdmirx_dev->content_type,
+				 V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+
+	hdmirx_dev->plugged = false;
+}
+
+static int hdmirx_set_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	u16 phys_addr;
+	int err;
+
+	if (edid->pad)
+		return -EINVAL;
+
+	if (edid->start_block)
+		return -EINVAL;
+
+	if (edid->blocks > EDID_NUM_BLOCKS_MAX) {
+		edid->blocks = EDID_NUM_BLOCKS_MAX;
+		return -E2BIG;
+	}
+
+	if (edid->blocks) {
+		phys_addr = cec_get_edid_phys_addr(edid->edid,
+						   edid->blocks * EDID_BLOCK_SIZE,
+						   NULL);
+
+		err = v4l2_phys_addr_validate(phys_addr, &phys_addr, NULL);
+		if (err)
+			return err;
+	}
+
+	/*
+	 * Touching HW registers in parallel with plugin/out handlers
+	 * will bring hardware into a bad state.
+	 */
+	mutex_lock(&hdmirx_dev->work_lock);
+
+	hdmirx_hpd_ctrl(hdmirx_dev, false);
+
+	if (edid->blocks) {
+		hdmirx_write_edid(hdmirx_dev, edid);
+		hdmirx_hpd_ctrl(hdmirx_dev, true);
+	} else {
+		cec_phys_addr_invalidate(hdmirx_dev->cec->adap);
+		hdmirx_dev->edid_blocks_written = 0;
+	}
+
+	mutex_unlock(&hdmirx_dev->work_lock);
+
+	return 0;
+}
+
+static int hdmirx_get_edid(struct file *file, void *fh, struct v4l2_edid *edid)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+
+	if (edid->pad)
+		return -EINVAL;
+
+	if (!edid->start_block && !edid->blocks) {
+		edid->blocks = hdmirx_dev->edid_blocks_written;
+		return 0;
+	}
+
+	if (!hdmirx_dev->edid_blocks_written)
+		return -ENODATA;
+
+	if (edid->start_block >= hdmirx_dev->edid_blocks_written || !edid->blocks)
+		return -EINVAL;
+
+	if (edid->start_block + edid->blocks > hdmirx_dev->edid_blocks_written)
+		edid->blocks = hdmirx_dev->edid_blocks_written - edid->start_block;
+
+	memcpy(edid->edid, hdmirx_dev->edid, edid->blocks * EDID_BLOCK_SIZE);
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: read EDID:\n", __func__);
+	if (debug > 0)
+		print_hex_dump(KERN_INFO, "", DUMP_PREFIX_NONE, 16, 1,
+			       edid->edid, edid->blocks * EDID_BLOCK_SIZE, false);
+
+	return 0;
+}
+
+static int hdmirx_g_parm(struct file *file, void *priv,
+			 struct v4l2_streamparm *parm)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+
+	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+		return -EINVAL;
+
+	parm->parm.capture.timeperframe = v4l2_calc_timeperframe(&hdmirx_dev->timings);
+
+	return 0;
+}
+
+static int hdmirx_dv_timings_cap(struct file *file, void *fh,
+				 struct v4l2_dv_timings_cap *cap)
+{
+	*cap = hdmirx_timings_cap;
+	return 0;
+}
+
+static int hdmirx_enum_dv_timings(struct file *file, void *_fh,
+				  struct v4l2_enum_dv_timings *timings)
+{
+	return v4l2_enum_dv_timings_cap(timings, &hdmirx_timings_cap, NULL, NULL);
+}
+
+static void hdmirx_scdc_init(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	hdmirx_update_bits(hdmirx_dev, I2C_SLAVE_CONFIG1,
+			   I2C_SDA_OUT_HOLD_VALUE_QST_MASK |
+			   I2C_SDA_IN_HOLD_VALUE_QST_MASK,
+			   I2C_SDA_OUT_HOLD_VALUE_QST(0x80) |
+			   I2C_SDA_IN_HOLD_VALUE_QST(0x15));
+	hdmirx_update_bits(hdmirx_dev, SCDC_REGBANK_CONFIG0,
+			   SCDC_SINKVERSION_QST_MASK,
+			   SCDC_SINKVERSION_QST(1));
+}
+
+static int wait_reg_bit_status(struct snps_hdmirx_dev *hdmirx_dev, u32 reg,
+			       u32 bit_mask, u32 expect_val, bool is_grf,
+			       u32 ms)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 i, val;
+
+	for (i = 0; i < ms; i++) {
+		if (is_grf)
+			regmap_read(hdmirx_dev->grf, reg, &val);
+		else
+			val = hdmirx_readl(hdmirx_dev, reg);
+
+		if ((val & bit_mask) == expect_val) {
+			v4l2_dbg(2, debug, v4l2_dev,
+				 "%s:  i:%d, time: %dms\n", __func__, i, ms);
+			break;
+		}
+		usleep_range(1000, 1010);
+	}
+
+	if (i == ms)
+		return -1;
+
+	return 0;
+}
+
+static int hdmirx_phy_register_write(struct snps_hdmirx_dev *hdmirx_dev,
+				     u32 phy_reg, u32 val)
+{
+	struct device *dev = hdmirx_dev->dev;
+
+	reinit_completion(&hdmirx_dev->cr_write_done);
+	/* clear irq status */
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_2_INT_CLEAR, 0xffffffff);
+	/* en irq */
+	hdmirx_update_bits(hdmirx_dev, MAINUNIT_2_INT_MASK_N,
+			   PHYCREG_CR_WRITE_DONE, PHYCREG_CR_WRITE_DONE);
+	/* write phy reg addr */
+	hdmirx_writel(hdmirx_dev, PHYCREG_CONFIG1, phy_reg);
+	/* write phy reg val */
+	hdmirx_writel(hdmirx_dev, PHYCREG_CONFIG2, val);
+	/* config write enable */
+	hdmirx_writel(hdmirx_dev, PHYCREG_CONTROL, PHYCREG_CR_PARA_WRITE_P);
+
+	if (!wait_for_completion_timeout(&hdmirx_dev->cr_write_done,
+					 msecs_to_jiffies(20))) {
+		dev_err(dev, "%s wait cr write done failed\n", __func__);
+		return -1;
+	}
+
+	return 0;
+}
+
+static void hdmirx_tmds_clk_ratio_config(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 val;
+
+	val = hdmirx_readl(hdmirx_dev, SCDC_REGBANK_STATUS1);
+	v4l2_dbg(3, debug, v4l2_dev, "%s: scdc_regbank_st:%#x\n", __func__, val);
+	hdmirx_dev->tmds_clk_ratio = (val & SCDC_TMDSBITCLKRATIO) > 0;
+
+	if (hdmirx_dev->tmds_clk_ratio) {
+		v4l2_dbg(3, debug, v4l2_dev, "%s: HDMITX greater than 3.4Gbps\n", __func__);
+		hdmirx_update_bits(hdmirx_dev, PHY_CONFIG,
+				   TMDS_CLOCK_RATIO, TMDS_CLOCK_RATIO);
+	} else {
+		v4l2_dbg(3, debug, v4l2_dev, "%s: HDMITX less than 3.4Gbps\n", __func__);
+		hdmirx_update_bits(hdmirx_dev, PHY_CONFIG,
+				   TMDS_CLOCK_RATIO, 0);
+	}
+}
+
+static void hdmirx_phy_config(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct device *dev = hdmirx_dev->dev;
+
+	hdmirx_clear_interrupt(hdmirx_dev, SCDC_INT_CLEAR, 0xffffffff);
+	hdmirx_update_bits(hdmirx_dev, SCDC_INT_MASK_N, SCDCTMDSCCFG_CHG,
+			   SCDCTMDSCCFG_CHG);
+	/* cr_para_clk 24M */
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, REFFREQ_SEL_MASK, REFFREQ_SEL(0));
+	/* rx data width 40bit valid */
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, RXDATA_WIDTH, RXDATA_WIDTH);
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, PHY_RESET, PHY_RESET);
+	usleep_range(100, 110);
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, PHY_RESET, 0);
+	usleep_range(100, 110);
+	/* select cr para interface */
+	hdmirx_writel(hdmirx_dev, PHYCREG_CONFIG0, 0x3);
+
+	if (wait_reg_bit_status(hdmirx_dev, SYS_GRF_SOC_STATUS1,
+				HDMIRXPHY_SRAM_INIT_DONE,
+				HDMIRXPHY_SRAM_INIT_DONE, true, 10))
+		dev_err(dev, "%s: phy SRAM init failed\n", __func__);
+
+	regmap_write(hdmirx_dev->grf, SYS_GRF_SOC_CON1,
+		     (HDMIRXPHY_SRAM_EXT_LD_DONE << 16) |
+		     HDMIRXPHY_SRAM_EXT_LD_DONE);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 2);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 3);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 2);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 2);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 3);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 2);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 0);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 1);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 0);
+	hdmirx_phy_register_write(hdmirx_dev, SUP_DIG_ANA_CREGS_SUP_ANA_NC, 0);
+
+	hdmirx_phy_register_write(hdmirx_dev,
+				  HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_3_REG,
+				  CDR_SETTING_BOUNDARY_3_DEFAULT);
+	hdmirx_phy_register_write(hdmirx_dev,
+				  HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_4_REG,
+				  CDR_SETTING_BOUNDARY_4_DEFAULT);
+	hdmirx_phy_register_write(hdmirx_dev,
+				  HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_5_REG,
+				  CDR_SETTING_BOUNDARY_5_DEFAULT);
+	hdmirx_phy_register_write(hdmirx_dev,
+				  HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_6_REG,
+				  CDR_SETTING_BOUNDARY_6_DEFAULT);
+	hdmirx_phy_register_write(hdmirx_dev,
+				  HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_7_REG,
+				  CDR_SETTING_BOUNDARY_7_DEFAULT);
+
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, PHY_PDDQ, 0);
+	if (wait_reg_bit_status(hdmirx_dev, PHY_STATUS, PDDQ_ACK, 0, false, 10))
+		dev_err(dev, "%s: wait pddq ack failed\n", __func__);
+
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, HDMI_DISABLE, 0);
+	if (wait_reg_bit_status(hdmirx_dev, PHY_STATUS, HDMI_DISABLE_ACK, 0,
+				false, 50))
+		dev_err(dev, "%s: wait hdmi disable ack failed\n", __func__);
+
+	hdmirx_tmds_clk_ratio_config(hdmirx_dev);
+}
+
+static void hdmirx_controller_init(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	const unsigned long iref_clk_freq_hz = 428571429;
+	struct device *dev = hdmirx_dev->dev;
+
+	reinit_completion(&hdmirx_dev->timer_base_lock);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_0_INT_CLEAR, 0xffffffff);
+	/* en irq */
+	hdmirx_update_bits(hdmirx_dev, MAINUNIT_0_INT_MASK_N,
+			   TIMER_BASE_LOCKED_IRQ, TIMER_BASE_LOCKED_IRQ);
+	/* write irefclk freq */
+	hdmirx_writel(hdmirx_dev, GLOBAL_TIMER_REF_BASE, iref_clk_freq_hz);
+
+	if (!wait_for_completion_timeout(&hdmirx_dev->timer_base_lock,
+					 msecs_to_jiffies(20)))
+		dev_err(dev, "%s wait timer base lock failed\n", __func__);
+
+	hdmirx_update_bits(hdmirx_dev, CMU_CONFIG0,
+			   TMDSQPCLK_STABLE_FREQ_MARGIN_MASK |
+			   AUDCLK_STABLE_FREQ_MARGIN_MASK,
+			   TMDSQPCLK_STABLE_FREQ_MARGIN(2) |
+			   AUDCLK_STABLE_FREQ_MARGIN(1));
+	hdmirx_update_bits(hdmirx_dev, DESCRAND_EN_CONTROL,
+			   SCRAMB_EN_SEL_QST_MASK, SCRAMB_EN_SEL_QST(1));
+	hdmirx_update_bits(hdmirx_dev, CED_CONFIG,
+			   CED_VIDDATACHECKEN_QST |
+			   CED_DATAISCHECKEN_QST |
+			   CED_GBCHECKEN_QST |
+			   CED_CTRLCHECKEN_QST |
+			   CED_CHLOCKMAXER_QST_MASK,
+			   CED_VIDDATACHECKEN_QST |
+			   CED_GBCHECKEN_QST |
+			   CED_CTRLCHECKEN_QST |
+			   CED_CHLOCKMAXER_QST(0x10));
+	hdmirx_update_bits(hdmirx_dev, DEFRAMER_CONFIG0,
+			   VS_REMAPFILTER_EN_QST | VS_FILTER_ORDER_QST_MASK,
+			   VS_REMAPFILTER_EN_QST | VS_FILTER_ORDER_QST(0x3));
+}
+
+static void hdmirx_get_colordepth(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 val, color_depth_reg;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS11);
+	color_depth_reg = (val & HDMIRX_COLOR_DEPTH_MASK) >> 3;
+
+	switch (color_depth_reg) {
+	case 0x4:
+		hdmirx_dev->color_depth = 24;
+		break;
+	case 0x5:
+		hdmirx_dev->color_depth = 30;
+		break;
+	case 0x6:
+		hdmirx_dev->color_depth = 36;
+		break;
+	case 0x7:
+		hdmirx_dev->color_depth = 48;
+		break;
+	default:
+		hdmirx_dev->color_depth = 24;
+		break;
+	}
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: color_depth: %d, reg_val:%d\n",
+		 __func__, hdmirx_dev->color_depth, color_depth_reg);
+}
+
+static void hdmirx_get_pix_fmt(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 val;
+
+	val = hdmirx_readl(hdmirx_dev, DMA_STATUS11);
+	hdmirx_dev->pix_fmt = val & HDMIRX_FORMAT_MASK;
+
+	switch (hdmirx_dev->pix_fmt) {
+	case HDMIRX_RGB888:
+		hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_BGR24;
+		break;
+	case HDMIRX_YUV422:
+		hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_NV16;
+		break;
+	case HDMIRX_YUV444:
+		hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_NV24;
+		break;
+	case HDMIRX_YUV420:
+		hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_NV12;
+		break;
+	default:
+		v4l2_err(v4l2_dev,
+			 "%s: err pix_fmt: %d, set RGB888 as default\n",
+			 __func__, hdmirx_dev->pix_fmt);
+		hdmirx_dev->pix_fmt = HDMIRX_RGB888;
+		hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_BGR24;
+		break;
+	}
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: pix_fmt: %s\n", __func__,
+		 pix_fmt_str[hdmirx_dev->pix_fmt]);
+}
+
+static void hdmirx_read_avi_infoframe(struct snps_hdmirx_dev *hdmirx_dev,
+				      u8 *aviif)
+{
+	unsigned int i, b, itr = 0;
+	u32 val;
+
+	aviif[itr++] = HDMI_INFOFRAME_TYPE_AVI;
+	val = hdmirx_readl(hdmirx_dev, PKTDEC_AVIIF_PH2_1);
+	aviif[itr++] = val & 0xff;
+	aviif[itr++] = (val >> 8) & 0xff;
+
+	for (i = 0; i < 7; i++) {
+		val = hdmirx_readl(hdmirx_dev, PKTDEC_AVIIF_PB3_0 + 4 * i);
+
+		for (b = 0; b < 4; b++)
+			aviif[itr++] = (val >> (8 * b)) & 0xff;
+	}
+}
+
+static void hdmirx_get_avi_infoframe(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	union hdmi_infoframe frame = {};
+	u8 aviif[3 + 7 * 4];
+	int err;
+
+	hdmirx_read_avi_infoframe(hdmirx_dev, aviif);
+
+	err = hdmi_infoframe_unpack(&frame, aviif, sizeof(aviif));
+	if (err) {
+		v4l2_err(v4l2_dev, "failed to unpack AVI infoframe\n");
+		return;
+	}
+
+	v4l2_ctrl_s_ctrl(hdmirx_dev->rgb_range, frame.avi.quantization_range);
+
+	if (frame.avi.itc)
+		v4l2_ctrl_s_ctrl(hdmirx_dev->content_type,
+				 frame.avi.content_type);
+	else
+		v4l2_ctrl_s_ctrl(hdmirx_dev->content_type,
+				 V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+}
+
+static ssize_t
+hdmirx_debugfs_if_read(u32 type, void *priv, struct file *filp,
+		       char __user *ubuf, size_t count, loff_t *ppos)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = priv;
+	u8 aviif[V4L2_DEBUGFS_IF_MAX_LEN] = {};
+	int len;
+
+	if (type != V4L2_DEBUGFS_IF_AVI)
+		return 0;
+
+	hdmirx_read_avi_infoframe(hdmirx_dev, aviif);
+
+	len = aviif[2] + 4;
+	if (len > V4L2_DEBUGFS_IF_MAX_LEN)
+		len = -ENOENT;
+	else
+		len = simple_read_from_buffer(ubuf, count, ppos, aviif, len);
+
+	return len < 0 ? 0 : len;
+}
+
+static void hdmirx_format_change(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct hdmirx_stream *stream = &hdmirx_dev->stream;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	static const struct v4l2_event ev_src_chg = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+
+	hdmirx_get_pix_fmt(hdmirx_dev);
+	hdmirx_get_colordepth(hdmirx_dev);
+	hdmirx_get_avi_infoframe(hdmirx_dev);
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: queue res_chg_event\n", __func__);
+	v4l2_event_queue(&stream->vdev, &ev_src_chg);
+}
+
+static void hdmirx_set_ddr_store_fmt(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	enum ddr_store_fmt store_fmt;
+	u32 dma_cfg1;
+
+	switch (hdmirx_dev->pix_fmt) {
+	case HDMIRX_RGB888:
+		store_fmt = STORE_RGB888;
+		break;
+	case HDMIRX_YUV444:
+		store_fmt = STORE_YUV444_8BIT;
+		break;
+	case HDMIRX_YUV422:
+		store_fmt = STORE_YUV422_8BIT;
+		break;
+	case HDMIRX_YUV420:
+		store_fmt = STORE_YUV420_8BIT;
+		break;
+	default:
+		store_fmt = STORE_RGB888;
+		break;
+	}
+
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG1,
+			   DDR_STORE_FORMAT_MASK, DDR_STORE_FORMAT(store_fmt));
+	dma_cfg1 = hdmirx_readl(hdmirx_dev, DMA_CONFIG1);
+	v4l2_dbg(1, debug, v4l2_dev, "%s: pix_fmt: %s, DMA_CONFIG1:%#x\n",
+		 __func__, pix_fmt_str[hdmirx_dev->pix_fmt], dma_cfg1);
+}
+
+static void hdmirx_dma_config(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	hdmirx_set_ddr_store_fmt(hdmirx_dev);
+
+	/* Note: uv_swap, rb can not swap, doc err */
+	if (hdmirx_dev->cur_fmt_fourcc != V4L2_PIX_FMT_NV16)
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, RB_SWAP_EN, RB_SWAP_EN);
+	else
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, RB_SWAP_EN, 0);
+
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG7,
+			   LOCK_FRAME_NUM_MASK,
+			   LOCK_FRAME_NUM(2));
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG1,
+			   UV_WID_MASK | Y_WID_MASK | ABANDON_EN,
+			   UV_WID(1) | Y_WID(2) | ABANDON_EN);
+}
+
+static void hdmirx_submodule_init(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	/* Note: if not config HDCP2_CONFIG, there will be some errors; */
+	hdmirx_update_bits(hdmirx_dev, HDCP2_CONFIG,
+			   HDCP2_SWITCH_OVR_VALUE |
+			   HDCP2_SWITCH_OVR_EN,
+			   HDCP2_SWITCH_OVR_EN);
+	hdmirx_scdc_init(hdmirx_dev);
+	hdmirx_controller_init(hdmirx_dev);
+}
+
+static int hdmirx_enum_input(struct file *file, void *priv,
+			     struct v4l2_input *input)
+{
+	if (input->index > 0)
+		return -EINVAL;
+
+	input->type = V4L2_INPUT_TYPE_CAMERA;
+	input->std = 0;
+	strscpy(input->name, "HDMI IN", sizeof(input->name));
+	input->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+
+	return 0;
+}
+
+static int hdmirx_get_input(struct file *file, void *priv, unsigned int *i)
+{
+	*i = 0;
+	return 0;
+}
+
+static int hdmirx_set_input(struct file *file, void *priv, unsigned int i)
+{
+	if (i)
+		return -EINVAL;
+	return 0;
+}
+
+static void hdmirx_set_fmt(struct hdmirx_stream *stream,
+			   struct v4l2_pix_format_mplane *pixm, bool try)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct v4l2_bt_timings *bt = &hdmirx_dev->timings.bt;
+	const struct v4l2_format_info *finfo;
+	unsigned int imagesize = 0;
+	unsigned int i;
+
+	memset(&pixm->plane_fmt[0], 0, sizeof(struct v4l2_plane_pix_format));
+	finfo = v4l2_format_info(pixm->pixelformat);
+	if (!finfo) {
+		finfo = v4l2_format_info(V4L2_PIX_FMT_BGR24);
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "%s: set_fmt:%#x not supported, use def_fmt:%x\n",
+			 __func__, pixm->pixelformat, finfo->format);
+	}
+
+	if (!bt->width || !bt->height)
+		v4l2_dbg(1, debug, v4l2_dev, "%s: invalid resolution:%#xx%#x\n",
+			 __func__, bt->width, bt->height);
+
+	pixm->pixelformat = finfo->format;
+	pixm->width = bt->width;
+	pixm->height = bt->height;
+	pixm->num_planes = finfo->mem_planes;
+	pixm->quantization = V4L2_QUANTIZATION_DEFAULT;
+	pixm->colorspace = V4L2_COLORSPACE_SRGB;
+	pixm->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+
+	if (bt->interlaced == V4L2_DV_INTERLACED)
+		pixm->field = V4L2_FIELD_INTERLACED_TB;
+	else
+		pixm->field = V4L2_FIELD_NONE;
+
+	memset(pixm->reserved, 0, sizeof(pixm->reserved));
+
+	v4l2_fill_pixfmt_mp(pixm, finfo->format, pixm->width, pixm->height);
+
+	for (i = 0; i < finfo->comp_planes; i++) {
+		struct v4l2_plane_pix_format *plane_fmt;
+		int width, height, bpl, size, bpp = 0;
+		const unsigned int hw_align = 64;
+
+		if (!i) {
+			width = pixm->width;
+			height = pixm->height;
+		} else {
+			width = pixm->width / finfo->hdiv;
+			height = pixm->height / finfo->vdiv;
+		}
+
+		switch (finfo->format) {
+		case V4L2_PIX_FMT_NV24:
+		case V4L2_PIX_FMT_NV16:
+		case V4L2_PIX_FMT_NV12:
+		case V4L2_PIX_FMT_BGR24:
+			bpp = finfo->bpp[i];
+			break;
+		default:
+			v4l2_dbg(1, debug, v4l2_dev,
+				 "fourcc: %#x is not supported\n",
+				 finfo->format);
+			break;
+		}
+
+		bpl = ALIGN(width * bpp, hw_align);
+		size = bpl * height;
+		imagesize += size;
+
+		if (finfo->mem_planes > i) {
+			/* Set bpl and size for each mplane */
+			plane_fmt = pixm->plane_fmt + i;
+			plane_fmt->bytesperline = bpl;
+			plane_fmt->sizeimage = size;
+		}
+
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "C-Plane %u size: %d, Total imagesize: %d\n",
+			 i, size, imagesize);
+	}
+
+	/* Convert to non-MPLANE format as we want to unify non-MPLANE and MPLANE */
+	if (finfo->mem_planes == 1)
+		pixm->plane_fmt[0].sizeimage = imagesize;
+
+	if (!try) {
+		stream->out_finfo = finfo;
+		stream->pixm = *pixm;
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "%s: req(%d, %d), out(%d, %d), fmt:%#x\n", __func__,
+			 pixm->width, pixm->height, stream->pixm.width,
+			 stream->pixm.height, finfo->format);
+	}
+}
+
+static int hdmirx_enum_fmt_vid_cap_mplane(struct file *file, void *priv,
+					  struct v4l2_fmtdesc *f)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+
+	if (f->index >= 1)
+		return -EINVAL;
+
+	f->pixelformat = hdmirx_dev->cur_fmt_fourcc;
+
+	return 0;
+}
+
+static int hdmirx_s_fmt_vid_cap_mplane(struct file *file,
+				       void *priv, struct v4l2_format *f)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	if (vb2_is_busy(&stream->buf_queue)) {
+		v4l2_err(v4l2_dev, "%s: queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	hdmirx_set_fmt(stream, &f->fmt.pix_mp, false);
+
+	return 0;
+}
+
+static int hdmirx_g_fmt_vid_cap_mplane(struct file *file, void *fh,
+				       struct v4l2_format *f)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_pix_format_mplane pixm = {};
+
+	pixm.pixelformat = hdmirx_dev->cur_fmt_fourcc;
+	hdmirx_set_fmt(stream, &pixm, true);
+	f->fmt.pix_mp = pixm;
+
+	return 0;
+}
+
+static int hdmirx_g_dv_timings(struct file *file, void *_fh,
+			       struct v4l2_dv_timings *timings)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 dma_cfg1;
+
+	*timings = hdmirx_dev->timings;
+	dma_cfg1 = hdmirx_readl(hdmirx_dev, DMA_CONFIG1);
+	v4l2_dbg(1, debug, v4l2_dev, "%s: pix_fmt: %s, DMA_CONFIG1:%#x\n",
+		 __func__, pix_fmt_str[hdmirx_dev->pix_fmt], dma_cfg1);
+
+	return 0;
+}
+
+static int hdmirx_s_dv_timings(struct file *file, void *_fh,
+			       struct v4l2_dv_timings *timings)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	if (!timings)
+		return -EINVAL;
+
+	if (debug)
+		v4l2_print_dv_timings(hdmirx_dev->v4l2_dev.name,
+				      "s_dv_timings: ", timings, false);
+
+	if (!v4l2_valid_dv_timings(timings, &hdmirx_timings_cap, NULL, NULL)) {
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "%s: timings out of range\n", __func__);
+		return -ERANGE;
+	}
+
+	/* Check if the timings are part of the CEA-861 timings. */
+	v4l2_find_dv_timings_cap(timings, &hdmirx_timings_cap, 0, NULL, NULL);
+
+	if (v4l2_match_dv_timings(&hdmirx_dev->timings, timings, 0, false)) {
+		v4l2_dbg(1, debug, v4l2_dev, "%s: no change\n", __func__);
+		return 0;
+	}
+
+	/*
+	 * Changing the timings implies a format change, which is not allowed
+	 * while buffers for use with streaming have already been allocated.
+	 */
+	if (vb2_is_busy(&stream->buf_queue))
+		return -EBUSY;
+
+	hdmirx_dev->timings = *timings;
+	/* Update the internal format */
+	hdmirx_set_fmt(stream, &stream->pixm, false);
+
+	return 0;
+}
+
+static int hdmirx_querycap(struct file *file, void *priv,
+			   struct v4l2_capability *cap)
+{
+	struct hdmirx_stream *stream = video_drvdata(file);
+	struct device *dev = stream->hdmirx_dev->dev;
+
+	strscpy(cap->driver, dev->driver->name, sizeof(cap->driver));
+	strscpy(cap->card, dev->driver->name, sizeof(cap->card));
+
+	return 0;
+}
+
+static int hdmirx_queue_setup(struct vb2_queue *queue,
+			      unsigned int *num_buffers,
+			      unsigned int *num_planes,
+			      unsigned int sizes[],
+			      struct device *alloc_ctxs[])
+{
+	struct hdmirx_stream *stream = vb2_get_drv_priv(queue);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	const struct v4l2_pix_format_mplane *pixm = NULL;
+	const struct v4l2_format_info *out_finfo;
+	u32 i;
+
+	pixm = &stream->pixm;
+	out_finfo = stream->out_finfo;
+
+	if (!out_finfo) {
+		v4l2_err(v4l2_dev, "%s: out_fmt not set\n", __func__);
+		return -EINVAL;
+	}
+
+	if (*num_planes) {
+		if (*num_planes != pixm->num_planes)
+			return -EINVAL;
+
+		for (i = 0; i < *num_planes; i++)
+			if (sizes[i] < pixm->plane_fmt[i].sizeimage)
+				return -EINVAL;
+		return 0;
+	}
+
+	*num_planes = out_finfo->mem_planes;
+
+	for (i = 0; i < out_finfo->mem_planes; i++)
+		sizes[i] = pixm->plane_fmt[i].sizeimage;
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: count %d, size %d\n",
+		 v4l2_type_names[queue->type], *num_buffers, sizes[0]);
+
+	return 0;
+}
+
+/*
+ * The vb2_buffer are stored in hdmirx_buffer, in order to unify
+ * mplane buffer and none-mplane buffer.
+ */
+static void hdmirx_buf_queue(struct vb2_buffer *vb)
+{
+	const struct v4l2_pix_format_mplane *pixm;
+	const struct v4l2_format_info *out_finfo;
+	struct hdmirx_buffer *hdmirx_buf;
+	struct vb2_v4l2_buffer *vbuf;
+	struct hdmirx_stream *stream;
+	struct vb2_queue *queue;
+	unsigned long flags;
+	unsigned int i;
+
+	vbuf = to_vb2_v4l2_buffer(vb);
+	hdmirx_buf = container_of(vbuf, struct hdmirx_buffer, vb);
+	queue = vb->vb2_queue;
+	stream = vb2_get_drv_priv(queue);
+	pixm = &stream->pixm;
+	out_finfo = stream->out_finfo;
+
+	memset(hdmirx_buf->buff_addr, 0, sizeof(hdmirx_buf->buff_addr));
+
+	/*
+	 * If mplanes > 1, every c-plane has its own m-plane,
+	 * otherwise, multiple c-planes are in the same m-plane
+	 */
+	for (i = 0; i < out_finfo->mem_planes; i++)
+		hdmirx_buf->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
+
+	if (out_finfo->mem_planes == 1) {
+		if (out_finfo->comp_planes == 1) {
+			hdmirx_buf->buff_addr[HDMIRX_PLANE_CBCR] =
+				hdmirx_buf->buff_addr[HDMIRX_PLANE_Y];
+		} else {
+			for (i = 0; i < out_finfo->comp_planes - 1; i++)
+				hdmirx_buf->buff_addr[i + 1] =
+					hdmirx_buf->buff_addr[i] +
+					pixm->plane_fmt[i].bytesperline *
+					pixm->height;
+		}
+	}
+
+	spin_lock_irqsave(&stream->vbq_lock, flags);
+	list_add_tail(&hdmirx_buf->queue, &stream->buf_head);
+	spin_unlock_irqrestore(&stream->vbq_lock, flags);
+}
+
+static void return_all_buffers(struct hdmirx_stream *stream,
+			       enum vb2_buffer_state state)
+{
+	struct hdmirx_buffer *buf, *tmp;
+	unsigned long flags;
+
+	spin_lock_irqsave(&stream->vbq_lock, flags);
+	if (stream->curr_buf)
+		list_add_tail(&stream->curr_buf->queue, &stream->buf_head);
+	if (stream->next_buf && stream->next_buf != stream->curr_buf)
+		list_add_tail(&stream->next_buf->queue, &stream->buf_head);
+	stream->curr_buf = NULL;
+	stream->next_buf = NULL;
+
+	list_for_each_entry_safe(buf, tmp, &stream->buf_head, queue) {
+		list_del(&buf->queue);
+		vb2_buffer_done(&buf->vb.vb2_buf, state);
+	}
+	spin_unlock_irqrestore(&stream->vbq_lock, flags);
+}
+
+static void hdmirx_stop_streaming(struct vb2_queue *queue)
+{
+	struct hdmirx_stream *stream = vb2_get_drv_priv(queue);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	int ret;
+
+	v4l2_dbg(1, debug, v4l2_dev, "stream start stopping\n");
+	mutex_lock(&hdmirx_dev->stream_lock);
+	WRITE_ONCE(stream->stopping, true);
+
+	/* wait last irq to return the buffer */
+	ret = wait_event_timeout(stream->wq_stopped, !stream->stopping,
+				 msecs_to_jiffies(500));
+	if (!ret)
+		v4l2_dbg(1, debug, v4l2_dev, "%s: timeout waiting last irq\n",
+			 __func__);
+
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, HDMIRX_DMA_EN, 0);
+	return_all_buffers(stream, VB2_BUF_STATE_ERROR);
+	mutex_unlock(&hdmirx_dev->stream_lock);
+	v4l2_dbg(1, debug, v4l2_dev, "stream stopping finished\n");
+}
+
+static int hdmirx_start_streaming(struct vb2_queue *queue, unsigned int count)
+{
+	struct hdmirx_stream *stream = vb2_get_drv_priv(queue);
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct v4l2_dv_timings timings = hdmirx_dev->timings;
+	struct v4l2_bt_timings *bt = &timings.bt;
+	unsigned long lock_flags = 0;
+	int line_flag;
+
+	mutex_lock(&hdmirx_dev->stream_lock);
+	stream->sequence = 0;
+	stream->line_flag_int_cnt = 0;
+	stream->curr_buf = NULL;
+	stream->next_buf = NULL;
+	stream->irq_stat = 0;
+
+	WRITE_ONCE(stream->stopping, false);
+
+	spin_lock_irqsave(&stream->vbq_lock, lock_flags);
+	if (!stream->curr_buf) {
+		if (!list_empty(&stream->buf_head)) {
+			stream->curr_buf = list_first_entry(&stream->buf_head,
+							    struct hdmirx_buffer,
+							    queue);
+			list_del(&stream->curr_buf->queue);
+		} else {
+			stream->curr_buf = NULL;
+		}
+	}
+	spin_unlock_irqrestore(&stream->vbq_lock, lock_flags);
+
+	if (!stream->curr_buf) {
+		mutex_unlock(&hdmirx_dev->stream_lock);
+		return -ENOMEM;
+	}
+
+	v4l2_dbg(2, debug, v4l2_dev,
+		 "%s: start_stream cur_buf y_addr:%#x, uv_addr:%#x\n",
+		 __func__, stream->curr_buf->buff_addr[HDMIRX_PLANE_Y],
+		 stream->curr_buf->buff_addr[HDMIRX_PLANE_CBCR]);
+	hdmirx_writel(hdmirx_dev, DMA_CONFIG2,
+		      stream->curr_buf->buff_addr[HDMIRX_PLANE_Y]);
+	hdmirx_writel(hdmirx_dev, DMA_CONFIG3,
+		      stream->curr_buf->buff_addr[HDMIRX_PLANE_CBCR]);
+
+	if (bt->height) {
+		if (bt->interlaced == V4L2_DV_INTERLACED)
+			line_flag = bt->height / 4;
+		else
+			line_flag = bt->height / 2;
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG7,
+				   LINE_FLAG_NUM_MASK,
+				   LINE_FLAG_NUM(line_flag));
+	} else {
+		v4l2_err(v4l2_dev, "invalid BT timing height=%d\n", bt->height);
+	}
+
+	hdmirx_writel(hdmirx_dev, DMA_CONFIG5, 0xffffffff);
+	hdmirx_writel(hdmirx_dev, CED_DYN_CONTROL, 0x1);
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG4,
+			   LINE_FLAG_INT_EN |
+			   HDMIRX_DMA_IDLE_INT |
+			   HDMIRX_LOCK_DISABLE_INT |
+			   LAST_FRAME_AXI_UNFINISH_INT_EN |
+			   FIFO_OVERFLOW_INT_EN |
+			   FIFO_UNDERFLOW_INT_EN |
+			   HDMIRX_AXI_ERROR_INT_EN,
+			   LINE_FLAG_INT_EN |
+			   HDMIRX_DMA_IDLE_INT |
+			   HDMIRX_LOCK_DISABLE_INT |
+			   LAST_FRAME_AXI_UNFINISH_INT_EN |
+			   FIFO_OVERFLOW_INT_EN |
+			   FIFO_UNDERFLOW_INT_EN |
+			   HDMIRX_AXI_ERROR_INT_EN);
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, HDMIRX_DMA_EN, HDMIRX_DMA_EN);
+	v4l2_dbg(1, debug, v4l2_dev, "%s: enable dma", __func__);
+	mutex_unlock(&hdmirx_dev->stream_lock);
+
+	return 0;
+}
+
+/* vb2 queue */
+static const struct vb2_ops hdmirx_vb2_ops = {
+	.queue_setup = hdmirx_queue_setup,
+	.buf_queue = hdmirx_buf_queue,
+	.stop_streaming = hdmirx_stop_streaming,
+	.start_streaming = hdmirx_start_streaming,
+};
+
+static int hdmirx_init_vb2_queue(struct vb2_queue *q,
+				 struct hdmirx_stream *stream,
+				 enum v4l2_buf_type buf_type)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+
+	q->type = buf_type;
+	q->io_modes = VB2_MMAP | VB2_DMABUF;
+	q->drv_priv = stream;
+	q->ops = &hdmirx_vb2_ops;
+	q->mem_ops = &vb2_dma_contig_memops;
+	q->buf_struct_size = sizeof(struct hdmirx_buffer);
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &stream->vlock;
+	q->dev = hdmirx_dev->dev;
+	q->min_queued_buffers = 1;
+
+	return vb2_queue_init(q);
+}
+
+/* video device */
+static const struct v4l2_ioctl_ops hdmirx_v4l2_ioctl_ops = {
+	.vidioc_querycap = hdmirx_querycap,
+	.vidioc_try_fmt_vid_cap_mplane = hdmirx_g_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_cap_mplane = hdmirx_s_fmt_vid_cap_mplane,
+	.vidioc_g_fmt_vid_cap_mplane = hdmirx_g_fmt_vid_cap_mplane,
+	.vidioc_enum_fmt_vid_cap = hdmirx_enum_fmt_vid_cap_mplane,
+
+	.vidioc_s_dv_timings = hdmirx_s_dv_timings,
+	.vidioc_g_dv_timings = hdmirx_g_dv_timings,
+	.vidioc_enum_dv_timings = hdmirx_enum_dv_timings,
+	.vidioc_query_dv_timings = hdmirx_query_dv_timings,
+	.vidioc_dv_timings_cap = hdmirx_dv_timings_cap,
+	.vidioc_enum_input = hdmirx_enum_input,
+	.vidioc_g_input = hdmirx_get_input,
+	.vidioc_s_input = hdmirx_set_input,
+	.vidioc_g_edid = hdmirx_get_edid,
+	.vidioc_s_edid = hdmirx_set_edid,
+	.vidioc_g_parm = hdmirx_g_parm,
+
+	.vidioc_reqbufs = vb2_ioctl_reqbufs,
+	.vidioc_querybuf = vb2_ioctl_querybuf,
+	.vidioc_create_bufs = vb2_ioctl_create_bufs,
+	.vidioc_qbuf = vb2_ioctl_qbuf,
+	.vidioc_expbuf = vb2_ioctl_expbuf,
+	.vidioc_dqbuf = vb2_ioctl_dqbuf,
+	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+	.vidioc_streamon = vb2_ioctl_streamon,
+	.vidioc_streamoff = vb2_ioctl_streamoff,
+
+	.vidioc_log_status = v4l2_ctrl_log_status,
+	.vidioc_subscribe_event = hdmirx_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+static const struct v4l2_file_operations hdmirx_fops = {
+	.owner = THIS_MODULE,
+	.open = v4l2_fh_open,
+	.release = vb2_fop_release,
+	.unlocked_ioctl = video_ioctl2,
+	.poll = vb2_fop_poll,
+	.mmap = vb2_fop_mmap,
+};
+
+static int hdmirx_register_stream_vdev(struct hdmirx_stream *stream)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = stream->hdmirx_dev;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct video_device *vdev = &stream->vdev;
+	int ret;
+
+	strscpy(vdev->name, "stream_hdmirx", sizeof(vdev->name));
+	INIT_LIST_HEAD(&stream->buf_head);
+	spin_lock_init(&stream->vbq_lock);
+	mutex_init(&stream->vlock);
+	init_waitqueue_head(&stream->wq_stopped);
+	stream->curr_buf = NULL;
+	stream->next_buf = NULL;
+
+	vdev->ioctl_ops = &hdmirx_v4l2_ioctl_ops;
+	vdev->release = video_device_release_empty;
+	vdev->fops = &hdmirx_fops;
+	vdev->minor = -1;
+	vdev->v4l2_dev = v4l2_dev;
+	vdev->lock = &stream->vlock;
+	vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
+			    V4L2_CAP_STREAMING;
+	vdev->vfl_dir = VFL_DIR_RX;
+
+	video_set_drvdata(vdev, stream);
+
+	hdmirx_init_vb2_queue(&stream->buf_queue, stream,
+			      V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
+	vdev->queue = &stream->buf_queue;
+
+	ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "video_register_device failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static void process_signal_change(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG6, HDMIRX_DMA_EN, 0);
+	hdmirx_update_bits(hdmirx_dev, DMA_CONFIG4,
+			   LINE_FLAG_INT_EN |
+			   HDMIRX_DMA_IDLE_INT |
+			   HDMIRX_LOCK_DISABLE_INT |
+			   LAST_FRAME_AXI_UNFINISH_INT_EN |
+			   FIFO_OVERFLOW_INT_EN |
+			   FIFO_UNDERFLOW_INT_EN |
+			   HDMIRX_AXI_ERROR_INT_EN, 0);
+	hdmirx_reset_dma(hdmirx_dev);
+	queue_delayed_work(system_unbound_wq,
+			   &hdmirx_dev->delayed_work_res_change,
+			   msecs_to_jiffies(50));
+}
+
+static void avpunit_0_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				  int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	if (status & (CED_DYN_CNT_CH2_IRQ |
+		      CED_DYN_CNT_CH1_IRQ |
+		      CED_DYN_CNT_CH0_IRQ)) {
+		process_signal_change(hdmirx_dev);
+		v4l2_dbg(2, debug, v4l2_dev, "%s: avp0_st:%#x\n",
+			 __func__, status);
+		*handled = true;
+	}
+
+	hdmirx_clear_interrupt(hdmirx_dev, AVPUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_writel(hdmirx_dev, AVPUNIT_0_INT_FORCE, 0x0);
+}
+
+static void avpunit_1_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				  int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	if (status & DEFRAMER_VSYNC_THR_REACHED_IRQ) {
+		v4l2_dbg(2, debug, v4l2_dev,
+			 "Vertical Sync threshold reached interrupt %#x", status);
+		hdmirx_update_bits(hdmirx_dev, AVPUNIT_1_INT_MASK_N,
+				   DEFRAMER_VSYNC_THR_REACHED_MASK_N, 0);
+		*handled = true;
+	}
+}
+
+static void mainunit_0_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				   int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	v4l2_dbg(2, debug, v4l2_dev, "mu0_st:%#x\n", status);
+	if (status & TIMER_BASE_LOCKED_IRQ) {
+		hdmirx_update_bits(hdmirx_dev, MAINUNIT_0_INT_MASK_N,
+				   TIMER_BASE_LOCKED_IRQ, 0);
+		complete(&hdmirx_dev->timer_base_lock);
+		*handled = true;
+	}
+
+	if (status & TMDSQPCLK_OFF_CHG) {
+		process_signal_change(hdmirx_dev);
+		v4l2_dbg(2, debug, v4l2_dev, "%s: TMDSQPCLK_OFF_CHG\n", __func__);
+		*handled = true;
+	}
+
+	if (status & TMDSQPCLK_LOCKED_CHG) {
+		process_signal_change(hdmirx_dev);
+		v4l2_dbg(2, debug, v4l2_dev, "%s: TMDSQPCLK_LOCKED_CHG\n", __func__);
+		*handled = true;
+	}
+
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_writel(hdmirx_dev, MAINUNIT_0_INT_FORCE, 0x0);
+}
+
+static void mainunit_2_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				   int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	v4l2_dbg(2, debug, v4l2_dev, "mu2_st:%#x\n", status);
+	if (status & PHYCREG_CR_WRITE_DONE) {
+		hdmirx_update_bits(hdmirx_dev, MAINUNIT_2_INT_MASK_N,
+				   PHYCREG_CR_WRITE_DONE, 0);
+		complete(&hdmirx_dev->cr_write_done);
+		*handled = true;
+	}
+
+	if (status & TMDSVALID_STABLE_CHG) {
+		process_signal_change(hdmirx_dev);
+		v4l2_dbg(2, debug, v4l2_dev, "%s: TMDSVALID_STABLE_CHG\n", __func__);
+		*handled = true;
+	}
+
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_writel(hdmirx_dev, MAINUNIT_2_INT_FORCE, 0x0);
+}
+
+static void pkt_2_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+			      int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	v4l2_dbg(2, debug, v4l2_dev, "%s: pk2_st:%#x\n", __func__, status);
+	if (status & PKTDEC_AVIIF_RCV_IRQ) {
+		hdmirx_update_bits(hdmirx_dev, PKT_2_INT_MASK_N,
+				   PKTDEC_AVIIF_RCV_IRQ, 0);
+		complete(&hdmirx_dev->avi_pkt_rcv);
+		v4l2_dbg(2, debug, v4l2_dev, "%s: AVIIF_RCV_IRQ\n", __func__);
+		*handled = true;
+	}
+
+	hdmirx_clear_interrupt(hdmirx_dev, PKT_2_INT_CLEAR, 0xffffffff);
+}
+
+static void scdc_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+			     int status, bool *handled)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	v4l2_dbg(2, debug, v4l2_dev, "%s: scdc_st:%#x\n", __func__, status);
+	if (status & SCDCTMDSCCFG_CHG) {
+		hdmirx_tmds_clk_ratio_config(hdmirx_dev);
+		*handled = true;
+	}
+
+	hdmirx_clear_interrupt(hdmirx_dev, SCDC_INT_CLEAR, 0xffffffff);
+}
+
+static irqreturn_t hdmirx_hdmi_irq_handler(int irq, void *dev_id)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_id;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 mu0_st, mu2_st, pk2_st, scdc_st, avp1_st, avp0_st;
+	u32 mu0_mask, mu2_mask, pk2_mask, scdc_mask, avp1_msk, avp0_msk;
+	bool handled = false;
+
+	mu0_mask = hdmirx_readl(hdmirx_dev, MAINUNIT_0_INT_MASK_N);
+	mu2_mask = hdmirx_readl(hdmirx_dev, MAINUNIT_2_INT_MASK_N);
+	pk2_mask = hdmirx_readl(hdmirx_dev, PKT_2_INT_MASK_N);
+	scdc_mask = hdmirx_readl(hdmirx_dev, SCDC_INT_MASK_N);
+	mu0_st = hdmirx_readl(hdmirx_dev, MAINUNIT_0_INT_STATUS);
+	mu2_st = hdmirx_readl(hdmirx_dev, MAINUNIT_2_INT_STATUS);
+	pk2_st = hdmirx_readl(hdmirx_dev, PKT_2_INT_STATUS);
+	scdc_st = hdmirx_readl(hdmirx_dev, SCDC_INT_STATUS);
+	avp0_st = hdmirx_readl(hdmirx_dev, AVPUNIT_0_INT_STATUS);
+	avp1_st = hdmirx_readl(hdmirx_dev, AVPUNIT_1_INT_STATUS);
+	avp0_msk = hdmirx_readl(hdmirx_dev, AVPUNIT_0_INT_MASK_N);
+	avp1_msk = hdmirx_readl(hdmirx_dev, AVPUNIT_1_INT_MASK_N);
+	mu0_st &= mu0_mask;
+	mu2_st &= mu2_mask;
+	pk2_st &= pk2_mask;
+	avp1_st &= avp1_msk;
+	avp0_st &= avp0_msk;
+	scdc_st &= scdc_mask;
+
+	if (avp0_st)
+		avpunit_0_int_handler(hdmirx_dev, avp0_st, &handled);
+	if (avp1_st)
+		avpunit_1_int_handler(hdmirx_dev, avp1_st, &handled);
+	if (mu0_st)
+		mainunit_0_int_handler(hdmirx_dev, mu0_st, &handled);
+	if (mu2_st)
+		mainunit_2_int_handler(hdmirx_dev, mu2_st, &handled);
+	if (pk2_st)
+		pkt_2_int_handler(hdmirx_dev, pk2_st, &handled);
+	if (scdc_st)
+		scdc_int_handler(hdmirx_dev, scdc_st, &handled);
+
+	if (!handled) {
+		v4l2_dbg(2, debug, v4l2_dev, "%s: hdmi irq not handled", __func__);
+		v4l2_dbg(2, debug, v4l2_dev,
+			 "avp0:%#x, avp1:%#x, mu0:%#x, mu2:%#x, pk2:%#x, scdc:%#x\n",
+			 avp0_st, avp1_st, mu0_st, mu2_st, pk2_st, scdc_st);
+	}
+
+	v4l2_dbg(2, debug, v4l2_dev, "%s: en_fiq", __func__);
+
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static void hdmirx_vb_done(struct hdmirx_stream *stream,
+			   struct vb2_v4l2_buffer *vb_done)
+{
+	const struct v4l2_format_info *finfo = stream->out_finfo;
+	u32 i;
+
+	/* Dequeue a filled buffer */
+	for (i = 0; i < finfo->mem_planes; i++) {
+		vb2_set_plane_payload(&vb_done->vb2_buf, i,
+				      stream->pixm.plane_fmt[i].sizeimage);
+	}
+
+	vb_done->vb2_buf.timestamp = ktime_get_ns();
+	vb2_buffer_done(&vb_done->vb2_buf, VB2_BUF_STATE_DONE);
+}
+
+static void dma_idle_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				 bool *handled)
+{
+	struct hdmirx_stream *stream = &hdmirx_dev->stream;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct v4l2_dv_timings timings = hdmirx_dev->timings;
+	struct v4l2_bt_timings *bt = &timings.bt;
+	struct vb2_v4l2_buffer *vb_done = NULL;
+
+	if (!(stream->irq_stat) && !(stream->irq_stat & LINE_FLAG_INT_EN))
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "%s: last time have no line_flag_irq\n", __func__);
+
+	/* skip first frames that are expected to come out zeroed from DMA */
+	if (stream->line_flag_int_cnt <= FILTER_FRAME_CNT)
+		goto DMA_IDLE_OUT;
+
+	if (bt->interlaced != V4L2_DV_INTERLACED ||
+	    !(stream->line_flag_int_cnt % 2)) {
+		if (stream->next_buf) {
+			if (stream->curr_buf)
+				vb_done = &stream->curr_buf->vb;
+
+			if (vb_done) {
+				vb_done->vb2_buf.timestamp = ktime_get_ns();
+				vb_done->sequence = stream->sequence;
+
+				if (bt->interlaced)
+					vb_done->field = V4L2_FIELD_INTERLACED_TB;
+				else
+					vb_done->field = V4L2_FIELD_NONE;
+
+				hdmirx_vb_done(stream, vb_done);
+			}
+
+			stream->curr_buf = NULL;
+			if (stream->next_buf) {
+				stream->curr_buf = stream->next_buf;
+				stream->next_buf = NULL;
+			}
+		} else {
+			v4l2_dbg(3, debug, v4l2_dev,
+				 "%s: next_buf NULL, skip vb_done\n", __func__);
+		}
+
+		stream->sequence++;
+		if (stream->sequence == 30)
+			v4l2_dbg(1, debug, v4l2_dev, "rcv frames\n");
+	}
+
+DMA_IDLE_OUT:
+	*handled = true;
+}
+
+static void line_flag_int_handler(struct snps_hdmirx_dev *hdmirx_dev,
+				  bool *handled)
+{
+	struct hdmirx_stream *stream = &hdmirx_dev->stream;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	struct v4l2_dv_timings timings = hdmirx_dev->timings;
+	struct v4l2_bt_timings *bt = &timings.bt;
+	u32 dma_cfg6;
+
+	stream->line_flag_int_cnt++;
+	if (!(stream->irq_stat) && !(stream->irq_stat & HDMIRX_DMA_IDLE_INT))
+		v4l2_dbg(1, debug, v4l2_dev,
+			 "%s: last have no dma_idle_irq\n", __func__);
+	dma_cfg6 = hdmirx_readl(hdmirx_dev, DMA_CONFIG6);
+	if (!(dma_cfg6 & HDMIRX_DMA_EN)) {
+		v4l2_dbg(2, debug, v4l2_dev, "%s: dma not on\n", __func__);
+		goto LINE_FLAG_OUT;
+	}
+
+	if (stream->line_flag_int_cnt <= FILTER_FRAME_CNT)
+		goto LINE_FLAG_OUT;
+
+	if (bt->interlaced != V4L2_DV_INTERLACED ||
+	    !(stream->line_flag_int_cnt % 2)) {
+		if (!stream->next_buf) {
+			spin_lock(&stream->vbq_lock);
+			if (!list_empty(&stream->buf_head)) {
+				stream->next_buf = list_first_entry(&stream->buf_head,
+								    struct hdmirx_buffer,
+								    queue);
+				list_del(&stream->next_buf->queue);
+			} else {
+				stream->next_buf = NULL;
+			}
+			spin_unlock(&stream->vbq_lock);
+
+			if (stream->next_buf) {
+				hdmirx_writel(hdmirx_dev, DMA_CONFIG2,
+					      stream->next_buf->buff_addr[HDMIRX_PLANE_Y]);
+				hdmirx_writel(hdmirx_dev, DMA_CONFIG3,
+					      stream->next_buf->buff_addr[HDMIRX_PLANE_CBCR]);
+			} else {
+				v4l2_dbg(3, debug, v4l2_dev,
+					 "%s: no buffer is available\n", __func__);
+			}
+		}
+	} else {
+		v4l2_dbg(3, debug, v4l2_dev, "%s: interlace:%d, line_flag_int_cnt:%d\n",
+			 __func__, bt->interlaced, stream->line_flag_int_cnt);
+	}
+
+LINE_FLAG_OUT:
+	*handled = true;
+}
+
+static irqreturn_t hdmirx_dma_irq_handler(int irq, void *dev_id)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_id;
+	struct hdmirx_stream *stream = &hdmirx_dev->stream;
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 dma_stat1, dma_stat13;
+	bool handled = false;
+
+	dma_stat1 = hdmirx_readl(hdmirx_dev, DMA_STATUS1);
+	dma_stat13 = hdmirx_readl(hdmirx_dev, DMA_STATUS13);
+	v4l2_dbg(3, debug, v4l2_dev, "dma_irq st1:%#x, st13:%d\n",
+		 dma_stat1, dma_stat13);
+
+	if (READ_ONCE(stream->stopping)) {
+		v4l2_dbg(1, debug, v4l2_dev, "%s: stop stream\n", __func__);
+		hdmirx_writel(hdmirx_dev, DMA_CONFIG5, 0xffffffff);
+		hdmirx_update_bits(hdmirx_dev, DMA_CONFIG4,
+				   LINE_FLAG_INT_EN |
+				   HDMIRX_DMA_IDLE_INT |
+				   HDMIRX_LOCK_DISABLE_INT |
+				   LAST_FRAME_AXI_UNFINISH_INT_EN |
+				   FIFO_OVERFLOW_INT_EN |
+				   FIFO_UNDERFLOW_INT_EN |
+				   HDMIRX_AXI_ERROR_INT_EN, 0);
+		WRITE_ONCE(stream->stopping, false);
+		wake_up(&stream->wq_stopped);
+		return IRQ_HANDLED;
+	}
+
+	if (dma_stat1 & HDMIRX_DMA_IDLE_INT)
+		dma_idle_int_handler(hdmirx_dev, &handled);
+
+	if (dma_stat1 & LINE_FLAG_INT_EN)
+		line_flag_int_handler(hdmirx_dev, &handled);
+
+	if (!handled)
+		v4l2_dbg(3, debug, v4l2_dev,
+			 "%s: dma irq not handled, dma_stat1:%#x\n",
+			 __func__, dma_stat1);
+
+	stream->irq_stat = dma_stat1;
+	hdmirx_writel(hdmirx_dev, DMA_CONFIG5, 0xffffffff);
+
+	return IRQ_HANDLED;
+}
+
+static int hdmirx_wait_signal_lock(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	u32 mu_status, scdc_status, dma_st10, cmu_st;
+	u32 i;
+
+	for (i = 0; i < 300; i++) {
+		mu_status = hdmirx_readl(hdmirx_dev, MAINUNIT_STATUS);
+		scdc_status = hdmirx_readl(hdmirx_dev, SCDC_REGBANK_STATUS3);
+		dma_st10 = hdmirx_readl(hdmirx_dev, DMA_STATUS10);
+		cmu_st = hdmirx_readl(hdmirx_dev, CMU_STATUS);
+
+		if ((mu_status & TMDSVALID_STABLE_ST) &&
+		    (dma_st10 & HDMIRX_LOCK) &&
+		    (cmu_st & TMDSQPCLK_LOCKED_ST))
+			break;
+
+		if (!tx_5v_power_present(hdmirx_dev)) {
+			v4l2_dbg(1, debug, v4l2_dev,
+				 "%s: HDMI pull out, return\n", __func__);
+			return -1;
+		}
+
+		hdmirx_tmds_clk_ratio_config(hdmirx_dev);
+	}
+
+	if (i == 300) {
+		v4l2_err(v4l2_dev, "%s: signal not lock, tmds_clk_ratio:%d\n",
+			 __func__, hdmirx_dev->tmds_clk_ratio);
+		v4l2_err(v4l2_dev, "%s: mu_st:%#x, scdc_st:%#x, dma_st10:%#x\n",
+			 __func__, mu_status, scdc_status, dma_st10);
+		return -1;
+	}
+
+	v4l2_dbg(1, debug, v4l2_dev, "%s: signal lock ok, i:%d\n", __func__, i);
+	hdmirx_writel(hdmirx_dev, GLOBAL_SWRESET_REQUEST, DATAPATH_SWRESETREQ);
+
+	reinit_completion(&hdmirx_dev->avi_pkt_rcv);
+	hdmirx_clear_interrupt(hdmirx_dev, PKT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_update_bits(hdmirx_dev, PKT_2_INT_MASK_N,
+			   PKTDEC_AVIIF_RCV_IRQ, PKTDEC_AVIIF_RCV_IRQ);
+
+	if (!wait_for_completion_timeout(&hdmirx_dev->avi_pkt_rcv,
+					 msecs_to_jiffies(300))) {
+		v4l2_err(v4l2_dev, "%s wait avi_pkt_rcv failed\n", __func__);
+		hdmirx_update_bits(hdmirx_dev, PKT_2_INT_MASK_N,
+				   PKTDEC_AVIIF_RCV_IRQ, 0);
+	}
+
+	msleep(50);
+	hdmirx_format_change(hdmirx_dev);
+
+	return 0;
+}
+
+static void hdmirx_plugin(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	if (hdmirx_dev->plugged)
+		return;
+
+	hdmirx_submodule_init(hdmirx_dev);
+	hdmirx_update_bits(hdmirx_dev, SCDC_CONFIG, POWERPROVIDED,
+			   POWERPROVIDED);
+	hdmirx_phy_config(hdmirx_dev);
+	hdmirx_interrupts_setup(hdmirx_dev, true);
+
+	hdmirx_dev->plugged = true;
+}
+
+static void hdmirx_delayed_work_hotplug(struct work_struct *work)
+{
+	struct snps_hdmirx_dev *hdmirx_dev;
+	bool plugin;
+
+	hdmirx_dev = container_of(work, struct snps_hdmirx_dev,
+				  delayed_work_hotplug.work);
+
+	mutex_lock(&hdmirx_dev->work_lock);
+	plugin = tx_5v_power_present(hdmirx_dev);
+	v4l2_ctrl_s_ctrl(hdmirx_dev->detect_tx_5v_ctrl, plugin);
+	v4l2_dbg(1, debug, &hdmirx_dev->v4l2_dev, "%s: plugin:%d\n",
+		 __func__, plugin);
+
+	hdmirx_plugout(hdmirx_dev);
+
+	if (plugin)
+		hdmirx_plugin(hdmirx_dev);
+
+	mutex_unlock(&hdmirx_dev->work_lock);
+}
+
+static void hdmirx_delayed_work_res_change(struct work_struct *work)
+{
+	struct snps_hdmirx_dev *hdmirx_dev;
+	bool plugin;
+
+	hdmirx_dev = container_of(work, struct snps_hdmirx_dev,
+				  delayed_work_res_change.work);
+
+	mutex_lock(&hdmirx_dev->work_lock);
+	plugin = tx_5v_power_present(hdmirx_dev);
+	v4l2_dbg(1, debug, &hdmirx_dev->v4l2_dev, "%s: plugin:%d\n",
+		 __func__, plugin);
+	if (plugin) {
+		hdmirx_interrupts_setup(hdmirx_dev, false);
+		hdmirx_submodule_init(hdmirx_dev);
+		hdmirx_update_bits(hdmirx_dev, SCDC_CONFIG, POWERPROVIDED,
+				   POWERPROVIDED);
+		hdmirx_phy_config(hdmirx_dev);
+
+		if (hdmirx_wait_signal_lock(hdmirx_dev)) {
+			hdmirx_plugout(hdmirx_dev);
+			queue_delayed_work(system_unbound_wq,
+					   &hdmirx_dev->delayed_work_hotplug,
+					   msecs_to_jiffies(200));
+		} else {
+			hdmirx_dma_config(hdmirx_dev);
+			hdmirx_interrupts_setup(hdmirx_dev, true);
+		}
+	}
+	mutex_unlock(&hdmirx_dev->work_lock);
+}
+
+static irqreturn_t hdmirx_5v_det_irq_handler(int irq, void *dev_id)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_id;
+	u32 val;
+
+	val = gpiod_get_value(hdmirx_dev->detect_5v_gpio);
+	v4l2_dbg(3, debug, &hdmirx_dev->v4l2_dev, "%s: 5v:%d\n", __func__, val);
+
+	queue_delayed_work(system_unbound_wq,
+			   &hdmirx_dev->delayed_work_hotplug,
+			   msecs_to_jiffies(10));
+
+	return IRQ_HANDLED;
+}
+
+static const struct hdmirx_cec_ops hdmirx_cec_ops = {
+	.write = hdmirx_writel,
+	.read = hdmirx_readl,
+};
+
+static void devm_hdmirx_of_reserved_mem_device_release(void *dev)
+{
+	of_reserved_mem_device_release(dev);
+}
+
+static int hdmirx_parse_dt(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct device *dev = hdmirx_dev->dev;
+	int ret;
+
+	hdmirx_dev->num_clks = devm_clk_bulk_get_all(dev, &hdmirx_dev->clks);
+	if (hdmirx_dev->num_clks < 1)
+		return -ENODEV;
+
+	hdmirx_dev->resets[HDMIRX_RST_A].id = "axi";
+	hdmirx_dev->resets[HDMIRX_RST_P].id = "apb";
+	hdmirx_dev->resets[HDMIRX_RST_REF].id = "ref";
+	hdmirx_dev->resets[HDMIRX_RST_BIU].id = "biu";
+
+	ret = devm_reset_control_bulk_get_exclusive(dev, HDMIRX_NUM_RST,
+						    hdmirx_dev->resets);
+	if (ret < 0) {
+		dev_err(dev, "failed to get reset controls\n");
+		return ret;
+	}
+
+	hdmirx_dev->detect_5v_gpio =
+		devm_gpiod_get_optional(dev, "hpd", GPIOD_IN);
+
+	if (IS_ERR(hdmirx_dev->detect_5v_gpio)) {
+		dev_err(dev, "failed to get hdmirx hot plug detection gpio\n");
+		return PTR_ERR(hdmirx_dev->detect_5v_gpio);
+	}
+
+	hdmirx_dev->grf = syscon_regmap_lookup_by_phandle(dev->of_node,
+							  "rockchip,grf");
+	if (IS_ERR(hdmirx_dev->grf)) {
+		dev_err(dev, "failed to get rockchip,grf\n");
+		return PTR_ERR(hdmirx_dev->grf);
+	}
+
+	hdmirx_dev->vo1_grf = syscon_regmap_lookup_by_phandle(dev->of_node,
+							      "rockchip,vo1-grf");
+	if (IS_ERR(hdmirx_dev->vo1_grf)) {
+		dev_err(dev, "failed to get rockchip,vo1-grf\n");
+		return PTR_ERR(hdmirx_dev->vo1_grf);
+	}
+
+	if (!device_property_read_bool(dev, "hpd-is-active-low"))
+		hdmirx_dev->hpd_trigger_level_high = true;
+
+	ret = of_reserved_mem_device_init(dev);
+	if (ret) {
+		dev_warn(dev, "no reserved memory for HDMIRX, use default CMA\n");
+	} else {
+		ret = devm_add_action_or_reset(dev,
+					       devm_hdmirx_of_reserved_mem_device_release,
+					       dev);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static void hdmirx_disable_all_interrupts(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	hdmirx_writel(hdmirx_dev, MAINUNIT_0_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, MAINUNIT_1_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, MAINUNIT_2_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, AVPUNIT_0_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, AVPUNIT_1_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, PKT_0_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, PKT_1_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, PKT_2_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, SCDC_INT_MASK_N, 0);
+	hdmirx_writel(hdmirx_dev, CEC_INT_MASK_N, 0);
+
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_1_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, MAINUNIT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, AVPUNIT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, AVPUNIT_1_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, PKT_0_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, PKT_1_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, PKT_2_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, SCDC_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, HDCP_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, HDCP_1_INT_CLEAR, 0xffffffff);
+	hdmirx_clear_interrupt(hdmirx_dev, CEC_INT_CLEAR, 0xffffffff);
+}
+
+static void hdmirx_init(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	hdmirx_update_bits(hdmirx_dev, PHY_CONFIG, PHY_RESET | PHY_PDDQ, 0);
+
+	regmap_write(hdmirx_dev->vo1_grf, VO1_GRF_VO1_CON2,
+		     (HDMIRX_SDAIN_MSK | HDMIRX_SCLIN_MSK) |
+		     ((HDMIRX_SDAIN_MSK | HDMIRX_SCLIN_MSK) << 16));
+	/*
+	 * Some interrupts are enabled by default, so we disable
+	 * all interrupts and clear interrupts status first.
+	 */
+	hdmirx_disable_all_interrupts(hdmirx_dev);
+}
+
+/* hdmi-4k-300mhz EDID produced by v4l2-ctl tool */
+static u8 __maybe_unused edid_default[] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x31, 0xd8, 0x34, 0x12, 0x00, 0x00, 0x00, 0x00,
+	0x22, 0x1a, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+	0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+	0x0f, 0x50, 0x54, 0x2f, 0xcf, 0x00, 0x31, 0x59,
+	0x45, 0x59, 0x81, 0x80, 0x81, 0x40, 0x90, 0x40,
+	0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x04, 0x74,
+	0x00, 0x30, 0xf2, 0x70, 0x5a, 0x80, 0xb0, 0x58,
+	0x8a, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x87, 0x1e, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x68,
+	0x64, 0x6d, 0x69, 0x2d, 0x34, 0x6b, 0x2d, 0x33,
+	0x30, 0x30, 0x0a, 0x20, 0x00, 0x00, 0x00, 0x10,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc5,
+
+	0x02, 0x03, 0x40, 0xf1, 0x4f, 0x5f, 0x5e, 0x5d,
+	0x10, 0x1f, 0x04, 0x13, 0x22, 0x21, 0x20, 0x05,
+	0x14, 0x02, 0x11, 0x01, 0x23, 0x09, 0x07, 0x07,
+	0x83, 0x01, 0x00, 0x00, 0x6d, 0x03, 0x0c, 0x00,
+	0x10, 0x00, 0x00, 0x3c, 0x21, 0x00, 0x60, 0x01,
+	0x02, 0x03, 0x67, 0xd8, 0x5d, 0xc4, 0x01, 0x00,
+	0x00, 0x00, 0xe2, 0x00, 0xca, 0xe3, 0x05, 0x00,
+	0x00, 0xe3, 0x06, 0x01, 0x00, 0xe2, 0x0d, 0x5f,
+	0xa3, 0x66, 0x00, 0xa0, 0xf0, 0x70, 0x1f, 0x80,
+	0x30, 0x20, 0x35, 0x00, 0xc0, 0x1c, 0x32, 0x00,
+	0x00, 0x1e, 0x1a, 0x36, 0x80, 0xa0, 0x70, 0x38,
+	0x1f, 0x40, 0x30, 0x20, 0x35, 0x00, 0xc0, 0x1c,
+	0x32, 0x00, 0x00, 0x1a, 0x1a, 0x1d, 0x00, 0x80,
+	0x51, 0xd0, 0x1c, 0x20, 0x40, 0x80, 0x35, 0x00,
+	0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1c, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa1,
+};
+
+static void hdmirx_load_default_edid(struct snps_hdmirx_dev *hdmirx_dev)
+{
+	struct v4l2_edid def_edid = {};
+
+	hdmirx_hpd_ctrl(hdmirx_dev, false);
+
+	if (!IS_ENABLED(CONFIG_VIDEO_SYNOPSYS_HDMIRX_LOAD_DEFAULT_EDID))
+		return;
+
+	/* disable hpd and write edid */
+	def_edid.blocks = sizeof(edid_default) / EDID_BLOCK_SIZE;
+	def_edid.edid = edid_default;
+
+	hdmirx_write_edid(hdmirx_dev, &def_edid);
+	hdmirx_hpd_ctrl(hdmirx_dev, true);
+}
+
+static int hdmirx_disable(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+
+	hdmirx_plugout(hdmirx_dev);
+	hdmirx_hpd_ctrl(hdmirx_dev, false);
+
+	clk_bulk_disable_unprepare(hdmirx_dev->num_clks, hdmirx_dev->clks);
+
+	v4l2_dbg(2, debug, v4l2_dev, "%s: suspend\n", __func__);
+
+	return pinctrl_pm_select_sleep_state(dev);
+}
+
+static int hdmirx_enable(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+	struct v4l2_device *v4l2_dev = &hdmirx_dev->v4l2_dev;
+	int ret;
+
+	v4l2_dbg(2, debug, v4l2_dev, "%s: resume\n", __func__);
+	ret = pinctrl_pm_select_default_state(dev);
+	if (ret < 0)
+		return ret;
+
+	ret = clk_bulk_prepare_enable(hdmirx_dev->num_clks, hdmirx_dev->clks);
+	if (ret) {
+		dev_err(dev, "failed to enable hdmirx bulk clks: %d\n", ret);
+		return ret;
+	}
+
+	reset_control_bulk_assert(HDMIRX_NUM_RST, hdmirx_dev->resets);
+	usleep_range(150, 160);
+	reset_control_bulk_deassert(HDMIRX_NUM_RST, hdmirx_dev->resets);
+	usleep_range(150, 160);
+
+	return 0;
+}
+
+static void hdmirx_disable_irq(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+
+	disable_irq(hdmirx_dev->det_irq);
+	disable_irq(hdmirx_dev->dma_irq);
+	disable_irq(hdmirx_dev->hdmi_irq);
+
+	cancel_delayed_work_sync(&hdmirx_dev->delayed_work_hotplug);
+	cancel_delayed_work_sync(&hdmirx_dev->delayed_work_res_change);
+}
+
+static void hdmirx_enable_irq(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+
+	enable_irq(hdmirx_dev->hdmi_irq);
+	enable_irq(hdmirx_dev->dma_irq);
+	enable_irq(hdmirx_dev->det_irq);
+
+	queue_delayed_work(system_unbound_wq,
+			   &hdmirx_dev->delayed_work_hotplug,
+			   msecs_to_jiffies(110));
+}
+
+static __maybe_unused int hdmirx_suspend(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+
+	hdmirx_disable_irq(dev);
+
+	/* TODO store CEC HW state */
+	disable_irq(hdmirx_dev->cec->irq);
+
+	return hdmirx_disable(dev);
+}
+
+static __maybe_unused int hdmirx_resume(struct device *dev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+	int ret = hdmirx_enable(dev);
+
+	if (ret)
+		return ret;
+
+	if (hdmirx_dev->edid_blocks_written) {
+		hdmirx_write_edid_data(hdmirx_dev, hdmirx_dev->edid,
+				       hdmirx_dev->edid_blocks_written);
+		hdmirx_hpd_ctrl(hdmirx_dev, true);
+	}
+
+	/* TODO restore CEC HW state */
+	enable_irq(hdmirx_dev->cec->irq);
+
+	hdmirx_enable_irq(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops snps_hdmirx_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(hdmirx_suspend, hdmirx_resume)
+};
+
+static int hdmirx_setup_irq(struct snps_hdmirx_dev *hdmirx_dev,
+			    struct platform_device *pdev)
+{
+	struct device *dev = hdmirx_dev->dev;
+	int ret, irq;
+
+	irq = platform_get_irq_byname(pdev, "hdmi");
+	if (irq < 0) {
+		dev_err_probe(dev, irq, "failed to get hdmi irq\n");
+		return irq;
+	}
+
+	irq_set_status_flags(irq, IRQ_NOAUTOEN);
+
+	hdmirx_dev->hdmi_irq = irq;
+	ret = devm_request_irq(dev, irq, hdmirx_hdmi_irq_handler, 0,
+			       "rk_hdmirx-hdmi", hdmirx_dev);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to request hdmi irq\n");
+		return ret;
+	}
+
+	irq = platform_get_irq_byname(pdev, "dma");
+	if (irq < 0) {
+		dev_err_probe(dev, irq, "failed to get dma irq\n");
+		return irq;
+	}
+
+	irq_set_status_flags(irq, IRQ_NOAUTOEN);
+
+	hdmirx_dev->dma_irq = irq;
+	ret = devm_request_threaded_irq(dev, irq, NULL, hdmirx_dma_irq_handler,
+					IRQF_ONESHOT, "rk_hdmirx-dma",
+					hdmirx_dev);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to request dma irq\n");
+		return ret;
+	}
+
+	irq = gpiod_to_irq(hdmirx_dev->detect_5v_gpio);
+	if (irq < 0) {
+		dev_err_probe(dev, irq, "failed to get hdmirx-5v irq\n");
+		return irq;
+	}
+
+	irq_set_status_flags(irq, IRQ_NOAUTOEN);
+
+	hdmirx_dev->det_irq = irq;
+	ret = devm_request_irq(dev, irq, hdmirx_5v_det_irq_handler,
+			       IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING,
+			       "rk_hdmirx-5v", hdmirx_dev);
+	if (ret) {
+		dev_err_probe(dev, ret, "failed to request hdmirx-5v irq\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int hdmirx_register_cec(struct snps_hdmirx_dev *hdmirx_dev,
+			       struct platform_device *pdev)
+{
+	struct device *dev = hdmirx_dev->dev;
+	struct hdmirx_cec_data cec_data;
+	int irq;
+
+	irq = platform_get_irq_byname(pdev, "cec");
+	if (irq < 0) {
+		dev_err_probe(dev, irq, "failed to get cec irq\n");
+		return irq;
+	}
+
+	cec_data.hdmirx = hdmirx_dev;
+	cec_data.dev = hdmirx_dev->dev;
+	cec_data.ops = &hdmirx_cec_ops;
+	cec_data.irq = irq;
+
+	hdmirx_dev->cec = snps_hdmirx_cec_register(&cec_data);
+	if (IS_ERR(hdmirx_dev->cec))
+		return dev_err_probe(dev, PTR_ERR(hdmirx_dev->cec),
+				     "failed to register cec\n");
+
+	return 0;
+}
+
+static int hdmirx_probe(struct platform_device *pdev)
+{
+	struct snps_hdmirx_dev *hdmirx_dev;
+	struct device *dev = &pdev->dev;
+	struct v4l2_ctrl_handler *hdl;
+	struct hdmirx_stream *stream;
+	struct v4l2_device *v4l2_dev;
+	int ret;
+
+	hdmirx_dev = devm_kzalloc(dev, sizeof(*hdmirx_dev), GFP_KERNEL);
+	if (!hdmirx_dev)
+		return -ENOMEM;
+
+	/*
+	 * RK3588 HDMIRX SoC integration doesn't use IOMMU and can
+	 * address only first 32bit of the physical address space.
+	 */
+	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
+	if (ret)
+		return ret;
+
+	hdmirx_dev->dev = dev;
+	dev_set_drvdata(dev, hdmirx_dev);
+
+	ret = hdmirx_parse_dt(hdmirx_dev);
+	if (ret)
+		return ret;
+
+	ret = hdmirx_setup_irq(hdmirx_dev, pdev);
+	if (ret)
+		return ret;
+
+	hdmirx_dev->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(hdmirx_dev->regs))
+		return dev_err_probe(dev, PTR_ERR(hdmirx_dev->regs),
+				     "failed to remap regs resource\n");
+
+	mutex_init(&hdmirx_dev->stream_lock);
+	mutex_init(&hdmirx_dev->work_lock);
+	spin_lock_init(&hdmirx_dev->rst_lock);
+
+	init_completion(&hdmirx_dev->cr_write_done);
+	init_completion(&hdmirx_dev->timer_base_lock);
+	init_completion(&hdmirx_dev->avi_pkt_rcv);
+
+	INIT_DELAYED_WORK(&hdmirx_dev->delayed_work_hotplug,
+			  hdmirx_delayed_work_hotplug);
+	INIT_DELAYED_WORK(&hdmirx_dev->delayed_work_res_change,
+			  hdmirx_delayed_work_res_change);
+
+	hdmirx_dev->cur_fmt_fourcc = V4L2_PIX_FMT_BGR24;
+	hdmirx_dev->timings = cea640x480;
+
+	hdmirx_enable(dev);
+	hdmirx_init(hdmirx_dev);
+
+	v4l2_dev = &hdmirx_dev->v4l2_dev;
+	strscpy(v4l2_dev->name, dev_name(dev), sizeof(v4l2_dev->name));
+
+	hdl = &hdmirx_dev->hdl;
+	v4l2_ctrl_handler_init(hdl, 3);
+
+	hdmirx_dev->detect_tx_5v_ctrl = v4l2_ctrl_new_std(hdl, NULL,
+							  V4L2_CID_DV_RX_POWER_PRESENT,
+							  0, 1, 0, 0);
+
+	hdmirx_dev->rgb_range = v4l2_ctrl_new_std_menu(hdl, NULL,
+						       V4L2_CID_DV_RX_RGB_RANGE,
+						       V4L2_DV_RGB_RANGE_FULL, 0,
+						       V4L2_DV_RGB_RANGE_AUTO);
+
+	hdmirx_dev->rgb_range->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+
+	hdmirx_dev->content_type =
+		v4l2_ctrl_new_std_menu(hdl, NULL, V4L2_CID_DV_RX_IT_CONTENT_TYPE,
+				       V4L2_DV_IT_CONTENT_TYPE_NO_ITC, 0,
+				       V4L2_DV_IT_CONTENT_TYPE_NO_ITC);
+
+	if (hdl->error) {
+		ret = hdl->error;
+		dev_err_probe(dev, ret, "v4l2 ctrl handler init failed\n");
+		goto err_pm;
+	}
+	hdmirx_dev->v4l2_dev.ctrl_handler = hdl;
+
+	ret = v4l2_device_register(dev, &hdmirx_dev->v4l2_dev);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "v4l2 device registration failed\n");
+		goto err_hdl;
+	}
+
+	stream = &hdmirx_dev->stream;
+	stream->hdmirx_dev = hdmirx_dev;
+	ret = hdmirx_register_stream_vdev(stream);
+	if (ret < 0) {
+		dev_err_probe(dev, ret, "video device registration failed\n");
+		goto err_unreg_v4l2_dev;
+	}
+
+	ret = hdmirx_register_cec(hdmirx_dev, pdev);
+	if (ret)
+		goto err_unreg_video_dev;
+
+	hdmirx_load_default_edid(hdmirx_dev);
+
+	hdmirx_enable_irq(dev);
+
+	hdmirx_dev->debugfs_dir = debugfs_create_dir(hdmirx_dev->v4l2_dev.name,
+						     v4l2_debugfs_root());
+
+	hdmirx_dev->infoframes = v4l2_debugfs_if_alloc(hdmirx_dev->debugfs_dir,
+						       V4L2_DEBUGFS_IF_AVI, hdmirx_dev,
+						       hdmirx_debugfs_if_read);
+
+	return 0;
+
+err_unreg_video_dev:
+	vb2_video_unregister_device(&hdmirx_dev->stream.vdev);
+err_unreg_v4l2_dev:
+	v4l2_device_unregister(&hdmirx_dev->v4l2_dev);
+err_hdl:
+	v4l2_ctrl_handler_free(&hdmirx_dev->hdl);
+err_pm:
+	hdmirx_disable(dev);
+
+	return ret;
+}
+
+static void hdmirx_remove(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct snps_hdmirx_dev *hdmirx_dev = dev_get_drvdata(dev);
+
+	v4l2_debugfs_if_free(hdmirx_dev->infoframes);
+	debugfs_remove_recursive(hdmirx_dev->debugfs_dir);
+
+	snps_hdmirx_cec_unregister(hdmirx_dev->cec);
+
+	hdmirx_disable_irq(dev);
+
+	vb2_video_unregister_device(&hdmirx_dev->stream.vdev);
+	v4l2_ctrl_handler_free(&hdmirx_dev->hdl);
+	v4l2_device_unregister(&hdmirx_dev->v4l2_dev);
+
+	/* touched by hdmirx_disable()->hdmirx_plugout() */
+	hdmirx_dev->rgb_range = NULL;
+	hdmirx_dev->content_type = NULL;
+
+	hdmirx_disable(dev);
+
+	reset_control_bulk_assert(HDMIRX_NUM_RST, hdmirx_dev->resets);
+}
+
+static const struct of_device_id hdmirx_id[] = {
+	{ .compatible = "rockchip,rk3588-hdmirx-ctrler" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, hdmirx_id);
+
+static struct platform_driver hdmirx_driver = {
+	.probe = hdmirx_probe,
+	.remove = hdmirx_remove,
+	.driver = {
+		.name = "snps_hdmirx",
+		.of_match_table = hdmirx_id,
+		.pm = &snps_hdmirx_pm_ops,
+	}
+};
+module_platform_driver(hdmirx_driver);
+
+MODULE_DESCRIPTION("Synopsys HDMI Receiver Driver");
+MODULE_AUTHOR("Dingxian Wen <shawn.wen@rock-chips.com>");
+MODULE_AUTHOR("Shreeya Patel <shreeya.patel@collabora.com>");
+MODULE_AUTHOR("Dmitry Osipenko <dmitry.osipenko@collabora.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.h b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.h
new file mode 100644
index 000000000000..220ab99ca611
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx.h
@@ -0,0 +1,394 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
+ *
+ * Author: Dingxian Wen <shawn.wen@rock-chips.com>
+ */
+
+#ifndef DW_HDMIRX_H
+#define DW_HDMIRX_H
+
+#include <linux/bitops.h>
+
+#define UPDATE(x, h, l)		(((x) << (l)) & GENMASK((h), (l)))
+#define HIWORD_UPDATE(v, h, l)	(((v) << (l)) | (GENMASK((h), (l)) << 16))
+
+/* SYS_GRF */
+#define SYS_GRF_SOC_CON1			0x0304
+#define HDMIRXPHY_SRAM_EXT_LD_DONE		BIT(1)
+#define HDMIRXPHY_SRAM_BYPASS			BIT(0)
+#define SYS_GRF_SOC_STATUS1			0x0384
+#define HDMIRXPHY_SRAM_INIT_DONE		BIT(10)
+#define SYS_GRF_CHIP_ID				0x0600
+
+/* VO1_GRF */
+#define VO1_GRF_VO1_CON2			0x0008
+#define HDMIRX_SDAIN_MSK			BIT(2)
+#define HDMIRX_SCLIN_MSK			BIT(1)
+
+/* HDMIRX PHY */
+#define SUP_DIG_ANA_CREGS_SUP_ANA_NC			0x004f
+
+#define	LANE0_DIG_ASIC_RX_OVRD_OUT_0			0x100f
+#define	LANE1_DIG_ASIC_RX_OVRD_OUT_0			0x110f
+#define	LANE2_DIG_ASIC_RX_OVRD_OUT_0			0x120f
+#define	LANE3_DIG_ASIC_RX_OVRD_OUT_0			0x130f
+#define ASIC_ACK_OVRD_EN				BIT(1)
+#define ASIC_ACK					BIT(0)
+
+#define	LANE0_DIG_RX_VCOCAL_RX_VCO_CAL_CTRL_2		0x104a
+#define	LANE1_DIG_RX_VCOCAL_RX_VCO_CAL_CTRL_2		0x114a
+#define	LANE2_DIG_RX_VCOCAL_RX_VCO_CAL_CTRL_2		0x124a
+#define	LANE3_DIG_RX_VCOCAL_RX_VCO_CAL_CTRL_2		0x134a
+#define FREQ_TUNE_START_VAL_MASK			GENMASK(9, 0)
+#define FREQ_TUNE_START_VAL(x)				UPDATE(x, 9, 0)
+
+#define	HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_FSM_CONFIG	0x20c4
+#define	HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_ADAPT_REF_FOM	0x20c7
+#define HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_3_REG	0x20e9
+#define CDR_SETTING_BOUNDARY_3_DEFAULT			0x52da
+#define HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_4_REG	0x20ea
+#define CDR_SETTING_BOUNDARY_4_DEFAULT			0x43cd
+#define HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_5_REG	0x20eb
+#define CDR_SETTING_BOUNDARY_5_DEFAULT			0x35b3
+#define HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_6_REG	0x20fb
+#define	CDR_SETTING_BOUNDARY_6_DEFAULT			0x2799
+#define HDMIPCS_DIG_CTRL_PATH_MAIN_FSM_RATE_CALC_HDMI14_CDR_SETTING_7_REG	0x20fc
+#define CDR_SETTING_BOUNDARY_7_DEFAULT			0x1b65
+
+#define	RAWLANE0_DIG_PCS_XF_RX_OVRD_OUT			0x300e
+#define	RAWLANE1_DIG_PCS_XF_RX_OVRD_OUT			0x310e
+#define	RAWLANE2_DIG_PCS_XF_RX_OVRD_OUT			0x320e
+#define	RAWLANE3_DIG_PCS_XF_RX_OVRD_OUT			0x330e
+#define PCS_ACK_WRITE_SELECT				BIT(14)
+#define PCS_EN_CTL					BIT(1)
+#define PCS_ACK						BIT(0)
+
+#define	RAWLANE0_DIG_AON_FAST_FLAGS			0x305c
+#define	RAWLANE1_DIG_AON_FAST_FLAGS			0x315c
+#define	RAWLANE2_DIG_AON_FAST_FLAGS			0x325c
+#define	RAWLANE3_DIG_AON_FAST_FLAGS			0x335c
+
+/* HDMIRX Ctrler */
+#define GLOBAL_SWRESET_REQUEST			0x0020
+#define DATAPATH_SWRESETREQ			BIT(12)
+#define GLOBAL_SWENABLE				0x0024
+#define PHYCTRL_ENABLE				BIT(21)
+#define CEC_ENABLE				BIT(16)
+#define TMDS_ENABLE				BIT(13)
+#define DATAPATH_ENABLE				BIT(12)
+#define PKTFIFO_ENABLE				BIT(11)
+#define AVPUNIT_ENABLE				BIT(8)
+#define MAIN_ENABLE				BIT(0)
+#define GLOBAL_TIMER_REF_BASE			0x0028
+#define CORE_CONFIG				0x0050
+#define CMU_CONFIG0				0x0060
+#define TMDSQPCLK_STABLE_FREQ_MARGIN_MASK	GENMASK(30, 16)
+#define TMDSQPCLK_STABLE_FREQ_MARGIN(x)		UPDATE(x, 30, 16)
+#define AUDCLK_STABLE_FREQ_MARGIN_MASK		GENMASK(11, 9)
+#define AUDCLK_STABLE_FREQ_MARGIN(x)		UPDATE(x, 11, 9)
+#define CMU_STATUS				0x007c
+#define TMDSQPCLK_LOCKED_ST			BIT(4)
+#define CMU_TMDSQPCLK_FREQ			0x0084
+#define PHY_CONFIG				0x00c0
+#define LDO_AFE_PROG_MASK			GENMASK(24, 23)
+#define LDO_AFE_PROG(x)				UPDATE(x, 24, 23)
+#define LDO_PWRDN				BIT(21)
+#define TMDS_CLOCK_RATIO			BIT(16)
+#define RXDATA_WIDTH				BIT(15)
+#define REFFREQ_SEL_MASK			GENMASK(11, 9)
+#define REFFREQ_SEL(x)				UPDATE(x, 11, 9)
+#define HDMI_DISABLE				BIT(8)
+#define PHY_PDDQ				BIT(1)
+#define PHY_RESET				BIT(0)
+#define PHY_STATUS				0x00c8
+#define HDMI_DISABLE_ACK			BIT(1)
+#define PDDQ_ACK				BIT(0)
+#define PHYCREG_CONFIG0				0x00e0
+#define PHYCREG_CR_PARA_SELECTION_MODE_MASK	GENMASK(1, 0)
+#define PHYCREG_CR_PARA_SELECTION_MODE(x)	UPDATE(x, 1, 0)
+#define PHYCREG_CONFIG1				0x00e4
+#define PHYCREG_CONFIG2				0x00e8
+#define PHYCREG_CONFIG3				0x00ec
+#define PHYCREG_CONTROL				0x00f0
+#define PHYCREG_CR_PARA_WRITE_P			BIT(1)
+#define PHYCREG_CR_PARA_READ_P			BIT(0)
+#define PHYCREG_STATUS				0x00f4
+
+#define MAINUNIT_STATUS				0x0150
+#define TMDSVALID_STABLE_ST			BIT(1)
+#define DESCRAND_EN_CONTROL			0x0210
+#define SCRAMB_EN_SEL_QST_MASK			GENMASK(1, 0)
+#define SCRAMB_EN_SEL_QST(x)			UPDATE(x, 1, 0)
+#define DESCRAND_SYNC_CONTROL			0x0214
+#define RECOVER_UNSYNC_STREAM_QST		BIT(0)
+#define DESCRAND_SYNC_SEQ_CONFIG		0x022c
+#define DESCRAND_SYNC_SEQ_ERR_CNT_EN		BIT(0)
+#define DESCRAND_SYNC_SEQ_STATUS		0x0234
+#define DEFRAMER_CONFIG0			0x0270
+#define VS_CNT_THR_QST_MASK			GENMASK(27, 20)
+#define VS_CNT_THR_QST(x)			UPDATE(x, 27, 20)
+#define HS_POL_QST_MASK				GENMASK(19, 18)
+#define HS_POL_QST(x)				UPDATE(x, 19, 18)
+#define VS_POL_QST_MASK				GENMASK(17, 16)
+#define VS_POL_QST(x)				UPDATE(x, 17, 16)
+#define VS_REMAPFILTER_EN_QST			BIT(8)
+#define VS_FILTER_ORDER_QST_MASK		GENMASK(1, 0)
+#define VS_FILTER_ORDER_QST(x)			UPDATE(x, 1, 0)
+#define DEFRAMER_VSYNC_CNT_CLEAR		0x0278
+#define VSYNC_CNT_CLR_P				BIT(0)
+#define DEFRAMER_STATUS				0x027c
+#define OPMODE_STS_MASK				GENMASK(6, 4)
+#define I2C_SLAVE_CONFIG1			0x0164
+#define I2C_SDA_OUT_HOLD_VALUE_QST_MASK		GENMASK(15, 8)
+#define I2C_SDA_OUT_HOLD_VALUE_QST(x)		UPDATE(x, 15, 8)
+#define I2C_SDA_IN_HOLD_VALUE_QST_MASK		GENMASK(7, 0)
+#define I2C_SDA_IN_HOLD_VALUE_QST(x)		UPDATE(x, 7, 0)
+#define OPMODE_STS_MASK				GENMASK(6, 4)
+#define REPEATER_QST				BIT(28)
+#define FASTREAUTH_QST				BIT(27)
+#define FEATURES_1DOT1_QST			BIT(26)
+#define FASTI2C_QST				BIT(25)
+#define EESS_CTL_THR_QST_MASK			GENMASK(19, 16)
+#define EESS_CTL_THR_QST(x)			UPDATE(x, 19, 16)
+#define OESS_CTL3_THR_QST_MASK			GENMASK(11, 8)
+#define OESS_CTL3_THR_QST(x)			UPDATE(x, 11, 8)
+#define EESS_OESS_SEL_QST_MASK			GENMASK(5, 4)
+#define EESS_OESS_SEL_QST(x)			UPDATE(x, 5, 4)
+#define KEY_DECRYPT_EN_QST			BIT(0)
+#define KEY_DECRYPT_SEED_QST_MASK		GENMASK(15, 0)
+#define KEY_DECRYPT_SEED_QST(x)			UPDATE(x, 15, 0)
+#define HDCP_INT_CLEAR				0x50d8
+#define HDCP_1_INT_CLEAR			0x50e8
+#define HDCP2_CONFIG				0x02f0
+#define HDCP2_SWITCH_OVR_VALUE			BIT(2)
+#define HDCP2_SWITCH_OVR_EN			BIT(1)
+
+#define VIDEO_CONFIG2				0x042c
+#define VPROC_VSYNC_POL_OVR_VALUE		BIT(19)
+#define VPROC_VSYNC_POL_OVR_EN			BIT(18)
+#define VPROC_HSYNC_POL_OVR_VALUE		BIT(17)
+#define VPROC_HSYNC_POL_OVR_EN			BIT(16)
+#define VPROC_FMT_OVR_VALUE_MASK		GENMASK(6, 4)
+#define VPROC_FMT_OVR_VALUE(x)			UPDATE(x, 6, 4)
+#define VPROC_FMT_OVR_EN			BIT(0)
+
+#define AFIFO_FILL_RESTART			BIT(0)
+#define AFIFO_INIT_P				BIT(0)
+#define AFIFO_THR_LOW_QST_MASK			GENMASK(25, 16)
+#define AFIFO_THR_LOW_QST(x)			UPDATE(x, 25, 16)
+#define AFIFO_THR_HIGH_QST_MASK			GENMASK(9, 0)
+#define AFIFO_THR_HIGH_QST(x)			UPDATE(x, 9, 0)
+#define AFIFO_THR_MUTE_LOW_QST_MASK		GENMASK(25, 16)
+#define AFIFO_THR_MUTE_LOW_QST(x)		UPDATE(x, 25, 16)
+#define AFIFO_THR_MUTE_HIGH_QST_MASK		GENMASK(9, 0)
+#define AFIFO_THR_MUTE_HIGH_QST(x)		UPDATE(x, 9, 0)
+
+#define AFIFO_UNDERFLOW_ST			BIT(25)
+#define AFIFO_OVERFLOW_ST			BIT(24)
+
+#define SPEAKER_ALLOC_OVR_EN			BIT(16)
+#define I2S_BPCUV_EN				BIT(4)
+#define SPDIF_EN				BIT(2)
+#define I2S_EN					BIT(1)
+#define AFIFO_THR_PASS_DEMUTEMASK_N		BIT(24)
+#define AVMUTE_DEMUTEMASK_N			BIT(16)
+#define AFIFO_THR_MUTE_LOW_MUTEMASK_N		BIT(9)
+#define AFIFO_THR_MUTE_HIGH_MUTEMASK_N		BIT(8)
+#define AVMUTE_MUTEMASK_N			BIT(0)
+#define SCDC_CONFIG				0x0580
+#define HPDLOW					BIT(1)
+#define POWERPROVIDED				BIT(0)
+#define SCDC_REGBANK_STATUS1			0x058c
+#define SCDC_TMDSBITCLKRATIO			BIT(1)
+#define SCDC_REGBANK_STATUS3			0x0594
+#define SCDC_REGBANK_CONFIG0			0x05c0
+#define SCDC_SINKVERSION_QST_MASK		GENMASK(7, 0)
+#define SCDC_SINKVERSION_QST(x)			UPDATE(x, 7, 0)
+#define AGEN_LAYOUT				BIT(4)
+#define AGEN_SPEAKER_ALLOC			GENMASK(15, 8)
+
+#define CED_CONFIG				0x0760
+#define CED_VIDDATACHECKEN_QST			BIT(27)
+#define CED_DATAISCHECKEN_QST			BIT(26)
+#define CED_GBCHECKEN_QST			BIT(25)
+#define CED_CTRLCHECKEN_QST			BIT(24)
+#define CED_CHLOCKMAXER_QST_MASK		GENMASK(14, 0)
+#define CED_CHLOCKMAXER_QST(x)			UPDATE(x, 14, 0)
+#define CED_DYN_CONFIG				0x0768
+#define CED_DYN_CONTROL				0x076c
+#define PKTEX_BCH_ERRFILT_CONFIG		0x07c4
+#define PKTEX_CHKSUM_ERRFILT_CONFIG		0x07c8
+
+#define PKTDEC_ACR_PH2_1			0x1100
+#define PKTDEC_ACR_PB3_0			0x1104
+#define PKTDEC_ACR_PB7_4			0x1108
+#define PKTDEC_AVIIF_PH2_1			0x1200
+#define PKTDEC_AVIIF_PB3_0			0x1204
+#define PKTDEC_AVIIF_PB7_4			0x1208
+#define VIC_VAL_MASK				GENMASK(6, 0)
+#define PKTDEC_AVIIF_PB11_8			0x120c
+#define PKTDEC_AVIIF_PB15_12			0x1210
+#define PKTDEC_AVIIF_PB19_16			0x1214
+#define PKTDEC_AVIIF_PB23_20			0x1218
+#define PKTDEC_AVIIF_PB27_24			0x121c
+
+#define PKTFIFO_CONFIG				0x1500
+#define PKTFIFO_STORE_FILT_CONFIG		0x1504
+#define PKTFIFO_THR_CONFIG0			0x1508
+#define PKTFIFO_THR_CONFIG1			0x150c
+#define PKTFIFO_CONTROL				0x1510
+
+#define VMON_STATUS1				0x1580
+#define VMON_STATUS2				0x1584
+#define VMON_STATUS3				0x1588
+#define VMON_STATUS4				0x158c
+#define VMON_STATUS5				0x1590
+#define VMON_STATUS6				0x1594
+#define VMON_STATUS7				0x1598
+#define VMON_ILACE_DETECT			BIT(4)
+
+#define CEC_TX_CONTROL				0x2000
+#define CEC_STATUS				0x2004
+#define CEC_CONFIG				0x2008
+#define RX_AUTO_DRIVE_ACKNOWLEDGE		BIT(9)
+#define CEC_ADDR				0x200c
+#define CEC_TX_COUNT				0x2020
+#define CEC_TX_DATA3_0				0x2024
+#define CEC_RX_COUNT_STATUS			0x2040
+#define CEC_RX_DATA3_0				0x2044
+#define CEC_LOCK_CONTROL			0x2054
+#define CEC_RXQUAL_BITTIME_CONFIG		0x2060
+#define CEC_RX_BITTIME_CONFIG			0x2064
+#define CEC_TX_BITTIME_CONFIG			0x2068
+
+#define DMA_CONFIG1				0x4400
+#define UV_WID_MASK				GENMASK(31, 28)
+#define UV_WID(x)				UPDATE(x, 31, 28)
+#define Y_WID_MASK				GENMASK(27, 24)
+#define Y_WID(x)				UPDATE(x, 27, 24)
+#define DDR_STORE_FORMAT_MASK			GENMASK(15, 12)
+#define DDR_STORE_FORMAT(x)			UPDATE(x, 15, 12)
+#define ABANDON_EN				BIT(0)
+#define DMA_CONFIG2				0x4404
+#define DMA_CONFIG3				0x4408
+#define DMA_CONFIG4				0x440c // dma irq en
+#define DMA_CONFIG5				0x4410 // dma irq clear status
+#define LINE_FLAG_INT_EN			BIT(8)
+#define HDMIRX_DMA_IDLE_INT			BIT(7)
+#define HDMIRX_LOCK_DISABLE_INT			BIT(6)
+#define LAST_FRAME_AXI_UNFINISH_INT_EN		BIT(5)
+#define FIFO_OVERFLOW_INT_EN			BIT(2)
+#define FIFO_UNDERFLOW_INT_EN			BIT(1)
+#define HDMIRX_AXI_ERROR_INT_EN			BIT(0)
+#define DMA_CONFIG6				0x4414
+#define RB_SWAP_EN				BIT(9)
+#define HSYNC_TOGGLE_EN				BIT(5)
+#define VSYNC_TOGGLE_EN				BIT(4)
+#define HDMIRX_DMA_EN				BIT(1)
+#define DMA_CONFIG7				0x4418
+#define LINE_FLAG_NUM_MASK			GENMASK(31, 16)
+#define LINE_FLAG_NUM(x)			UPDATE(x, 31, 16)
+#define LOCK_FRAME_NUM_MASK			GENMASK(11, 0)
+#define LOCK_FRAME_NUM(x)			UPDATE(x, 11, 0)
+#define DMA_CONFIG8				0x441c
+#define REG_MIRROR_EN				BIT(0)
+#define DMA_CONFIG9				0x4420
+#define DMA_CONFIG10				0x4424
+#define DMA_CONFIG11				0x4428
+#define EDID_READ_EN_MASK			BIT(8)
+#define EDID_READ_EN(x)				UPDATE(x, 8, 8)
+#define EDID_WRITE_EN_MASK			BIT(7)
+#define EDID_WRITE_EN(x)			UPDATE(x, 7, 7)
+#define EDID_SLAVE_ADDR_MASK			GENMASK(6, 0)
+#define EDID_SLAVE_ADDR(x)			UPDATE(x, 6, 0)
+#define DMA_STATUS1				0x4430 // dma irq status
+#define DMA_STATUS2				0x4434
+#define DMA_STATUS3				0x4438
+#define DMA_STATUS4				0x443c
+#define DMA_STATUS5				0x4440
+#define DMA_STATUS6				0x4444
+#define DMA_STATUS7				0x4448
+#define DMA_STATUS8				0x444c
+#define DMA_STATUS9				0x4450
+#define DMA_STATUS10				0x4454
+#define HDMIRX_LOCK				BIT(3)
+#define DMA_STATUS11				0x4458
+#define HDMIRX_TYPE_MASK			GENMASK(8, 7)
+#define HDMIRX_COLOR_DEPTH_MASK			GENMASK(6, 3)
+#define HDMIRX_FORMAT_MASK			GENMASK(2, 0)
+#define DMA_STATUS12				0x445c
+#define DMA_STATUS13				0x4460
+#define DMA_STATUS14				0x4464
+
+#define MAINUNIT_INTVEC_INDEX			0x5000
+#define MAINUNIT_0_INT_STATUS			0x5010
+#define CECRX_NOTIFY_ERR			BIT(12)
+#define CECRX_EOM				BIT(11)
+#define CECTX_DRIVE_ERR				BIT(10)
+#define CECRX_BUSY				BIT(9)
+#define CECTX_BUSY				BIT(8)
+#define CECTX_FRAME_DISCARDED			BIT(5)
+#define CECTX_NRETRANSMIT_FAIL			BIT(4)
+#define CECTX_LINE_ERR				BIT(3)
+#define CECTX_ARBLOST				BIT(2)
+#define CECTX_NACK				BIT(1)
+#define CECTX_DONE				BIT(0)
+#define MAINUNIT_0_INT_MASK_N			0x5014
+#define MAINUNIT_0_INT_CLEAR			0x5018
+#define MAINUNIT_0_INT_FORCE			0x501c
+#define TIMER_BASE_LOCKED_IRQ			BIT(26)
+#define TMDSQPCLK_OFF_CHG			BIT(5)
+#define TMDSQPCLK_LOCKED_CHG			BIT(4)
+#define MAINUNIT_1_INT_STATUS			0x5020
+#define MAINUNIT_1_INT_MASK_N			0x5024
+#define MAINUNIT_1_INT_CLEAR			0x5028
+#define MAINUNIT_1_INT_FORCE			0x502c
+#define MAINUNIT_2_INT_STATUS			0x5030
+#define MAINUNIT_2_INT_MASK_N			0x5034
+#define MAINUNIT_2_INT_CLEAR			0x5038
+#define MAINUNIT_2_INT_FORCE			0x503c
+#define PHYCREG_CR_READ_DONE			BIT(11)
+#define PHYCREG_CR_WRITE_DONE			BIT(10)
+#define TMDSVALID_STABLE_CHG			BIT(1)
+
+#define AVPUNIT_0_INT_STATUS			0x5040
+#define AVPUNIT_0_INT_MASK_N			0x5044
+#define AVPUNIT_0_INT_CLEAR			0x5048
+#define AVPUNIT_0_INT_FORCE			0x504c
+#define CED_DYN_CNT_CH2_IRQ			BIT(22)
+#define CED_DYN_CNT_CH1_IRQ			BIT(21)
+#define CED_DYN_CNT_CH0_IRQ			BIT(20)
+#define AVPUNIT_1_INT_STATUS			0x5050
+#define DEFRAMER_VSYNC_THR_REACHED_IRQ		BIT(1)
+#define AVPUNIT_1_INT_MASK_N			0x5054
+#define DEFRAMER_VSYNC_THR_REACHED_MASK_N	BIT(1)
+#define DEFRAMER_VSYNC_MASK_N			BIT(0)
+#define AVPUNIT_1_INT_CLEAR			0x5058
+#define DEFRAMER_VSYNC_THR_REACHED_CLEAR	BIT(1)
+#define PKT_0_INT_STATUS			0x5080
+#define PKTDEC_ACR_CHG_IRQ			BIT(3)
+#define PKT_0_INT_MASK_N			0x5084
+#define PKTDEC_ACR_CHG_MASK_N			BIT(3)
+#define PKT_0_INT_CLEAR				0x5088
+#define PKT_1_INT_STATUS			0x5090
+#define PKT_1_INT_MASK_N			0x5094
+#define PKT_1_INT_CLEAR				0x5098
+#define PKT_2_INT_STATUS			0x50a0
+#define PKTDEC_ACR_RCV_IRQ			BIT(3)
+#define PKT_2_INT_MASK_N			0x50a4
+#define PKTDEC_AVIIF_RCV_IRQ			BIT(11)
+#define PKTDEC_ACR_RCV_MASK_N			BIT(3)
+#define PKT_2_INT_CLEAR				0x50a8
+#define PKTDEC_AVIIF_RCV_CLEAR			BIT(11)
+#define PKTDEC_ACR_RCV_CLEAR			BIT(3)
+#define SCDC_INT_STATUS				0x50c0
+#define SCDC_INT_MASK_N				0x50c4
+#define SCDC_INT_CLEAR				0x50c8
+#define SCDCTMDSCCFG_CHG			BIT(2)
+
+#define CEC_INT_STATUS				0x5100
+#define CEC_INT_MASK_N				0x5104
+#define CEC_INT_CLEAR				0x5108
+
+#endif
diff --git a/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.c b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.c
new file mode 100644
index 000000000000..8e470c0376d6
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.c
@@ -0,0 +1,275 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
+ *
+ * Author: Shunqing Chen <csq@rock-chips.com>
+ */
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <media/cec.h>
+
+#include "snps_hdmirx.h"
+#include "snps_hdmirx_cec.h"
+
+static void hdmirx_cec_write(struct hdmirx_cec *cec, int reg, u32 val)
+{
+	cec->ops->write(cec->hdmirx, reg, val);
+}
+
+static u32 hdmirx_cec_read(struct hdmirx_cec *cec, int reg)
+{
+	return cec->ops->read(cec->hdmirx, reg);
+}
+
+static void hdmirx_cec_update_bits(struct hdmirx_cec *cec, int reg, u32 mask,
+				   u32 data)
+{
+	u32 val = hdmirx_cec_read(cec, reg) & ~mask;
+
+	val |= (data & mask);
+	hdmirx_cec_write(cec, reg, val);
+}
+
+static int hdmirx_cec_log_addr(struct cec_adapter *adap, u8 logical_addr)
+{
+	struct hdmirx_cec *cec = cec_get_drvdata(adap);
+
+	if (logical_addr == CEC_LOG_ADDR_INVALID)
+		cec->addresses = 0;
+	else
+		cec->addresses |= BIT(logical_addr) | BIT(15);
+
+	hdmirx_cec_write(cec, CEC_ADDR, cec->addresses);
+
+	return 0;
+}
+
+/* signal_free_time is handled by the Synopsys Designware
+ * HDMIRX Controller hardware.
+ */
+static int hdmirx_cec_transmit(struct cec_adapter *adap, u8 attempts,
+			       u32 signal_free_time, struct cec_msg *msg)
+{
+	struct hdmirx_cec *cec = cec_get_drvdata(adap);
+	u32 data[4] = {0};
+	int i, data_len, msg_len;
+
+	msg_len = msg->len;
+
+	hdmirx_cec_write(cec, CEC_TX_COUNT, msg_len - 1);
+	for (i = 0; i < msg_len; i++)
+		data[i / 4] |= msg->msg[i] << (i % 4) * 8;
+
+	data_len = DIV_ROUND_UP(msg_len, 4);
+
+	for (i = 0; i < data_len; i++)
+		hdmirx_cec_write(cec, CEC_TX_DATA3_0 + i * 4, data[i]);
+
+	hdmirx_cec_write(cec, CEC_TX_CONTROL, 0x1);
+
+	return 0;
+}
+
+static irqreturn_t hdmirx_cec_hardirq(int irq, void *data)
+{
+	struct cec_adapter *adap = data;
+	struct hdmirx_cec *cec = cec_get_drvdata(adap);
+	u32 stat = hdmirx_cec_read(cec, CEC_INT_STATUS);
+	irqreturn_t ret = IRQ_HANDLED;
+	u32 val;
+
+	if (!stat)
+		return IRQ_NONE;
+
+	hdmirx_cec_write(cec, CEC_INT_CLEAR, stat);
+
+	if (stat & CECTX_LINE_ERR) {
+		cec->tx_status = CEC_TX_STATUS_ERROR;
+		cec->tx_done = true;
+		ret = IRQ_WAKE_THREAD;
+	} else if (stat & CECTX_DONE) {
+		cec->tx_status = CEC_TX_STATUS_OK;
+		cec->tx_done = true;
+		ret = IRQ_WAKE_THREAD;
+	} else if (stat & CECTX_NACK) {
+		cec->tx_status = CEC_TX_STATUS_NACK;
+		cec->tx_done = true;
+		ret = IRQ_WAKE_THREAD;
+	} else if (stat & CECTX_ARBLOST) {
+		cec->tx_status = CEC_TX_STATUS_ARB_LOST;
+		cec->tx_done = true;
+		ret = IRQ_WAKE_THREAD;
+	}
+
+	if (stat & CECRX_EOM) {
+		unsigned int len, i;
+
+		val = hdmirx_cec_read(cec, CEC_RX_COUNT_STATUS);
+		/* rxbuffer locked status */
+		if ((val & 0x80))
+			return ret;
+
+		len = (val & 0xf) + 1;
+		if (len > sizeof(cec->rx_msg.msg))
+			len = sizeof(cec->rx_msg.msg);
+
+		for (i = 0; i < len; i++) {
+			if (!(i % 4))
+				val = hdmirx_cec_read(cec, CEC_RX_DATA3_0 + i / 4 * 4);
+			cec->rx_msg.msg[i] = (val >> ((i % 4) * 8)) & 0xff;
+		}
+
+		cec->rx_msg.len = len;
+		smp_wmb(); /* receive RX msg */
+		cec->rx_done = true;
+		hdmirx_cec_write(cec, CEC_LOCK_CONTROL, 0x1);
+
+		ret = IRQ_WAKE_THREAD;
+	}
+
+	return ret;
+}
+
+static irqreturn_t hdmirx_cec_thread(int irq, void *data)
+{
+	struct cec_adapter *adap = data;
+	struct hdmirx_cec *cec = cec_get_drvdata(adap);
+
+	if (cec->tx_done) {
+		cec->tx_done = false;
+		cec_transmit_attempt_done(adap, cec->tx_status);
+	}
+	if (cec->rx_done) {
+		cec->rx_done = false;
+		smp_rmb(); /* RX msg has been received */
+		cec_received_msg(adap, &cec->rx_msg);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int hdmirx_cec_enable(struct cec_adapter *adap, bool enable)
+{
+	struct hdmirx_cec *cec = cec_get_drvdata(adap);
+
+	if (!enable) {
+		hdmirx_cec_write(cec, CEC_INT_MASK_N, 0);
+		hdmirx_cec_write(cec, CEC_INT_CLEAR, 0);
+		if (cec->ops->disable)
+			cec->ops->disable(cec->hdmirx);
+	} else {
+		unsigned int irqs;
+
+		hdmirx_cec_log_addr(cec->adap, CEC_LOG_ADDR_INVALID);
+		if (cec->ops->enable)
+			cec->ops->enable(cec->hdmirx);
+		hdmirx_cec_update_bits(cec, GLOBAL_SWENABLE, CEC_ENABLE, CEC_ENABLE);
+
+		irqs = CECTX_LINE_ERR | CECTX_NACK | CECRX_EOM | CECTX_DONE;
+		hdmirx_cec_write(cec, CEC_INT_MASK_N, irqs);
+	}
+
+	return 0;
+}
+
+static const struct cec_adap_ops hdmirx_cec_ops = {
+	.adap_enable = hdmirx_cec_enable,
+	.adap_log_addr = hdmirx_cec_log_addr,
+	.adap_transmit = hdmirx_cec_transmit,
+};
+
+static void hdmirx_cec_del(void *data)
+{
+	struct hdmirx_cec *cec = data;
+
+	cec_delete_adapter(cec->adap);
+}
+
+struct hdmirx_cec *snps_hdmirx_cec_register(struct hdmirx_cec_data *data)
+{
+	struct hdmirx_cec *cec;
+	unsigned int irqs;
+	int ret;
+
+	/*
+	 * Our device is just a convenience - we want to link to the real
+	 * hardware device here, so that userspace can see the association
+	 * between the HDMI hardware and its associated CEC chardev.
+	 */
+	cec = devm_kzalloc(data->dev, sizeof(*cec), GFP_KERNEL);
+	if (!cec)
+		return ERR_PTR(-ENOMEM);
+
+	cec->dev = data->dev;
+	cec->irq = data->irq;
+	cec->ops = data->ops;
+	cec->hdmirx = data->hdmirx;
+
+	hdmirx_cec_update_bits(cec, GLOBAL_SWENABLE, CEC_ENABLE, CEC_ENABLE);
+	hdmirx_cec_update_bits(cec, CEC_CONFIG, RX_AUTO_DRIVE_ACKNOWLEDGE,
+			       RX_AUTO_DRIVE_ACKNOWLEDGE);
+
+	hdmirx_cec_write(cec, CEC_TX_COUNT, 0);
+	hdmirx_cec_write(cec, CEC_INT_MASK_N, 0);
+	hdmirx_cec_write(cec, CEC_INT_CLEAR, ~0);
+
+	cec->adap = cec_allocate_adapter(&hdmirx_cec_ops, cec, "snps-hdmirx",
+					 CEC_CAP_DEFAULTS | CEC_CAP_MONITOR_ALL,
+					 CEC_MAX_LOG_ADDRS);
+	if (IS_ERR(cec->adap)) {
+		dev_err(cec->dev, "cec adapter allocation failed\n");
+		return ERR_CAST(cec->adap);
+	}
+
+	/* override the module pointer */
+	cec->adap->owner = THIS_MODULE;
+
+	ret = devm_add_action(cec->dev, hdmirx_cec_del, cec);
+	if (ret) {
+		cec_delete_adapter(cec->adap);
+		return ERR_PTR(ret);
+	}
+
+	irq_set_status_flags(cec->irq, IRQ_NOAUTOEN);
+
+	ret = devm_request_threaded_irq(cec->dev, cec->irq,
+					hdmirx_cec_hardirq,
+					hdmirx_cec_thread, IRQF_ONESHOT,
+					"rk_hdmirx_cec", cec->adap);
+	if (ret) {
+		dev_err(cec->dev, "cec irq request failed\n");
+		return ERR_PTR(ret);
+	}
+
+	ret = cec_register_adapter(cec->adap, cec->dev);
+	if (ret < 0) {
+		dev_err(cec->dev, "cec adapter registration failed\n");
+		return ERR_PTR(ret);
+	}
+
+	irqs = CECTX_LINE_ERR | CECTX_NACK | CECRX_EOM | CECTX_DONE;
+	hdmirx_cec_write(cec, CEC_INT_MASK_N, irqs);
+
+	/*
+	 * CEC documentation says we must not call cec_delete_adapter
+	 * after a successful call to cec_register_adapter().
+	 */
+	devm_remove_action(cec->dev, hdmirx_cec_del, cec);
+
+	enable_irq(cec->irq);
+
+	return cec;
+}
+
+void snps_hdmirx_cec_unregister(struct hdmirx_cec *cec)
+{
+	disable_irq(cec->irq);
+
+	cec_unregister_adapter(cec->adap);
+}
diff --git a/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.h b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.h
new file mode 100644
index 000000000000..1b10da5b8fd4
--- /dev/null
+++ b/drivers/media/platform/synopsys/hdmirx/snps_hdmirx_cec.h
@@ -0,0 +1,43 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (c) 2021 Rockchip Electronics Co. Ltd.
+ *
+ * Author: Shunqing Chen <csq@rock-chips.com>
+ */
+
+#ifndef DW_HDMI_RX_CEC_H
+#define DW_HDMI_RX_CEC_H
+
+struct snps_hdmirx_dev;
+
+struct hdmirx_cec_ops {
+	void (*write)(struct snps_hdmirx_dev *hdmirx_dev, int reg, u32 val);
+	u32 (*read)(struct snps_hdmirx_dev *hdmirx_dev, int reg);
+	void (*enable)(struct snps_hdmirx_dev *hdmirx);
+	void (*disable)(struct snps_hdmirx_dev *hdmirx);
+};
+
+struct hdmirx_cec_data {
+	struct snps_hdmirx_dev *hdmirx;
+	const struct hdmirx_cec_ops *ops;
+	struct device *dev;
+	int irq;
+};
+
+struct hdmirx_cec {
+	struct snps_hdmirx_dev *hdmirx;
+	struct device *dev;
+	const struct hdmirx_cec_ops *ops;
+	u32 addresses;
+	struct cec_adapter *adap;
+	struct cec_msg rx_msg;
+	unsigned int tx_status;
+	bool tx_done;
+	bool rx_done;
+	int irq;
+};
+
+struct hdmirx_cec *snps_hdmirx_cec_register(struct hdmirx_cec_data *data);
+void snps_hdmirx_cec_unregister(struct hdmirx_cec *cec);
+
+#endif /* DW_HDMI_RX_CEC_H */
diff --git a/drivers/media/platform/ti/am437x/am437x-vpfe.c b/drivers/media/platform/ti/am437x/am437x-vpfe.c
index 77e12457d149..1ca559df7e59 100644
--- a/drivers/media/platform/ti/am437x/am437x-vpfe.c
+++ b/drivers/media/platform/ti/am437x/am437x-vpfe.c
@@ -2030,7 +2030,7 @@ vpfe_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
 	vpfe->fmt.fmt.pix.sizeimage = vpfe->fmt.fmt.pix.bytesperline *
 						vpfe->fmt.fmt.pix.height;
 
-	vpfe_dbg(1, vpfe, "cropped (%d,%d)/%dx%d of %dx%d\n",
+	vpfe_dbg(1, vpfe, "cropped (%d,%d)/%ux%u of %dx%d\n",
 		 r.left, r.top, r.width, r.height, cr.width, cr.height);
 
 	return 0;
@@ -2079,8 +2079,6 @@ static long vpfe_ioctl_default(struct file *file, void *priv,
 }
 
 static const struct vb2_ops vpfe_video_qops = {
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.queue_setup		= vpfe_queue_setup,
 	.buf_prepare		= vpfe_buffer_prepare,
 	.buf_queue		= vpfe_buffer_queue,
@@ -2287,7 +2285,7 @@ static const struct v4l2_async_notifier_operations vpfe_async_ops = {
 static struct vpfe_config *
 vpfe_get_pdata(struct vpfe_device *vpfe)
 {
-	struct device_node *endpoint = NULL;
+	struct device_node *endpoint;
 	struct device *dev = vpfe->pdev;
 	struct vpfe_subdev_info *sdinfo;
 	struct vpfe_config *pdata;
@@ -2306,14 +2304,11 @@ vpfe_get_pdata(struct vpfe_device *vpfe)
 	if (!pdata)
 		return NULL;
 
-	for (i = 0; ; i++) {
+	i = 0;
+	for_each_endpoint_of_node(dev->of_node, endpoint) {
 		struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
 		struct device_node *rem;
 
-		endpoint = of_graph_get_next_endpoint(dev->of_node, endpoint);
-		if (!endpoint)
-			break;
-
 		sdinfo = &pdata->sub_devs[i];
 		sdinfo->grp_id = 0;
 
@@ -2371,9 +2366,10 @@ vpfe_get_pdata(struct vpfe_device *vpfe)
 		of_node_put(rem);
 		if (IS_ERR(pdata->asd[i]))
 			goto cleanup;
+
+		i++;
 	}
 
-	of_node_put(endpoint);
 	return pdata;
 
 cleanup:
@@ -2619,7 +2615,7 @@ MODULE_DEVICE_TABLE(of, vpfe_of_match);
 
 static struct platform_driver vpfe_driver = {
 	.probe		= vpfe_probe,
-	.remove_new	= vpfe_remove,
+	.remove		= vpfe_remove,
 	.driver = {
 		.name	= VPFE_MODULE_NAME,
 		.pm	= &vpfe_pm_ops,
diff --git a/drivers/media/platform/ti/cal/cal-camerarx.c b/drivers/media/platform/ti/cal/cal-camerarx.c
index 4afc2ad00330..00a71dac0ff4 100644
--- a/drivers/media/platform/ti/cal/cal-camerarx.c
+++ b/drivers/media/platform/ti/cal/cal-camerarx.c
@@ -49,23 +49,41 @@ static s64 cal_camerarx_get_ext_link_freq(struct cal_camerarx *phy)
 {
 	struct v4l2_mbus_config_mipi_csi2 *mipi_csi2 = &phy->endpoint.bus.mipi_csi2;
 	u32 num_lanes = mipi_csi2->num_data_lanes;
-	const struct cal_format_info *fmtinfo;
 	struct v4l2_subdev_state *state;
-	struct v4l2_mbus_framefmt *fmt;
 	u32 bpp;
 	s64 freq;
 
+	/*
+	 * v4l2_get_link_freq() uses V4L2_CID_LINK_FREQ first, and falls back
+	 * to V4L2_CID_PIXEL_RATE if V4L2_CID_LINK_FREQ is not available.
+	 *
+	 * With multistream input there is no single pixel rate, and thus we
+	 * cannot use V4L2_CID_PIXEL_RATE, so we pass 0 as the bpp which
+	 * causes v4l2_get_link_freq() to return an error if it falls back to
+	 * V4L2_CID_PIXEL_RATE.
+	 */
+
 	state = v4l2_subdev_get_locked_active_state(&phy->subdev);
 
-	fmt = v4l2_subdev_state_get_format(state, CAL_CAMERARX_PAD_SINK);
+	if (state->routing.num_routes > 1) {
+		bpp = 0;
+	} else {
+		struct v4l2_subdev_route *route = &state->routing.routes[0];
+		const struct cal_format_info *fmtinfo;
+		struct v4l2_mbus_framefmt *fmt;
 
-	fmtinfo = cal_format_by_code(fmt->code);
-	if (!fmtinfo)
-		return -EINVAL;
+		fmt = v4l2_subdev_state_get_format(state, route->sink_pad,
+						   route->sink_stream);
 
-	bpp = fmtinfo->bpp;
+		fmtinfo = cal_format_by_code(fmt->code);
+		if (!fmtinfo)
+			return -EINVAL;
 
-	freq = v4l2_get_link_freq(phy->source->ctrl_handler, bpp, 2 * num_lanes);
+		bpp = fmtinfo->bpp;
+	}
+
+	freq = v4l2_get_link_freq(&phy->source->entity.pads[phy->source_pad],
+				  bpp, 2 * num_lanes);
 	if (freq < 0) {
 		phy_err(phy, "failed to get link freq for subdev '%s'\n",
 			phy->source->name);
@@ -284,15 +302,32 @@ static void cal_camerarx_ppi_disable(struct cal_camerarx *phy)
 			0, CAL_CSI2_PPI_CTRL_IF_EN_MASK);
 }
 
-static int cal_camerarx_start(struct cal_camerarx *phy)
+static int cal_camerarx_start(struct cal_camerarx *phy, u32 sink_stream)
 {
+	struct media_pad *remote_pad;
 	s64 link_freq;
 	u32 sscounter;
 	u32 val;
 	int ret;
 
+	remote_pad = media_pad_remote_pad_first(&phy->pads[CAL_CAMERARX_PAD_SINK]);
+
+	/*
+	 * We need to enable the PHY hardware when enabling the first stream,
+	 * but for the following streams we just propagate the enable_streams
+	 * to the source.
+	 */
+
 	if (phy->enable_count > 0) {
+		ret = v4l2_subdev_enable_streams(phy->source, remote_pad->index,
+						 BIT(sink_stream));
+		if (ret) {
+			phy_err(phy, "enable streams failed in source: %d\n", ret);
+			return ret;
+		}
+
 		phy->enable_count++;
+
 		return 0;
 	}
 
@@ -394,7 +429,8 @@ static int cal_camerarx_start(struct cal_camerarx *phy)
 	 * Start the source to enable the CSI-2 HS clock. We can now wait for
 	 * CSI-2 PHY reset to complete.
 	 */
-	ret = v4l2_subdev_call(phy->source, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(phy->source, remote_pad->index,
+					 BIT(sink_stream));
 	if (ret) {
 		v4l2_subdev_call(phy->source, core, s_power, 0);
 		cal_camerarx_disable_irqs(phy);
@@ -425,12 +461,22 @@ static int cal_camerarx_start(struct cal_camerarx *phy)
 	return 0;
 }
 
-static void cal_camerarx_stop(struct cal_camerarx *phy)
+static void cal_camerarx_stop(struct cal_camerarx *phy, u32 sink_stream)
 {
+	struct media_pad *remote_pad;
 	int ret;
 
-	if (--phy->enable_count > 0)
+	remote_pad = media_pad_remote_pad_first(&phy->pads[CAL_CAMERARX_PAD_SINK]);
+
+	if (--phy->enable_count > 0) {
+		ret = v4l2_subdev_disable_streams(phy->source,
+						  remote_pad->index,
+						  BIT(sink_stream));
+		if (ret)
+			phy_err(phy, "stream off failed in subdev\n");
+
 		return;
+	}
 
 	cal_camerarx_ppi_disable(phy);
 
@@ -450,7 +496,9 @@ static void cal_camerarx_stop(struct cal_camerarx *phy)
 	/* Disable the phy */
 	cal_camerarx_disable(phy);
 
-	if (v4l2_subdev_call(phy->source, video, s_stream, 0))
+	ret = v4l2_subdev_disable_streams(phy->source, remote_pad->index,
+					  BIT(sink_stream));
+	if (ret)
 		phy_err(phy, "stream off failed in subdev\n");
 
 	ret = v4l2_subdev_call(phy->source, core, s_power, 0);
@@ -599,22 +647,50 @@ static inline struct cal_camerarx *to_cal_camerarx(struct v4l2_subdev *sd)
 	return container_of(sd, struct cal_camerarx, subdev);
 }
 
-static int cal_camerarx_sd_s_stream(struct v4l2_subdev *sd, int enable)
+struct cal_camerarx *
+cal_camerarx_get_phy_from_entity(struct media_entity *entity)
+{
+	struct v4l2_subdev *sd;
+
+	sd = media_entity_to_v4l2_subdev(entity);
+	if (!sd)
+		return NULL;
+
+	return to_cal_camerarx(sd);
+}
+
+static int cal_camerarx_sd_enable_streams(struct v4l2_subdev *sd,
+					  struct v4l2_subdev_state *state,
+					  u32 pad, u64 streams_mask)
 {
 	struct cal_camerarx *phy = to_cal_camerarx(sd);
-	struct v4l2_subdev_state *state;
-	int ret = 0;
+	u32 sink_stream;
+	int ret;
 
-	state = v4l2_subdev_lock_and_get_active_state(sd);
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,
+						    NULL, &sink_stream);
+	if (ret)
+		return ret;
 
-	if (enable)
-		ret = cal_camerarx_start(phy);
-	else
-		cal_camerarx_stop(phy);
+	return cal_camerarx_start(phy, sink_stream);
+}
 
-	v4l2_subdev_unlock_state(state);
+static int cal_camerarx_sd_disable_streams(struct v4l2_subdev *sd,
+					   struct v4l2_subdev_state *state,
+					   u32 pad, u64 streams_mask)
+{
+	struct cal_camerarx *phy = to_cal_camerarx(sd);
+	u32 sink_stream;
+	int ret;
 
-	return ret;
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing, pad, 0,
+						    NULL, &sink_stream);
+	if (ret)
+		return ret;
+
+	cal_camerarx_stop(phy, sink_stream);
+
+	return 0;
 }
 
 static int cal_camerarx_sd_enum_mbus_code(struct v4l2_subdev *sd,
@@ -628,8 +704,12 @@ static int cal_camerarx_sd_enum_mbus_code(struct v4l2_subdev *sd,
 		if (code->index > 0)
 			return -EINVAL;
 
-		fmt = v4l2_subdev_state_get_format(state,
-						   CAL_CAMERARX_PAD_SINK);
+		fmt = v4l2_subdev_state_get_opposite_stream_format(state,
+								   code->pad,
+								   code->stream);
+		if (!fmt)
+			return -EINVAL;
+
 		code->code = fmt->code;
 	} else {
 		if (code->index >= cal_num_formats)
@@ -654,8 +734,12 @@ static int cal_camerarx_sd_enum_frame_size(struct v4l2_subdev *sd,
 	if (cal_rx_pad_is_source(fse->pad)) {
 		struct v4l2_mbus_framefmt *fmt;
 
-		fmt = v4l2_subdev_state_get_format(state,
-						   CAL_CAMERARX_PAD_SINK);
+		fmt = v4l2_subdev_state_get_opposite_stream_format(state,
+								   fse->pad,
+								   fse->stream);
+		if (!fmt)
+			return -EINVAL;
+
 		if (fse->code != fmt->code)
 			return -EINVAL;
 
@@ -711,36 +795,77 @@ static int cal_camerarx_sd_set_fmt(struct v4l2_subdev *sd,
 
 	/* Store the format and propagate it to the source pad. */
 
-	fmt = v4l2_subdev_state_get_format(state, CAL_CAMERARX_PAD_SINK);
+	fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
+	if (!fmt)
+		return -EINVAL;
+
 	*fmt = format->format;
 
-	fmt = v4l2_subdev_state_get_format(state,
-					   CAL_CAMERARX_PAD_FIRST_SOURCE);
+	fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
+							   format->stream);
+	if (!fmt)
+		return -EINVAL;
+
 	*fmt = format->format;
 
 	return 0;
 }
 
+static int cal_camerarx_set_routing(struct v4l2_subdev *sd,
+				    struct v4l2_subdev_state *state,
+				    struct v4l2_subdev_krouting *routing)
+{
+	static const struct v4l2_mbus_framefmt format = {
+		.width = 640,
+		.height = 480,
+		.code = MEDIA_BUS_FMT_UYVY8_1X16,
+		.field = V4L2_FIELD_NONE,
+		.colorspace = V4L2_COLORSPACE_SRGB,
+		.ycbcr_enc = V4L2_YCBCR_ENC_601,
+		.quantization = V4L2_QUANTIZATION_LIM_RANGE,
+		.xfer_func = V4L2_XFER_FUNC_SRGB,
+	};
+	int ret;
+
+	ret = v4l2_subdev_routing_validate(sd, routing,
+					   V4L2_SUBDEV_ROUTING_ONLY_1_TO_1 |
+					   V4L2_SUBDEV_ROUTING_NO_SOURCE_MULTIPLEXING);
+	if (ret)
+		return ret;
+
+	ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int cal_camerarx_sd_set_routing(struct v4l2_subdev *sd,
+				       struct v4l2_subdev_state *state,
+				       enum v4l2_subdev_format_whence which,
+				       struct v4l2_subdev_krouting *routing)
+{
+	return cal_camerarx_set_routing(sd, state, routing);
+}
+
 static int cal_camerarx_sd_init_state(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_state *state)
 {
-	struct v4l2_subdev_format format = {
-		.which = state ? V4L2_SUBDEV_FORMAT_TRY
-		: V4L2_SUBDEV_FORMAT_ACTIVE,
-		.pad = CAL_CAMERARX_PAD_SINK,
-		.format = {
-			.width = 640,
-			.height = 480,
-			.code = MEDIA_BUS_FMT_UYVY8_1X16,
-			.field = V4L2_FIELD_NONE,
-			.colorspace = V4L2_COLORSPACE_SRGB,
-			.ycbcr_enc = V4L2_YCBCR_ENC_601,
-			.quantization = V4L2_QUANTIZATION_LIM_RANGE,
-			.xfer_func = V4L2_XFER_FUNC_SRGB,
-		},
+	struct v4l2_subdev_route routes[] = { {
+		.sink_pad = 0,
+		.sink_stream = 0,
+		.source_pad = 1,
+		.source_stream = 0,
+		.flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
+	} };
+
+	struct v4l2_subdev_krouting routing = {
+		.num_routes = 1,
+		.routes = routes,
 	};
 
-	return cal_camerarx_sd_set_fmt(sd, state, &format);
+	/* Initialize routing to single route to the fist source pad */
+	return cal_camerarx_set_routing(sd, state, &routing);
 }
 
 static int cal_camerarx_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
@@ -749,48 +874,71 @@ static int cal_camerarx_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 	struct cal_camerarx *phy = to_cal_camerarx(sd);
 	struct v4l2_mbus_frame_desc remote_desc;
 	const struct media_pad *remote_pad;
+	struct v4l2_subdev_state *state;
+	u32 sink_stream;
+	unsigned int i;
 	int ret;
 
+	state = v4l2_subdev_lock_and_get_active_state(sd);
+
+	ret = v4l2_subdev_routing_find_opposite_end(&state->routing,
+						    pad, 0,
+						    NULL, &sink_stream);
+	if (ret)
+		goto out_unlock;
+
 	remote_pad = media_pad_remote_pad_first(&phy->pads[CAL_CAMERARX_PAD_SINK]);
-	if (!remote_pad)
-		return -EPIPE;
+	if (!remote_pad) {
+		ret = -EPIPE;
+		goto out_unlock;
+	}
 
 	ret = v4l2_subdev_call(phy->source, pad, get_frame_desc,
 			       remote_pad->index, &remote_desc);
 	if (ret)
-		return ret;
+		goto out_unlock;
 
 	if (remote_desc.type != V4L2_MBUS_FRAME_DESC_TYPE_CSI2) {
 		cal_err(phy->cal,
 			"Frame descriptor does not describe CSI-2 link");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto out_unlock;
 	}
 
-	if (remote_desc.num_entries > 1)
-		cal_err(phy->cal,
-			"Multiple streams not supported in remote frame descriptor, using the first one\n");
+	for (i = 0; i < remote_desc.num_entries; i++) {
+		if (remote_desc.entry[i].stream == sink_stream)
+			break;
+	}
+
+	if (i == remote_desc.num_entries) {
+		cal_err(phy->cal, "Stream %u not found in remote frame desc\n",
+			sink_stream);
+		ret = -EINVAL;
+		goto out_unlock;
+	}
 
 	fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
 	fd->num_entries = 1;
-	fd->entry[0] = remote_desc.entry[0];
+	fd->entry[0] = remote_desc.entry[i];
 
-	return 0;
-}
+out_unlock:
+	v4l2_subdev_unlock_state(state);
 
-static const struct v4l2_subdev_video_ops cal_camerarx_video_ops = {
-	.s_stream = cal_camerarx_sd_s_stream,
-};
+	return ret;
+}
 
 static const struct v4l2_subdev_pad_ops cal_camerarx_pad_ops = {
+	.enable_streams = cal_camerarx_sd_enable_streams,
+	.disable_streams = cal_camerarx_sd_disable_streams,
 	.enum_mbus_code = cal_camerarx_sd_enum_mbus_code,
 	.enum_frame_size = cal_camerarx_sd_enum_frame_size,
 	.get_fmt = v4l2_subdev_get_fmt,
 	.set_fmt = cal_camerarx_sd_set_fmt,
+	.set_routing = cal_camerarx_sd_set_routing,
 	.get_frame_desc = cal_camerarx_get_frame_desc,
 };
 
 static const struct v4l2_subdev_ops cal_camerarx_subdev_ops = {
-	.video = &cal_camerarx_video_ops,
 	.pad = &cal_camerarx_pad_ops,
 };
 
@@ -798,8 +946,9 @@ static const struct v4l2_subdev_internal_ops cal_camerarx_internal_ops = {
 	.init_state = cal_camerarx_sd_init_state,
 };
 
-static struct media_entity_operations cal_camerarx_media_ops = {
+static const struct media_entity_operations cal_camerarx_media_ops = {
 	.link_validate = v4l2_subdev_link_validate,
+	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
 };
 
 /* ------------------------------------------------------------------
@@ -851,7 +1000,7 @@ struct cal_camerarx *cal_camerarx_create(struct cal_dev *cal,
 	v4l2_subdev_init(sd, &cal_camerarx_subdev_ops);
 	sd->internal_ops = &cal_camerarx_internal_ops;
 	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
-	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
+	sd->flags = V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
 	snprintf(sd->name, sizeof(sd->name), "CAMERARX%u", instance);
 	sd->dev = cal->dev;
 
diff --git a/drivers/media/platform/ti/cal/cal-video.c b/drivers/media/platform/ti/cal/cal-video.c
index e1ba5dfc217e..d40e24ab1127 100644
--- a/drivers/media/platform/ti/cal/cal-video.c
+++ b/drivers/media/platform/ti/cal/cal-video.c
@@ -25,20 +25,6 @@
 
 #include "cal.h"
 
-/*  Print Four-character-code (FOURCC) */
-static char *fourcc_to_str(u32 fmt)
-{
-	static char code[5];
-
-	code[0] = (unsigned char)(fmt & 0xff);
-	code[1] = (unsigned char)((fmt >> 8) & 0xff);
-	code[2] = (unsigned char)((fmt >> 16) & 0xff);
-	code[3] = (unsigned char)((fmt >> 24) & 0xff);
-	code[4] = '\0';
-
-	return code;
-}
-
 /* ------------------------------------------------------------------
  *	V4L2 Common IOCTLs
  * ------------------------------------------------------------------
@@ -122,9 +108,10 @@ static int __subdev_get_format(struct cal_ctx *ctx,
 		.pad = 0,
 	};
 	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
+	struct v4l2_subdev *sd = ctx->phy->source;
 	int ret;
 
-	ret = v4l2_subdev_call(ctx->phy->source, pad, get_fmt, NULL, &sd_fmt);
+	ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &sd_fmt);
 	if (ret)
 		return ret;
 
@@ -144,11 +131,12 @@ static int __subdev_set_format(struct cal_ctx *ctx,
 		.pad = 0,
 	};
 	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
+	struct v4l2_subdev *sd = ctx->phy->source;
 	int ret;
 
 	*mbus_fmt = *fmt;
 
-	ret = v4l2_subdev_call(ctx->phy->source, pad, set_fmt, NULL, &sd_fmt);
+	ret = v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
 	if (ret)
 		return ret;
 
@@ -180,8 +168,8 @@ static void cal_calc_format_size(struct cal_ctx *ctx,
 	f->fmt.pix.sizeimage = f->fmt.pix.height *
 			       f->fmt.pix.bytesperline;
 
-	ctx_dbg(3, ctx, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n",
-		__func__, fourcc_to_str(f->fmt.pix.pixelformat),
+	ctx_dbg(3, ctx, "%s: fourcc: %p4cc size: %dx%d bpl:%d img_size:%d\n",
+		__func__, &f->fmt.pix.pixelformat,
 		f->fmt.pix.width, f->fmt.pix.height,
 		f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
 }
@@ -190,6 +178,7 @@ static int cal_legacy_try_fmt_vid_cap(struct file *file, void *priv,
 				      struct v4l2_format *f)
 {
 	struct cal_ctx *ctx = video_drvdata(file);
+	struct v4l2_subdev *sd = ctx->phy->source;
 	const struct cal_format_info *fmtinfo;
 	struct v4l2_subdev_frame_size_enum fse = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -215,8 +204,8 @@ static int cal_legacy_try_fmt_vid_cap(struct file *file, void *priv,
 	for (fse.index = 0; ; fse.index++) {
 		int ret;
 
-		ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_size,
-				       NULL, &fse);
+		ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size,
+						    &fse);
 		if (ret)
 			break;
 
@@ -252,6 +241,7 @@ static int cal_legacy_s_fmt_vid_cap(struct file *file, void *priv,
 				    struct v4l2_format *f)
 {
 	struct cal_ctx *ctx = video_drvdata(file);
+	struct v4l2_subdev *sd = &ctx->phy->subdev;
 	struct vb2_queue *q = &ctx->vb_vidq;
 	struct v4l2_subdev_format sd_fmt = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -291,7 +281,7 @@ static int cal_legacy_s_fmt_vid_cap(struct file *file, void *priv,
 	ctx->v_fmt.fmt.pix.field = sd_fmt.format.field;
 	cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
 
-	v4l2_subdev_call(&ctx->phy->subdev, pad, set_fmt, NULL, &sd_fmt);
+	v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
 
 	ctx->fmtinfo = fmtinfo;
 	*f = ctx->v_fmt;
@@ -303,6 +293,7 @@ static int cal_legacy_enum_framesizes(struct file *file, void *fh,
 				      struct v4l2_frmsizeenum *fsize)
 {
 	struct cal_ctx *ctx = video_drvdata(file);
+	struct v4l2_subdev *sd = ctx->phy->source;
 	const struct cal_format_info *fmtinfo;
 	struct v4l2_subdev_frame_size_enum fse = {
 		.index = fsize->index,
@@ -321,8 +312,7 @@ static int cal_legacy_enum_framesizes(struct file *file, void *fh,
 
 	fse.code = fmtinfo->code;
 
-	ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_size, NULL,
-			       &fse);
+	ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size, &fse);
 	if (ret)
 		return ret;
 
@@ -364,6 +354,7 @@ static int cal_legacy_enum_frameintervals(struct file *file, void *priv,
 					  struct v4l2_frmivalenum *fival)
 {
 	struct cal_ctx *ctx = video_drvdata(file);
+	struct v4l2_subdev *sd = ctx->phy->source;
 	const struct cal_format_info *fmtinfo;
 	struct v4l2_subdev_frame_interval_enum fie = {
 		.index = fival->index,
@@ -378,8 +369,8 @@ static int cal_legacy_enum_frameintervals(struct file *file, void *priv,
 		return -EINVAL;
 
 	fie.code = fmtinfo->code;
-	ret = v4l2_subdev_call(ctx->phy->source, pad, enum_frame_interval,
-			       NULL, &fie);
+
+	ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_interval, &fie);
 	if (ret)
 		return ret;
 	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
@@ -446,6 +437,9 @@ static int cal_mc_enum_fmt_vid_cap(struct file *file, void  *priv,
 	idx = 0;
 
 	for (i = 0; i < cal_num_formats; ++i) {
+		if (cal_formats[i].meta)
+			continue;
+
 		if (f->mbus_code && cal_formats[i].code != f->mbus_code)
 			continue;
 
@@ -473,7 +467,7 @@ static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f,
 	 * supported.
 	 */
 	fmtinfo = cal_format_by_fourcc(f->fmt.pix.pixelformat);
-	if (!fmtinfo)
+	if (!fmtinfo || fmtinfo->meta)
 		fmtinfo = &cal_formats[0];
 
 	/*
@@ -509,8 +503,8 @@ static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f,
 	if (info)
 		*info = fmtinfo;
 
-	ctx_dbg(3, ctx, "%s: %s %ux%u (bytesperline %u sizeimage %u)\n",
-		__func__, fourcc_to_str(format->pixelformat),
+	ctx_dbg(3, ctx, "%s: %p4cc %ux%u (bytesperline %u sizeimage %u)\n",
+		__func__, &format->pixelformat,
 		format->width, format->height,
 		format->bytesperline, format->sizeimage);
 }
@@ -689,16 +683,16 @@ static int cal_video_check_format(struct cal_ctx *ctx)
 {
 	const struct v4l2_mbus_framefmt *format;
 	struct v4l2_subdev_state *state;
-	struct media_pad *remote_pad;
+	struct media_pad *phy_source_pad;
 	int ret = 0;
 
-	remote_pad = media_pad_remote_pad_first(&ctx->pad);
-	if (!remote_pad)
+	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
+	if (!phy_source_pad)
 		return -ENODEV;
 
 	state = v4l2_subdev_lock_and_get_active_state(&ctx->phy->subdev);
 
-	format = v4l2_subdev_state_get_format(state, remote_pad->index);
+	format = v4l2_subdev_state_get_format(state, phy_source_pad->index, 0);
 	if (!format) {
 		ret = -EINVAL;
 		goto out;
@@ -721,16 +715,28 @@ out:
 static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
+	struct media_pad *phy_source_pad;
 	struct cal_buffer *buf;
 	dma_addr_t addr;
 	int ret;
 
+	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
+	if (!phy_source_pad) {
+		ctx_err(ctx, "Context not connected\n");
+		ret = -ENODEV;
+		goto error_release_buffers;
+	}
+
 	ret = video_device_pipeline_alloc_start(&ctx->vdev);
 	if (ret < 0) {
 		ctx_err(ctx, "Failed to start media pipeline: %d\n", ret);
 		goto error_release_buffers;
 	}
 
+	/* Find the PHY connected to this video device */
+	if (cal_mc_api)
+		ctx->phy = cal_camerarx_get_phy_from_entity(phy_source_pad->entity);
+
 	/*
 	 * Verify that the currently configured format matches the output of
 	 * the connected CAMERARX.
@@ -758,12 +764,13 @@ static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	ret = pm_runtime_resume_and_get(ctx->cal->dev);
 	if (ret < 0)
-		goto error_pipeline;
+		goto error_unprepare;
 
 	cal_ctx_set_dma_addr(ctx, addr);
 	cal_ctx_start(ctx);
 
-	ret = v4l2_subdev_call(&ctx->phy->subdev, video, s_stream, 1);
+	ret = v4l2_subdev_enable_streams(&ctx->phy->subdev,
+					 phy_source_pad->index, BIT(0));
 	if (ret)
 		goto error_stop;
 
@@ -775,8 +782,8 @@ static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 error_stop:
 	cal_ctx_stop(ctx);
 	pm_runtime_put_sync(ctx->cal->dev);
+error_unprepare:
 	cal_ctx_unprepare(ctx);
-
 error_pipeline:
 	video_device_pipeline_stop(&ctx->vdev);
 error_release_buffers:
@@ -788,10 +795,14 @@ error_release_buffers:
 static void cal_stop_streaming(struct vb2_queue *vq)
 {
 	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
+	struct media_pad *phy_source_pad;
 
 	cal_ctx_stop(ctx);
 
-	v4l2_subdev_call(&ctx->phy->subdev, video, s_stream, 0);
+	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
+
+	v4l2_subdev_disable_streams(&ctx->phy->subdev, phy_source_pad->index,
+				    BIT(0));
 
 	pm_runtime_put_sync(ctx->cal->dev);
 
@@ -800,6 +811,9 @@ static void cal_stop_streaming(struct vb2_queue *vq)
 	cal_release_buffers(ctx, VB2_BUF_STATE_ERROR);
 
 	video_device_pipeline_stop(&ctx->vdev);
+
+	if (cal_mc_api)
+		ctx->phy = NULL;
 }
 
 static const struct vb2_ops cal_video_qops = {
@@ -808,8 +822,6 @@ static const struct vb2_ops cal_video_qops = {
 	.buf_queue		= cal_buffer_queue,
 	.start_streaming	= cal_start_streaming,
 	.stop_streaming		= cal_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------
@@ -828,6 +840,7 @@ static const struct v4l2_file_operations cal_fops = {
 
 static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 {
+	struct v4l2_subdev *sd = ctx->phy->source;
 	struct v4l2_mbus_framefmt mbus_fmt;
 	const struct cal_format_info *fmtinfo;
 	unsigned int i, j, k;
@@ -847,20 +860,20 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 		};
 
-		ret = v4l2_subdev_call(ctx->phy->source, pad, enum_mbus_code,
-				       NULL, &mbus_code);
+		ret = v4l2_subdev_call_state_active(sd, pad, enum_mbus_code,
+						    &mbus_code);
 		if (ret == -EINVAL)
 			break;
 
 		if (ret) {
 			ctx_err(ctx, "Error enumerating mbus codes in subdev %s: %d\n",
-				ctx->phy->source->name, ret);
+				sd->name, ret);
 			return ret;
 		}
 
 		ctx_dbg(2, ctx,
 			"subdev %s: code: %04x idx: %u\n",
-			ctx->phy->source->name, mbus_code.code, j);
+			sd->name, mbus_code.code, j);
 
 		for (k = 0; k < cal_num_formats; k++) {
 			fmtinfo = &cal_formats[k];
@@ -868,8 +881,8 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 			if (mbus_code.code == fmtinfo->code) {
 				ctx->active_fmt[i] = fmtinfo;
 				ctx_dbg(2, ctx,
-					"matched fourcc: %s: code: %04x idx: %u\n",
-					fourcc_to_str(fmtinfo->fourcc),
+					"matched fourcc: %p4cc: code: %04x idx: %u\n",
+					&fmtinfo->fourcc,
 					fmtinfo->code, i);
 				ctx->num_active_fmt = ++i;
 			}
@@ -878,7 +891,7 @@ static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 
 	if (i == 0) {
 		ctx_err(ctx, "No suitable format reported by subdev %s\n",
-			ctx->phy->source->name);
+			sd->name);
 		return -EINVAL;
 	}
 
@@ -964,16 +977,52 @@ int cal_ctx_v4l2_register(struct cal_ctx *ctx)
 		return ret;
 	}
 
-	ret = media_create_pad_link(&ctx->phy->subdev.entity,
-				    CAL_CAMERARX_PAD_FIRST_SOURCE,
-				    &vfd->entity, 0,
-				    MEDIA_LNK_FL_IMMUTABLE |
-				    MEDIA_LNK_FL_ENABLED);
-	if (ret) {
-		ctx_err(ctx, "Failed to create media link for context %u\n",
-			ctx->dma_ctx);
-		video_unregister_device(vfd);
-		return ret;
+	if (cal_mc_api) {
+		u16 phy_idx;
+		u16 pad_idx;
+
+		/* Create links from all video nodes to all PHYs */
+
+		for (phy_idx = 0; phy_idx < ctx->cal->data->num_csi2_phy;
+		     ++phy_idx) {
+			struct media_entity *phy_entity =
+				&ctx->cal->phy[phy_idx]->subdev.entity;
+
+			for (pad_idx = 1; pad_idx < CAL_CAMERARX_NUM_PADS;
+			     ++pad_idx) {
+				/*
+				 * Enable only links from video0 to PHY0 pad 1,
+				 * and video1 to PHY1 pad 1.
+				 */
+				bool enable = (ctx->dma_ctx == 0 &&
+					       phy_idx == 0 && pad_idx == 1) ||
+					      (ctx->dma_ctx == 1 &&
+					       phy_idx == 1 && pad_idx == 1);
+
+				ret = media_create_pad_link(phy_entity, pad_idx,
+							    &vfd->entity, 0,
+							    enable ? MEDIA_LNK_FL_ENABLED : 0);
+				if (ret) {
+					ctx_err(ctx,
+						"Failed to create media link for context %u\n",
+						ctx->dma_ctx);
+					video_unregister_device(vfd);
+					return ret;
+				}
+			}
+		}
+	} else {
+		ret = media_create_pad_link(&ctx->phy->subdev.entity,
+					    CAL_CAMERARX_PAD_FIRST_SOURCE,
+					    &vfd->entity, 0,
+					    MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
+		if (ret) {
+			ctx_err(ctx,
+				"Failed to create media link for context %u\n",
+				ctx->dma_ctx);
+			video_unregister_device(vfd);
+			return ret;
+		}
 	}
 
 	ctx_info(ctx, "V4L2 device registered as %s\n",
diff --git a/drivers/media/platform/ti/cal/cal.c b/drivers/media/platform/ti/cal/cal.c
index 528909ae4bd6..b644ed890412 100644
--- a/drivers/media/platform/ti/cal/cal.c
+++ b/drivers/media/platform/ti/cal/cal.c
@@ -481,8 +481,9 @@ int cal_ctx_prepare(struct cal_ctx *ctx)
 		ctx->vc = 0;
 		ctx->datatype = CAL_CSI2_CTX_DT_ANY;
 	} else if (!ret) {
-		ctx_dbg(2, ctx, "Framedesc: len %u, vc %u, dt %#x\n",
-			entry.length, entry.bus.csi2.vc, entry.bus.csi2.dt);
+		ctx_dbg(2, ctx, "Framedesc: stream %u, len %u, vc %u, dt %#x\n",
+			entry.stream, entry.length, entry.bus.csi2.vc,
+			entry.bus.csi2.dt);
 
 		ctx->vc = entry.bus.csi2.vc;
 		ctx->datatype = entry.bus.csi2.dt;
@@ -490,7 +491,7 @@ int cal_ctx_prepare(struct cal_ctx *ctx)
 		return ret;
 	}
 
-	ctx->use_pix_proc = !ctx->fmtinfo->meta;
+	ctx->use_pix_proc = ctx->vb_vidq.type == V4L2_BUF_TYPE_VIDEO_CAPTURE;
 
 	if (ctx->use_pix_proc) {
 		ret = cal_reserve_pix_proc(ctx->cal);
@@ -549,7 +550,7 @@ void cal_ctx_start(struct cal_ctx *ctx)
 void cal_ctx_stop(struct cal_ctx *ctx)
 {
 	struct cal_camerarx *phy = ctx->phy;
-	long timeout;
+	long time_left;
 
 	WARN_ON(phy->vc_enable_count[ctx->vc] == 0);
 
@@ -565,9 +566,9 @@ void cal_ctx_stop(struct cal_ctx *ctx)
 	ctx->dma.state = CAL_DMA_STOP_REQUESTED;
 	spin_unlock_irq(&ctx->dma.lock);
 
-	timeout = wait_event_timeout(ctx->dma.wait, cal_ctx_wr_dma_stopped(ctx),
-				     msecs_to_jiffies(500));
-	if (!timeout) {
+	time_left = wait_event_timeout(ctx->dma.wait, cal_ctx_wr_dma_stopped(ctx),
+				       msecs_to_jiffies(500));
+	if (!time_left) {
 		ctx_err(ctx, "failed to disable dma cleanly\n");
 		cal_ctx_wr_dma_disable(ctx);
 	}
@@ -798,7 +799,6 @@ static int cal_async_notifier_bound(struct v4l2_async_notifier *notifier,
 		return 0;
 	}
 
-	phy->source = subdev;
 	phy_dbg(1, phy, "Using source %s for capture\n", subdev->name);
 
 	pad = media_entity_get_fwnode_pad(&subdev->entity,
@@ -820,6 +820,9 @@ static int cal_async_notifier_bound(struct v4l2_async_notifier *notifier,
 		return ret;
 	}
 
+	phy->source = subdev;
+	phy->source_pad = pad;
+
 	return 0;
 }
 
@@ -1014,7 +1017,6 @@ static struct cal_ctx *cal_ctx_create(struct cal_dev *cal, int inst)
 		return NULL;
 
 	ctx->cal = cal;
-	ctx->phy = cal->phy[inst];
 	ctx->dma_ctx = inst;
 	ctx->csi2_ctx = inst;
 	ctx->cport = inst;
@@ -1226,18 +1228,37 @@ static int cal_probe(struct platform_device *pdev)
 	}
 
 	/* Create contexts. */
-	for (i = 0; i < cal->data->num_csi2_phy; ++i) {
-		if (!cal->phy[i]->source_node)
-			continue;
+	if (!cal_mc_api) {
+		for (i = 0; i < cal->data->num_csi2_phy; ++i) {
+			struct cal_ctx *ctx;
+
+			if (!cal->phy[i]->source_node)
+				continue;
 
-		cal->ctx[cal->num_contexts] = cal_ctx_create(cal, i);
-		if (!cal->ctx[cal->num_contexts]) {
-			cal_err(cal, "Failed to create context %u\n", cal->num_contexts);
-			ret = -ENODEV;
-			goto error_context;
+			ctx = cal_ctx_create(cal, i);
+			if (!ctx) {
+				cal_err(cal, "Failed to create context %u\n", cal->num_contexts);
+				ret = -ENODEV;
+				goto error_context;
+			}
+
+			ctx->phy = cal->phy[i];
+
+			cal->ctx[cal->num_contexts++] = ctx;
 		}
+	} else {
+		for (i = 0; i < ARRAY_SIZE(cal->ctx); ++i) {
+			struct cal_ctx *ctx;
+
+			ctx = cal_ctx_create(cal, i);
+			if (!ctx) {
+				cal_err(cal, "Failed to create context %u\n", i);
+				ret = -ENODEV;
+				goto error_context;
+			}
 
-		cal->num_contexts++;
+			cal->ctx[cal->num_contexts++] = ctx;
+		}
 	}
 
 	/* Register the media device. */
@@ -1332,7 +1353,7 @@ static const struct dev_pm_ops cal_pm_ops = {
 
 static struct platform_driver cal_pdrv = {
 	.probe		= cal_probe,
-	.remove_new	= cal_remove,
+	.remove		= cal_remove,
 	.driver		= {
 		.name	= CAL_MODULE_NAME,
 		.pm	= &cal_pm_ops,
diff --git a/drivers/media/platform/ti/cal/cal.h b/drivers/media/platform/ti/cal/cal.h
index 0856297adc0b..33b1885aafe1 100644
--- a/drivers/media/platform/ti/cal/cal.h
+++ b/drivers/media/platform/ti/cal/cal.h
@@ -45,7 +45,7 @@
 
 #define CAL_CAMERARX_PAD_SINK		0
 #define CAL_CAMERARX_PAD_FIRST_SOURCE	1
-#define CAL_CAMERARX_NUM_SOURCE_PADS	1
+#define CAL_CAMERARX_NUM_SOURCE_PADS	8
 #define CAL_CAMERARX_NUM_PADS		(1 + CAL_CAMERARX_NUM_SOURCE_PADS)
 
 static inline bool cal_rx_pad_is_sink(u32 pad)
@@ -174,6 +174,7 @@ struct cal_camerarx {
 	struct device_node	*source_ep_node;
 	struct device_node	*source_node;
 	struct v4l2_subdev	*source;
+	unsigned int		source_pad;
 
 	struct v4l2_subdev	subdev;
 	struct media_pad	pads[CAL_CAMERARX_NUM_PADS];
@@ -319,6 +320,7 @@ const struct cal_format_info *cal_format_by_code(u32 code);
 
 void cal_quickdump_regs(struct cal_dev *cal);
 
+struct cal_camerarx *cal_camerarx_get_phy_from_entity(struct media_entity *entity);
 void cal_camerarx_disable(struct cal_camerarx *phy);
 void cal_camerarx_i913_errata(struct cal_camerarx *phy);
 struct cal_camerarx *cal_camerarx_create(struct cal_dev *cal,
diff --git a/drivers/media/platform/ti/davinci/vpif.c b/drivers/media/platform/ti/davinci/vpif.c
index f4e1fa76bf37..969d623fc842 100644
--- a/drivers/media/platform/ti/davinci/vpif.c
+++ b/drivers/media/platform/ti/davinci/vpif.c
@@ -504,7 +504,7 @@ static int vpif_probe(struct platform_device *pdev)
 	pdev_display = kzalloc(sizeof(*pdev_display), GFP_KERNEL);
 	if (!pdev_display) {
 		ret = -ENOMEM;
-		goto err_put_pdev_capture;
+		goto err_del_pdev_capture;
 	}
 
 	pdev_display->name = "vpif_display";
@@ -527,6 +527,8 @@ static int vpif_probe(struct platform_device *pdev)
 
 err_put_pdev_display:
 	platform_device_put(pdev_display);
+err_del_pdev_capture:
+	platform_device_del(pdev_capture);
 err_put_pdev_capture:
 	platform_device_put(pdev_capture);
 err_put_rpm:
@@ -589,7 +591,7 @@ static struct platform_driver vpif_driver = {
 		.name	= VPIF_DRIVER_NAME,
 		.pm	= vpif_pm_ops,
 	},
-	.remove_new = vpif_remove,
+	.remove = vpif_remove,
 	.probe = vpif_probe,
 };
 
diff --git a/drivers/media/platform/ti/davinci/vpif_capture.c b/drivers/media/platform/ti/davinci/vpif_capture.c
index c31a5566fc5a..d053972888d1 100644
--- a/drivers/media/platform/ti/davinci/vpif_capture.c
+++ b/drivers/media/platform/ti/davinci/vpif_capture.c
@@ -310,8 +310,6 @@ static const struct vb2_ops video_qops = {
 	.start_streaming	= vpif_start_streaming,
 	.stop_streaming		= vpif_stop_streaming,
 	.buf_queue		= vpif_buffer_queue,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /**
@@ -1132,7 +1130,7 @@ vpif_query_dv_timings(struct file *file, void *priv,
 	if (input.capabilities != V4L2_IN_CAP_DV_TIMINGS)
 		return -ENODATA;
 
-	ret = v4l2_subdev_call(ch->sd, video, query_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, pad, query_dv_timings, 0, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		return -ENODATA;
 
@@ -1177,7 +1175,7 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 		return -EBUSY;
 
 	/* Configure subdevice timings, if any */
-	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, pad, s_dv_timings, 0, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		ret = 0;
 	if (ret < 0) {
@@ -1487,7 +1485,7 @@ static struct vpif_capture_config *
 vpif_capture_get_pdata(struct platform_device *pdev,
 		       struct v4l2_device *v4l2_dev)
 {
-	struct device_node *endpoint = NULL;
+	struct device_node *endpoint;
 	struct device_node *rem = NULL;
 	struct vpif_capture_config *pdata;
 	struct vpif_subdev_info *sdinfo;
@@ -1517,16 +1515,12 @@ vpif_capture_get_pdata(struct platform_device *pdev,
 	if (!pdata->subdev_info)
 		return NULL;
 
-	for (i = 0; i < VPIF_CAPTURE_NUM_CHANNELS; i++) {
+	i = 0;
+	for_each_endpoint_of_node(pdev->dev.of_node, endpoint) {
 		struct v4l2_fwnode_endpoint bus_cfg = { .bus_type = 0 };
 		unsigned int flags;
 		int err;
 
-		endpoint = of_graph_get_next_endpoint(pdev->dev.of_node,
-						      endpoint);
-		if (!endpoint)
-			break;
-
 		rem = of_graph_get_remote_port_parent(endpoint);
 		if (!rem) {
 			dev_dbg(&pdev->dev, "Remote device at %pOF not found\n",
@@ -1577,6 +1571,10 @@ vpif_capture_get_pdata(struct platform_device *pdev,
 			goto err_cleanup;
 
 		of_node_put(rem);
+
+		i++;
+		if (i >= VPIF_CAPTURE_NUM_CHANNELS)
+			break;
 	}
 
 done:
@@ -1815,7 +1813,7 @@ static __refdata struct platform_driver vpif_driver = {
 		.pm	= &vpif_pm_ops,
 	},
 	.probe = vpif_probe,
-	.remove_new = vpif_remove,
+	.remove = vpif_remove,
 };
 
 module_platform_driver(vpif_driver);
diff --git a/drivers/media/platform/ti/davinci/vpif_display.c b/drivers/media/platform/ti/davinci/vpif_display.c
index 02ede1fe12cb..70c89549f4b6 100644
--- a/drivers/media/platform/ti/davinci/vpif_display.c
+++ b/drivers/media/platform/ti/davinci/vpif_display.c
@@ -293,8 +293,6 @@ static void vpif_stop_streaming(struct vb2_queue *vq)
 
 static const struct vb2_ops video_qops = {
 	.queue_setup		= vpif_buffer_queue_setup,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 	.buf_prepare		= vpif_buffer_prepare,
 	.start_streaming	= vpif_start_streaming,
 	.stop_streaming		= vpif_stop_streaming,
@@ -934,7 +932,7 @@ static int vpif_s_dv_timings(struct file *file, void *priv,
 	}
 
 	/* Configure subdevice timings, if any */
-	ret = v4l2_subdev_call(ch->sd, video, s_dv_timings, timings);
+	ret = v4l2_subdev_call(ch->sd, pad, s_dv_timings, 0, timings);
 	if (ret == -ENOIOCTLCMD || ret == -ENODEV)
 		ret = 0;
 	if (ret < 0) {
@@ -1398,7 +1396,7 @@ static __refdata struct platform_driver vpif_driver = {
 			.pm	= &vpif_pm_ops,
 	},
 	.probe	= vpif_probe,
-	.remove_new = vpif_remove,
+	.remove = vpif_remove,
 };
 
 module_platform_driver(vpif_driver);
diff --git a/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c b/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c
index 6da83d0cffaa..6412a00be8ea 100644
--- a/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c
+++ b/drivers/media/platform/ti/j721e-csi2rx/j721e-csi2rx.c
@@ -786,15 +786,14 @@ static void ti_csi2rx_buffer_queue(struct vb2_buffer *vb)
 			dev_warn(csi->dev,
 				 "Failed to drain DMA. Next frame might be bogus\n");
 
+		spin_lock_irqsave(&dma->lock, flags);
 		ret = ti_csi2rx_start_dma(csi, buf);
 		if (ret) {
-			dev_err(csi->dev, "Failed to start DMA: %d\n", ret);
-			spin_lock_irqsave(&dma->lock, flags);
 			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 			dma->state = TI_CSI2RX_DMA_IDLE;
 			spin_unlock_irqrestore(&dma->lock, flags);
+			dev_err(csi->dev, "Failed to start DMA: %d\n", ret);
 		} else {
-			spin_lock_irqsave(&dma->lock, flags);
 			list_add_tail(&buf->list, &dma->submitted);
 			spin_unlock_irqrestore(&dma->lock, flags);
 		}
@@ -879,8 +878,6 @@ static const struct vb2_ops csi_vb2_qops = {
 	.buf_queue = ti_csi2rx_buffer_queue,
 	.start_streaming = ti_csi2rx_start_streaming,
 	.stop_streaming = ti_csi2rx_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int ti_csi2rx_init_vb2q(struct ti_csi2rx_dev *csi)
@@ -1015,9 +1012,9 @@ static int ti_csi2rx_v4l2_init(struct ti_csi2rx_dev *csi)
 	pix_fmt->height = 480;
 	pix_fmt->field = V4L2_FIELD_NONE;
 	pix_fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	pix_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601,
-	pix_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE,
-	pix_fmt->xfer_func = V4L2_XFER_FUNC_SRGB,
+	pix_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
+	pix_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+	pix_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
 
 	ti_csi2rx_fill_fmt(fmt, &csi->v_fmt);
 
@@ -1164,7 +1161,7 @@ MODULE_DEVICE_TABLE(of, ti_csi2rx_of_match);
 
 static struct platform_driver ti_csi2rx_pdrv = {
 	.probe = ti_csi2rx_probe,
-	.remove_new = ti_csi2rx_remove,
+	.remove = ti_csi2rx_remove,
 	.driver = {
 		.name = TI_CSI2RX_MODULE_NAME,
 		.of_match_table = ti_csi2rx_of_match,
diff --git a/drivers/media/platform/ti/omap/omap_vout.c b/drivers/media/platform/ti/omap/omap_vout.c
index 1c56b6a87ced..a87d5030ac35 100644
--- a/drivers/media/platform/ti/omap/omap_vout.c
+++ b/drivers/media/platform/ti/omap/omap_vout.c
@@ -1300,8 +1300,6 @@ static const struct vb2_ops omap_vout_vb2_ops = {
 	.buf_prepare		= omap_vout_vb2_prepare,
 	.start_streaming	= omap_vout_vb2_start_streaming,
 	.stop_streaming		= omap_vout_vb2_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* Init functions used during driver initialization */
@@ -1721,7 +1719,7 @@ static struct platform_driver omap_vout_driver = {
 	.driver = {
 		.name = VOUT_NAME,
 	},
-	.remove_new = omap_vout_remove,
+	.remove = omap_vout_remove,
 };
 
 static int __init omap_vout_init(void)
diff --git a/drivers/media/platform/ti/omap/omap_voutdef.h b/drivers/media/platform/ti/omap/omap_voutdef.h
index b586193341d2..159e5e670d91 100644
--- a/drivers/media/platform/ti/omap/omap_voutdef.h
+++ b/drivers/media/platform/ti/omap/omap_voutdef.h
@@ -48,7 +48,7 @@
 #define VRFB_TX_TIMEOUT         1000
 #define VRFB_NUM_BUFS		4
 
-/* Max buffer size tobe allocated during init */
+/* Max buffer size to be allocated during init */
 #define OMAP_VOUT_MAX_BUF_SIZE (VID_MAX_WIDTH*VID_MAX_HEIGHT*4)
 
 enum dma_channel_state {
diff --git a/drivers/media/platform/ti/omap3isp/isp.c b/drivers/media/platform/ti/omap3isp/isp.c
index 1cda23244c7b..f51cf6119e97 100644
--- a/drivers/media/platform/ti/omap3isp/isp.c
+++ b/drivers/media/platform/ti/omap3isp/isp.c
@@ -1475,43 +1475,6 @@ void omap3isp_put(struct isp_device *isp)
  * Platform device driver
  */
 
-/*
- * omap3isp_print_status - Prints the values of the ISP Control Module registers
- * @isp: OMAP3 ISP device
- */
-#define ISP_PRINT_REGISTER(isp, name)\
-	dev_dbg(isp->dev, "###ISP " #name "=0x%08x\n", \
-		isp_reg_readl(isp, OMAP3_ISP_IOMEM_MAIN, ISP_##name))
-#define SBL_PRINT_REGISTER(isp, name)\
-	dev_dbg(isp->dev, "###SBL " #name "=0x%08x\n", \
-		isp_reg_readl(isp, OMAP3_ISP_IOMEM_SBL, ISPSBL_##name))
-
-void omap3isp_print_status(struct isp_device *isp)
-{
-	dev_dbg(isp->dev, "-------------ISP Register dump--------------\n");
-
-	ISP_PRINT_REGISTER(isp, SYSCONFIG);
-	ISP_PRINT_REGISTER(isp, SYSSTATUS);
-	ISP_PRINT_REGISTER(isp, IRQ0ENABLE);
-	ISP_PRINT_REGISTER(isp, IRQ0STATUS);
-	ISP_PRINT_REGISTER(isp, TCTRL_GRESET_LENGTH);
-	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_REPLAY);
-	ISP_PRINT_REGISTER(isp, CTRL);
-	ISP_PRINT_REGISTER(isp, TCTRL_CTRL);
-	ISP_PRINT_REGISTER(isp, TCTRL_FRAME);
-	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_DELAY);
-	ISP_PRINT_REGISTER(isp, TCTRL_STRB_DELAY);
-	ISP_PRINT_REGISTER(isp, TCTRL_SHUT_DELAY);
-	ISP_PRINT_REGISTER(isp, TCTRL_PSTRB_LENGTH);
-	ISP_PRINT_REGISTER(isp, TCTRL_STRB_LENGTH);
-	ISP_PRINT_REGISTER(isp, TCTRL_SHUT_LENGTH);
-
-	SBL_PRINT_REGISTER(isp, PCR);
-	SBL_PRINT_REGISTER(isp, SDR_REQ_EXP);
-
-	dev_dbg(isp->dev, "--------------------------------------------\n");
-}
-
 #ifdef CONFIG_PM
 
 /*
@@ -1961,11 +1924,18 @@ static int isp_attach_iommu(struct isp_device *isp)
 	struct dma_iommu_mapping *mapping;
 	int ret;
 
+	/* We always want to replace any default mapping from the arch code */
+	mapping = to_dma_iommu_mapping(isp->dev);
+	if (mapping) {
+		arm_iommu_detach_device(isp->dev);
+		arm_iommu_release_mapping(mapping);
+	}
+
 	/*
 	 * Create the ARM mapping, used by the ARM DMA mapping core to allocate
 	 * VAs. This will allocate a corresponding IOMMU domain.
 	 */
-	mapping = arm_iommu_create_mapping(&platform_bus_type, SZ_1G, SZ_2G);
+	mapping = arm_iommu_create_mapping(isp->dev, SZ_1G, SZ_2G);
 	if (IS_ERR(mapping)) {
 		dev_err(isp->dev, "failed to create ARM IOMMU mapping\n");
 		return PTR_ERR(mapping);
@@ -2272,18 +2242,14 @@ static int isp_probe(struct platform_device *pdev)
 	if (ret)
 		goto error_release_isp;
 
-	isp->syscon = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
-						      "syscon");
+	isp->syscon = syscon_regmap_lookup_by_phandle_args(pdev->dev.of_node,
+							   "syscon", 1,
+							   &isp->syscon_offset);
 	if (IS_ERR(isp->syscon)) {
 		ret = PTR_ERR(isp->syscon);
 		goto error_release_isp;
 	}
 
-	ret = of_property_read_u32_index(pdev->dev.of_node,
-					 "syscon", 1, &isp->syscon_offset);
-	if (ret)
-		goto error_release_isp;
-
 	isp->autoidle = autoidle;
 
 	mutex_init(&isp->isp_mutex);
@@ -2472,7 +2438,7 @@ MODULE_DEVICE_TABLE(of, omap3isp_of_table);
 
 static struct platform_driver omap3isp_driver = {
 	.probe = isp_probe,
-	.remove_new = isp_remove,
+	.remove = isp_remove,
 	.id_table = omap3isp_id_table,
 	.driver = {
 		.name = "omap3isp",
diff --git a/drivers/media/platform/ti/omap3isp/isp.h b/drivers/media/platform/ti/omap3isp/isp.h
index b4793631ad97..60acf3401ac9 100644
--- a/drivers/media/platform/ti/omap3isp/isp.h
+++ b/drivers/media/platform/ti/omap3isp/isp.h
@@ -260,8 +260,6 @@ void omap3isp_configure_bridge(struct isp_device *isp,
 struct isp_device *omap3isp_get(struct isp_device *isp);
 void omap3isp_put(struct isp_device *isp);
 
-void omap3isp_print_status(struct isp_device *isp);
-
 void omap3isp_sbl_enable(struct isp_device *isp, enum isp_sbl_resource res);
 void omap3isp_sbl_disable(struct isp_device *isp, enum isp_sbl_resource res);
 
diff --git a/drivers/media/platform/ti/omap3isp/ispccdc.c b/drivers/media/platform/ti/omap3isp/ispccdc.c
index dd375c4e180d..7d0c723dcd11 100644
--- a/drivers/media/platform/ti/omap3isp/ispccdc.c
+++ b/drivers/media/platform/ti/omap3isp/ispccdc.c
@@ -446,8 +446,8 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 		if (ret < 0)
 			goto done;
 
-		dma_sync_sg_for_cpu(isp->dev, req->table.sgt.sgl,
-				    req->table.sgt.nents, DMA_TO_DEVICE);
+		dma_sync_sgtable_for_cpu(isp->dev, &req->table.sgt,
+					 DMA_TO_DEVICE);
 
 		if (copy_from_user(req->table.addr, config->lsc,
 				   req->config.size)) {
@@ -455,8 +455,8 @@ static int ccdc_lsc_config(struct isp_ccdc_device *ccdc,
 			goto done;
 		}
 
-		dma_sync_sg_for_device(isp->dev, req->table.sgt.sgl,
-				       req->table.sgt.nents, DMA_TO_DEVICE);
+		dma_sync_sgtable_for_device(isp->dev, &req->table.sgt,
+					    DMA_TO_DEVICE);
 	}
 
 	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
diff --git a/drivers/media/platform/ti/omap3isp/ispstat.c b/drivers/media/platform/ti/omap3isp/ispstat.c
index 359a846205b0..d3da68408ecb 100644
--- a/drivers/media/platform/ti/omap3isp/ispstat.c
+++ b/drivers/media/platform/ti/omap3isp/ispstat.c
@@ -161,8 +161,7 @@ static void isp_stat_buf_sync_for_device(struct ispstat *stat,
 	if (ISP_STAT_USES_DMAENGINE(stat))
 		return;
 
-	dma_sync_sg_for_device(stat->isp->dev, buf->sgt.sgl,
-			       buf->sgt.nents, DMA_FROM_DEVICE);
+	dma_sync_sgtable_for_device(stat->isp->dev, &buf->sgt, DMA_FROM_DEVICE);
 }
 
 static void isp_stat_buf_sync_for_cpu(struct ispstat *stat,
@@ -171,8 +170,7 @@ static void isp_stat_buf_sync_for_cpu(struct ispstat *stat,
 	if (ISP_STAT_USES_DMAENGINE(stat))
 		return;
 
-	dma_sync_sg_for_cpu(stat->isp->dev, buf->sgt.sgl,
-			    buf->sgt.nents, DMA_FROM_DEVICE);
+	dma_sync_sgtable_for_cpu(stat->isp->dev, &buf->sgt, DMA_FROM_DEVICE);
 }
 
 static void isp_stat_buf_clear(struct ispstat *stat)
diff --git a/drivers/media/platform/ti/omap3isp/ispvideo.c b/drivers/media/platform/ti/omap3isp/ispvideo.c
index daca689dc082..78e30298c7ad 100644
--- a/drivers/media/platform/ti/omap3isp/ispvideo.c
+++ b/drivers/media/platform/ti/omap3isp/ispvideo.c
@@ -1320,6 +1320,7 @@ static int isp_video_open(struct file *file)
 	queue->buf_struct_size = sizeof(struct isp_buffer);
 	queue->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	queue->dev = video->isp->dev;
+	queue->lock = &video->queue_lock;
 
 	ret = vb2_queue_init(&handle->queue);
 	if (ret < 0) {
diff --git a/drivers/media/platform/ti/vpe/vpdma.c b/drivers/media/platform/ti/vpe/vpdma.c
index f8998a8ad371..da90d7f03f82 100644
--- a/drivers/media/platform/ti/vpe/vpdma.c
+++ b/drivers/media/platform/ti/vpe/vpdma.c
@@ -1173,4 +1173,5 @@ EXPORT_SYMBOL(vpdma_create);
 
 MODULE_AUTHOR("Texas Instruments Inc.");
 MODULE_FIRMWARE(VPDMA_FIRMWARE);
+MODULE_DESCRIPTION("TI VPDMA helper library");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/platform/ti/vpe/vpe.c b/drivers/media/platform/ti/vpe/vpe.c
index 6848cbc82f52..636d76ecebcd 100644
--- a/drivers/media/platform/ti/vpe/vpe.c
+++ b/drivers/media/platform/ti/vpe/vpe.c
@@ -2210,8 +2210,6 @@ static const struct vb2_ops vpe_qops = {
 	.queue_setup	 = vpe_queue_setup,
 	.buf_prepare	 = vpe_buf_prepare,
 	.buf_queue	 = vpe_buf_queue,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.start_streaming = vpe_start_streaming,
 	.stop_streaming  = vpe_stop_streaming,
 };
@@ -2649,7 +2647,7 @@ MODULE_DEVICE_TABLE(of, vpe_of_match);
 
 static struct platform_driver vpe_pdrv = {
 	.probe		= vpe_probe,
-	.remove_new	= vpe_remove,
+	.remove		= vpe_remove,
 	.driver		= {
 		.name	= VPE_MODULE_NAME,
 		.of_match_table = of_match_ptr(vpe_of_match),
diff --git a/drivers/media/platform/verisilicon/Kconfig b/drivers/media/platform/verisilicon/Kconfig
index 9a34d14c6e40..3272a24db71d 100644
--- a/drivers/media/platform/verisilicon/Kconfig
+++ b/drivers/media/platform/verisilicon/Kconfig
@@ -12,6 +12,7 @@ config VIDEO_HANTRO
 	select VIDEOBUF2_VMALLOC
 	select V4L2_MEM2MEM_DEV
 	select V4L2_H264
+	select V4L2_JPEG_HELPER
 	select V4L2_VP9
 	help
 	  Support for the Hantro IP based Video Processing Units present on
@@ -20,6 +21,14 @@ config VIDEO_HANTRO
 	  To compile this driver as a module, choose M here: the module
 	  will be called hantro-vpu.
 
+config VIDEO_HANTRO_HEVC_RFC
+	bool "Use reference frame compression for HEVC"
+	depends on VIDEO_HANTRO
+	default n
+	help
+	  Enable the reference frame compression feature for the HEVC codec.
+	  It will use more memory but save bandwidth on memory bus.
+
 config VIDEO_HANTRO_IMX8M
 	bool "Hantro VPU i.MX8M support"
 	depends on VIDEO_HANTRO
diff --git a/drivers/media/platform/verisilicon/Makefile b/drivers/media/platform/verisilicon/Makefile
index eb38a1833b02..f6f019d04ff0 100644
--- a/drivers/media/platform/verisilicon/Makefile
+++ b/drivers/media/platform/verisilicon/Makefile
@@ -14,13 +14,6 @@ hantro-vpu-y += \
 		hantro_g2.o \
 		hantro_g2_hevc_dec.o \
 		hantro_g2_vp9_dec.o \
-		rockchip_vpu2_hw_jpeg_enc.o \
-		rockchip_vpu2_hw_h264_dec.o \
-		rockchip_vpu2_hw_mpeg2_dec.o \
-		rockchip_vpu2_hw_vp8_dec.o \
-		rockchip_vpu981_hw_av1_dec.o \
-		rockchip_av1_filmgrain.o \
-		rockchip_av1_entropymode.o \
 		hantro_jpeg.o \
 		hantro_h264.o \
 		hantro_hevc.o \
@@ -35,6 +28,13 @@ hantro-vpu-$(CONFIG_VIDEO_HANTRO_SAMA5D4) += \
 		sama5d4_vdec_hw.o
 
 hantro-vpu-$(CONFIG_VIDEO_HANTRO_ROCKCHIP) += \
+		rockchip_vpu2_hw_jpeg_enc.o \
+		rockchip_vpu2_hw_h264_dec.o \
+		rockchip_vpu2_hw_mpeg2_dec.o \
+		rockchip_vpu2_hw_vp8_dec.o \
+		rockchip_vpu981_hw_av1_dec.o \
+		rockchip_av1_filmgrain.o \
+		rockchip_av1_entropymode.o \
 		rockchip_vpu_hw.o
 
 hantro-vpu-$(CONFIG_VIDEO_HANTRO_SUNXI) += \
diff --git a/drivers/media/platform/verisilicon/hantro.h b/drivers/media/platform/verisilicon/hantro.h
index 811260dc3c77..edc217eed293 100644
--- a/drivers/media/platform/verisilicon/hantro.h
+++ b/drivers/media/platform/verisilicon/hantro.h
@@ -227,6 +227,7 @@ struct hantro_dev {
  * @src_fmt:		V4L2 pixel format of active source format.
  * @vpu_dst_fmt:	Descriptor of active destination format.
  * @dst_fmt:		V4L2 pixel format of active destination format.
+ * @ref_fmt:		V4L2 pixel format of the reference frames format.
  *
  * @ctrl_handler:	Control handler used to register controls.
  * @jpeg_quality:	User-specified JPEG compression quality.
@@ -255,6 +256,7 @@ struct hantro_ctx {
 	struct v4l2_pix_format_mplane src_fmt;
 	const struct hantro_fmt *vpu_dst_fmt;
 	struct v4l2_pix_format_mplane dst_fmt;
+	struct v4l2_pix_format_mplane ref_fmt;
 
 	struct v4l2_ctrl_handler ctrl_handler;
 	int jpeg_quality;
@@ -332,12 +334,19 @@ struct hantro_vp9_decoded_buffer_info {
 	u32 bit_depth : 4;
 };
 
+struct hantro_av1_decoded_buffer_info {
+	/* Info needed when the decoded frame serves as a reference frame. */
+	size_t chroma_offset;
+	size_t mv_offset;
+};
+
 struct hantro_decoded_buffer {
 	/* Must be the first field in this struct. */
 	struct v4l2_m2m_buffer base;
 
 	union {
 		struct hantro_vp9_decoded_buffer_info vp9;
+		struct hantro_av1_decoded_buffer_info av1;
 	};
 };
 
diff --git a/drivers/media/platform/verisilicon/hantro_drv.c b/drivers/media/platform/verisilicon/hantro_drv.c
index 34b123dafd89..8542238e0fb1 100644
--- a/drivers/media/platform/verisilicon/hantro_drv.c
+++ b/drivers/media/platform/verisilicon/hantro_drv.c
@@ -722,6 +722,7 @@ static const struct of_device_id of_hantro_match[] = {
 	{ .compatible = "rockchip,rk3399-vpu", .data = &rk3399_vpu_variant, },
 	{ .compatible = "rockchip,rk3568-vepu", .data = &rk3568_vepu_variant, },
 	{ .compatible = "rockchip,rk3568-vpu", .data = &rk3568_vpu_variant, },
+	{ .compatible = "rockchip,rk3588-vepu121", .data = &rk3568_vepu_variant, },
 	{ .compatible = "rockchip,rk3588-av1-vpu", .data = &rk3588_vpu981_variant, },
 #endif
 #ifdef CONFIG_VIDEO_HANTRO_IMX8M
@@ -992,6 +993,49 @@ static const struct media_device_ops hantro_m2m_media_ops = {
 	.req_queue = v4l2_m2m_request_queue,
 };
 
+/*
+ * Some SoCs, like RK3588 have multiple identical Hantro cores, but the
+ * kernel is currently missing support for multi-core handling. Exposing
+ * separate devices for each core to userspace is bad, since that does
+ * not allow scheduling tasks properly (and creates ABI). With this workaround
+ * the driver will only probe for the first core and early exit for the other
+ * cores. Once the driver gains multi-core support, the same technique
+ * for detecting the main core can be used to cluster all cores together.
+ */
+static int hantro_disable_multicore(struct hantro_dev *vpu)
+{
+	struct device_node *node = NULL;
+	const char *compatible;
+	bool is_main_core;
+	int ret;
+
+	/* Intentionally ignores the fallback strings */
+	ret = of_property_read_string(vpu->dev->of_node, "compatible", &compatible);
+	if (ret)
+		return ret;
+
+	/* The first compatible and available node found is considered the main core */
+	do {
+		node = of_find_compatible_node(node, NULL, compatible);
+		if (of_device_is_available(node))
+			break;
+	} while (node);
+
+	if (!node)
+		return -EINVAL;
+
+	is_main_core = (vpu->dev->of_node == node);
+
+	of_node_put(node);
+
+	if (!is_main_core) {
+		dev_info(vpu->dev, "missing multi-core support, ignoring this instance\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
 static int hantro_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match;
@@ -1011,6 +1055,10 @@ static int hantro_probe(struct platform_device *pdev)
 	match = of_match_node(of_hantro_match, pdev->dev.of_node);
 	vpu->variant = match->data;
 
+	ret = hantro_disable_multicore(vpu);
+	if (ret)
+		return ret;
+
 	/*
 	 * Support for nxp,imx8mq-vpu is kept for backwards compatibility
 	 * but it's deprecated. Please update your DTS file to use
@@ -1229,7 +1277,7 @@ static const struct dev_pm_ops hantro_pm_ops = {
 
 static struct platform_driver hantro_driver = {
 	.probe = hantro_probe,
-	.remove_new = hantro_remove,
+	.remove = hantro_remove,
 	.driver = {
 		   .name = DRIVER_NAME,
 		   .of_match_table = of_hantro_match,
diff --git a/drivers/media/platform/verisilicon/hantro_g1_mpeg2_dec.c b/drivers/media/platform/verisilicon/hantro_g1_mpeg2_dec.c
index 9aea331e1a3c..e0d6bd0a6e44 100644
--- a/drivers/media/platform/verisilicon/hantro_g1_mpeg2_dec.c
+++ b/drivers/media/platform/verisilicon/hantro_g1_mpeg2_dec.c
@@ -5,7 +5,7 @@
  * Copyright (C) 2018 Rockchip Electronics Co., Ltd.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/bitfield.h>
 #include <media/v4l2-mem2mem.h>
 #include "hantro.h"
diff --git a/drivers/media/platform/verisilicon/hantro_g2.c b/drivers/media/platform/verisilicon/hantro_g2.c
index b880a6849d58..aae0b562fabb 100644
--- a/drivers/media/platform/verisilicon/hantro_g2.c
+++ b/drivers/media/platform/verisilicon/hantro_g2.c
@@ -47,7 +47,7 @@ irqreturn_t hantro_g2_irq(int irq, void *dev_id)
 
 size_t hantro_g2_chroma_offset(struct hantro_ctx *ctx)
 {
-	return ctx->dst_fmt.width * ctx->dst_fmt.height * ctx->bit_depth / 8;
+	return ctx->ref_fmt.plane_fmt[0].bytesperline *	ctx->ref_fmt.height;
 }
 
 size_t hantro_g2_motion_vectors_offset(struct hantro_ctx *ctx)
@@ -56,3 +56,32 @@ size_t hantro_g2_motion_vectors_offset(struct hantro_ctx *ctx)
 
 	return ALIGN((cr_offset * 3) / 2, G2_ALIGN);
 }
+
+static size_t hantro_g2_mv_size(struct hantro_ctx *ctx)
+{
+	const struct hantro_hevc_dec_ctrls *ctrls = &ctx->hevc_dec.ctrls;
+	const struct v4l2_ctrl_hevc_sps *sps = ctrls->sps;
+	unsigned int pic_width_in_ctbs, pic_height_in_ctbs;
+	unsigned int max_log2_ctb_size;
+
+	max_log2_ctb_size = sps->log2_min_luma_coding_block_size_minus3 + 3 +
+			    sps->log2_diff_max_min_luma_coding_block_size;
+	pic_width_in_ctbs = (sps->pic_width_in_luma_samples +
+			    (1 << max_log2_ctb_size) - 1) >> max_log2_ctb_size;
+	pic_height_in_ctbs = (sps->pic_height_in_luma_samples + (1 << max_log2_ctb_size) - 1)
+			     >> max_log2_ctb_size;
+
+	return pic_width_in_ctbs * pic_height_in_ctbs * (1 << (2 * (max_log2_ctb_size - 4))) * 16;
+}
+
+size_t hantro_g2_luma_compress_offset(struct hantro_ctx *ctx)
+{
+	return hantro_g2_motion_vectors_offset(ctx) +
+	       hantro_g2_mv_size(ctx);
+}
+
+size_t hantro_g2_chroma_compress_offset(struct hantro_ctx *ctx)
+{
+	return hantro_g2_luma_compress_offset(ctx) +
+	       hantro_hevc_luma_compressed_size(ctx->dst_fmt.width, ctx->dst_fmt.height);
+}
diff --git a/drivers/media/platform/verisilicon/hantro_g2_hevc_dec.c b/drivers/media/platform/verisilicon/hantro_g2_hevc_dec.c
index d3f8c33eb16c..0e212198dd65 100644
--- a/drivers/media/platform/verisilicon/hantro_g2_hevc_dec.c
+++ b/drivers/media/platform/verisilicon/hantro_g2_hevc_dec.c
@@ -367,11 +367,14 @@ static int set_ref(struct hantro_ctx *ctx)
 	const struct v4l2_ctrl_hevc_decode_params *decode_params = ctrls->decode_params;
 	const struct v4l2_hevc_dpb_entry *dpb = decode_params->dpb;
 	dma_addr_t luma_addr, chroma_addr, mv_addr = 0;
+	dma_addr_t compress_luma_addr, compress_chroma_addr = 0;
 	struct hantro_dev *vpu = ctx->dev;
 	struct vb2_v4l2_buffer *vb2_dst;
 	struct hantro_decoded_buffer *dst;
 	size_t cr_offset = hantro_g2_chroma_offset(ctx);
 	size_t mv_offset = hantro_g2_motion_vectors_offset(ctx);
+	size_t compress_luma_offset = hantro_g2_luma_compress_offset(ctx);
+	size_t compress_chroma_offset = hantro_g2_chroma_compress_offset(ctx);
 	u32 max_ref_frames;
 	u16 dpb_longterm_e;
 	static const struct hantro_reg cur_poc[] = {
@@ -445,6 +448,8 @@ static int set_ref(struct hantro_ctx *ctx)
 
 		chroma_addr = luma_addr + cr_offset;
 		mv_addr = luma_addr + mv_offset;
+		compress_luma_addr = luma_addr + compress_luma_offset;
+		compress_chroma_addr = luma_addr + compress_chroma_offset;
 
 		if (dpb[i].flags & V4L2_HEVC_DPB_ENTRY_LONG_TERM_REFERENCE)
 			dpb_longterm_e |= BIT(V4L2_HEVC_DPB_ENTRIES_NUM_MAX - 1 - i);
@@ -452,6 +457,8 @@ static int set_ref(struct hantro_ctx *ctx)
 		hantro_write_addr(vpu, G2_REF_LUMA_ADDR(i), luma_addr);
 		hantro_write_addr(vpu, G2_REF_CHROMA_ADDR(i), chroma_addr);
 		hantro_write_addr(vpu, G2_REF_MV_ADDR(i), mv_addr);
+		hantro_write_addr(vpu, G2_REF_COMP_LUMA_ADDR(i), compress_luma_addr);
+		hantro_write_addr(vpu, G2_REF_COMP_CHROMA_ADDR(i), compress_chroma_addr);
 	}
 
 	vb2_dst = hantro_get_dst_buf(ctx);
@@ -465,19 +472,27 @@ static int set_ref(struct hantro_ctx *ctx)
 
 	chroma_addr = luma_addr + cr_offset;
 	mv_addr = luma_addr + mv_offset;
+	compress_luma_addr = luma_addr + compress_luma_offset;
+	compress_chroma_addr = luma_addr + compress_chroma_offset;
 
 	hantro_write_addr(vpu, G2_REF_LUMA_ADDR(i), luma_addr);
 	hantro_write_addr(vpu, G2_REF_CHROMA_ADDR(i), chroma_addr);
-	hantro_write_addr(vpu, G2_REF_MV_ADDR(i++), mv_addr);
+	hantro_write_addr(vpu, G2_REF_MV_ADDR(i), mv_addr);
+	hantro_write_addr(vpu, G2_REF_COMP_LUMA_ADDR(i), compress_luma_addr);
+	hantro_write_addr(vpu, G2_REF_COMP_CHROMA_ADDR(i++), compress_chroma_addr);
 
 	hantro_write_addr(vpu, G2_OUT_LUMA_ADDR, luma_addr);
 	hantro_write_addr(vpu, G2_OUT_CHROMA_ADDR, chroma_addr);
 	hantro_write_addr(vpu, G2_OUT_MV_ADDR, mv_addr);
+	hantro_write_addr(vpu, G2_OUT_COMP_LUMA_ADDR, compress_luma_addr);
+	hantro_write_addr(vpu, G2_OUT_COMP_CHROMA_ADDR, compress_chroma_addr);
 
 	for (; i < V4L2_HEVC_DPB_ENTRIES_NUM_MAX; i++) {
 		hantro_write_addr(vpu, G2_REF_LUMA_ADDR(i), 0);
 		hantro_write_addr(vpu, G2_REF_CHROMA_ADDR(i), 0);
 		hantro_write_addr(vpu, G2_REF_MV_ADDR(i), 0);
+		hantro_write_addr(vpu, G2_REF_COMP_LUMA_ADDR(i), 0);
+		hantro_write_addr(vpu, G2_REF_COMP_CHROMA_ADDR(i), 0);
 	}
 
 	hantro_reg_write(vpu, &g2_refer_lterm_e, dpb_longterm_e);
@@ -503,6 +518,7 @@ static void set_buffers(struct hantro_ctx *ctx)
 	hantro_reg_write(vpu, &g2_stream_len, src_len);
 	hantro_reg_write(vpu, &g2_strm_buffer_len, src_buf_len);
 	hantro_reg_write(vpu, &g2_strm_start_offset, 0);
+	hantro_reg_write(vpu, &g2_start_bit, 0);
 	hantro_reg_write(vpu, &g2_write_mvs_e, 1);
 
 	hantro_write_addr(vpu, G2_TILE_SIZES_ADDR, ctx->hevc_dec.tile_sizes.dma);
@@ -594,8 +610,7 @@ int hantro_g2_hevc_dec_run(struct hantro_ctx *ctx)
 	/* Don't disable output */
 	hantro_reg_write(vpu, &g2_out_dis, 0);
 
-	/* Don't compress buffers */
-	hantro_reg_write(vpu, &g2_ref_compress_bypass, 1);
+	hantro_reg_write(vpu, &g2_ref_compress_bypass, !ctx->hevc_dec.use_compression);
 
 	/* Bus width and max burst */
 	hantro_reg_write(vpu, &g2_buswidth, BUS_WIDTH_128);
diff --git a/drivers/media/platform/verisilicon/hantro_g2_regs.h b/drivers/media/platform/verisilicon/hantro_g2_regs.h
index 82606783591a..b943b1816db7 100644
--- a/drivers/media/platform/verisilicon/hantro_g2_regs.h
+++ b/drivers/media/platform/verisilicon/hantro_g2_regs.h
@@ -318,6 +318,10 @@
 #define G2_TILE_BSD_ADDR		(G2_SWREG(183))
 #define G2_DS_DST			(G2_SWREG(186))
 #define G2_DS_DST_CHR			(G2_SWREG(188))
+#define G2_OUT_COMP_LUMA_ADDR		(G2_SWREG(190))
+#define G2_REF_COMP_LUMA_ADDR(i)	(G2_SWREG(192) + ((i) * 0x8))
+#define G2_OUT_COMP_CHROMA_ADDR		(G2_SWREG(224))
+#define G2_REF_COMP_CHROMA_ADDR(i)	(G2_SWREG(226) + ((i) * 0x8))
 
 #define g2_strm_buffer_len	G2_DEC_REG(258, 0, 0xffffffff)
 #define g2_strm_start_offset	G2_DEC_REG(259, 0, 0xffffffff)
diff --git a/drivers/media/platform/verisilicon/hantro_g2_vp9_dec.c b/drivers/media/platform/verisilicon/hantro_g2_vp9_dec.c
index 342e543dee4c..82a478ac645e 100644
--- a/drivers/media/platform/verisilicon/hantro_g2_vp9_dec.c
+++ b/drivers/media/platform/verisilicon/hantro_g2_vp9_dec.c
@@ -776,15 +776,15 @@ config_source(struct hantro_ctx *ctx, const struct v4l2_ctrl_vp9_frame *dec_para
 	      struct vb2_v4l2_buffer *vb2_src)
 {
 	dma_addr_t stream_base, tmp_addr;
-	unsigned int headres_size;
+	unsigned int headers_size;
 	u32 src_len, start_bit, src_buf_len;
 
-	headres_size = dec_params->uncompressed_header_size
+	headers_size = dec_params->uncompressed_header_size
 		     + dec_params->compressed_header_size;
 
 	stream_base = vb2_dma_contig_plane_dma_addr(&vb2_src->vb2_buf, 0);
 
-	tmp_addr = stream_base + headres_size;
+	tmp_addr = stream_base + headers_size;
 	if (ctx->dev->variant->legacy_regs)
 		hantro_write_addr(ctx->dev, G2_STREAM_ADDR, (tmp_addr & ~0xf));
 	else
@@ -794,7 +794,7 @@ config_source(struct hantro_ctx *ctx, const struct v4l2_ctrl_vp9_frame *dec_para
 	hantro_reg_write(ctx->dev, &g2_start_bit, start_bit);
 
 	src_len = vb2_get_plane_payload(&vb2_src->vb2_buf, 0);
-	src_len += start_bit / 8 - headres_size;
+	src_len += start_bit / 8 - headers_size;
 	hantro_reg_write(ctx->dev, &g2_stream_len, src_len);
 
 	if (!ctx->dev->variant->legacy_regs) {
diff --git a/drivers/media/platform/verisilicon/hantro_h1_jpeg_enc.c b/drivers/media/platform/verisilicon/hantro_h1_jpeg_enc.c
index 12d69503d6ba..86cc1a07026f 100644
--- a/drivers/media/platform/verisilicon/hantro_h1_jpeg_enc.c
+++ b/drivers/media/platform/verisilicon/hantro_h1_jpeg_enc.c
@@ -5,7 +5,7 @@
  * Copyright (C) 2018 Rockchip Electronics Co., Ltd.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <media/v4l2-mem2mem.h>
 #include "hantro_jpeg.h"
 #include "hantro.h"
diff --git a/drivers/media/platform/verisilicon/hantro_h1_regs.h b/drivers/media/platform/verisilicon/hantro_h1_regs.h
index 30e7e7b920b5..8650cc489392 100644
--- a/drivers/media/platform/verisilicon/hantro_h1_regs.h
+++ b/drivers/media/platform/verisilicon/hantro_h1_regs.h
@@ -62,7 +62,7 @@
 #define    H1_REG_ENC_CTRL1_INTRA_PRED_MODE(x)		((x) << 16)
 #define    H1_REG_ENC_CTRL1_FRAME_NUM(x)		((x))
 #define H1_REG_ENC_CTRL2				0x048
-#define    H1_REG_ENC_CTRL2_DEBLOCKING_FILETER_MODE(x)	((x) << 30)
+#define    H1_REG_ENC_CTRL2_DEBLOCKING_FILTER_MODE(x)	((x) << 30)
 #define    H1_REG_ENC_CTRL2_H264_SLICE_SIZE(x)		((x) << 23)
 #define    H1_REG_ENC_CTRL2_DISABLE_QUARTER_PIXMV	BIT(22)
 #define    H1_REG_ENC_CTRL2_TRANS8X8_MODE_EN		BIT(21)
@@ -89,7 +89,7 @@
 #define H1_REG_STR_BUF_LIMIT				0x060
 #define H1_REG_MAD_CTRL					0x064
 #define    H1_REG_MAD_CTRL_QP_ADJUST(x)			((x) << 28)
-#define    H1_REG_MAD_CTRL_MAD_THREDHOLD(x)		((x) << 22)
+#define    H1_REG_MAD_CTRL_MAD_THRESHOLD(x)		((x) << 22)
 #define    H1_REG_MAD_CTRL_QP_SUM_DIV2(x)		((x))
 #define H1_REG_ADDR_VP8_PROB_CNT			0x068
 #define H1_REG_QP_VAL					0x06c
diff --git a/drivers/media/platform/verisilicon/hantro_hevc.c b/drivers/media/platform/verisilicon/hantro_hevc.c
index 2c14330bc562..83cd12b0ddd6 100644
--- a/drivers/media/platform/verisilicon/hantro_hevc.c
+++ b/drivers/media/platform/verisilicon/hantro_hevc.c
@@ -25,6 +25,11 @@
 #define MAX_TILE_COLS 20
 #define MAX_TILE_ROWS 22
 
+static bool hevc_use_compression = IS_ENABLED(CONFIG_VIDEO_HANTRO_HEVC_RFC);
+module_param_named(hevc_use_compression, hevc_use_compression, bool, 0644);
+MODULE_PARM_DESC(hevc_use_compression,
+		 "Use reference frame compression for HEVC");
+
 void hantro_hevc_ref_init(struct hantro_ctx *ctx)
 {
 	struct hantro_hevc_dec_hw_ctx *hevc_dec = &ctx->hevc_dec;
@@ -275,5 +280,8 @@ int hantro_hevc_dec_init(struct hantro_ctx *ctx)
 
 	hantro_hevc_ref_init(ctx);
 
+	hevc_dec->use_compression =
+		hevc_use_compression & hantro_needs_postproc(ctx, ctx->vpu_dst_fmt);
+
 	return 0;
 }
diff --git a/drivers/media/platform/verisilicon/hantro_hw.h b/drivers/media/platform/verisilicon/hantro_hw.h
index 7737320cc8cc..c9b6556f8b2b 100644
--- a/drivers/media/platform/verisilicon/hantro_hw.h
+++ b/drivers/media/platform/verisilicon/hantro_hw.h
@@ -42,6 +42,13 @@
 
 #define MAX_POSTPROC_BUFFERS	64
 
+#define CBS_SIZE	16	/* compression table size in bytes */
+#define CBS_LUMA	8	/* luminance CBS is composed of 1 8x8 coded block */
+#define CBS_CHROMA_W	(8 * 2)	/* chrominance CBS is composed of two 8x4 coded
+				 * blocks, with Cb CB first then Cr CB following
+				 */
+#define CBS_CHROMA_H	4
+
 struct hantro_dev;
 struct hantro_ctx;
 struct hantro_buf;
@@ -144,6 +151,7 @@ struct hantro_hevc_dec_ctrls {
  * @ref_bufs_used:	Bitfield of used reference buffers
  * @ctrls:		V4L2 controls attached to a run
  * @num_tile_cols_allocated: number of allocated tiles
+ * @use_compression:	use reference buffer compression
  */
 struct hantro_hevc_dec_hw_ctx {
 	struct hantro_aux_buf tile_sizes;
@@ -156,6 +164,7 @@ struct hantro_hevc_dec_hw_ctx {
 	u32 ref_bufs_used;
 	struct hantro_hevc_dec_ctrls ctrls;
 	unsigned int num_tile_cols_allocated;
+	bool use_compression;
 };
 
 /**
@@ -510,6 +519,33 @@ hantro_hevc_mv_size(unsigned int width, unsigned int height)
 	return width * height / 16;
 }
 
+static inline size_t
+hantro_hevc_luma_compressed_size(unsigned int width, unsigned int height)
+{
+	u32 pic_width_in_cbsy =
+		round_up((width + CBS_LUMA - 1) / CBS_LUMA, CBS_SIZE);
+	u32 pic_height_in_cbsy = (height + CBS_LUMA - 1) / CBS_LUMA;
+
+	return round_up(pic_width_in_cbsy * pic_height_in_cbsy, CBS_SIZE);
+}
+
+static inline size_t
+hantro_hevc_chroma_compressed_size(unsigned int width, unsigned int height)
+{
+	u32 pic_width_in_cbsc =
+		round_up((width + CBS_CHROMA_W - 1) / CBS_CHROMA_W, CBS_SIZE);
+	u32 pic_height_in_cbsc = (height / 2 + CBS_CHROMA_H - 1) / CBS_CHROMA_H;
+
+	return round_up(pic_width_in_cbsc * pic_height_in_cbsc, CBS_SIZE);
+}
+
+static inline size_t
+hantro_hevc_compressed_size(unsigned int width, unsigned int height)
+{
+	return hantro_hevc_luma_compressed_size(width, height) +
+	       hantro_hevc_chroma_compressed_size(width, height);
+}
+
 static inline unsigned short hantro_av1_num_sbs(unsigned short dimension)
 {
 	return DIV_ROUND_UP(dimension, 64);
@@ -525,6 +561,8 @@ hantro_av1_mv_size(unsigned int width, unsigned int height)
 
 size_t hantro_g2_chroma_offset(struct hantro_ctx *ctx);
 size_t hantro_g2_motion_vectors_offset(struct hantro_ctx *ctx);
+size_t hantro_g2_luma_compress_offset(struct hantro_ctx *ctx);
+size_t hantro_g2_chroma_compress_offset(struct hantro_ctx *ctx);
 
 int hantro_g1_mpeg2_dec_run(struct hantro_ctx *ctx);
 int rockchip_vpu2_mpeg2_dec_run(struct hantro_ctx *ctx);
diff --git a/drivers/media/platform/verisilicon/hantro_jpeg.c b/drivers/media/platform/verisilicon/hantro_jpeg.c
index d07b1b449b61..13a60638e43f 100644
--- a/drivers/media/platform/verisilicon/hantro_jpeg.c
+++ b/drivers/media/platform/verisilicon/hantro_jpeg.c
@@ -11,6 +11,7 @@
 #include <linux/build_bug.h>
 #include <linux/kernel.h>
 #include <linux/string.h>
+#include <media/v4l2-jpeg.h>
 #include "hantro_jpeg.h"
 #include "hantro.h"
 
@@ -24,42 +25,6 @@
 #define HUFF_CHROMA_DC_OFF	394
 #define HUFF_CHROMA_AC_OFF	427
 
-/* Default tables from JPEG ITU-T.81
- * (ISO/IEC 10918-1) Annex K, tables K.1 and K.2
- */
-static const unsigned char luma_q_table[] = {
-	0x10, 0x0b, 0x0a, 0x10, 0x18, 0x28, 0x33, 0x3d,
-	0x0c, 0x0c, 0x0e, 0x13, 0x1a, 0x3a, 0x3c, 0x37,
-	0x0e, 0x0d, 0x10, 0x18, 0x28, 0x39, 0x45, 0x38,
-	0x0e, 0x11, 0x16, 0x1d, 0x33, 0x57, 0x50, 0x3e,
-	0x12, 0x16, 0x25, 0x38, 0x44, 0x6d, 0x67, 0x4d,
-	0x18, 0x23, 0x37, 0x40, 0x51, 0x68, 0x71, 0x5c,
-	0x31, 0x40, 0x4e, 0x57, 0x67, 0x79, 0x78, 0x65,
-	0x48, 0x5c, 0x5f, 0x62, 0x70, 0x64, 0x67, 0x63
-};
-
-static const unsigned char chroma_q_table[] = {
-	0x11, 0x12, 0x18, 0x2f, 0x63, 0x63, 0x63, 0x63,
-	0x12, 0x15, 0x1a, 0x42, 0x63, 0x63, 0x63, 0x63,
-	0x18, 0x1a, 0x38, 0x63, 0x63, 0x63, 0x63, 0x63,
-	0x2f, 0x42, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
-	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
-	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
-	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
-	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63
-};
-
-static const unsigned char zigzag[] = {
-	 0,  1,  8, 16,  9,  2,  3, 10,
-	17, 24, 32, 25, 18, 11,  4,  5,
-	12, 19, 26, 33, 40, 48, 41, 34,
-	27, 20, 13,  6,  7, 14, 21, 28,
-	35, 42, 49, 56, 57, 50, 43, 36,
-	29, 22, 15, 23, 30, 37, 44, 51,
-	58, 59, 52, 45, 38, 31, 39, 46,
-	53, 60, 61, 54, 47, 55, 62, 63
-};
-
 static const u32 hw_reorder[] = {
 	 0,  8, 16, 24,  1,  9, 17, 25,
 	32, 40, 48, 56, 33, 41, 49, 57,
@@ -71,73 +36,6 @@ static const u32 hw_reorder[] = {
 	38, 46, 54, 62, 39, 47, 55, 63
 };
 
-/* Huffman tables are shared with CODA */
-static const unsigned char luma_dc_table[] = {
-	0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01,
-	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	0x08, 0x09, 0x0a, 0x0b,
-};
-
-static const unsigned char chroma_dc_table[] = {
-	0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
-	0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
-	0x08, 0x09, 0x0a, 0x0b,
-};
-
-static const unsigned char luma_ac_table[] = {
-	0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03,
-	0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7d,
-	0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
-	0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
-	0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
-	0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
-	0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
-	0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
-	0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
-	0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
-	0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
-	0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
-	0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
-	0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
-	0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
-	0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
-	0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
-	0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
-	0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
-	0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
-	0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
-	0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
-	0xf9, 0xfa,
-};
-
-static const unsigned char chroma_ac_table[] = {
-	0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04,
-	0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77,
-	0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21,
-	0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71,
-	0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
-	0xa1, 0xb1, 0xc1, 0x09, 0x23, 0x33, 0x52, 0xf0,
-	0x15, 0x62, 0x72, 0xd1, 0x0a, 0x16, 0x24, 0x34,
-	0xe1, 0x25, 0xf1, 0x17, 0x18, 0x19, 0x1a, 0x26,
-	0x27, 0x28, 0x29, 0x2a, 0x35, 0x36, 0x37, 0x38,
-	0x39, 0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48,
-	0x49, 0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
-	0x59, 0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
-	0x69, 0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78,
-	0x79, 0x7a, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87,
-	0x88, 0x89, 0x8a, 0x92, 0x93, 0x94, 0x95, 0x96,
-	0x97, 0x98, 0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5,
-	0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4,
-	0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3,
-	0xc4, 0xc5, 0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2,
-	0xd3, 0xd4, 0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda,
-	0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9,
-	0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
-	0xf9, 0xfa,
-};
-
 /* For simplicity, we keep a pre-formatted JPEG header,
  * and we'll use fixed offsets to change the width, height
  * quantization tables, etc.
@@ -292,11 +190,11 @@ jpeg_scale_quant_table(unsigned char *file_q_tab,
 {
 	int i;
 
-	BUILD_BUG_ON(ARRAY_SIZE(zigzag) != JPEG_QUANT_SIZE);
+	BUILD_BUG_ON(ARRAY_SIZE(v4l2_jpeg_zigzag_scan_index) != JPEG_QUANT_SIZE);
 	BUILD_BUG_ON(ARRAY_SIZE(hw_reorder) != JPEG_QUANT_SIZE);
 
 	for (i = 0; i < JPEG_QUANT_SIZE; i++) {
-		file_q_tab[i] = jpeg_scale_qp(tab[zigzag[i]], scale);
+		file_q_tab[i] = jpeg_scale_qp(tab[v4l2_jpeg_zigzag_scan_index[i]], scale);
 		reordered_q_tab[i] = jpeg_scale_qp(tab[hw_reorder[i]], scale);
 	}
 }
@@ -304,7 +202,6 @@ jpeg_scale_quant_table(unsigned char *file_q_tab,
 static void jpeg_set_quality(struct hantro_jpeg_ctx *ctx)
 {
 	int scale;
-
 	/*
 	 * Non-linear scaling factor:
 	 * [5,50] -> [1000..100], [51,100] -> [98..0]
@@ -314,15 +211,17 @@ static void jpeg_set_quality(struct hantro_jpeg_ctx *ctx)
 	else
 		scale = 200 - 2 * ctx->quality;
 
-	BUILD_BUG_ON(ARRAY_SIZE(luma_q_table) != JPEG_QUANT_SIZE);
-	BUILD_BUG_ON(ARRAY_SIZE(chroma_q_table) != JPEG_QUANT_SIZE);
+	BUILD_BUG_ON(ARRAY_SIZE(v4l2_jpeg_ref_table_luma_qt) != JPEG_QUANT_SIZE);
+	BUILD_BUG_ON(ARRAY_SIZE(v4l2_jpeg_ref_table_chroma_qt) != JPEG_QUANT_SIZE);
 	BUILD_BUG_ON(ARRAY_SIZE(ctx->hw_luma_qtable) != JPEG_QUANT_SIZE);
 	BUILD_BUG_ON(ARRAY_SIZE(ctx->hw_chroma_qtable) != JPEG_QUANT_SIZE);
 
 	jpeg_scale_quant_table(ctx->buffer + LUMA_QUANT_OFF,
-			       ctx->hw_luma_qtable, luma_q_table, scale);
+			       ctx->hw_luma_qtable,
+			       (const unsigned char *)v4l2_jpeg_ref_table_luma_qt, scale);
 	jpeg_scale_quant_table(ctx->buffer + CHROMA_QUANT_OFF,
-			       ctx->hw_chroma_qtable, chroma_q_table, scale);
+			       ctx->hw_chroma_qtable,
+			       (const unsigned char *)v4l2_jpeg_ref_table_chroma_qt, scale);
 }
 
 void hantro_jpeg_header_assemble(struct hantro_jpeg_ctx *ctx)
@@ -337,12 +236,10 @@ void hantro_jpeg_header_assemble(struct hantro_jpeg_ctx *ctx)
 	buf[WIDTH_OFF + 0] = ctx->width >> 8;
 	buf[WIDTH_OFF + 1] = ctx->width;
 
-	memcpy(buf + HUFF_LUMA_DC_OFF, luma_dc_table, sizeof(luma_dc_table));
-	memcpy(buf + HUFF_LUMA_AC_OFF, luma_ac_table, sizeof(luma_ac_table));
-	memcpy(buf + HUFF_CHROMA_DC_OFF, chroma_dc_table,
-	       sizeof(chroma_dc_table));
-	memcpy(buf + HUFF_CHROMA_AC_OFF, chroma_ac_table,
-	       sizeof(chroma_ac_table));
+	memcpy(buf + HUFF_LUMA_DC_OFF, v4l2_jpeg_ref_table_luma_dc_ht, V4L2_JPEG_REF_HT_DC_LEN);
+	memcpy(buf + HUFF_LUMA_AC_OFF, v4l2_jpeg_ref_table_luma_ac_ht, V4L2_JPEG_REF_HT_AC_LEN);
+	memcpy(buf + HUFF_CHROMA_DC_OFF, v4l2_jpeg_ref_table_chroma_dc_ht, V4L2_JPEG_REF_HT_DC_LEN);
+	memcpy(buf + HUFF_CHROMA_AC_OFF, v4l2_jpeg_ref_table_chroma_ac_ht, V4L2_JPEG_REF_HT_AC_LEN);
 
 	jpeg_set_quality(ctx);
 }
diff --git a/drivers/media/platform/verisilicon/hantro_postproc.c b/drivers/media/platform/verisilicon/hantro_postproc.c
index 41e93176300b..9f559a13d409 100644
--- a/drivers/media/platform/verisilicon/hantro_postproc.c
+++ b/drivers/media/platform/verisilicon/hantro_postproc.c
@@ -194,31 +194,25 @@ void hantro_postproc_free(struct hantro_ctx *ctx)
 
 static unsigned int hantro_postproc_buffer_size(struct hantro_ctx *ctx)
 {
-	struct v4l2_pix_format_mplane pix_mp;
-	const struct hantro_fmt *fmt;
 	unsigned int buf_size;
 
-	/* this should always pick native format */
-	fmt = hantro_get_default_fmt(ctx, false, ctx->bit_depth, HANTRO_AUTO_POSTPROC);
-	if (!fmt)
-		return 0;
-
-	v4l2_fill_pixfmt_mp(&pix_mp, fmt->fourcc, ctx->src_fmt.width,
-			    ctx->src_fmt.height);
-
-	buf_size = pix_mp.plane_fmt[0].sizeimage;
+	buf_size = ctx->ref_fmt.plane_fmt[0].sizeimage;
 	if (ctx->vpu_src_fmt->fourcc == V4L2_PIX_FMT_H264_SLICE)
-		buf_size += hantro_h264_mv_size(pix_mp.width,
-						pix_mp.height);
+		buf_size += hantro_h264_mv_size(ctx->ref_fmt.width,
+						ctx->ref_fmt.height);
 	else if (ctx->vpu_src_fmt->fourcc == V4L2_PIX_FMT_VP9_FRAME)
-		buf_size += hantro_vp9_mv_size(pix_mp.width,
-					       pix_mp.height);
-	else if (ctx->vpu_src_fmt->fourcc == V4L2_PIX_FMT_HEVC_SLICE)
-		buf_size += hantro_hevc_mv_size(pix_mp.width,
-						pix_mp.height);
+		buf_size += hantro_vp9_mv_size(ctx->ref_fmt.width,
+					       ctx->ref_fmt.height);
+	else if (ctx->vpu_src_fmt->fourcc == V4L2_PIX_FMT_HEVC_SLICE) {
+		buf_size += hantro_hevc_mv_size(ctx->ref_fmt.width,
+						ctx->ref_fmt.height);
+		if (ctx->hevc_dec.use_compression)
+			buf_size += hantro_hevc_compressed_size(ctx->ref_fmt.width,
+								ctx->ref_fmt.height);
+	}
 	else if (ctx->vpu_src_fmt->fourcc == V4L2_PIX_FMT_AV1_FRAME)
-		buf_size += hantro_av1_mv_size(pix_mp.width,
-					       pix_mp.height);
+		buf_size += hantro_av1_mv_size(ctx->ref_fmt.width,
+					       ctx->ref_fmt.height);
 
 	return buf_size;
 }
@@ -256,8 +250,10 @@ int hantro_postproc_init(struct hantro_ctx *ctx)
 
 	for (i = 0; i < num_buffers; i++) {
 		ret = hantro_postproc_alloc(ctx, i);
-		if (ret)
+		if (ret) {
+			hantro_postproc_free(ctx);
 			return ret;
+		}
 	}
 
 	return 0;
diff --git a/drivers/media/platform/verisilicon/hantro_v4l2.c b/drivers/media/platform/verisilicon/hantro_v4l2.c
index 941fa23c211a..7c3515cf7d64 100644
--- a/drivers/media/platform/verisilicon/hantro_v4l2.c
+++ b/drivers/media/platform/verisilicon/hantro_v4l2.c
@@ -77,6 +77,7 @@ int hantro_get_format_depth(u32 fourcc)
 	switch (fourcc) {
 	case V4L2_PIX_FMT_P010:
 	case V4L2_PIX_FMT_P010_4L4:
+	case V4L2_PIX_FMT_NV15:
 	case V4L2_PIX_FMT_NV15_4L4:
 		return 10;
 	default:
@@ -126,6 +127,24 @@ hantro_find_format(const struct hantro_ctx *ctx, u32 fourcc)
 	return NULL;
 }
 
+static int
+hantro_set_reference_frames_format(struct hantro_ctx *ctx)
+{
+	const struct hantro_fmt *fmt;
+	int dst_bit_depth = hantro_get_format_depth(ctx->vpu_dst_fmt->fourcc);
+
+	fmt = hantro_get_default_fmt(ctx, false, dst_bit_depth, HANTRO_AUTO_POSTPROC);
+	if (!fmt)
+		return -EINVAL;
+
+	ctx->ref_fmt.width = ctx->src_fmt.width;
+	ctx->ref_fmt.height = ctx->src_fmt.height;
+
+	v4l2_apply_frmsize_constraints(&ctx->ref_fmt.width, &ctx->ref_fmt.height, &fmt->frmsize);
+	return v4l2_fill_pixfmt_mp(&ctx->ref_fmt, fmt->fourcc,
+				   ctx->ref_fmt.width, ctx->ref_fmt.height);
+}
+
 const struct hantro_fmt *
 hantro_get_default_fmt(const struct hantro_ctx *ctx, bool bitstream,
 		       int bit_depth, bool need_postproc)
@@ -201,7 +220,15 @@ static int vidioc_enum_fmt(struct file *file, void *priv,
 	struct hantro_ctx *ctx = fh_to_ctx(priv);
 	const struct hantro_fmt *fmt, *formats;
 	unsigned int num_fmts, i, j = 0;
-	bool skip_mode_none;
+	bool skip_mode_none, enum_all_formats;
+	u32 index = f->index & ~V4L2_FMTDESC_FLAG_ENUM_ALL;
+
+	/*
+	 * If the V4L2_FMTDESC_FLAG_ENUM_ALL flag is set, we want to enumerate all
+	 * hardware supported pixel formats
+	 */
+	enum_all_formats = !!(f->index & V4L2_FMTDESC_FLAG_ENUM_ALL);
+	f->index = index;
 
 	/*
 	 * When dealing with an encoder:
@@ -222,9 +249,9 @@ static int vidioc_enum_fmt(struct file *file, void *priv,
 
 		if (skip_mode_none == mode_none)
 			continue;
-		if (!hantro_check_depth_match(fmt, ctx->bit_depth))
+		if (!hantro_check_depth_match(fmt, ctx->bit_depth) && !enum_all_formats)
 			continue;
-		if (j == f->index) {
+		if (j == index) {
 			f->pixelformat = fmt->fourcc;
 			return 0;
 		}
@@ -242,9 +269,9 @@ static int vidioc_enum_fmt(struct file *file, void *priv,
 	for (i = 0; i < num_fmts; i++) {
 		fmt = &formats[i];
 
-		if (!hantro_check_depth_match(fmt, ctx->bit_depth))
+		if (!hantro_check_depth_match(fmt, ctx->bit_depth) && !enum_all_formats)
 			continue;
-		if (j == f->index) {
+		if (j == index) {
 			f->pixelformat = fmt->fourcc;
 			return 0;
 		}
@@ -303,11 +330,7 @@ static int hantro_try_fmt(const struct hantro_ctx *ctx,
 
 	coded = capture == ctx->is_encoder;
 
-	vpu_debug(4, "trying format %c%c%c%c\n",
-		  (pix_mp->pixelformat & 0x7f),
-		  (pix_mp->pixelformat >> 8) & 0x7f,
-		  (pix_mp->pixelformat >> 16) & 0x7f,
-		  (pix_mp->pixelformat >> 24) & 0x7f);
+	vpu_debug(4, "trying format %p4cc\n", &pix_mp->pixelformat);
 
 	fmt = hantro_find_format(ctx, pix_mp->pixelformat);
 	if (!fmt) {
@@ -591,6 +614,9 @@ static int hantro_set_fmt_cap(struct hantro_ctx *ctx,
 
 	ctx->vpu_dst_fmt = hantro_find_format(ctx, pix_mp->pixelformat);
 	ctx->dst_fmt = *pix_mp;
+	ret = hantro_set_reference_frames_format(ctx);
+	if (ret)
+		return ret;
 
 	/*
 	 * Current raw format might have become invalid with newly
@@ -756,6 +782,7 @@ const struct v4l2_ioctl_ops hantro_ioctl_ops = {
 	.vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
 	.vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
 	.vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+	.vidioc_remove_bufs = v4l2_m2m_ioctl_remove_bufs,
 	.vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
 
 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
@@ -999,6 +1026,4 @@ const struct vb2_ops hantro_queue_ops = {
 	.buf_request_complete = hantro_buf_request_complete,
 	.start_streaming = hantro_start_streaming,
 	.stop_streaming = hantro_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
diff --git a/drivers/media/platform/verisilicon/imx8m_vpu_hw.c b/drivers/media/platform/verisilicon/imx8m_vpu_hw.c
index f850d8bddef6..35799da534ed 100644
--- a/drivers/media/platform/verisilicon/imx8m_vpu_hw.c
+++ b/drivers/media/platform/verisilicon/imx8m_vpu_hw.c
@@ -187,23 +187,23 @@ static const struct hantro_fmt imx8m_vpu_g2_dec_fmts[] = {
 		.frmsize = {
 			.min_width = FMT_MIN_WIDTH,
 			.max_width = FMT_UHD_WIDTH,
-			.step_width = TILE_MB_DIM,
+			.step_width = 8,
 			.min_height = FMT_MIN_HEIGHT,
 			.max_height = FMT_UHD_HEIGHT,
-			.step_height = TILE_MB_DIM,
+			.step_height = 32,
 		},
 	},
 	{
-		.fourcc = V4L2_PIX_FMT_P010_4L4,
+		.fourcc = V4L2_PIX_FMT_NV15_4L4,
 		.codec_mode = HANTRO_MODE_NONE,
 		.match_depth = true,
 		.frmsize = {
 			.min_width = FMT_MIN_WIDTH,
 			.max_width = FMT_UHD_WIDTH,
-			.step_width = TILE_MB_DIM,
+			.step_width = 8,
 			.min_height = FMT_MIN_HEIGHT,
 			.max_height = FMT_UHD_HEIGHT,
-			.step_height = TILE_MB_DIM,
+			.step_height = 32,
 		},
 	},
 	{
diff --git a/drivers/media/platform/verisilicon/rockchip_vpu2_hw_jpeg_enc.c b/drivers/media/platform/verisilicon/rockchip_vpu2_hw_jpeg_enc.c
index 8395c4d48dd0..61621b1be8a2 100644
--- a/drivers/media/platform/verisilicon/rockchip_vpu2_hw_jpeg_enc.c
+++ b/drivers/media/platform/verisilicon/rockchip_vpu2_hw_jpeg_enc.c
@@ -22,7 +22,7 @@
  * zigzag, nor linear.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <media/v4l2-mem2mem.h>
 #include "hantro_jpeg.h"
 #include "hantro.h"
diff --git a/drivers/media/platform/verisilicon/rockchip_vpu2_hw_mpeg2_dec.c b/drivers/media/platform/verisilicon/rockchip_vpu2_hw_mpeg2_dec.c
index b66737fab46b..50a3a3eeaa00 100644
--- a/drivers/media/platform/verisilicon/rockchip_vpu2_hw_mpeg2_dec.c
+++ b/drivers/media/platform/verisilicon/rockchip_vpu2_hw_mpeg2_dec.c
@@ -5,7 +5,7 @@
  * Copyright (C) 2018 Rockchip Electronics Co., Ltd.
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/bitfield.h>
 #include <media/v4l2-mem2mem.h>
 #include "hantro.h"
diff --git a/drivers/media/platform/verisilicon/rockchip_vpu981_hw_av1_dec.c b/drivers/media/platform/verisilicon/rockchip_vpu981_hw_av1_dec.c
index cc4483857489..e4703bb6be7c 100644
--- a/drivers/media/platform/verisilicon/rockchip_vpu981_hw_av1_dec.c
+++ b/drivers/media/platform/verisilicon/rockchip_vpu981_hw_av1_dec.c
@@ -161,8 +161,7 @@ static int rockchip_vpu981_av1_dec_frame_ref(struct hantro_ctx *ctx,
 		av1_dec->frame_refs[i].timestamp = timestamp;
 		av1_dec->frame_refs[i].frame_type = frame->frame_type;
 		av1_dec->frame_refs[i].order_hint = frame->order_hint;
-		if (!av1_dec->frame_refs[i].vb2_ref)
-			av1_dec->frame_refs[i].vb2_ref = hantro_get_dst_buf(ctx);
+		av1_dec->frame_refs[i].vb2_ref = hantro_get_dst_buf(ctx);
 
 		for (j = 0; j < V4L2_AV1_TOTAL_REFS_PER_FRAME; j++)
 			av1_dec->frame_refs[i].order_hints[j] = frame->order_hints[j];
@@ -257,7 +256,8 @@ static int rockchip_vpu981_av1_dec_tiles_reallocate(struct hantro_ctx *ctx)
 	struct hantro_dev *vpu = ctx->dev;
 	struct hantro_av1_dec_hw_ctx *av1_dec = &ctx->av1_dec;
 	struct hantro_av1_dec_ctrls *ctrls = &av1_dec->ctrls;
-	unsigned int num_tile_cols = 1 << ctrls->tile_group_entry->tile_col;
+	const struct v4l2_av1_tile_info *tile_info = &ctrls->frame->tile_info;
+	unsigned int num_tile_cols = tile_info->tile_cols;
 	unsigned int height = ALIGN(ctrls->frame->frame_height_minus_1 + 1, 64);
 	unsigned int height_in_sb = height / 64;
 	unsigned int stripe_num = ((height + 8) + 63) / 64;
@@ -686,8 +686,6 @@ rockchip_vpu981_av1_dec_set_ref(struct hantro_ctx *ctx, int ref, int idx,
 	struct hantro_dev *vpu = ctx->dev;
 	struct hantro_decoded_buffer *dst;
 	dma_addr_t luma_addr, chroma_addr, mv_addr = 0;
-	size_t cr_offset = rockchip_vpu981_av1_dec_luma_size(ctx);
-	size_t mv_offset = rockchip_vpu981_av1_dec_chroma_size(ctx);
 	int cur_width = frame->frame_width_minus_1 + 1;
 	int cur_height = frame->frame_height_minus_1 + 1;
 	int scale_width =
@@ -744,8 +742,8 @@ rockchip_vpu981_av1_dec_set_ref(struct hantro_ctx *ctx, int ref, int idx,
 
 	dst = vb2_to_hantro_decoded_buf(&av1_dec->frame_refs[idx].vb2_ref->vb2_buf);
 	luma_addr = hantro_get_dec_buf_addr(ctx, &dst->base.vb.vb2_buf);
-	chroma_addr = luma_addr + cr_offset;
-	mv_addr = luma_addr + mv_offset;
+	chroma_addr = luma_addr + dst->av1.chroma_offset;
+	mv_addr = luma_addr + dst->av1.mv_offset;
 
 	hantro_write_addr(vpu, AV1_REFERENCE_Y(ref), luma_addr);
 	hantro_write_addr(vpu, AV1_REFERENCE_CB(ref), chroma_addr);
@@ -2089,6 +2087,9 @@ rockchip_vpu981_av1_dec_set_output_buffer(struct hantro_ctx *ctx)
 	chroma_addr = luma_addr + cr_offset;
 	mv_addr = luma_addr + mv_offset;
 
+	dst->av1.chroma_offset = cr_offset;
+	dst->av1.mv_offset = mv_offset;
+
 	hantro_write_addr(vpu, AV1_TILE_OUT_LU, luma_addr);
 	hantro_write_addr(vpu, AV1_TILE_OUT_CH, chroma_addr);
 	hantro_write_addr(vpu, AV1_TILE_OUT_MV, mv_addr);
@@ -2201,6 +2202,10 @@ static void rockchip_vpu981_postproc_enable(struct hantro_ctx *ctx)
 	case V4L2_PIX_FMT_NV12:
 		hantro_reg_write(vpu, &av1_pp_out_format, 3);
 		break;
+	case V4L2_PIX_FMT_NV15:
+		/* this mapping is RK specific */
+		hantro_reg_write(vpu, &av1_pp_out_format, 10);
+		break;
 	default:
 		hantro_reg_write(vpu, &av1_pp_out_format, 0);
 	}
diff --git a/drivers/media/platform/verisilicon/rockchip_vpu981_regs.h b/drivers/media/platform/verisilicon/rockchip_vpu981_regs.h
index 850ff0f84424..e4008da64f19 100644
--- a/drivers/media/platform/verisilicon/rockchip_vpu981_regs.h
+++ b/drivers/media/platform/verisilicon/rockchip_vpu981_regs.h
@@ -327,7 +327,7 @@
 
 #define av1_apf_threshold		AV1_DEC_REG(55, 0, 0xffff)
 #define av1_apf_single_pu_mode		AV1_DEC_REG(55, 30, 0x1)
-#define av1_apf_disable			AV1_DEC_REG(55, 30, 0x1)
+#define av1_apf_disable			AV1_DEC_REG(55, 31, 0x1)
 
 #define av1_dec_max_burst		AV1_DEC_REG(58, 0, 0xff)
 #define av1_dec_buswidth		AV1_DEC_REG(58, 8, 0x7)
@@ -337,10 +337,10 @@
 #define av1_dec_mc_polltime		AV1_DEC_REG(58, 17, 0x3ff)
 #define av1_dec_mc_pollmode		AV1_DEC_REG(58,	27, 0x3)
 
-#define av1_filt_ref_adj_3		AV1_DEC_REG(59, 0, 0x3f)
-#define av1_filt_ref_adj_2		AV1_DEC_REG(59, 7, 0x3f)
-#define av1_filt_ref_adj_1		AV1_DEC_REG(59, 14, 0x3f)
-#define av1_filt_ref_adj_0		AV1_DEC_REG(59, 21, 0x3f)
+#define av1_filt_ref_adj_3		AV1_DEC_REG(59, 0, 0x7f)
+#define av1_filt_ref_adj_2		AV1_DEC_REG(59, 7, 0x7f)
+#define av1_filt_ref_adj_1		AV1_DEC_REG(59, 14, 0x7f)
+#define av1_filt_ref_adj_0		AV1_DEC_REG(59, 21, 0x7f)
 #define av1_ref0_sign_bias		AV1_DEC_REG(59, 28, 0x1)
 #define av1_ref1_sign_bias		AV1_DEC_REG(59, 29, 0x1)
 #define av1_ref2_sign_bias		AV1_DEC_REG(59, 30, 0x1)
diff --git a/drivers/media/platform/verisilicon/rockchip_vpu_hw.c b/drivers/media/platform/verisilicon/rockchip_vpu_hw.c
index f97527670783..acd29fa41d2d 100644
--- a/drivers/media/platform/verisilicon/rockchip_vpu_hw.c
+++ b/drivers/media/platform/verisilicon/rockchip_vpu_hw.c
@@ -82,14 +82,27 @@ static const struct hantro_fmt rockchip_vpu981_postproc_fmts[] = {
 	{
 		.fourcc = V4L2_PIX_FMT_NV12,
 		.codec_mode = HANTRO_MODE_NONE,
+		.postprocessed = true,
+		.frmsize = {
+			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
+			.max_width = FMT_4K_WIDTH,
+			.step_width = MB_DIM,
+			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
+			.max_height = FMT_4K_HEIGHT,
+			.step_height = MB_DIM,
+		},
+	},
+	{
+		.fourcc = V4L2_PIX_FMT_NV15,
+		.codec_mode = HANTRO_MODE_NONE,
 		.match_depth = true,
 		.postprocessed = true,
 		.frmsize = {
 			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_width = FMT_UHD_WIDTH,
+			.max_width = FMT_4K_WIDTH,
 			.step_width = MB_DIM,
 			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_height = FMT_UHD_HEIGHT,
+			.max_height = FMT_4K_HEIGHT,
 			.step_height = MB_DIM,
 		},
 	},
@@ -100,10 +113,10 @@ static const struct hantro_fmt rockchip_vpu981_postproc_fmts[] = {
 		.postprocessed = true,
 		.frmsize = {
 			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_width = FMT_UHD_WIDTH,
+			.max_width = FMT_4K_WIDTH,
 			.step_width = MB_DIM,
 			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_height = FMT_UHD_HEIGHT,
+			.max_height = FMT_4K_HEIGHT,
 			.step_height = MB_DIM,
 		},
 	},
@@ -319,10 +332,10 @@ static const struct hantro_fmt rockchip_vpu981_dec_fmts[] = {
 		.match_depth = true,
 		.frmsize = {
 			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_width = FMT_UHD_WIDTH,
+			.max_width = FMT_4K_WIDTH,
 			.step_width = MB_DIM,
 			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_height = FMT_UHD_HEIGHT,
+			.max_height = FMT_4K_HEIGHT,
 			.step_height = MB_DIM,
 		},
 	},
@@ -332,10 +345,10 @@ static const struct hantro_fmt rockchip_vpu981_dec_fmts[] = {
 		.match_depth = true,
 		.frmsize = {
 			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_width = FMT_UHD_WIDTH,
+			.max_width = FMT_4K_WIDTH,
 			.step_width = MB_DIM,
 			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_height = FMT_UHD_HEIGHT,
+			.max_height = FMT_4K_HEIGHT,
 			.step_height = MB_DIM,
 		},
 	},
@@ -345,10 +358,10 @@ static const struct hantro_fmt rockchip_vpu981_dec_fmts[] = {
 		.max_depth = 2,
 		.frmsize = {
 			.min_width = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_width = FMT_UHD_WIDTH,
+			.max_width = FMT_4K_WIDTH,
 			.step_width = MB_DIM,
 			.min_height = ROCKCHIP_VPU981_MIN_SIZE,
-			.max_height = FMT_UHD_HEIGHT,
+			.max_height = FMT_4K_HEIGHT,
 			.step_height = MB_DIM,
 		},
 	},
diff --git a/drivers/media/platform/via/via-camera.c b/drivers/media/platform/via/via-camera.c
index 4cb8f29e2f14..5702eff664d4 100644
--- a/drivers/media/platform/via/via-camera.c
+++ b/drivers/media/platform/via/via-camera.c
@@ -666,8 +666,6 @@ static const struct vb2_ops viacam_vb2_ops = {
 	.buf_prepare		= viacam_vb2_prepare,
 	.start_streaming	= viacam_vb2_start_streaming,
 	.stop_streaming		= viacam_vb2_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* --------------------------------------------------------------------------*/
@@ -1307,7 +1305,7 @@ static struct platform_driver viacam_driver = {
 		.name = "viafb-camera",
 	},
 	.probe = viacam_probe,
-	.remove_new = viacam_remove,
+	.remove = viacam_remove,
 };
 
 module_platform_driver(viacam_driver);
diff --git a/drivers/media/platform/video-mux.c b/drivers/media/platform/video-mux.c
index 31e9e92e723e..cba34893258a 100644
--- a/drivers/media/platform/video-mux.c
+++ b/drivers/media/platform/video-mux.c
@@ -52,6 +52,7 @@ static int video_mux_link_setup(struct media_entity *entity,
 				const struct media_pad *remote, u32 flags)
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
+	struct v4l2_subdev_state *sd_state;
 	struct video_mux *vmux = v4l2_subdev_to_video_mux(sd);
 	u16 source_pad = entity->num_pads - 1;
 	int ret = 0;
@@ -67,10 +68,10 @@ static int video_mux_link_setup(struct media_entity *entity,
 		remote->entity->name, remote->index, local->entity->name,
 		local->index, flags & MEDIA_LNK_FL_ENABLED);
 
+	sd_state = v4l2_subdev_lock_and_get_active_state(sd);
 	mutex_lock(&vmux->lock);
 
 	if (flags & MEDIA_LNK_FL_ENABLED) {
-		struct v4l2_subdev_state *sd_state;
 		struct v4l2_mbus_framefmt *source_mbusformat;
 
 		if (vmux->active == local->index)
@@ -88,12 +89,10 @@ static int video_mux_link_setup(struct media_entity *entity,
 		vmux->active = local->index;
 
 		/* Propagate the active format to the source */
-		sd_state = v4l2_subdev_lock_and_get_active_state(sd);
 		source_mbusformat = v4l2_subdev_state_get_format(sd_state,
 								 source_pad);
 		*source_mbusformat = *v4l2_subdev_state_get_format(sd_state,
 								   vmux->active);
-		v4l2_subdev_unlock_state(sd_state);
 	} else {
 		if (vmux->active != local->index)
 			goto out;
@@ -105,6 +104,7 @@ static int video_mux_link_setup(struct media_entity *entity,
 
 out:
 	mutex_unlock(&vmux->lock);
+	v4l2_subdev_unlock_state(sd_state);
 	return ret;
 }
 
@@ -486,7 +486,7 @@ MODULE_DEVICE_TABLE(of, video_mux_dt_ids);
 
 static struct platform_driver video_mux_driver = {
 	.probe		= video_mux_probe,
-	.remove_new	= video_mux_remove,
+	.remove		= video_mux_remove,
 	.driver		= {
 		.of_match_table = video_mux_dt_ids,
 		.name = "video-mux",
diff --git a/drivers/media/platform/xilinx/xilinx-csi2rxss.c b/drivers/media/platform/xilinx/xilinx-csi2rxss.c
index f953d5474ae0..146131b8f37e 100644
--- a/drivers/media/platform/xilinx/xilinx-csi2rxss.c
+++ b/drivers/media/platform/xilinx/xilinx-csi2rxss.c
@@ -1028,7 +1028,7 @@ static struct platform_driver xcsi2rxss_driver = {
 		.of_match_table	= xcsi2rxss_of_id_table,
 	},
 	.probe			= xcsi2rxss_probe,
-	.remove_new		= xcsi2rxss_remove,
+	.remove			= xcsi2rxss_remove,
 };
 
 module_platform_driver(xcsi2rxss_driver);
diff --git a/drivers/media/platform/xilinx/xilinx-dma.c b/drivers/media/platform/xilinx/xilinx-dma.c
index a96de5d388a1..18bfa6001909 100644
--- a/drivers/media/platform/xilinx/xilinx-dma.c
+++ b/drivers/media/platform/xilinx/xilinx-dma.c
@@ -348,8 +348,8 @@ static void xvip_dma_buffer_queue(struct vb2_buffer *vb)
 	}
 
 	dma->xt.frame_size = 1;
-	dma->sgl[0].size = dma->format.width * dma->fmtinfo->bpp;
-	dma->sgl[0].icg = dma->format.bytesperline - dma->sgl[0].size;
+	dma->sgl.size = dma->format.width * dma->fmtinfo->bpp;
+	dma->sgl.icg = dma->format.bytesperline - dma->sgl.size;
 	dma->xt.numf = dma->format.height;
 
 	desc = dmaengine_prep_interleaved_dma(dma->dma, &dma->xt, flags);
@@ -458,8 +458,6 @@ static const struct vb2_ops xvip_dma_queue_qops = {
 	.queue_setup = xvip_dma_queue_setup,
 	.buf_prepare = xvip_dma_buffer_prepare,
 	.buf_queue = xvip_dma_buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = xvip_dma_start_streaming,
 	.stop_streaming = xvip_dma_stop_streaming,
 };
diff --git a/drivers/media/platform/xilinx/xilinx-dma.h b/drivers/media/platform/xilinx/xilinx-dma.h
index 9c6d4c18d1a9..18f77e1a7b39 100644
--- a/drivers/media/platform/xilinx/xilinx-dma.h
+++ b/drivers/media/platform/xilinx/xilinx-dma.h
@@ -97,7 +97,7 @@ struct xvip_dma {
 	struct dma_chan *dma;
 	unsigned int align;
 	struct dma_interleaved_template xt;
-	struct data_chunk sgl[1];
+	struct data_chunk sgl;
 };
 
 #define to_xvip_dma(vdev)	container_of(vdev, struct xvip_dma, video)
diff --git a/drivers/media/platform/xilinx/xilinx-tpg.c b/drivers/media/platform/xilinx/xilinx-tpg.c
index e05e528ffc6f..7deec6e37edc 100644
--- a/drivers/media/platform/xilinx/xilinx-tpg.c
+++ b/drivers/media/platform/xilinx/xilinx-tpg.c
@@ -13,6 +13,7 @@
 #include <linux/gpio/consumer.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_graph.h>
 #include <linux/platform_device.h>
 #include <linux/xilinx-v4l2-controls.h>
 
@@ -711,26 +712,16 @@ static int xtpg_parse_of(struct xtpg_device *xtpg)
 {
 	struct device *dev = xtpg->xvip.dev;
 	struct device_node *node = xtpg->xvip.dev->of_node;
-	struct device_node *ports;
-	struct device_node *port;
 	unsigned int nports = 0;
 	bool has_endpoint = false;
 
-	ports = of_get_child_by_name(node, "ports");
-	if (ports == NULL)
-		ports = node;
-
-	for_each_child_of_node(ports, port) {
+	for_each_of_graph_port(node, port) {
 		const struct xvip_video_format *format;
 		struct device_node *endpoint;
 
-		if (!of_node_name_eq(port, "port"))
-			continue;
-
 		format = xvip_of_get_format(port);
 		if (IS_ERR(format)) {
 			dev_err(dev, "invalid format in DT");
-			of_node_put(port);
 			return PTR_ERR(format);
 		}
 
@@ -739,12 +730,11 @@ static int xtpg_parse_of(struct xtpg_device *xtpg)
 			xtpg->vip_format = format;
 		} else if (xtpg->vip_format != format) {
 			dev_err(dev, "in/out format mismatch in DT");
-			of_node_put(port);
 			return -EINVAL;
 		}
 
 		if (nports == 0) {
-			endpoint = of_get_next_child(port, NULL);
+			endpoint = of_graph_get_next_port_endpoint(port, NULL);
 			if (endpoint)
 				has_endpoint = true;
 			of_node_put(endpoint);
@@ -920,7 +910,7 @@ static struct platform_driver xtpg_driver = {
 		.of_match_table	= xtpg_of_id_table,
 	},
 	.probe			= xtpg_probe,
-	.remove_new		= xtpg_remove,
+	.remove			= xtpg_remove,
 };
 
 module_platform_driver(xtpg_driver);
diff --git a/drivers/media/platform/xilinx/xilinx-vipp.c b/drivers/media/platform/xilinx/xilinx-vipp.c
index 996684a73038..024b439feec9 100644
--- a/drivers/media/platform/xilinx/xilinx-vipp.c
+++ b/drivers/media/platform/xilinx/xilinx-vipp.c
@@ -199,18 +199,13 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 	struct media_pad *sink_pad;
 	struct xvip_graph_entity *ent;
 	struct v4l2_fwnode_link link;
-	struct device_node *ep = NULL;
+	struct device_node *ep;
 	struct xvip_dma *dma;
 	int ret = 0;
 
 	dev_dbg(xdev->dev, "creating links for DMA engines\n");
 
-	while (1) {
-		/* Get the next endpoint and parse its link. */
-		ep = of_graph_get_next_endpoint(node, ep);
-		if (ep == NULL)
-			break;
-
+	for_each_endpoint_of_node(node, ep) {
 		dev_dbg(xdev->dev, "processing endpoint %pOF\n", ep);
 
 		ret = v4l2_fwnode_parse_link(of_fwnode_handle(ep), &link);
@@ -623,7 +618,7 @@ static struct platform_driver xvip_composite_driver = {
 		.of_match_table = xvip_composite_of_id_table,
 	},
 	.probe = xvip_composite_probe,
-	.remove_new = xvip_composite_remove,
+	.remove = xvip_composite_remove,
 };
 
 module_platform_driver(xvip_composite_driver);
diff --git a/drivers/media/platform/xilinx/xilinx-vtc.c b/drivers/media/platform/xilinx/xilinx-vtc.c
index dda70719f004..92fec7bb47da 100644
--- a/drivers/media/platform/xilinx/xilinx-vtc.c
+++ b/drivers/media/platform/xilinx/xilinx-vtc.c
@@ -365,7 +365,7 @@ static struct platform_driver xvtc_driver = {
 		.of_match_table = xvtc_of_id_table,
 	},
 	.probe = xvtc_probe,
-	.remove_new = xvtc_remove,
+	.remove = xvtc_remove,
 };
 
 module_platform_driver(xvtc_driver);
diff --git a/drivers/media/radio/Kconfig b/drivers/media/radio/Kconfig
index d52eccdc7eb9..72776d08046a 100644
--- a/drivers/media/radio/Kconfig
+++ b/drivers/media/radio/Kconfig
@@ -221,10 +221,6 @@ config USB_RAREMONO
 source "drivers/media/radio/si470x/Kconfig"
 source "drivers/media/radio/si4713/Kconfig"
 
-# TI's ST based wl128x FM radio
-
-source "drivers/media/radio/wl128x/Kconfig"
-
 #
 # ISA drivers configuration
 #
diff --git a/drivers/media/radio/Makefile b/drivers/media/radio/Makefile
index cfb6af7d3bc3..1ff46f3a6ed3 100644
--- a/drivers/media/radio/Makefile
+++ b/drivers/media/radio/Makefile
@@ -31,7 +31,6 @@ obj-$(CONFIG_RADIO_TIMBERDALE) += radio-timb.o
 obj-$(CONFIG_RADIO_TRUST) += radio-trust.o
 obj-$(CONFIG_RADIO_TYPHOON) += radio-typhoon.o
 obj-$(CONFIG_RADIO_WL1273) += radio-wl1273.o
-obj-$(CONFIG_RADIO_WL128X) += wl128x/
 obj-$(CONFIG_RADIO_ZOLTRIX) += radio-zoltrix.o
 
 obj-$(CONFIG_USB_DSBR) += dsbr100.o
diff --git a/drivers/media/radio/radio-aimslab.c b/drivers/media/radio/radio-aimslab.c
index 0e9a3787724c..3c8c17d64821 100644
--- a/drivers/media/radio/radio-aimslab.c
+++ b/drivers/media/radio/radio-aimslab.c
@@ -4,7 +4,7 @@
  *
  * Copyright 1997 M. Kirkwood
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
+ * Converted to the radio-isa framework by Hans Verkuil <hansverk@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@kernel.org>
  * Converted to new API by Alan Cox <alan@lxorguk.ukuu.org.uk>
  * Various bugfixes and enhancements by Russell Kroll <rkroll@exploits.org>
diff --git a/drivers/media/radio/radio-aztech.c b/drivers/media/radio/radio-aztech.c
index 4909c337b027..d989c0b3966f 100644
--- a/drivers/media/radio/radio-aztech.c
+++ b/drivers/media/radio/radio-aztech.c
@@ -2,7 +2,7 @@
 /*
  * radio-aztech.c - Aztech radio card driver
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@xs4all.nl>
+ * Converted to the radio-isa framework by Hans Verkuil <hverkuil@xs4all.nl>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@kernel.org>
  * Adapted to support the Video for Linux API by
  * Russell Kroll <rkroll@exploits.org>.  Based on original tuner code by:
diff --git a/drivers/media/radio/radio-cadet.c b/drivers/media/radio/radio-cadet.c
index a5db9b4dc3de..5110754e1a31 100644
--- a/drivers/media/radio/radio-cadet.c
+++ b/drivers/media/radio/radio-cadet.c
@@ -284,7 +284,7 @@ static bool cadet_has_rds_data(struct cadet *dev)
 
 static void cadet_handler(struct timer_list *t)
 {
-	struct cadet *dev = from_timer(dev, t, readtimer);
+	struct cadet *dev = timer_container_of(dev, t, readtimer);
 
 	/* Service the RDS fifo */
 	if (mutex_trylock(&dev->lock)) {
@@ -471,7 +471,7 @@ static int cadet_release(struct file *file)
 
 	mutex_lock(&dev->lock);
 	if (v4l2_fh_is_singular_file(file) && dev->rdsstat) {
-		del_timer_sync(&dev->readtimer);
+		timer_delete_sync(&dev->readtimer);
 		dev->rdsstat = 0;
 	}
 	v4l2_fh_release(file);
diff --git a/drivers/media/radio/radio-gemtek.c b/drivers/media/radio/radio-gemtek.c
index a532f63aa9d9..5ca6274c45bd 100644
--- a/drivers/media/radio/radio-gemtek.c
+++ b/drivers/media/radio/radio-gemtek.c
@@ -15,7 +15,7 @@
  *    Converted to new API by Alan Cox <alan@lxorguk.ukuu.org.uk>
  *    Various bugfixes and enhancements by Russell Kroll <rkroll@exploits.org>
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
+ * Converted to the radio-isa framework by Hans Verkuil <hansverk@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@kernel.org>
  *
  * Note: this card seems to swap the left and right audio channels!
diff --git a/drivers/media/radio/radio-isa.c b/drivers/media/radio/radio-isa.c
index ad49151f5ff0..4f87c76a2a96 100644
--- a/drivers/media/radio/radio-isa.c
+++ b/drivers/media/radio/radio-isa.c
@@ -4,7 +4,7 @@
  * This takes care of all the V4L2 scaffolding, allowing the ISA drivers
  * to concentrate on the actual hardware operation.
  *
- * Copyright (C) 2012 Hans Verkuil <hans.verkuil@cisco.com>
+ * Copyright (C) 2012 Hans Verkuil <hansverk@cisco.com>
  */
 
 #include <linux/module.h>
diff --git a/drivers/media/radio/radio-isa.h b/drivers/media/radio/radio-isa.h
index c9159958203e..0f3db473da5e 100644
--- a/drivers/media/radio/radio-isa.h
+++ b/drivers/media/radio/radio-isa.h
@@ -4,7 +4,7 @@
  * This takes care of all the V4L2 scaffolding, allowing the ISA drivers
  * to concentrate on the actual hardware operation.
  *
- * Copyright (C) 2012 Hans Verkuil <hans.verkuil@cisco.com>
+ * Copyright (C) 2012 Hans Verkuil <hansverk@cisco.com>
  */
 
 #ifndef _RADIO_ISA_H_
diff --git a/drivers/media/radio/radio-miropcm20.c b/drivers/media/radio/radio-miropcm20.c
index 08be77b8f3b7..27f058c5e677 100644
--- a/drivers/media/radio/radio-miropcm20.c
+++ b/drivers/media/radio/radio-miropcm20.c
@@ -23,7 +23,7 @@
  * This code has been reintroduced and converted to use
  * the new V4L2 RDS API by:
  *
- * Hans Verkuil <hans.verkuil@cisco.com>
+ * Hans Verkuil <hansverk@cisco.com>
  */
 
 #include <linux/module.h>
diff --git a/drivers/media/radio/radio-raremono.c b/drivers/media/radio/radio-raremono.c
index c3180d53c282..64c7452c05b5 100644
--- a/drivers/media/radio/radio-raremono.c
+++ b/drivers/media/radio/radio-raremono.c
@@ -12,7 +12,7 @@
 #include <linux/hid.h>
 #include <linux/mutex.h>
 #include <linux/videodev2.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/radio/radio-rtrack2.c b/drivers/media/radio/radio-rtrack2.c
index 73d2c187f122..16b13a63bfed 100644
--- a/drivers/media/radio/radio-rtrack2.c
+++ b/drivers/media/radio/radio-rtrack2.c
@@ -7,7 +7,7 @@
  * Converted to new API by Alan Cox <alan@lxorguk.ukuu.org.uk>
  * Various bugfixes and enhancements by Russell Kroll <rkroll@exploits.org>
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
+ * Converted to the radio-isa framework by Hans Verkuil <hansverk@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@kernel.org>
  *
  * Fully tested with actual hardware and the v4l2-compliance tool.
diff --git a/drivers/media/radio/radio-shark2.c b/drivers/media/radio/radio-shark2.c
index f1c5c0a6a335..e3e6aa87fe08 100644
--- a/drivers/media/radio/radio-shark2.c
+++ b/drivers/media/radio/radio-shark2.c
@@ -62,7 +62,7 @@ struct shark_device {
 #ifdef SHARK_USE_LEDS
 	struct work_struct led_work;
 	struct led_classdev leds[NO_LEDS];
-	char led_names[NO_LEDS][32];
+	char led_names[NO_LEDS][64];
 	atomic_t brightness[NO_LEDS];
 	unsigned long brightness_new;
 #endif
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
index b2c5809a8bc7..9980346cb5ea 100644
--- a/drivers/media/radio/radio-si476x.c
+++ b/drivers/media/radio/radio-si476x.c
@@ -1513,7 +1513,7 @@ static struct platform_driver si476x_radio_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= si476x_radio_probe,
-	.remove_new	= si476x_radio_remove,
+	.remove		= si476x_radio_remove,
 };
 module_platform_driver(si476x_radio_driver);
 
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 14e7dd3889ff..dd85b0b1bcd9 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -502,7 +502,7 @@ static void tea5764_i2c_remove(struct i2c_client *client)
 
 /* I2C subsystem interface */
 static const struct i2c_device_id tea5764_id[] = {
-	{ "radio-tea5764", 0 },
+	{ "radio-tea5764" },
 	{ }					/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(i2c, tea5764_id);
diff --git a/drivers/media/radio/radio-terratec.c b/drivers/media/radio/radio-terratec.c
index 621bb8523271..720080634454 100644
--- a/drivers/media/radio/radio-terratec.c
+++ b/drivers/media/radio/radio-terratec.c
@@ -17,7 +17,7 @@
  *  Frequency control is done digitally -- ie out(port,encodefreq(95.8));
  *  Volume Control is done digitally
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
+ * Converted to the radio-isa framework by Hans Verkuil <hansverk@cisco.com>
  * Converted to V4L2 API by Mauro Carvalho Chehab <mchehab@kernel.org>
  */
 
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index 04daa9c358c2..a6069b106fd3 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -164,7 +164,7 @@ static struct platform_driver timbradio_platform_driver = {
 		.name	= DRIVER_NAME,
 	},
 	.probe		= timbradio_probe,
-	.remove_new	= timbradio_remove,
+	.remove		= timbradio_remove,
 };
 
 module_platform_driver(timbradio_platform_driver);
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index f6b98c304b72..f55217ccf2b8 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -1407,7 +1407,7 @@ static inline struct wl1273_device *to_radio(struct v4l2_ctrl *ctrl)
 	return container_of(ctrl->handler, struct wl1273_device, ctrl_handler);
 }
 
-static int wl1273_fm_vidioc_s_ctrl(struct v4l2_ctrl *ctrl)
+static int wl1273_fm_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct wl1273_device *radio = to_radio(ctrl);
 	struct wl1273_core *core = radio->core;
@@ -1945,7 +1945,7 @@ static void wl1273_vdev_release(struct video_device *dev)
 }
 
 static const struct v4l2_ctrl_ops wl1273_ctrl_ops = {
-	.s_ctrl = wl1273_fm_vidioc_s_ctrl,
+	.s_ctrl = wl1273_fm_s_ctrl,
 	.g_volatile_ctrl = wl1273_fm_g_volatile_ctrl,
 };
 
@@ -2145,7 +2145,7 @@ pdata_err:
 
 static struct platform_driver wl1273_fm_radio_driver = {
 	.probe		= wl1273_fm_radio_probe,
-	.remove_new	= wl1273_fm_radio_remove,
+	.remove		= wl1273_fm_radio_remove,
 	.driver		= {
 		.name	= "wl1273_fm_radio",
 	},
diff --git a/drivers/media/radio/radio-zoltrix.c b/drivers/media/radio/radio-zoltrix.c
index f3dc57c75131..099b7af6a410 100644
--- a/drivers/media/radio/radio-zoltrix.c
+++ b/drivers/media/radio/radio-zoltrix.c
@@ -30,7 +30,7 @@
  * 2006-07-24 - Converted to V4L2 API
  *		by Mauro Carvalho Chehab <mchehab@kernel.org>
  *
- * Converted to the radio-isa framework by Hans Verkuil <hans.verkuil@cisco.com>
+ * Converted to the radio-isa framework by Hans Verkuil <hansverk@cisco.com>
  *
  * Note that this is the driver for the Zoltrix Radio Plus.
  * This driver does not work for the Zoltrix Radio Plus 108 or the
diff --git a/drivers/media/radio/saa7706h.c b/drivers/media/radio/saa7706h.c
index 91345198bbf1..d9eecddffd91 100644
--- a/drivers/media/radio/saa7706h.c
+++ b/drivers/media/radio/saa7706h.c
@@ -395,8 +395,8 @@ static void saa7706h_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id saa7706h_id[] = {
-	{DRIVER_NAME, 0},
-	{},
+	{ DRIVER_NAME },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, saa7706h_id);
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index dc0c1d8d23f0..af0a70910099 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -790,4 +790,5 @@ const struct video_device si470x_viddev_template = {
 };
 EXPORT_SYMBOL_GPL(si470x_viddev_template);
 
+MODULE_DESCRIPTION("Core radio driver for Si470x FM Radio Receivers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/si470x/radio-si470x-i2c.c b/drivers/media/radio/si470x/radio-si470x-i2c.c
index fd449e42c191..cdd2ac198f2c 100644
--- a/drivers/media/radio/si470x/radio-si470x-i2c.c
+++ b/drivers/media/radio/si470x/radio-si470x-i2c.c
@@ -28,7 +28,7 @@
 /* I2C Device ID List */
 static const struct i2c_device_id si470x_i2c_id[] = {
 	/* Generic Entry */
-	{ "si470x", 0 },
+	{ "si470x" },
 	/* Terminating entry */
 	{ }
 };
diff --git a/drivers/media/radio/si470x/radio-si470x.h b/drivers/media/radio/si470x/radio-si470x.h
index e57ab54a27fc..2915c0023fcd 100644
--- a/drivers/media/radio/si470x/radio-si470x.h
+++ b/drivers/media/radio/si470x/radio-si470x.h
@@ -26,7 +26,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-device.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 
 
diff --git a/drivers/media/radio/si4713/radio-platform-si4713.c b/drivers/media/radio/si4713/radio-platform-si4713.c
index 9fdaed68a962..67b4afadc95a 100644
--- a/drivers/media/radio/si4713/radio-platform-si4713.c
+++ b/drivers/media/radio/si4713/radio-platform-si4713.c
@@ -205,7 +205,7 @@ static struct platform_driver radio_si4713_pdriver = {
 		.name	= "radio-si4713",
 	},
 	.probe		= radio_si4713_pdriver_probe,
-	.remove_new     = radio_si4713_pdriver_remove,
+	.remove         = radio_si4713_pdriver_remove,
 };
 
 module_platform_driver(radio_si4713_pdriver);
diff --git a/drivers/media/radio/si4713/si4713.c b/drivers/media/radio/si4713/si4713.c
index ddaf7a60b7d0..e71272c6de37 100644
--- a/drivers/media/radio/si4713/si4713.c
+++ b/drivers/media/radio/si4713/si4713.c
@@ -1639,8 +1639,8 @@ static void si4713_remove(struct i2c_client *client)
 
 /* si4713_i2c_driver - i2c driver interface */
 static const struct i2c_device_id si4713_id[] = {
-	{ "si4713" , 0 },
-	{ },
+	{ "si4713" },
+	{ }
 };
 MODULE_DEVICE_TABLE(i2c, si4713_id);
 
diff --git a/drivers/media/radio/tef6862.c b/drivers/media/radio/tef6862.c
index 215168aa1588..b00ccf651922 100644
--- a/drivers/media/radio/tef6862.c
+++ b/drivers/media/radio/tef6862.c
@@ -173,8 +173,8 @@ static void tef6862_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tef6862_id[] = {
-	{DRIVER_NAME, 0},
-	{},
+	{ DRIVER_NAME },
+	{}
 };
 
 MODULE_DEVICE_TABLE(i2c, tef6862_id);
diff --git a/drivers/media/radio/wl128x/Kconfig b/drivers/media/radio/wl128x/Kconfig
deleted file mode 100644
index 3e7713872e3f..000000000000
--- a/drivers/media/radio/wl128x/Kconfig
+++ /dev/null
@@ -1,15 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0-only
-#
-# TI's wl128x FM driver based on TI's ST driver.
-#
-config RADIO_WL128X
-	tristate "Texas Instruments WL128x FM Radio"
-	depends on VIDEO_DEV && RFKILL && TTY && TI_ST
-	depends on GPIOLIB || COMPILE_TEST
-	help
-	  Choose Y here if you have this FM radio chip.
-
-	  In order to control your radio card, you will need to use programs
-	  that are compatible with the Video For Linux 2 API.  Information on
-	  this API and pointers to "v4l2" programs may be found at
-	  <file:Documentation/userspace-api/media/index.rst>.
diff --git a/drivers/media/radio/wl128x/Makefile b/drivers/media/radio/wl128x/Makefile
deleted file mode 100644
index 4396ca416cfa..000000000000
--- a/drivers/media/radio/wl128x/Makefile
+++ /dev/null
@@ -1,7 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0-only
-#
-# Makefile for TI's shared transport driver based wl128x
-# FM radio.
-#
-obj-$(CONFIG_RADIO_WL128X)	+= fm_drv.o
-fm_drv-objs		:= fmdrv_common.o fmdrv_rx.o fmdrv_tx.o fmdrv_v4l2.o
diff --git a/drivers/media/radio/wl128x/fmdrv.h b/drivers/media/radio/wl128x/fmdrv.h
deleted file mode 100644
index da8920169df8..000000000000
--- a/drivers/media/radio/wl128x/fmdrv.h
+++ /dev/null
@@ -1,228 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *
- *  Common header for all FM driver sub-modules.
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#ifndef _FM_DRV_H
-#define _FM_DRV_H
-
-#include <linux/skbuff.h>
-#include <linux/interrupt.h>
-#include <sound/core.h>
-#include <sound/initval.h>
-#include <linux/timer.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ctrls.h>
-
-#define FM_DRV_VERSION            "0.1.1"
-#define FM_DRV_NAME               "ti_fmdrv"
-#define FM_DRV_CARD_SHORT_NAME    "TI FM Radio"
-#define FM_DRV_CARD_LONG_NAME     "Texas Instruments FM Radio"
-
-/* Flag info */
-#define FM_INTTASK_RUNNING            0
-#define FM_INTTASK_SCHEDULE_PENDING   1
-#define FM_FW_DW_INPROGRESS     2
-#define FM_CORE_READY                 3
-#define FM_CORE_TRANSPORT_READY       4
-#define FM_AF_SWITCH_INPROGRESS	      5
-#define FM_CORE_TX_XMITING	      6
-
-#define FM_TUNE_COMPLETE	      0x1
-#define FM_BAND_LIMIT		      0x2
-
-#define FM_DRV_TX_TIMEOUT      (5*HZ)	/* 5 seconds */
-#define FM_DRV_RX_SEEK_TIMEOUT (20*HZ)	/* 20 seconds */
-
-#define fmerr(format, ...) \
-	printk(KERN_ERR "fmdrv: " format, ## __VA_ARGS__)
-#define fmwarn(format, ...) \
-	printk(KERN_WARNING "fmdrv: " format, ##__VA_ARGS__)
-#ifdef DEBUG
-#define fmdbg(format, ...) \
-	printk(KERN_DEBUG "fmdrv: " format, ## __VA_ARGS__)
-#else /* DEBUG */
-#define fmdbg(format, ...) do {} while(0)
-#endif
-enum {
-	FM_MODE_OFF,
-	FM_MODE_TX,
-	FM_MODE_RX,
-	FM_MODE_ENTRY_MAX
-};
-
-#define FM_RX_RDS_INFO_FIELD_MAX	8	/* 4 Group * 2 Bytes */
-
-/* RX RDS data format */
-struct fm_rdsdata_format {
-	union {
-		struct {
-			u8 buff[FM_RX_RDS_INFO_FIELD_MAX];
-		} groupdatabuff;
-		struct {
-			u16 pidata;
-			u8 blk_b[2];
-			u8 blk_c[2];
-			u8 blk_d[2];
-		} groupgeneral;
-		struct {
-			u16 pidata;
-			u8 blk_b[2];
-			u8 af[2];
-			u8 ps[2];
-		} group0A;
-		struct {
-			u16 pi[2];
-			u8 blk_b[2];
-			u8 ps[2];
-		} group0B;
-	} data;
-};
-
-/* FM region (Europe/US, Japan) info */
-struct region_info {
-	u32 chanl_space;
-	u32 bot_freq;
-	u32 top_freq;
-	u8 fm_band;
-};
-struct fmdev;
-typedef void (*int_handler_prototype) (struct fmdev *);
-
-/* FM Interrupt processing related info */
-struct fm_irq {
-	u8 stage;
-	u16 flag;	/* FM interrupt flag */
-	u16 mask;	/* FM interrupt mask */
-	/* Interrupt process timeout handler */
-	struct timer_list timer;
-	u8 retry;
-	int_handler_prototype *handlers;
-};
-
-/* RDS info */
-struct fm_rds {
-	u8 flag;	/* RX RDS on/off status */
-	u8 last_blk_idx;	/* Last received RDS block */
-
-	/* RDS buffer */
-	wait_queue_head_t read_queue;
-	u32 buf_size;	/* Size is always multiple of 3 */
-	u32 wr_idx;
-	u32 rd_idx;
-	u8 *buff;
-};
-
-#define FM_RDS_MAX_AF_LIST		25
-
-/*
- * Current RX channel Alternate Frequency cache.
- * This info is used to switch to other freq (AF)
- * when current channel signal strength is below RSSI threshold.
- */
-struct tuned_station_info {
-	u16 picode;
-	u32 af_cache[FM_RDS_MAX_AF_LIST];
-	u8 afcache_size;
-	u8 af_list_max;
-};
-
-/* FM RX mode info */
-struct fm_rx {
-	struct region_info region;	/* Current selected band */
-	u32 freq;	/* Current RX frquency */
-	u8 mute_mode;	/* Current mute mode */
-	u8 deemphasis_mode; /* Current deemphasis mode */
-	/* RF dependent soft mute mode */
-	u8 rf_depend_mute;
-	u16 volume;	/* Current volume level */
-	u16 rssi_threshold;	/* Current RSSI threshold level */
-	/* Holds the index of the current AF jump */
-	u8 afjump_idx;
-	/* Will hold the frequency before the jump */
-	u32 freq_before_jump;
-	u8 rds_mode;	/* RDS operation mode (RDS/RDBS) */
-	u8 af_mode;	/* Alternate frequency on/off */
-	struct tuned_station_info stat_info;
-	struct fm_rds rds;
-};
-
-#define FMTX_RDS_TXT_STR_SIZE	25
-/*
- * FM TX RDS data
- *
- * @ text_type: is the text following PS or RT
- * @ text: radio text string which could either be PS or RT
- * @ af_freq: alternate frequency for Tx
- * TODO: to be declared in application
- */
-struct tx_rds {
-	u8 text_type;
-	u8 text[FMTX_RDS_TXT_STR_SIZE];
-	u8 flag;
-	u32 af_freq;
-};
-/*
- * FM TX global data
- *
- * @ pwr_lvl: Power Level of the Transmission from mixer control
- * @ xmit_state: Transmission state = Updated locally upon Start/Stop
- * @ audio_io: i2S/Analog
- * @ tx_frq: Transmission frequency
- */
-struct fmtx_data {
-	u8 pwr_lvl;
-	u8 xmit_state;
-	u8 audio_io;
-	u8 region;
-	u16 aud_mode;
-	u32 preemph;
-	u32 tx_frq;
-	struct tx_rds rds;
-};
-
-/* FM driver operation structure */
-struct fmdev {
-	struct video_device *radio_dev;	/* V4L2 video device pointer */
-	struct v4l2_device v4l2_dev;	/* V4L2 top level struct */
-	struct snd_card *card;	/* Card which holds FM mixer controls */
-	u16 asci_id;
-	spinlock_t rds_buff_lock; /* To protect access to RDS buffer */
-	spinlock_t resp_skb_lock; /* To protect access to received SKB */
-
-	long flag;		/*  FM driver state machine info */
-	int streg_cbdata; /* status of ST registration */
-
-	struct sk_buff_head rx_q;	/* RX queue */
-	struct tasklet_struct rx_task;	/* RX Tasklet */
-
-	struct sk_buff_head tx_q;	/* TX queue */
-	struct tasklet_struct tx_task;	/* TX Tasklet */
-	unsigned long last_tx_jiffies;	/* Timestamp of last pkt sent */
-	atomic_t tx_cnt;	/* Number of packets can send at a time */
-
-	struct sk_buff *resp_skb;	/* Response from the chip */
-	/* Main task completion handler */
-	struct completion maintask_comp;
-	/* Opcode of last command sent to the chip */
-	u8 pre_op;
-	/* Handler used for wakeup when response packet is received */
-	struct completion *resp_comp;
-	struct fm_irq irq_info;
-	u8 curr_fmmode; /* Current FM chip mode (TX, RX, OFF) */
-	struct fm_rx rx;	/* FM receiver info */
-	struct fmtx_data tx_data;
-
-	/* V4L2 ctrl framework handler*/
-	struct v4l2_ctrl_handler ctrl_handler;
-
-	/* For core assisted locking */
-	struct mutex mutex;
-};
-#endif
diff --git a/drivers/media/radio/wl128x/fmdrv_common.c b/drivers/media/radio/wl128x/fmdrv_common.c
deleted file mode 100644
index 3da8e5102bec..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_common.c
+++ /dev/null
@@ -1,1675 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *
- *  This sub-module of FM driver is common for FM RX and TX
- *  functionality. This module is responsible for:
- *  1) Forming group of Channel-8 commands to perform particular
- *     functionality (eg., frequency set require more than
- *     one Channel-8 command to be sent to the chip).
- *  2) Sending each Channel-8 command to the chip and reading
- *     response back over Shared Transport.
- *  3) Managing TX and RX Queues and Tasklets.
- *  4) Handling FM Interrupt packet and taking appropriate action.
- *  5) Loading FM firmware to the chip (common, FM TX, and FM RX
- *     firmware files based on mode selection)
- *
- *  Copyright (C) 2011 Texas Instruments
- *  Author: Raja Mani <raja_mani@ti.com>
- *  Author: Manjunatha Halli <manjunatha_halli@ti.com>
- */
-
-#include <linux/delay.h>
-#include <linux/firmware.h>
-#include <linux/module.h>
-#include <linux/nospec.h>
-#include <linux/jiffies.h>
-
-#include "fmdrv.h"
-#include "fmdrv_v4l2.h"
-#include "fmdrv_common.h"
-#include <linux/ti_wilink_st.h>
-#include "fmdrv_rx.h"
-#include "fmdrv_tx.h"
-
-/* Region info */
-static struct region_info region_configs[] = {
-	/* Europe/US */
-	{
-	 .chanl_space = FM_CHANNEL_SPACING_200KHZ * FM_FREQ_MUL,
-	 .bot_freq = 87500,	/* 87.5 MHz */
-	 .top_freq = 108000,	/* 108 MHz */
-	 .fm_band = 0,
-	 },
-	/* Japan */
-	{
-	 .chanl_space = FM_CHANNEL_SPACING_200KHZ * FM_FREQ_MUL,
-	 .bot_freq = 76000,	/* 76 MHz */
-	 .top_freq = 90000,	/* 90 MHz */
-	 .fm_band = 1,
-	 },
-};
-
-/* Band selection */
-static u8 default_radio_region;	/* Europe/US */
-module_param(default_radio_region, byte, 0);
-MODULE_PARM_DESC(default_radio_region, "Region: 0=Europe/US, 1=Japan");
-
-/* RDS buffer blocks */
-static u32 default_rds_buf = 300;
-module_param(default_rds_buf, uint, 0444);
-MODULE_PARM_DESC(default_rds_buf, "RDS buffer entries");
-
-/* Radio Nr */
-static u32 radio_nr = -1;
-module_param(radio_nr, int, 0444);
-MODULE_PARM_DESC(radio_nr, "Radio Nr");
-
-/* FM irq handlers forward declaration */
-static void fm_irq_send_flag_getcmd(struct fmdev *);
-static void fm_irq_handle_flag_getcmd_resp(struct fmdev *);
-static void fm_irq_handle_hw_malfunction(struct fmdev *);
-static void fm_irq_handle_rds_start(struct fmdev *);
-static void fm_irq_send_rdsdata_getcmd(struct fmdev *);
-static void fm_irq_handle_rdsdata_getcmd_resp(struct fmdev *);
-static void fm_irq_handle_rds_finish(struct fmdev *);
-static void fm_irq_handle_tune_op_ended(struct fmdev *);
-static void fm_irq_handle_power_enb(struct fmdev *);
-static void fm_irq_handle_low_rssi_start(struct fmdev *);
-static void fm_irq_afjump_set_pi(struct fmdev *);
-static void fm_irq_handle_set_pi_resp(struct fmdev *);
-static void fm_irq_afjump_set_pimask(struct fmdev *);
-static void fm_irq_handle_set_pimask_resp(struct fmdev *);
-static void fm_irq_afjump_setfreq(struct fmdev *);
-static void fm_irq_handle_setfreq_resp(struct fmdev *);
-static void fm_irq_afjump_enableint(struct fmdev *);
-static void fm_irq_afjump_enableint_resp(struct fmdev *);
-static void fm_irq_start_afjump(struct fmdev *);
-static void fm_irq_handle_start_afjump_resp(struct fmdev *);
-static void fm_irq_afjump_rd_freq(struct fmdev *);
-static void fm_irq_afjump_rd_freq_resp(struct fmdev *);
-static void fm_irq_handle_low_rssi_finish(struct fmdev *);
-static void fm_irq_send_intmsk_cmd(struct fmdev *);
-static void fm_irq_handle_intmsk_cmd_resp(struct fmdev *);
-
-/*
- * When FM common module receives interrupt packet, following handlers
- * will be executed one after another to service the interrupt(s)
- */
-enum fmc_irq_handler_index {
-	FM_SEND_FLAG_GETCMD_IDX,
-	FM_HANDLE_FLAG_GETCMD_RESP_IDX,
-
-	/* HW malfunction irq handler */
-	FM_HW_MAL_FUNC_IDX,
-
-	/* RDS threshold reached irq handler */
-	FM_RDS_START_IDX,
-	FM_RDS_SEND_RDS_GETCMD_IDX,
-	FM_RDS_HANDLE_RDS_GETCMD_RESP_IDX,
-	FM_RDS_FINISH_IDX,
-
-	/* Tune operation ended irq handler */
-	FM_HW_TUNE_OP_ENDED_IDX,
-
-	/* TX power enable irq handler */
-	FM_HW_POWER_ENB_IDX,
-
-	/* Low RSSI irq handler */
-	FM_LOW_RSSI_START_IDX,
-	FM_AF_JUMP_SETPI_IDX,
-	FM_AF_JUMP_HANDLE_SETPI_RESP_IDX,
-	FM_AF_JUMP_SETPI_MASK_IDX,
-	FM_AF_JUMP_HANDLE_SETPI_MASK_RESP_IDX,
-	FM_AF_JUMP_SET_AF_FREQ_IDX,
-	FM_AF_JUMP_HANDLE_SET_AFFREQ_RESP_IDX,
-	FM_AF_JUMP_ENABLE_INT_IDX,
-	FM_AF_JUMP_ENABLE_INT_RESP_IDX,
-	FM_AF_JUMP_START_AFJUMP_IDX,
-	FM_AF_JUMP_HANDLE_START_AFJUMP_RESP_IDX,
-	FM_AF_JUMP_RD_FREQ_IDX,
-	FM_AF_JUMP_RD_FREQ_RESP_IDX,
-	FM_LOW_RSSI_FINISH_IDX,
-
-	/* Interrupt process post action */
-	FM_SEND_INTMSK_CMD_IDX,
-	FM_HANDLE_INTMSK_CMD_RESP_IDX,
-};
-
-/* FM interrupt handler table */
-static int_handler_prototype int_handler_table[] = {
-	fm_irq_send_flag_getcmd,
-	fm_irq_handle_flag_getcmd_resp,
-	fm_irq_handle_hw_malfunction,
-	fm_irq_handle_rds_start, /* RDS threshold reached irq handler */
-	fm_irq_send_rdsdata_getcmd,
-	fm_irq_handle_rdsdata_getcmd_resp,
-	fm_irq_handle_rds_finish,
-	fm_irq_handle_tune_op_ended,
-	fm_irq_handle_power_enb, /* TX power enable irq handler */
-	fm_irq_handle_low_rssi_start,
-	fm_irq_afjump_set_pi,
-	fm_irq_handle_set_pi_resp,
-	fm_irq_afjump_set_pimask,
-	fm_irq_handle_set_pimask_resp,
-	fm_irq_afjump_setfreq,
-	fm_irq_handle_setfreq_resp,
-	fm_irq_afjump_enableint,
-	fm_irq_afjump_enableint_resp,
-	fm_irq_start_afjump,
-	fm_irq_handle_start_afjump_resp,
-	fm_irq_afjump_rd_freq,
-	fm_irq_afjump_rd_freq_resp,
-	fm_irq_handle_low_rssi_finish,
-	fm_irq_send_intmsk_cmd, /* Interrupt process post action */
-	fm_irq_handle_intmsk_cmd_resp
-};
-
-static long (*g_st_write) (struct sk_buff *skb);
-static struct completion wait_for_fmdrv_reg_comp;
-
-static inline void fm_irq_call(struct fmdev *fmdev)
-{
-	fmdev->irq_info.handlers[fmdev->irq_info.stage](fmdev);
-}
-
-/* Continue next function in interrupt handler table */
-static inline void fm_irq_call_stage(struct fmdev *fmdev, u8 stage)
-{
-	fmdev->irq_info.stage = stage;
-	fm_irq_call(fmdev);
-}
-
-static inline void fm_irq_timeout_stage(struct fmdev *fmdev, u8 stage)
-{
-	fmdev->irq_info.stage = stage;
-	mod_timer(&fmdev->irq_info.timer, jiffies + FM_DRV_TX_TIMEOUT);
-}
-
-#ifdef FM_DUMP_TXRX_PKT
- /* To dump outgoing FM Channel-8 packets */
-inline void dump_tx_skb_data(struct sk_buff *skb)
-{
-	int len, len_org;
-	u8 index;
-	struct fm_cmd_msg_hdr *cmd_hdr;
-
-	cmd_hdr = (struct fm_cmd_msg_hdr *)skb->data;
-	printk(KERN_INFO "<<%shdr:%02x len:%02x opcode:%02x type:%s dlen:%02x",
-	       fm_cb(skb)->completion ? " " : "*", cmd_hdr->hdr,
-	       cmd_hdr->len, cmd_hdr->op,
-	       cmd_hdr->rd_wr ? "RD" : "WR", cmd_hdr->dlen);
-
-	len_org = skb->len - FM_CMD_MSG_HDR_SIZE;
-	if (len_org > 0) {
-		printk(KERN_CONT "\n   data(%d): ", cmd_hdr->dlen);
-		len = min(len_org, 14);
-		for (index = 0; index < len; index++)
-			printk(KERN_CONT "%x ",
-			       skb->data[FM_CMD_MSG_HDR_SIZE + index]);
-		printk(KERN_CONT "%s", (len_org > 14) ? ".." : "");
-	}
-	printk(KERN_CONT "\n");
-}
-
- /* To dump incoming FM Channel-8 packets */
-inline void dump_rx_skb_data(struct sk_buff *skb)
-{
-	int len, len_org;
-	u8 index;
-	struct fm_event_msg_hdr *evt_hdr;
-
-	evt_hdr = (struct fm_event_msg_hdr *)skb->data;
-	printk(KERN_INFO ">> hdr:%02x len:%02x sts:%02x numhci:%02x opcode:%02x type:%s dlen:%02x",
-	       evt_hdr->hdr, evt_hdr->len,
-	       evt_hdr->status, evt_hdr->num_fm_hci_cmds, evt_hdr->op,
-	       (evt_hdr->rd_wr) ? "RD" : "WR", evt_hdr->dlen);
-
-	len_org = skb->len - FM_EVT_MSG_HDR_SIZE;
-	if (len_org > 0) {
-		printk(KERN_CONT "\n   data(%d): ", evt_hdr->dlen);
-		len = min(len_org, 14);
-		for (index = 0; index < len; index++)
-			printk(KERN_CONT "%x ",
-			       skb->data[FM_EVT_MSG_HDR_SIZE + index]);
-		printk(KERN_CONT "%s", (len_org > 14) ? ".." : "");
-	}
-	printk(KERN_CONT "\n");
-}
-#endif
-
-void fmc_update_region_info(struct fmdev *fmdev, u8 region_to_set)
-{
-	fmdev->rx.region = region_configs[region_to_set];
-}
-
-/*
- * FM common sub-module will schedule this tasklet whenever it receives
- * FM packet from ST driver.
- */
-static void recv_tasklet(struct tasklet_struct *t)
-{
-	struct fmdev *fmdev;
-	struct fm_irq *irq_info;
-	struct fm_event_msg_hdr *evt_hdr;
-	struct sk_buff *skb;
-	u8 num_fm_hci_cmds;
-	unsigned long flags;
-
-	fmdev = from_tasklet(fmdev, t, tx_task);
-	irq_info = &fmdev->irq_info;
-	/* Process all packets in the RX queue */
-	while ((skb = skb_dequeue(&fmdev->rx_q))) {
-		if (skb->len < sizeof(struct fm_event_msg_hdr)) {
-			fmerr("skb(%p) has only %d bytes, at least need %zu bytes to decode\n",
-			      skb,
-			      skb->len, sizeof(struct fm_event_msg_hdr));
-			kfree_skb(skb);
-			continue;
-		}
-
-		evt_hdr = (void *)skb->data;
-		num_fm_hci_cmds = evt_hdr->num_fm_hci_cmds;
-
-		/* FM interrupt packet? */
-		if (evt_hdr->op == FM_INTERRUPT) {
-			/* FM interrupt handler started already? */
-			if (!test_bit(FM_INTTASK_RUNNING, &fmdev->flag)) {
-				set_bit(FM_INTTASK_RUNNING, &fmdev->flag);
-				if (irq_info->stage != 0) {
-					fmerr("Inval stage resetting to zero\n");
-					irq_info->stage = 0;
-				}
-
-				/*
-				 * Execute first function in interrupt handler
-				 * table.
-				 */
-				irq_info->handlers[irq_info->stage](fmdev);
-			} else {
-				set_bit(FM_INTTASK_SCHEDULE_PENDING, &fmdev->flag);
-			}
-			kfree_skb(skb);
-		}
-		/* Anyone waiting for this with completion handler? */
-		else if (evt_hdr->op == fmdev->pre_op && fmdev->resp_comp != NULL) {
-
-			spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
-			fmdev->resp_skb = skb;
-			spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
-			complete(fmdev->resp_comp);
-
-			fmdev->resp_comp = NULL;
-			atomic_set(&fmdev->tx_cnt, 1);
-		}
-		/* Is this for interrupt handler? */
-		else if (evt_hdr->op == fmdev->pre_op && fmdev->resp_comp == NULL) {
-			if (fmdev->resp_skb != NULL)
-				fmerr("Response SKB ptr not NULL\n");
-
-			spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
-			fmdev->resp_skb = skb;
-			spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
-
-			/* Execute interrupt handler where state index points */
-			irq_info->handlers[irq_info->stage](fmdev);
-
-			kfree_skb(skb);
-			atomic_set(&fmdev->tx_cnt, 1);
-		} else {
-			fmerr("Nobody claimed SKB(%p),purging\n", skb);
-		}
-
-		/*
-		 * Check flow control field. If Num_FM_HCI_Commands field is
-		 * not zero, schedule FM TX tasklet.
-		 */
-		if (num_fm_hci_cmds && atomic_read(&fmdev->tx_cnt))
-			if (!skb_queue_empty(&fmdev->tx_q))
-				tasklet_schedule(&fmdev->tx_task);
-	}
-}
-
-/* FM send tasklet: is scheduled when FM packet has to be sent to chip */
-static void send_tasklet(struct tasklet_struct *t)
-{
-	struct fmdev *fmdev;
-	struct sk_buff *skb;
-	int len;
-
-	fmdev = from_tasklet(fmdev, t, tx_task);
-
-	if (!atomic_read(&fmdev->tx_cnt))
-		return;
-
-	/* Check, is there any timeout happened to last transmitted packet */
-	if (time_is_before_jiffies(fmdev->last_tx_jiffies + FM_DRV_TX_TIMEOUT)) {
-		fmerr("TX timeout occurred\n");
-		atomic_set(&fmdev->tx_cnt, 1);
-	}
-
-	/* Send queued FM TX packets */
-	skb = skb_dequeue(&fmdev->tx_q);
-	if (!skb)
-		return;
-
-	atomic_dec(&fmdev->tx_cnt);
-	fmdev->pre_op = fm_cb(skb)->fm_op;
-
-	if (fmdev->resp_comp != NULL)
-		fmerr("Response completion handler is not NULL\n");
-
-	fmdev->resp_comp = fm_cb(skb)->completion;
-
-	/* Write FM packet to ST driver */
-	len = g_st_write(skb);
-	if (len < 0) {
-		kfree_skb(skb);
-		fmdev->resp_comp = NULL;
-		fmerr("TX tasklet failed to send skb(%p)\n", skb);
-		atomic_set(&fmdev->tx_cnt, 1);
-	} else {
-		fmdev->last_tx_jiffies = jiffies;
-	}
-}
-
-/*
- * Queues FM Channel-8 packet to FM TX queue and schedules FM TX tasklet for
- * transmission
- */
-static int fm_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type,	void *payload,
-		int payload_len, struct completion *wait_completion)
-{
-	struct sk_buff *skb;
-	struct fm_cmd_msg_hdr *hdr;
-	int size;
-
-	if (fm_op >= FM_INTERRUPT) {
-		fmerr("Invalid fm opcode - %d\n", fm_op);
-		return -EINVAL;
-	}
-	if (test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag) && payload == NULL) {
-		fmerr("Payload data is NULL during fw download\n");
-		return -EINVAL;
-	}
-	if (!test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag))
-		size =
-		    FM_CMD_MSG_HDR_SIZE + ((payload == NULL) ? 0 : payload_len);
-	else
-		size = payload_len;
-
-	skb = alloc_skb(size, GFP_ATOMIC);
-	if (!skb) {
-		fmerr("No memory to create new SKB\n");
-		return -ENOMEM;
-	}
-	/*
-	 * Don't fill FM header info for the commands which come from
-	 * FM firmware file.
-	 */
-	if (!test_bit(FM_FW_DW_INPROGRESS, &fmdev->flag) ||
-			test_bit(FM_INTTASK_RUNNING, &fmdev->flag)) {
-		/* Fill command header info */
-		hdr = skb_put(skb, FM_CMD_MSG_HDR_SIZE);
-		hdr->hdr = FM_PKT_LOGICAL_CHAN_NUMBER;	/* 0x08 */
-
-		/* 3 (fm_opcode,rd_wr,dlen) + payload len) */
-		hdr->len = ((payload == NULL) ? 0 : payload_len) + 3;
-
-		/* FM opcode */
-		hdr->op = fm_op;
-
-		/* read/write type */
-		hdr->rd_wr = type;
-		hdr->dlen = payload_len;
-		fm_cb(skb)->fm_op = fm_op;
-
-		/*
-		 * If firmware download has finished and the command is
-		 * not a read command then payload is != NULL - a write
-		 * command with u16 payload - convert to be16
-		 */
-		if (payload != NULL)
-			*(__be16 *)payload = cpu_to_be16(*(u16 *)payload);
-
-	} else if (payload != NULL) {
-		fm_cb(skb)->fm_op = *((u8 *)payload + 2);
-	}
-	if (payload != NULL)
-		skb_put_data(skb, payload, payload_len);
-
-	fm_cb(skb)->completion = wait_completion;
-	skb_queue_tail(&fmdev->tx_q, skb);
-	tasklet_schedule(&fmdev->tx_task);
-
-	return 0;
-}
-
-/* Sends FM Channel-8 command to the chip and waits for the response */
-int fmc_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type, void *payload,
-		unsigned int payload_len, void *response, int *response_len)
-{
-	struct sk_buff *skb;
-	struct fm_event_msg_hdr *evt_hdr;
-	unsigned long flags;
-	int ret;
-
-	init_completion(&fmdev->maintask_comp);
-	ret = fm_send_cmd(fmdev, fm_op, type, payload, payload_len,
-			    &fmdev->maintask_comp);
-	if (ret)
-		return ret;
-
-	if (!wait_for_completion_timeout(&fmdev->maintask_comp,
-					 FM_DRV_TX_TIMEOUT)) {
-		fmerr("Timeout(%d sec),didn't get regcompletion signal from RX tasklet\n",
-			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
-		return -ETIMEDOUT;
-	}
-	if (!fmdev->resp_skb) {
-		fmerr("Response SKB is missing\n");
-		return -EFAULT;
-	}
-	spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
-	skb = fmdev->resp_skb;
-	fmdev->resp_skb = NULL;
-	spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
-
-	evt_hdr = (void *)skb->data;
-	if (evt_hdr->status != 0) {
-		fmerr("Received event pkt status(%d) is not zero\n",
-			   evt_hdr->status);
-		kfree_skb(skb);
-		return -EIO;
-	}
-	/* Send response data to caller */
-	if (response != NULL && response_len != NULL && evt_hdr->dlen &&
-	    evt_hdr->dlen <= payload_len) {
-		/* Skip header info and copy only response data */
-		skb_pull(skb, sizeof(struct fm_event_msg_hdr));
-		memcpy(response, skb->data, evt_hdr->dlen);
-		*response_len = evt_hdr->dlen;
-	} else if (response_len != NULL && evt_hdr->dlen == 0) {
-		*response_len = 0;
-	}
-	kfree_skb(skb);
-
-	return 0;
-}
-
-/* --- Helper functions used in FM interrupt handlers ---*/
-static inline int check_cmdresp_status(struct fmdev *fmdev,
-		struct sk_buff **skb)
-{
-	struct fm_event_msg_hdr *fm_evt_hdr;
-	unsigned long flags;
-
-	del_timer(&fmdev->irq_info.timer);
-
-	spin_lock_irqsave(&fmdev->resp_skb_lock, flags);
-	*skb = fmdev->resp_skb;
-	fmdev->resp_skb = NULL;
-	spin_unlock_irqrestore(&fmdev->resp_skb_lock, flags);
-
-	fm_evt_hdr = (void *)(*skb)->data;
-	if (fm_evt_hdr->status != 0) {
-		fmerr("irq: opcode %x response status is not zero Initiating irq recovery process\n",
-				fm_evt_hdr->op);
-
-		mod_timer(&fmdev->irq_info.timer, jiffies + FM_DRV_TX_TIMEOUT);
-		return -1;
-	}
-
-	return 0;
-}
-
-static inline void fm_irq_common_cmd_resp_helper(struct fmdev *fmdev, u8 stage)
-{
-	struct sk_buff *skb;
-
-	if (!check_cmdresp_status(fmdev, &skb))
-		fm_irq_call_stage(fmdev, stage);
-}
-
-/*
- * Interrupt process timeout handler.
- * One of the irq handler did not get proper response from the chip. So take
- * recovery action here. FM interrupts are disabled in the beginning of
- * interrupt process. Therefore reset stage index to re-enable default
- * interrupts. So that next interrupt will be processed as usual.
- */
-static void int_timeout_handler(struct timer_list *t)
-{
-	struct fmdev *fmdev;
-	struct fm_irq *fmirq;
-
-	fmdbg("irq: timeout,trying to re-enable fm interrupts\n");
-	fmdev = from_timer(fmdev, t, irq_info.timer);
-	fmirq = &fmdev->irq_info;
-	fmirq->retry++;
-
-	if (fmirq->retry > FM_IRQ_TIMEOUT_RETRY_MAX) {
-		/* Stop recovery action (interrupt reenable process) and
-		 * reset stage index & retry count values */
-		fmirq->stage = 0;
-		fmirq->retry = 0;
-		fmerr("Recovery action failed duringirq processing, max retry reached\n");
-		return;
-	}
-	fm_irq_call_stage(fmdev, FM_SEND_INTMSK_CMD_IDX);
-}
-
-/* --------- FM interrupt handlers ------------*/
-static void fm_irq_send_flag_getcmd(struct fmdev *fmdev)
-{
-	u16 flag;
-
-	/* Send FLAG_GET command , to know the source of interrupt */
-	if (!fm_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, sizeof(flag), NULL))
-		fm_irq_timeout_stage(fmdev, FM_HANDLE_FLAG_GETCMD_RESP_IDX);
-}
-
-static void fm_irq_handle_flag_getcmd_resp(struct fmdev *fmdev)
-{
-	struct sk_buff *skb;
-	struct fm_event_msg_hdr *fm_evt_hdr;
-
-	if (check_cmdresp_status(fmdev, &skb))
-		return;
-
-	fm_evt_hdr = (void *)skb->data;
-	if (fm_evt_hdr->dlen > sizeof(fmdev->irq_info.flag))
-		return;
-
-	/* Skip header info and copy only response data */
-	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
-	memcpy(&fmdev->irq_info.flag, skb->data, fm_evt_hdr->dlen);
-
-	fmdev->irq_info.flag = be16_to_cpu((__force __be16)fmdev->irq_info.flag);
-	fmdbg("irq: flag register(0x%x)\n", fmdev->irq_info.flag);
-
-	/* Continue next function in interrupt handler table */
-	fm_irq_call_stage(fmdev, FM_HW_MAL_FUNC_IDX);
-}
-
-static void fm_irq_handle_hw_malfunction(struct fmdev *fmdev)
-{
-	if (fmdev->irq_info.flag & FM_MAL_EVENT & fmdev->irq_info.mask)
-		fmerr("irq: HW MAL int received - do nothing\n");
-
-	/* Continue next function in interrupt handler table */
-	fm_irq_call_stage(fmdev, FM_RDS_START_IDX);
-}
-
-static void fm_irq_handle_rds_start(struct fmdev *fmdev)
-{
-	if (fmdev->irq_info.flag & FM_RDS_EVENT & fmdev->irq_info.mask) {
-		fmdbg("irq: rds threshold reached\n");
-		fmdev->irq_info.stage = FM_RDS_SEND_RDS_GETCMD_IDX;
-	} else {
-		/* Continue next function in interrupt handler table */
-		fmdev->irq_info.stage = FM_HW_TUNE_OP_ENDED_IDX;
-	}
-
-	fm_irq_call(fmdev);
-}
-
-static void fm_irq_send_rdsdata_getcmd(struct fmdev *fmdev)
-{
-	/* Send the command to read RDS data from the chip */
-	if (!fm_send_cmd(fmdev, RDS_DATA_GET, REG_RD, NULL,
-			    (FM_RX_RDS_FIFO_THRESHOLD * 3), NULL))
-		fm_irq_timeout_stage(fmdev, FM_RDS_HANDLE_RDS_GETCMD_RESP_IDX);
-}
-
-/* Keeps track of current RX channel AF (Alternate Frequency) */
-static void fm_rx_update_af_cache(struct fmdev *fmdev, u8 af)
-{
-	struct tuned_station_info *stat_info = &fmdev->rx.stat_info;
-	u8 reg_idx = fmdev->rx.region.fm_band;
-	u8 index;
-	u32 freq;
-
-	/* First AF indicates the number of AF follows. Reset the list */
-	if ((af >= FM_RDS_1_AF_FOLLOWS) && (af <= FM_RDS_25_AF_FOLLOWS)) {
-		fmdev->rx.stat_info.af_list_max = (af - FM_RDS_1_AF_FOLLOWS + 1);
-		fmdev->rx.stat_info.afcache_size = 0;
-		fmdbg("No of expected AF : %d\n", fmdev->rx.stat_info.af_list_max);
-		return;
-	}
-
-	if (af < FM_RDS_MIN_AF)
-		return;
-	if (reg_idx == FM_BAND_EUROPE_US && af > FM_RDS_MAX_AF)
-		return;
-	if (reg_idx == FM_BAND_JAPAN && af > FM_RDS_MAX_AF_JAPAN)
-		return;
-
-	freq = fmdev->rx.region.bot_freq + (af * 100);
-	if (freq == fmdev->rx.freq) {
-		fmdbg("Current freq(%d) is matching with received AF(%d)\n",
-				fmdev->rx.freq, freq);
-		return;
-	}
-	/* Do check in AF cache */
-	for (index = 0; index < stat_info->afcache_size; index++) {
-		if (stat_info->af_cache[index] == freq)
-			break;
-	}
-	/* Reached the limit of the list - ignore the next AF */
-	if (index == stat_info->af_list_max) {
-		fmdbg("AF cache is full\n");
-		return;
-	}
-	/*
-	 * If we reached the end of the list then this AF is not
-	 * in the list - add it.
-	 */
-	if (index == stat_info->afcache_size) {
-		fmdbg("Storing AF %d to cache index %d\n", freq, index);
-		stat_info->af_cache[index] = freq;
-		stat_info->afcache_size++;
-	}
-}
-
-/*
- * Converts RDS buffer data from big endian format
- * to little endian format.
- */
-static void fm_rdsparse_swapbytes(struct fmdev *fmdev,
-		struct fm_rdsdata_format *rds_format)
-{
-	u8 index = 0;
-	u8 *rds_buff;
-
-	/*
-	 * Since in Orca the 2 RDS Data bytes are in little endian and
-	 * in Dolphin they are in big endian, the parsing of the RDS data
-	 * is chip dependent
-	 */
-	if (fmdev->asci_id != 0x6350) {
-		rds_buff = &rds_format->data.groupdatabuff.buff[0];
-		while (index + 1 < FM_RX_RDS_INFO_FIELD_MAX) {
-			swap(rds_buff[index], rds_buff[index + 1]);
-			index += 2;
-		}
-	}
-}
-
-static void fm_irq_handle_rdsdata_getcmd_resp(struct fmdev *fmdev)
-{
-	struct sk_buff *skb;
-	struct fm_rdsdata_format rds_fmt;
-	struct fm_rds *rds = &fmdev->rx.rds;
-	unsigned long group_idx, flags;
-	u8 *rds_data, meta_data, tmpbuf[FM_RDS_BLK_SIZE];
-	u8 type, blk_idx, idx;
-	u16 cur_picode;
-	u32 rds_len;
-
-	if (check_cmdresp_status(fmdev, &skb))
-		return;
-
-	/* Skip header info */
-	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
-	rds_data = skb->data;
-	rds_len = skb->len;
-
-	/* Parse the RDS data */
-	while (rds_len >= FM_RDS_BLK_SIZE) {
-		meta_data = rds_data[2];
-		/* Get the type: 0=A, 1=B, 2=C, 3=C', 4=D, 5=E */
-		type = (meta_data & 0x07);
-
-		/* Transform the blk type into index sequence (0, 1, 2, 3, 4) */
-		blk_idx = (type <= FM_RDS_BLOCK_C ? type : (type - 1));
-		fmdbg("Block index:%d(%s)\n", blk_idx,
-			   (meta_data & FM_RDS_STATUS_ERR_MASK) ? "Bad" : "Ok");
-
-		if ((meta_data & FM_RDS_STATUS_ERR_MASK) != 0)
-			break;
-
-		if (blk_idx > FM_RDS_BLK_IDX_D) {
-			fmdbg("Block sequence mismatch\n");
-			rds->last_blk_idx = -1;
-			break;
-		}
-
-		/* Skip checkword (control) byte and copy only data byte */
-		idx = array_index_nospec(blk_idx * (FM_RDS_BLK_SIZE - 1),
-					 FM_RX_RDS_INFO_FIELD_MAX - (FM_RDS_BLK_SIZE - 1));
-
-		memcpy(&rds_fmt.data.groupdatabuff.buff[idx], rds_data,
-		       FM_RDS_BLK_SIZE - 1);
-
-		rds->last_blk_idx = blk_idx;
-
-		/* If completed a whole group then handle it */
-		if (blk_idx == FM_RDS_BLK_IDX_D) {
-			fmdbg("Good block received\n");
-			fm_rdsparse_swapbytes(fmdev, &rds_fmt);
-
-			/*
-			 * Extract PI code and store in local cache.
-			 * We need this during AF switch processing.
-			 */
-			cur_picode = be16_to_cpu((__force __be16)rds_fmt.data.groupgeneral.pidata);
-			if (fmdev->rx.stat_info.picode != cur_picode)
-				fmdev->rx.stat_info.picode = cur_picode;
-
-			fmdbg("picode:%d\n", cur_picode);
-
-			group_idx = (rds_fmt.data.groupgeneral.blk_b[0] >> 3);
-			fmdbg("(fmdrv):Group:%ld%s\n", group_idx/2,
-					(group_idx % 2) ? "B" : "A");
-
-			group_idx = 1 << (rds_fmt.data.groupgeneral.blk_b[0] >> 3);
-			if (group_idx == FM_RDS_GROUP_TYPE_MASK_0A) {
-				fm_rx_update_af_cache(fmdev, rds_fmt.data.group0A.af[0]);
-				fm_rx_update_af_cache(fmdev, rds_fmt.data.group0A.af[1]);
-			}
-		}
-		rds_len -= FM_RDS_BLK_SIZE;
-		rds_data += FM_RDS_BLK_SIZE;
-	}
-
-	/* Copy raw rds data to internal rds buffer */
-	rds_data = skb->data;
-	rds_len = skb->len;
-
-	spin_lock_irqsave(&fmdev->rds_buff_lock, flags);
-	while (rds_len > 0) {
-		/*
-		 * Fill RDS buffer as per V4L2 specification.
-		 * Store control byte
-		 */
-		type = (rds_data[2] & 0x07);
-		blk_idx = (type <= FM_RDS_BLOCK_C ? type : (type - 1));
-		tmpbuf[2] = blk_idx;	/* Offset name */
-		tmpbuf[2] |= blk_idx << 3;	/* Received offset */
-
-		/* Store data byte */
-		tmpbuf[0] = rds_data[0];
-		tmpbuf[1] = rds_data[1];
-
-		memcpy(&rds->buff[rds->wr_idx], &tmpbuf, FM_RDS_BLK_SIZE);
-		rds->wr_idx = (rds->wr_idx + FM_RDS_BLK_SIZE) % rds->buf_size;
-
-		/* Check for overflow & start over */
-		if (rds->wr_idx == rds->rd_idx) {
-			fmdbg("RDS buffer overflow\n");
-			rds->wr_idx = 0;
-			rds->rd_idx = 0;
-			break;
-		}
-		rds_len -= FM_RDS_BLK_SIZE;
-		rds_data += FM_RDS_BLK_SIZE;
-	}
-	spin_unlock_irqrestore(&fmdev->rds_buff_lock, flags);
-
-	/* Wakeup read queue */
-	if (rds->wr_idx != rds->rd_idx)
-		wake_up_interruptible(&rds->read_queue);
-
-	fm_irq_call_stage(fmdev, FM_RDS_FINISH_IDX);
-}
-
-static void fm_irq_handle_rds_finish(struct fmdev *fmdev)
-{
-	fm_irq_call_stage(fmdev, FM_HW_TUNE_OP_ENDED_IDX);
-}
-
-static void fm_irq_handle_tune_op_ended(struct fmdev *fmdev)
-{
-	if (fmdev->irq_info.flag & (FM_FR_EVENT | FM_BL_EVENT) & fmdev->
-	    irq_info.mask) {
-		fmdbg("irq: tune ended/bandlimit reached\n");
-		if (test_and_clear_bit(FM_AF_SWITCH_INPROGRESS, &fmdev->flag)) {
-			fmdev->irq_info.stage = FM_AF_JUMP_RD_FREQ_IDX;
-		} else {
-			complete(&fmdev->maintask_comp);
-			fmdev->irq_info.stage = FM_HW_POWER_ENB_IDX;
-		}
-	} else
-		fmdev->irq_info.stage = FM_HW_POWER_ENB_IDX;
-
-	fm_irq_call(fmdev);
-}
-
-static void fm_irq_handle_power_enb(struct fmdev *fmdev)
-{
-	if (fmdev->irq_info.flag & FM_POW_ENB_EVENT) {
-		fmdbg("irq: Power Enabled/Disabled\n");
-		complete(&fmdev->maintask_comp);
-	}
-
-	fm_irq_call_stage(fmdev, FM_LOW_RSSI_START_IDX);
-}
-
-static void fm_irq_handle_low_rssi_start(struct fmdev *fmdev)
-{
-	if ((fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON) &&
-	    (fmdev->irq_info.flag & FM_LEV_EVENT & fmdev->irq_info.mask) &&
-	    (fmdev->rx.freq != FM_UNDEFINED_FREQ) &&
-	    (fmdev->rx.stat_info.afcache_size != 0)) {
-		fmdbg("irq: rssi level has fallen below threshold level\n");
-
-		/* Disable further low RSSI interrupts */
-		fmdev->irq_info.mask &= ~FM_LEV_EVENT;
-
-		fmdev->rx.afjump_idx = 0;
-		fmdev->rx.freq_before_jump = fmdev->rx.freq;
-		fmdev->irq_info.stage = FM_AF_JUMP_SETPI_IDX;
-	} else {
-		/* Continue next function in interrupt handler table */
-		fmdev->irq_info.stage = FM_SEND_INTMSK_CMD_IDX;
-	}
-
-	fm_irq_call(fmdev);
-}
-
-static void fm_irq_afjump_set_pi(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	/* Set PI code - must be updated if the AF list is not empty */
-	payload = fmdev->rx.stat_info.picode;
-	if (!fm_send_cmd(fmdev, RDS_PI_SET, REG_WR, &payload, sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SETPI_RESP_IDX);
-}
-
-static void fm_irq_handle_set_pi_resp(struct fmdev *fmdev)
-{
-	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_SETPI_MASK_IDX);
-}
-
-/*
- * Set PI mask.
- * 0xFFFF = Enable PI code matching
- * 0x0000 = Disable PI code matching
- */
-static void fm_irq_afjump_set_pimask(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	payload = 0x0000;
-	if (!fm_send_cmd(fmdev, RDS_PI_MASK_SET, REG_WR, &payload, sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SETPI_MASK_RESP_IDX);
-}
-
-static void fm_irq_handle_set_pimask_resp(struct fmdev *fmdev)
-{
-	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_SET_AF_FREQ_IDX);
-}
-
-static void fm_irq_afjump_setfreq(struct fmdev *fmdev)
-{
-	u16 frq_index;
-	u16 payload;
-
-	fmdbg("Switch to %d KHz\n", fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx]);
-	frq_index = (fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx] -
-	     fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
-
-	payload = frq_index;
-	if (!fm_send_cmd(fmdev, AF_FREQ_SET, REG_WR, &payload, sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_SET_AFFREQ_RESP_IDX);
-}
-
-static void fm_irq_handle_setfreq_resp(struct fmdev *fmdev)
-{
-	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_ENABLE_INT_IDX);
-}
-
-static void fm_irq_afjump_enableint(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	/* Enable FR (tuning operation ended) interrupt */
-	payload = FM_FR_EVENT;
-	if (!fm_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload, sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_ENABLE_INT_RESP_IDX);
-}
-
-static void fm_irq_afjump_enableint_resp(struct fmdev *fmdev)
-{
-	fm_irq_common_cmd_resp_helper(fmdev, FM_AF_JUMP_START_AFJUMP_IDX);
-}
-
-static void fm_irq_start_afjump(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	payload = FM_TUNER_AF_JUMP_MODE;
-	if (!fm_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
-			sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_HANDLE_START_AFJUMP_RESP_IDX);
-}
-
-static void fm_irq_handle_start_afjump_resp(struct fmdev *fmdev)
-{
-	struct sk_buff *skb;
-
-	if (check_cmdresp_status(fmdev, &skb))
-		return;
-
-	fmdev->irq_info.stage = FM_SEND_FLAG_GETCMD_IDX;
-	set_bit(FM_AF_SWITCH_INPROGRESS, &fmdev->flag);
-	clear_bit(FM_INTTASK_RUNNING, &fmdev->flag);
-}
-
-static void fm_irq_afjump_rd_freq(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	if (!fm_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_AF_JUMP_RD_FREQ_RESP_IDX);
-}
-
-static void fm_irq_afjump_rd_freq_resp(struct fmdev *fmdev)
-{
-	struct sk_buff *skb;
-	u16 read_freq;
-	u32 curr_freq, jumped_freq;
-
-	if (check_cmdresp_status(fmdev, &skb))
-		return;
-
-	/* Skip header info and copy only response data */
-	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
-	memcpy(&read_freq, skb->data, sizeof(read_freq));
-	read_freq = be16_to_cpu((__force __be16)read_freq);
-	curr_freq = fmdev->rx.region.bot_freq + ((u32)read_freq * FM_FREQ_MUL);
-
-	jumped_freq = fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx];
-
-	/* If the frequency was changed the jump succeeded */
-	if ((curr_freq != fmdev->rx.freq_before_jump) && (curr_freq == jumped_freq)) {
-		fmdbg("Successfully switched to alternate freq %d\n", curr_freq);
-		fmdev->rx.freq = curr_freq;
-		fm_rx_reset_rds_cache(fmdev);
-
-		/* AF feature is on, enable low level RSSI interrupt */
-		if (fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
-			fmdev->irq_info.mask |= FM_LEV_EVENT;
-
-		fmdev->irq_info.stage = FM_LOW_RSSI_FINISH_IDX;
-	} else {		/* jump to the next freq in the AF list */
-		fmdev->rx.afjump_idx++;
-
-		/* If we reached the end of the list - stop searching */
-		if (fmdev->rx.afjump_idx >= fmdev->rx.stat_info.afcache_size) {
-			fmdbg("AF switch processing failed\n");
-			fmdev->irq_info.stage = FM_LOW_RSSI_FINISH_IDX;
-		} else {	/* AF List is not over - try next one */
-
-			fmdbg("Trying next freq in AF cache\n");
-			fmdev->irq_info.stage = FM_AF_JUMP_SETPI_IDX;
-		}
-	}
-	fm_irq_call(fmdev);
-}
-
-static void fm_irq_handle_low_rssi_finish(struct fmdev *fmdev)
-{
-	fm_irq_call_stage(fmdev, FM_SEND_INTMSK_CMD_IDX);
-}
-
-static void fm_irq_send_intmsk_cmd(struct fmdev *fmdev)
-{
-	u16 payload;
-
-	/* Re-enable FM interrupts */
-	payload = fmdev->irq_info.mask;
-
-	if (!fm_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL))
-		fm_irq_timeout_stage(fmdev, FM_HANDLE_INTMSK_CMD_RESP_IDX);
-}
-
-static void fm_irq_handle_intmsk_cmd_resp(struct fmdev *fmdev)
-{
-	struct sk_buff *skb;
-
-	if (check_cmdresp_status(fmdev, &skb))
-		return;
-	/*
-	 * This is last function in interrupt table to be executed.
-	 * So, reset stage index to 0.
-	 */
-	fmdev->irq_info.stage = FM_SEND_FLAG_GETCMD_IDX;
-
-	/* Start processing any pending interrupt */
-	if (test_and_clear_bit(FM_INTTASK_SCHEDULE_PENDING, &fmdev->flag))
-		fmdev->irq_info.handlers[fmdev->irq_info.stage](fmdev);
-	else
-		clear_bit(FM_INTTASK_RUNNING, &fmdev->flag);
-}
-
-/* Returns availability of RDS data in internal buffer */
-int fmc_is_rds_data_available(struct fmdev *fmdev, struct file *file,
-				struct poll_table_struct *pts)
-{
-	poll_wait(file, &fmdev->rx.rds.read_queue, pts);
-	if (fmdev->rx.rds.rd_idx != fmdev->rx.rds.wr_idx)
-		return 0;
-
-	return -EAGAIN;
-}
-
-/* Copies RDS data from internal buffer to user buffer */
-int fmc_transfer_rds_from_internal_buff(struct fmdev *fmdev, struct file *file,
-		u8 __user *buf, size_t count)
-{
-	u32 block_count;
-	u8 tmpbuf[FM_RDS_BLK_SIZE];
-	unsigned long flags;
-	int ret;
-
-	if (fmdev->rx.rds.wr_idx == fmdev->rx.rds.rd_idx) {
-		if (file->f_flags & O_NONBLOCK)
-			return -EWOULDBLOCK;
-
-		ret = wait_event_interruptible(fmdev->rx.rds.read_queue,
-				(fmdev->rx.rds.wr_idx != fmdev->rx.rds.rd_idx));
-		if (ret)
-			return -EINTR;
-	}
-
-	/* Calculate block count from byte count */
-	count /= FM_RDS_BLK_SIZE;
-	block_count = 0;
-	ret = 0;
-
-	while (block_count < count) {
-		spin_lock_irqsave(&fmdev->rds_buff_lock, flags);
-
-		if (fmdev->rx.rds.wr_idx == fmdev->rx.rds.rd_idx) {
-			spin_unlock_irqrestore(&fmdev->rds_buff_lock, flags);
-			break;
-		}
-		memcpy(tmpbuf, &fmdev->rx.rds.buff[fmdev->rx.rds.rd_idx],
-					FM_RDS_BLK_SIZE);
-		fmdev->rx.rds.rd_idx += FM_RDS_BLK_SIZE;
-		if (fmdev->rx.rds.rd_idx >= fmdev->rx.rds.buf_size)
-			fmdev->rx.rds.rd_idx = 0;
-
-		spin_unlock_irqrestore(&fmdev->rds_buff_lock, flags);
-
-		if (copy_to_user(buf, tmpbuf, FM_RDS_BLK_SIZE))
-			break;
-
-		block_count++;
-		buf += FM_RDS_BLK_SIZE;
-		ret += FM_RDS_BLK_SIZE;
-	}
-	return ret;
-}
-
-int fmc_set_freq(struct fmdev *fmdev, u32 freq_to_set)
-{
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		return fm_rx_set_freq(fmdev, freq_to_set);
-
-	case FM_MODE_TX:
-		return fm_tx_set_freq(fmdev, freq_to_set);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-int fmc_get_freq(struct fmdev *fmdev, u32 *cur_tuned_frq)
-{
-	if (fmdev->rx.freq == FM_UNDEFINED_FREQ) {
-		fmerr("RX frequency is not set\n");
-		return -EPERM;
-	}
-	if (cur_tuned_frq == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		*cur_tuned_frq = fmdev->rx.freq;
-		return 0;
-
-	case FM_MODE_TX:
-		*cur_tuned_frq = 0;	/* TODO : Change this later */
-		return 0;
-
-	default:
-		return -EINVAL;
-	}
-
-}
-
-int fmc_set_region(struct fmdev *fmdev, u8 region_to_set)
-{
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		return fm_rx_set_region(fmdev, region_to_set);
-
-	case FM_MODE_TX:
-		return fm_tx_set_region(fmdev, region_to_set);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-int fmc_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
-{
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		return fm_rx_set_mute_mode(fmdev, mute_mode_toset);
-
-	case FM_MODE_TX:
-		return fm_tx_set_mute_mode(fmdev, mute_mode_toset);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-int fmc_set_stereo_mono(struct fmdev *fmdev, u16 mode)
-{
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		return fm_rx_set_stereo_mono(fmdev, mode);
-
-	case FM_MODE_TX:
-		return fm_tx_set_stereo_mono(fmdev, mode);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-int fmc_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
-{
-	switch (fmdev->curr_fmmode) {
-	case FM_MODE_RX:
-		return fm_rx_set_rds_mode(fmdev, rds_en_dis);
-
-	case FM_MODE_TX:
-		return fm_tx_set_rds_mode(fmdev, rds_en_dis);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-/* Sends power off command to the chip */
-static int fm_power_down(struct fmdev *fmdev)
-{
-	u16 payload;
-	int ret;
-
-	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
-		fmerr("FM core is not ready\n");
-		return -EPERM;
-	}
-	if (fmdev->curr_fmmode == FM_MODE_OFF) {
-		fmdbg("FM chip is already in OFF state\n");
-		return 0;
-	}
-
-	payload = 0x0;
-	ret = fmc_send_cmd(fmdev, FM_POWER_MODE, REG_WR, &payload,
-		sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return fmc_release(fmdev);
-}
-
-/* Reads init command from FM firmware file and loads to the chip */
-static int fm_download_firmware(struct fmdev *fmdev, const u8 *fw_name)
-{
-	const struct firmware *fw_entry;
-	struct bts_header *fw_header;
-	struct bts_action *action;
-	struct bts_action_delay *delay;
-	u8 *fw_data;
-	int ret, fw_len;
-
-	set_bit(FM_FW_DW_INPROGRESS, &fmdev->flag);
-
-	ret = request_firmware(&fw_entry, fw_name,
-				&fmdev->radio_dev->dev);
-	if (ret < 0) {
-		fmerr("Unable to read firmware(%s) content\n", fw_name);
-		return ret;
-	}
-	fmdbg("Firmware(%s) length : %zu bytes\n", fw_name, fw_entry->size);
-
-	fw_data = (void *)fw_entry->data;
-	fw_len = fw_entry->size;
-
-	fw_header = (struct bts_header *)fw_data;
-	if (fw_header->magic != FM_FW_FILE_HEADER_MAGIC) {
-		fmerr("%s not a legal TI firmware file\n", fw_name);
-		ret = -EINVAL;
-		goto rel_fw;
-	}
-	fmdbg("FW(%s) magic number : 0x%x\n", fw_name, fw_header->magic);
-
-	/* Skip file header info , we already verified it */
-	fw_data += sizeof(struct bts_header);
-	fw_len -= sizeof(struct bts_header);
-
-	while (fw_data && fw_len > 0) {
-		action = (struct bts_action *)fw_data;
-
-		switch (action->type) {
-		case ACTION_SEND_COMMAND:	/* Send */
-			ret = fmc_send_cmd(fmdev, 0, 0, action->data,
-					   action->size, NULL, NULL);
-			if (ret)
-				goto rel_fw;
-
-			break;
-
-		case ACTION_DELAY:	/* Delay */
-			delay = (struct bts_action_delay *)action->data;
-			mdelay(delay->msec);
-			break;
-		}
-
-		fw_data += (sizeof(struct bts_action) + (action->size));
-		fw_len -= (sizeof(struct bts_action) + (action->size));
-	}
-	fmdbg("Transferred only %d of %d bytes of the firmware to chip\n",
-	      fw_entry->size - fw_len, fw_entry->size);
-rel_fw:
-	release_firmware(fw_entry);
-	clear_bit(FM_FW_DW_INPROGRESS, &fmdev->flag);
-
-	return ret;
-}
-
-/* Loads default RX configuration to the chip */
-static int load_default_rx_configuration(struct fmdev *fmdev)
-{
-	int ret;
-
-	ret = fm_rx_set_volume(fmdev, FM_DEFAULT_RX_VOLUME);
-	if (ret < 0)
-		return ret;
-
-	return fm_rx_set_rssi_threshold(fmdev, FM_DEFAULT_RSSI_THRESHOLD);
-}
-
-/* Does FM power on sequence */
-static int fm_power_up(struct fmdev *fmdev, u8 mode)
-{
-	u16 payload;
-	__be16 asic_id = 0, asic_ver = 0;
-	int resp_len, ret;
-	u8 fw_name[50];
-
-	if (mode >= FM_MODE_ENTRY_MAX) {
-		fmerr("Invalid firmware download option\n");
-		return -EINVAL;
-	}
-
-	/*
-	 * Initialize FM common module. FM GPIO toggling is
-	 * taken care in Shared Transport driver.
-	 */
-	ret = fmc_prepare(fmdev);
-	if (ret < 0) {
-		fmerr("Unable to prepare FM Common\n");
-		return ret;
-	}
-
-	payload = FM_ENABLE;
-	if (fmc_send_cmd(fmdev, FM_POWER_MODE, REG_WR, &payload,
-			sizeof(payload), NULL, NULL))
-		goto rel;
-
-	/* Allow the chip to settle down in Channel-8 mode */
-	msleep(20);
-
-	if (fmc_send_cmd(fmdev, ASIC_ID_GET, REG_RD, NULL,
-			sizeof(asic_id), &asic_id, &resp_len))
-		goto rel;
-
-	if (fmc_send_cmd(fmdev, ASIC_VER_GET, REG_RD, NULL,
-			sizeof(asic_ver), &asic_ver, &resp_len))
-		goto rel;
-
-	fmdbg("ASIC ID: 0x%x , ASIC Version: %d\n",
-		be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
-
-	sprintf(fw_name, "%s_%x.%d.bts", FM_FMC_FW_FILE_START,
-		be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
-
-	ret = fm_download_firmware(fmdev, fw_name);
-	if (ret < 0) {
-		fmdbg("Failed to download firmware file %s\n", fw_name);
-		goto rel;
-	}
-	sprintf(fw_name, "%s_%x.%d.bts", (mode == FM_MODE_RX) ?
-			FM_RX_FW_FILE_START : FM_TX_FW_FILE_START,
-			be16_to_cpu(asic_id), be16_to_cpu(asic_ver));
-
-	ret = fm_download_firmware(fmdev, fw_name);
-	if (ret < 0) {
-		fmdbg("Failed to download firmware file %s\n", fw_name);
-		goto rel;
-	} else
-		return ret;
-rel:
-	return fmc_release(fmdev);
-}
-
-/* Set FM Modes(TX, RX, OFF) */
-int fmc_set_mode(struct fmdev *fmdev, u8 fm_mode)
-{
-	int ret = 0;
-
-	if (fm_mode >= FM_MODE_ENTRY_MAX) {
-		fmerr("Invalid FM mode\n");
-		return -EINVAL;
-	}
-	if (fmdev->curr_fmmode == fm_mode) {
-		fmdbg("Already fm is in mode(%d)\n", fm_mode);
-		return ret;
-	}
-
-	switch (fm_mode) {
-	case FM_MODE_OFF:	/* OFF Mode */
-		ret = fm_power_down(fmdev);
-		if (ret < 0) {
-			fmerr("Failed to set OFF mode\n");
-			return ret;
-		}
-		break;
-
-	case FM_MODE_TX:	/* TX Mode */
-	case FM_MODE_RX:	/* RX Mode */
-		/* Power down before switching to TX or RX mode */
-		if (fmdev->curr_fmmode != FM_MODE_OFF) {
-			ret = fm_power_down(fmdev);
-			if (ret < 0) {
-				fmerr("Failed to set OFF mode\n");
-				return ret;
-			}
-			msleep(30);
-		}
-		ret = fm_power_up(fmdev, fm_mode);
-		if (ret < 0) {
-			fmerr("Failed to load firmware\n");
-			return ret;
-		}
-	}
-	fmdev->curr_fmmode = fm_mode;
-
-	/* Set default configuration */
-	if (fmdev->curr_fmmode == FM_MODE_RX) {
-		fmdbg("Loading default rx configuration..\n");
-		ret = load_default_rx_configuration(fmdev);
-		if (ret < 0)
-			fmerr("Failed to load default values\n");
-	}
-
-	return ret;
-}
-
-/* Returns current FM mode (TX, RX, OFF) */
-int fmc_get_mode(struct fmdev *fmdev, u8 *fmmode)
-{
-	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
-		fmerr("FM core is not ready\n");
-		return -EPERM;
-	}
-	if (fmmode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*fmmode = fmdev->curr_fmmode;
-	return 0;
-}
-
-/* Called by ST layer when FM packet is available */
-static long fm_st_receive(void *arg, struct sk_buff *skb)
-{
-	struct fmdev *fmdev;
-
-	fmdev = arg;
-
-	if (skb == NULL) {
-		fmerr("Invalid SKB received from ST\n");
-		return -EFAULT;
-	}
-
-	if (skb->cb[0] != FM_PKT_LOGICAL_CHAN_NUMBER) {
-		fmerr("Received SKB (%p) is not FM Channel 8 pkt\n", skb);
-		return -EINVAL;
-	}
-
-	memcpy(skb_push(skb, 1), &skb->cb[0], 1);
-	skb_queue_tail(&fmdev->rx_q, skb);
-	tasklet_schedule(&fmdev->rx_task);
-
-	return 0;
-}
-
-/*
- * Called by ST layer to indicate protocol registration completion
- * status.
- */
-static void fm_st_reg_comp_cb(void *arg, int data)
-{
-	struct fmdev *fmdev;
-
-	fmdev = (struct fmdev *)arg;
-	fmdev->streg_cbdata = data;
-	complete(&wait_for_fmdrv_reg_comp);
-}
-
-/*
- * This function will be called from FM V4L2 open function.
- * Register with ST driver and initialize driver data.
- */
-int fmc_prepare(struct fmdev *fmdev)
-{
-	static struct st_proto_s fm_st_proto;
-	int ret;
-
-	if (test_bit(FM_CORE_READY, &fmdev->flag)) {
-		fmdbg("FM Core is already up\n");
-		return 0;
-	}
-
-	memset(&fm_st_proto, 0, sizeof(fm_st_proto));
-	fm_st_proto.recv = fm_st_receive;
-	fm_st_proto.match_packet = NULL;
-	fm_st_proto.reg_complete_cb = fm_st_reg_comp_cb;
-	fm_st_proto.write = NULL; /* TI ST driver will fill write pointer */
-	fm_st_proto.priv_data = fmdev;
-	fm_st_proto.chnl_id = 0x08;
-	fm_st_proto.max_frame_size = 0xff;
-	fm_st_proto.hdr_len = 1;
-	fm_st_proto.offset_len_in_hdr = 0;
-	fm_st_proto.len_size = 1;
-	fm_st_proto.reserve = 1;
-
-	ret = st_register(&fm_st_proto);
-	if (ret == -EINPROGRESS) {
-		init_completion(&wait_for_fmdrv_reg_comp);
-		fmdev->streg_cbdata = -EINPROGRESS;
-		fmdbg("%s waiting for ST reg completion signal\n", __func__);
-
-		if (!wait_for_completion_timeout(&wait_for_fmdrv_reg_comp,
-						 FM_ST_REG_TIMEOUT)) {
-			fmerr("Timeout(%d sec), didn't get reg completion signal from ST\n",
-					jiffies_to_msecs(FM_ST_REG_TIMEOUT) / 1000);
-			return -ETIMEDOUT;
-		}
-		if (fmdev->streg_cbdata != 0) {
-			fmerr("ST reg comp CB called with error status %d\n",
-			      fmdev->streg_cbdata);
-			return -EAGAIN;
-		}
-
-		ret = 0;
-	} else if (ret < 0) {
-		fmerr("st_register failed %d\n", ret);
-		return -EAGAIN;
-	}
-
-	if (fm_st_proto.write != NULL) {
-		g_st_write = fm_st_proto.write;
-	} else {
-		fmerr("Failed to get ST write func pointer\n");
-		ret = st_unregister(&fm_st_proto);
-		if (ret < 0)
-			fmerr("st_unregister failed %d\n", ret);
-		return -EAGAIN;
-	}
-
-	spin_lock_init(&fmdev->rds_buff_lock);
-	spin_lock_init(&fmdev->resp_skb_lock);
-
-	/* Initialize TX queue and TX tasklet */
-	skb_queue_head_init(&fmdev->tx_q);
-	tasklet_setup(&fmdev->tx_task, send_tasklet);
-
-	/* Initialize RX Queue and RX tasklet */
-	skb_queue_head_init(&fmdev->rx_q);
-	tasklet_setup(&fmdev->rx_task, recv_tasklet);
-
-	fmdev->irq_info.stage = 0;
-	atomic_set(&fmdev->tx_cnt, 1);
-	fmdev->resp_comp = NULL;
-
-	timer_setup(&fmdev->irq_info.timer, int_timeout_handler, 0);
-	/*TODO: add FM_STIC_EVENT later */
-	fmdev->irq_info.mask = FM_MAL_EVENT;
-
-	/* Region info */
-	fmdev->rx.region = region_configs[default_radio_region];
-
-	fmdev->rx.mute_mode = FM_MUTE_OFF;
-	fmdev->rx.rf_depend_mute = FM_RX_RF_DEPENDENT_MUTE_OFF;
-	fmdev->rx.rds.flag = FM_RDS_DISABLE;
-	fmdev->rx.freq = FM_UNDEFINED_FREQ;
-	fmdev->rx.rds_mode = FM_RDS_SYSTEM_RDS;
-	fmdev->rx.af_mode = FM_RX_RDS_AF_SWITCH_MODE_OFF;
-	fmdev->irq_info.retry = 0;
-
-	fm_rx_reset_rds_cache(fmdev);
-	init_waitqueue_head(&fmdev->rx.rds.read_queue);
-
-	fm_rx_reset_station_info(fmdev);
-	set_bit(FM_CORE_READY, &fmdev->flag);
-
-	return ret;
-}
-
-/*
- * This function will be called from FM V4L2 release function.
- * Unregister from ST driver.
- */
-int fmc_release(struct fmdev *fmdev)
-{
-	static struct st_proto_s fm_st_proto;
-	int ret;
-
-	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
-		fmdbg("FM Core is already down\n");
-		return 0;
-	}
-	/* Service pending read */
-	wake_up_interruptible(&fmdev->rx.rds.read_queue);
-
-	tasklet_kill(&fmdev->tx_task);
-	tasklet_kill(&fmdev->rx_task);
-
-	skb_queue_purge(&fmdev->tx_q);
-	skb_queue_purge(&fmdev->rx_q);
-
-	fmdev->resp_comp = NULL;
-	fmdev->rx.freq = 0;
-
-	memset(&fm_st_proto, 0, sizeof(fm_st_proto));
-	fm_st_proto.chnl_id = 0x08;
-
-	ret = st_unregister(&fm_st_proto);
-
-	if (ret < 0)
-		fmerr("Failed to de-register FM from ST %d\n", ret);
-	else
-		fmdbg("Successfully unregistered from ST\n");
-
-	clear_bit(FM_CORE_READY, &fmdev->flag);
-	return ret;
-}
-
-/*
- * Module init function. Ask FM V4L module to register video device.
- * Allocate memory for FM driver context and RX RDS buffer.
- */
-static int __init fm_drv_init(void)
-{
-	struct fmdev *fmdev = NULL;
-	int ret = -ENOMEM;
-
-	fmdbg("FM driver version %s\n", FM_DRV_VERSION);
-
-	fmdev = kzalloc(sizeof(struct fmdev), GFP_KERNEL);
-	if (NULL == fmdev) {
-		fmerr("Can't allocate operation structure memory\n");
-		return ret;
-	}
-	fmdev->rx.rds.buf_size = default_rds_buf * FM_RDS_BLK_SIZE;
-	fmdev->rx.rds.buff = kzalloc(fmdev->rx.rds.buf_size, GFP_KERNEL);
-	if (NULL == fmdev->rx.rds.buff) {
-		fmerr("Can't allocate rds ring buffer\n");
-		goto rel_dev;
-	}
-
-	ret = fm_v4l2_init_video_device(fmdev, radio_nr);
-	if (ret < 0)
-		goto rel_rdsbuf;
-
-	fmdev->irq_info.handlers = int_handler_table;
-	fmdev->curr_fmmode = FM_MODE_OFF;
-	fmdev->tx_data.pwr_lvl = FM_PWR_LVL_DEF;
-	fmdev->tx_data.preemph = FM_TX_PREEMPH_50US;
-	return ret;
-
-rel_rdsbuf:
-	kfree(fmdev->rx.rds.buff);
-rel_dev:
-	kfree(fmdev);
-
-	return ret;
-}
-
-/* Module exit function. Ask FM V4L module to unregister video device */
-static void __exit fm_drv_exit(void)
-{
-	struct fmdev *fmdev = NULL;
-
-	fmdev = fm_v4l2_deinit_video_device();
-	if (fmdev != NULL) {
-		kfree(fmdev->rx.rds.buff);
-		kfree(fmdev);
-	}
-}
-
-module_init(fm_drv_init);
-module_exit(fm_drv_exit);
-
-/* ------------- Module Info ------------- */
-MODULE_AUTHOR("Manjunatha Halli <manjunatha_halli@ti.com>");
-MODULE_DESCRIPTION("FM Driver for TI's Connectivity chip. " FM_DRV_VERSION);
-MODULE_VERSION(FM_DRV_VERSION);
-MODULE_LICENSE("GPL");
diff --git a/drivers/media/radio/wl128x/fmdrv_common.h b/drivers/media/radio/wl128x/fmdrv_common.h
deleted file mode 100644
index 6a287eadae75..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_common.h
+++ /dev/null
@@ -1,389 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  FM Common module header file
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#ifndef _FMDRV_COMMON_H
-#define _FMDRV_COMMON_H
-
-#define FM_ST_REG_TIMEOUT   msecs_to_jiffies(6000)	/* 6 sec */
-#define FM_PKT_LOGICAL_CHAN_NUMBER  0x08   /* Logical channel 8 */
-
-#define REG_RD       0x1
-#define REG_WR      0x0
-
-struct fm_reg_table {
-	u8 opcode;
-	u8 type;
-	u8 *name;
-};
-
-#define STEREO_GET               0
-#define RSSI_LVL_GET             1
-#define IF_COUNT_GET             2
-#define FLAG_GET                 3
-#define RDS_SYNC_GET             4
-#define RDS_DATA_GET             5
-#define FREQ_SET                 10
-#define AF_FREQ_SET              11
-#define MOST_MODE_SET            12
-#define MOST_BLEND_SET           13
-#define DEMPH_MODE_SET           14
-#define SEARCH_LVL_SET           15
-#define BAND_SET                 16
-#define MUTE_STATUS_SET          17
-#define RDS_PAUSE_LVL_SET        18
-#define RDS_PAUSE_DUR_SET        19
-#define RDS_MEM_SET              20
-#define RDS_BLK_B_SET            21
-#define RDS_MSK_B_SET            22
-#define RDS_PI_MASK_SET          23
-#define RDS_PI_SET               24
-#define RDS_SYSTEM_SET           25
-#define INT_MASK_SET             26
-#define SEARCH_DIR_SET           27
-#define VOLUME_SET               28
-#define AUDIO_ENABLE_SET         29
-#define PCM_MODE_SET             30
-#define I2S_MODE_CONFIG_SET      31
-#define POWER_SET                32
-#define INTX_CONFIG_SET          33
-#define PULL_EN_SET              34
-#define HILO_SET                 35
-#define SWITCH2FREF              36
-#define FREQ_DRIFT_REPORT        37
-
-#define PCE_GET                  40
-#define FIRM_VER_GET             41
-#define ASIC_VER_GET             42
-#define ASIC_ID_GET              43
-#define MAN_ID_GET               44
-#define TUNER_MODE_SET           45
-#define STOP_SEARCH              46
-#define RDS_CNTRL_SET            47
-
-#define WRITE_HARDWARE_REG       100
-#define CODE_DOWNLOAD            101
-#define RESET                    102
-
-#define FM_POWER_MODE            254
-#define FM_INTERRUPT             255
-
-/* Transmitter API */
-
-#define CHANL_SET                55
-#define CHANL_BW_SET		56
-#define REF_SET                  57
-#define POWER_ENB_SET            90
-#define POWER_ATT_SET            58
-#define POWER_LEV_SET            59
-#define AUDIO_DEV_SET            60
-#define PILOT_DEV_SET            61
-#define RDS_DEV_SET              62
-#define TX_BAND_SET              65
-#define PUPD_SET                 91
-#define AUDIO_IO_SET             63
-#define PREMPH_SET               64
-#define MONO_SET                 66
-#define MUTE                     92
-#define MPX_LMT_ENABLE           67
-#define PI_SET                   93
-#define ECC_SET                  69
-#define PTY                      70
-#define AF                       71
-#define DISPLAY_MODE             74
-#define RDS_REP_SET              77
-#define RDS_CONFIG_DATA_SET      98
-#define RDS_DATA_SET             99
-#define RDS_DATA_ENB             94
-#define TA_SET                   78
-#define TP_SET                   79
-#define DI_SET                   80
-#define MS_SET                   81
-#define PS_SCROLL_SPEED          82
-#define TX_AUDIO_LEVEL_TEST      96
-#define TX_AUDIO_LEVEL_TEST_THRESHOLD    73
-#define TX_AUDIO_INPUT_LEVEL_RANGE_SET   54
-#define RX_ANTENNA_SELECT        87
-#define I2C_DEV_ADDR_SET         86
-#define REF_ERR_CALIB_PARAM_SET          88
-#define REF_ERR_CALIB_PERIODICITY_SET    89
-#define SOC_INT_TRIGGER                  52
-#define SOC_AUDIO_PATH_SET               83
-#define SOC_PCMI_OVERRIDE                84
-#define SOC_I2S_OVERRIDE         85
-#define RSSI_BLOCK_SCAN_FREQ_SET 95
-#define RSSI_BLOCK_SCAN_START    97
-#define RSSI_BLOCK_SCAN_DATA_GET  5
-#define READ_FMANT_TUNE_VALUE            104
-
-/* SKB helpers */
-struct fm_skb_cb {
-	__u8 fm_op;
-	struct completion *completion;
-};
-
-#define fm_cb(skb) ((struct fm_skb_cb *)(skb->cb))
-
-/* FM Channel-8 command message format */
-struct fm_cmd_msg_hdr {
-	__u8 hdr;		/* Logical Channel-8 */
-	__u8 len;		/* Number of bytes follows */
-	__u8 op;		/* FM Opcode */
-	__u8 rd_wr;		/* Read/Write command */
-	__u8 dlen;		/* Length of payload */
-} __attribute__ ((packed));
-
-#define FM_CMD_MSG_HDR_SIZE    5	/* sizeof(struct fm_cmd_msg_hdr) */
-
-/* FM Channel-8 event messgage format */
-struct fm_event_msg_hdr {
-	__u8 header;		/* Logical Channel-8 */
-	__u8 len;		/* Number of bytes follows */
-	__u8 status;		/* Event status */
-	__u8 num_fm_hci_cmds;	/* Number of pkts the host allowed to send */
-	__u8 op;		/* FM Opcode */
-	__u8 rd_wr;		/* Read/Write command */
-	__u8 dlen;		/* Length of payload */
-} __attribute__ ((packed));
-
-#define FM_EVT_MSG_HDR_SIZE     7	/* sizeof(struct fm_event_msg_hdr) */
-
-/* TI's magic number in firmware file */
-#define FM_FW_FILE_HEADER_MAGIC	     0x42535442
-
-#define FM_ENABLE   1
-#define FM_DISABLE  0
-
-/* FLAG_GET register bits */
-#define FM_FR_EVENT		BIT(0)
-#define FM_BL_EVENT		BIT(1)
-#define FM_RDS_EVENT		BIT(2)
-#define FM_BBLK_EVENT		BIT(3)
-#define FM_LSYNC_EVENT		BIT(4)
-#define FM_LEV_EVENT		BIT(5)
-#define FM_IFFR_EVENT		BIT(6)
-#define FM_PI_EVENT		BIT(7)
-#define FM_PD_EVENT		BIT(8)
-#define FM_STIC_EVENT		BIT(9)
-#define FM_MAL_EVENT		BIT(10)
-#define FM_POW_ENB_EVENT	BIT(11)
-
-/*
- * Firmware files of FM. ASIC ID and ASIC version will be appened to this,
- * later.
- */
-#define FM_FMC_FW_FILE_START      ("fmc_ch8")
-#define FM_RX_FW_FILE_START       ("fm_rx_ch8")
-#define FM_TX_FW_FILE_START       ("fm_tx_ch8")
-
-#define FM_UNDEFINED_FREQ		   0xFFFFFFFF
-
-/* Band types */
-#define FM_BAND_EUROPE_US	0
-#define FM_BAND_JAPAN		1
-
-/* Seek directions */
-#define FM_SEARCH_DIRECTION_DOWN	0
-#define FM_SEARCH_DIRECTION_UP		1
-
-/* Tunner modes */
-#define FM_TUNER_STOP_SEARCH_MODE	0
-#define FM_TUNER_PRESET_MODE		1
-#define FM_TUNER_AUTONOMOUS_SEARCH_MODE	2
-#define FM_TUNER_AF_JUMP_MODE		3
-
-/* Min and Max volume */
-#define FM_RX_VOLUME_MIN	0
-#define FM_RX_VOLUME_MAX	70
-
-/* Volume gain step */
-#define FM_RX_VOLUME_GAIN_STEP	0x370
-
-/* Mute modes */
-#define	FM_MUTE_ON		0
-#define FM_MUTE_OFF		1
-#define	FM_MUTE_ATTENUATE	2
-
-#define FM_RX_UNMUTE_MODE		0x00
-#define FM_RX_RF_DEP_MODE		0x01
-#define FM_RX_AC_MUTE_MODE		0x02
-#define FM_RX_HARD_MUTE_LEFT_MODE	0x04
-#define FM_RX_HARD_MUTE_RIGHT_MODE	0x08
-#define FM_RX_SOFT_MUTE_FORCE_MODE	0x10
-
-/* RF dependent mute mode */
-#define FM_RX_RF_DEPENDENT_MUTE_ON	1
-#define FM_RX_RF_DEPENDENT_MUTE_OFF	0
-
-/* RSSI threshold min and max */
-#define FM_RX_RSSI_THRESHOLD_MIN	-128
-#define FM_RX_RSSI_THRESHOLD_MAX	127
-
-/* Stereo/Mono mode */
-#define FM_STEREO_MODE		0
-#define FM_MONO_MODE		1
-#define FM_STEREO_SOFT_BLEND	1
-
-/* FM RX De-emphasis filter modes */
-#define FM_RX_EMPHASIS_FILTER_50_USEC	0
-#define FM_RX_EMPHASIS_FILTER_75_USEC	1
-
-/* FM RDS modes */
-#define FM_RDS_DISABLE	0
-#define FM_RDS_ENABLE	1
-
-#define FM_NO_PI_CODE	0
-
-/* FM and RX RDS block enable/disable  */
-#define FM_RX_PWR_SET_FM_ON_RDS_OFF		0x1
-#define FM_RX_PWR_SET_FM_AND_RDS_BLK_ON		0x3
-#define FM_RX_PWR_SET_FM_AND_RDS_BLK_OFF	0x0
-
-/* RX RDS */
-#define FM_RX_RDS_FLUSH_FIFO		0x1
-#define FM_RX_RDS_FIFO_THRESHOLD	64	/* tuples */
-#define FM_RDS_BLK_SIZE		3	/* 3 bytes */
-
-/* RDS block types */
-#define FM_RDS_BLOCK_A		0
-#define FM_RDS_BLOCK_B		1
-#define FM_RDS_BLOCK_C		2
-#define FM_RDS_BLOCK_Ctag	3
-#define FM_RDS_BLOCK_D		4
-#define FM_RDS_BLOCK_E		5
-
-#define FM_RDS_BLK_IDX_A		0
-#define FM_RDS_BLK_IDX_B		1
-#define FM_RDS_BLK_IDX_C		2
-#define FM_RDS_BLK_IDX_D		3
-#define FM_RDS_BLK_IDX_UNKNOWN	0xF0
-
-#define FM_RDS_STATUS_ERR_MASK	0x18
-
-/*
- * Represents an RDS group type & version.
- * There are 15 groups, each group has 2 versions: A and B.
- */
-#define FM_RDS_GROUP_TYPE_MASK_0A	    BIT(0)
-#define FM_RDS_GROUP_TYPE_MASK_0B	    BIT(1)
-#define FM_RDS_GROUP_TYPE_MASK_1A	    BIT(2)
-#define FM_RDS_GROUP_TYPE_MASK_1B	    BIT(3)
-#define FM_RDS_GROUP_TYPE_MASK_2A	    BIT(4)
-#define FM_RDS_GROUP_TYPE_MASK_2B	    BIT(5)
-#define FM_RDS_GROUP_TYPE_MASK_3A	    BIT(6)
-#define FM_RDS_GROUP_TYPE_MASK_3B	    BIT(7)
-#define FM_RDS_GROUP_TYPE_MASK_4A	    BIT(8)
-#define FM_RDS_GROUP_TYPE_MASK_4B	    BIT(9)
-#define FM_RDS_GROUP_TYPE_MASK_5A	    BIT(10)
-#define FM_RDS_GROUP_TYPE_MASK_5B	    BIT(11)
-#define FM_RDS_GROUP_TYPE_MASK_6A	    BIT(12)
-#define FM_RDS_GROUP_TYPE_MASK_6B	    BIT(13)
-#define FM_RDS_GROUP_TYPE_MASK_7A	    BIT(14)
-#define FM_RDS_GROUP_TYPE_MASK_7B	    BIT(15)
-#define FM_RDS_GROUP_TYPE_MASK_8A	    BIT(16)
-#define FM_RDS_GROUP_TYPE_MASK_8B	    BIT(17)
-#define FM_RDS_GROUP_TYPE_MASK_9A	    BIT(18)
-#define FM_RDS_GROUP_TYPE_MASK_9B	    BIT(19)
-#define FM_RDS_GROUP_TYPE_MASK_10A	    BIT(20)
-#define FM_RDS_GROUP_TYPE_MASK_10B	    BIT(21)
-#define FM_RDS_GROUP_TYPE_MASK_11A	    BIT(22)
-#define FM_RDS_GROUP_TYPE_MASK_11B	    BIT(23)
-#define FM_RDS_GROUP_TYPE_MASK_12A	    BIT(24)
-#define FM_RDS_GROUP_TYPE_MASK_12B	    BIT(25)
-#define FM_RDS_GROUP_TYPE_MASK_13A	    BIT(26)
-#define FM_RDS_GROUP_TYPE_MASK_13B	    BIT(27)
-#define FM_RDS_GROUP_TYPE_MASK_14A	    BIT(28)
-#define FM_RDS_GROUP_TYPE_MASK_14B	    BIT(29)
-#define FM_RDS_GROUP_TYPE_MASK_15A	    BIT(30)
-#define FM_RDS_GROUP_TYPE_MASK_15B	    BIT(31)
-
-/* RX Alternate Frequency info */
-#define FM_RDS_MIN_AF			  1
-#define FM_RDS_MAX_AF			204
-#define FM_RDS_MAX_AF_JAPAN		140
-#define FM_RDS_1_AF_FOLLOWS		225
-#define FM_RDS_25_AF_FOLLOWS		249
-
-/* RDS system type (RDS/RBDS) */
-#define FM_RDS_SYSTEM_RDS		0
-#define FM_RDS_SYSTEM_RBDS		1
-
-/* AF on/off */
-#define FM_RX_RDS_AF_SWITCH_MODE_ON	1
-#define FM_RX_RDS_AF_SWITCH_MODE_OFF	0
-
-/* Retry count when interrupt process goes wrong */
-#define FM_IRQ_TIMEOUT_RETRY_MAX	5	/* 5 times */
-
-/* Audio IO set values */
-#define FM_RX_AUDIO_ENABLE_I2S	0x01
-#define FM_RX_AUDIO_ENABLE_ANALOG	0x02
-#define FM_RX_AUDIO_ENABLE_I2S_AND_ANALOG	0x03
-#define FM_RX_AUDIO_ENABLE_DISABLE	0x00
-
-/* HI/LO set values */
-#define FM_RX_IFFREQ_TO_HI_SIDE		0x0
-#define FM_RX_IFFREQ_TO_LO_SIDE		0x1
-#define FM_RX_IFFREQ_HILO_AUTOMATIC	0x2
-
-/*
- * Default RX mode configuration. Chip will be configured
- * with this default values after loading RX firmware.
- */
-#define FM_DEFAULT_RX_VOLUME		10
-#define FM_DEFAULT_RSSI_THRESHOLD	3
-
-/* Range for TX power level in units for dB/uV */
-#define FM_PWR_LVL_LOW			91
-#define FM_PWR_LVL_HIGH			122
-
-/* Chip specific default TX power level value */
-#define FM_PWR_LVL_DEF			4
-
-/* FM TX Pre-emphasis filter values */
-#define FM_TX_PREEMPH_OFF		1
-#define FM_TX_PREEMPH_50US		0
-#define FM_TX_PREEMPH_75US		2
-
-/* FM TX antenna impedance values */
-#define FM_TX_ANT_IMP_50		0
-#define FM_TX_ANT_IMP_200		1
-#define FM_TX_ANT_IMP_500		2
-
-/* Functions exported by FM common sub-module */
-int fmc_prepare(struct fmdev *);
-int fmc_release(struct fmdev *);
-
-void fmc_update_region_info(struct fmdev *, u8);
-int fmc_send_cmd(struct fmdev *, u8, u16,
-				void *, unsigned int, void *, int *);
-int fmc_is_rds_data_available(struct fmdev *, struct file *,
-				struct poll_table_struct *);
-int fmc_transfer_rds_from_internal_buff(struct fmdev *, struct file *,
-					u8 __user *, size_t);
-
-int fmc_set_freq(struct fmdev *, u32);
-int fmc_set_mode(struct fmdev *, u8);
-int fmc_set_region(struct fmdev *, u8);
-int fmc_set_mute_mode(struct fmdev *, u8);
-int fmc_set_stereo_mono(struct fmdev *, u16);
-int fmc_set_rds_mode(struct fmdev *, u8);
-
-int fmc_get_freq(struct fmdev *, u32 *);
-int fmc_get_region(struct fmdev *, u8 *);
-int fmc_get_mode(struct fmdev *, u8 *);
-
-/*
- * channel spacing
- */
-#define FM_CHANNEL_SPACING_50KHZ 1
-#define FM_CHANNEL_SPACING_100KHZ 2
-#define FM_CHANNEL_SPACING_200KHZ 4
-#define FM_FREQ_MUL 50
-
-#endif
-
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.c b/drivers/media/radio/wl128x/fmdrv_rx.c
deleted file mode 100644
index 419cf2e03bcf..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_rx.c
+++ /dev/null
@@ -1,820 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  This sub-module of FM driver implements FM RX functionality.
- *
- *  Copyright (C) 2011 Texas Instruments
- *  Author: Raja Mani <raja_mani@ti.com>
- *  Author: Manjunatha Halli <manjunatha_halli@ti.com>
- */
-
-#include "fmdrv.h"
-#include "fmdrv_common.h"
-#include "fmdrv_rx.h"
-
-void fm_rx_reset_rds_cache(struct fmdev *fmdev)
-{
-	fmdev->rx.rds.flag = FM_RDS_DISABLE;
-	fmdev->rx.rds.last_blk_idx = 0;
-	fmdev->rx.rds.wr_idx = 0;
-	fmdev->rx.rds.rd_idx = 0;
-
-	if (fmdev->rx.af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
-		fmdev->irq_info.mask |= FM_LEV_EVENT;
-}
-
-void fm_rx_reset_station_info(struct fmdev *fmdev)
-{
-	fmdev->rx.stat_info.picode = FM_NO_PI_CODE;
-	fmdev->rx.stat_info.afcache_size = 0;
-	fmdev->rx.stat_info.af_list_max = 0;
-}
-
-int fm_rx_set_freq(struct fmdev *fmdev, u32 freq)
-{
-	unsigned long timeleft;
-	u16 payload, curr_frq, intr_flag;
-	u32 curr_frq_in_khz;
-	u32 resp_len;
-	int ret;
-
-	if (freq < fmdev->rx.region.bot_freq || freq > fmdev->rx.region.top_freq) {
-		fmerr("Invalid frequency %d\n", freq);
-		return -EINVAL;
-	}
-
-	/* Set audio enable */
-	payload = FM_RX_AUDIO_ENABLE_I2S_AND_ANALOG;
-
-	ret = fmc_send_cmd(fmdev, AUDIO_ENABLE_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Set hilo to automatic selection */
-	payload = FM_RX_IFFREQ_HILO_AUTOMATIC;
-	ret = fmc_send_cmd(fmdev, HILO_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Calculate frequency index and set*/
-	payload = (freq - fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
-
-	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Read flags - just to clear any pending interrupts if we had */
-	ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2, NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Enable FR, BL interrupts */
-	intr_flag = fmdev->irq_info.mask;
-	fmdev->irq_info.mask = (FM_FR_EVENT | FM_BL_EVENT);
-	payload = fmdev->irq_info.mask;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Start tune */
-	payload = FM_TUNER_PRESET_MODE;
-	ret = fmc_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		goto exit;
-
-	/* Wait for tune ended interrupt */
-	init_completion(&fmdev->maintask_comp);
-	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
-			FM_DRV_TX_TIMEOUT);
-	if (!timeleft) {
-		fmerr("Timeout(%d sec),didn't get tune ended int\n",
-			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
-		ret = -ETIMEDOUT;
-		goto exit;
-	}
-
-	/* Read freq back to confirm */
-	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, 2, &curr_frq, &resp_len);
-	if (ret < 0)
-		goto exit;
-
-	curr_frq = be16_to_cpu((__force __be16)curr_frq);
-	curr_frq_in_khz = (fmdev->rx.region.bot_freq + ((u32)curr_frq * FM_FREQ_MUL));
-
-	if (curr_frq_in_khz != freq) {
-		pr_info("Frequency is set to (%d) but requested freq is (%d)\n",
-			curr_frq_in_khz, freq);
-	}
-
-	/* Update local cache  */
-	fmdev->rx.freq = curr_frq_in_khz;
-exit:
-	/* Re-enable default FM interrupts */
-	fmdev->irq_info.mask = intr_flag;
-	payload = fmdev->irq_info.mask;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Reset RDS cache and current station pointers */
-	fm_rx_reset_rds_cache(fmdev);
-	fm_rx_reset_station_info(fmdev);
-
-	return ret;
-}
-
-static int fm_rx_set_channel_spacing(struct fmdev *fmdev, u32 spacing)
-{
-	u16 payload;
-	int ret;
-
-	if (spacing > 0 && spacing <= 50000)
-		spacing = FM_CHANNEL_SPACING_50KHZ;
-	else if (spacing > 50000 && spacing <= 100000)
-		spacing = FM_CHANNEL_SPACING_100KHZ;
-	else
-		spacing = FM_CHANNEL_SPACING_200KHZ;
-
-	/* set channel spacing */
-	payload = spacing;
-	ret = fmc_send_cmd(fmdev, CHANL_BW_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.region.chanl_space = spacing * FM_FREQ_MUL;
-
-	return ret;
-}
-
-int fm_rx_seek(struct fmdev *fmdev, u32 seek_upward,
-		u32 wrap_around, u32 spacing)
-{
-	u32 resp_len;
-	u16 curr_frq, next_frq, last_frq;
-	u16 payload, int_reason, intr_flag;
-	u16 offset, space_idx;
-	unsigned long timeleft;
-	int ret;
-
-	/* Set channel spacing */
-	ret = fm_rx_set_channel_spacing(fmdev, spacing);
-	if (ret < 0) {
-		fmerr("Failed to set channel spacing\n");
-		return ret;
-	}
-
-	/* Read the current frequency from chip */
-	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL,
-			sizeof(curr_frq), &curr_frq, &resp_len);
-	if (ret < 0)
-		return ret;
-
-	curr_frq = be16_to_cpu((__force __be16)curr_frq);
-	last_frq = (fmdev->rx.region.top_freq - fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
-
-	/* Check the offset in order to be aligned to the channel spacing*/
-	space_idx = fmdev->rx.region.chanl_space / FM_FREQ_MUL;
-	offset = curr_frq % space_idx;
-
-	next_frq = seek_upward ? curr_frq + space_idx /* Seek Up */ :
-				curr_frq - space_idx /* Seek Down */ ;
-
-	/*
-	 * Add or subtract offset in order to stay aligned to the channel
-	 * spacing.
-	 */
-	if ((short)next_frq < 0)
-		next_frq = last_frq - offset;
-	else if (next_frq > last_frq)
-		next_frq = 0 + offset;
-
-again:
-	/* Set calculated next frequency to perform seek */
-	payload = next_frq;
-	ret = fmc_send_cmd(fmdev, FREQ_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Set search direction (0:Seek Down, 1:Seek Up) */
-	payload = (seek_upward ? FM_SEARCH_DIRECTION_UP : FM_SEARCH_DIRECTION_DOWN);
-	ret = fmc_send_cmd(fmdev, SEARCH_DIR_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Read flags - just to clear any pending interrupts if we had */
-	ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2, NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Enable FR, BL interrupts */
-	intr_flag = fmdev->irq_info.mask;
-	fmdev->irq_info.mask = (FM_FR_EVENT | FM_BL_EVENT);
-	payload = fmdev->irq_info.mask;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Start seek */
-	payload = FM_TUNER_AUTONOMOUS_SEARCH_MODE;
-	ret = fmc_send_cmd(fmdev, TUNER_MODE_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Wait for tune ended/band limit reached interrupt */
-	init_completion(&fmdev->maintask_comp);
-	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
-			FM_DRV_RX_SEEK_TIMEOUT);
-	if (!timeleft) {
-		fmerr("Timeout(%d sec),didn't get tune ended int\n",
-			   jiffies_to_msecs(FM_DRV_RX_SEEK_TIMEOUT) / 1000);
-		return -ENODATA;
-	}
-
-	int_reason = fmdev->irq_info.flag & (FM_TUNE_COMPLETE | FM_BAND_LIMIT);
-
-	/* Re-enable default FM interrupts */
-	fmdev->irq_info.mask = intr_flag;
-	payload = fmdev->irq_info.mask;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	if (int_reason & FM_BL_EVENT) {
-		if (wrap_around == 0) {
-			fmdev->rx.freq = seek_upward ?
-				fmdev->rx.region.top_freq :
-				fmdev->rx.region.bot_freq;
-		} else {
-			fmdev->rx.freq = seek_upward ?
-				fmdev->rx.region.bot_freq :
-				fmdev->rx.region.top_freq;
-			/* Calculate frequency index to write */
-			next_frq = (fmdev->rx.freq -
-					fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
-			goto again;
-		}
-	} else {
-		/* Read freq to know where operation tune operation stopped */
-		ret = fmc_send_cmd(fmdev, FREQ_SET, REG_RD, NULL, 2,
-				&curr_frq, &resp_len);
-		if (ret < 0)
-			return ret;
-
-		curr_frq = be16_to_cpu((__force __be16)curr_frq);
-		fmdev->rx.freq = (fmdev->rx.region.bot_freq +
-				((u32)curr_frq * FM_FREQ_MUL));
-
-	}
-	/* Reset RDS cache and current station pointers */
-	fm_rx_reset_rds_cache(fmdev);
-	fm_rx_reset_station_info(fmdev);
-
-	return ret;
-}
-
-int fm_rx_set_volume(struct fmdev *fmdev, u16 vol_to_set)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (vol_to_set > FM_RX_VOLUME_MAX) {
-		fmerr("Volume is not within(%d-%d) range\n",
-			   FM_RX_VOLUME_MIN, FM_RX_VOLUME_MAX);
-		return -EINVAL;
-	}
-	vol_to_set *= FM_RX_VOLUME_GAIN_STEP;
-
-	payload = vol_to_set;
-	ret = fmc_send_cmd(fmdev, VOLUME_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.volume = vol_to_set;
-	return ret;
-}
-
-/* Get volume */
-int fm_rx_get_volume(struct fmdev *fmdev, u16 *curr_vol)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_vol == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_vol = fmdev->rx.volume / FM_RX_VOLUME_GAIN_STEP;
-
-	return 0;
-}
-
-/* To get current band's bottom and top frequency */
-int fm_rx_get_band_freq_range(struct fmdev *fmdev, u32 *bot_freq, u32 *top_freq)
-{
-	if (bot_freq != NULL)
-		*bot_freq = fmdev->rx.region.bot_freq;
-
-	if (top_freq != NULL)
-		*top_freq = fmdev->rx.region.top_freq;
-
-	return 0;
-}
-
-/* Returns current band index (0-Europe/US; 1-Japan) */
-void fm_rx_get_region(struct fmdev *fmdev, u8 *region)
-{
-	*region = fmdev->rx.region.fm_band;
-}
-
-/* Sets band (0-Europe/US; 1-Japan) */
-int fm_rx_set_region(struct fmdev *fmdev, u8 region_to_set)
-{
-	u16 payload;
-	u32 new_frq = 0;
-	int ret;
-
-	if (region_to_set != FM_BAND_EUROPE_US &&
-	    region_to_set != FM_BAND_JAPAN) {
-		fmerr("Invalid band\n");
-		return -EINVAL;
-	}
-
-	if (fmdev->rx.region.fm_band == region_to_set) {
-		fmerr("Requested band is already configured\n");
-		return 0;
-	}
-
-	/* Send cmd to set the band  */
-	payload = (u16)region_to_set;
-	ret = fmc_send_cmd(fmdev, BAND_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmc_update_region_info(fmdev, region_to_set);
-
-	/* Check whether current RX frequency is within band boundary */
-	if (fmdev->rx.freq < fmdev->rx.region.bot_freq)
-		new_frq = fmdev->rx.region.bot_freq;
-	else if (fmdev->rx.freq > fmdev->rx.region.top_freq)
-		new_frq = fmdev->rx.region.top_freq;
-
-	if (new_frq) {
-		fmdbg("Current freq is not within band limit boundary,switching to %d KHz\n",
-		      new_frq);
-		 /* Current RX frequency is not in range. So, update it */
-		ret = fm_rx_set_freq(fmdev, new_frq);
-	}
-
-	return ret;
-}
-
-/* Reads current mute mode (Mute Off/On/Attenuate)*/
-int fm_rx_get_mute_mode(struct fmdev *fmdev, u8 *curr_mute_mode)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_mute_mode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_mute_mode = fmdev->rx.mute_mode;
-
-	return 0;
-}
-
-static int fm_config_rx_mute_reg(struct fmdev *fmdev)
-{
-	u16 payload, muteval;
-	int ret;
-
-	muteval = 0;
-	switch (fmdev->rx.mute_mode) {
-	case FM_MUTE_ON:
-		muteval = FM_RX_AC_MUTE_MODE;
-		break;
-
-	case FM_MUTE_OFF:
-		muteval = FM_RX_UNMUTE_MODE;
-		break;
-
-	case FM_MUTE_ATTENUATE:
-		muteval = FM_RX_SOFT_MUTE_FORCE_MODE;
-		break;
-	}
-	if (fmdev->rx.rf_depend_mute == FM_RX_RF_DEPENDENT_MUTE_ON)
-		muteval |= FM_RX_RF_DEP_MODE;
-	else
-		muteval &= ~FM_RX_RF_DEP_MODE;
-
-	payload = muteval;
-	ret = fmc_send_cmd(fmdev, MUTE_STATUS_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-/* Configures mute mode (Mute Off/On/Attenuate) */
-int fm_rx_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
-{
-	u8 org_state;
-	int ret;
-
-	if (fmdev->rx.mute_mode == mute_mode_toset)
-		return 0;
-
-	org_state = fmdev->rx.mute_mode;
-	fmdev->rx.mute_mode = mute_mode_toset;
-
-	ret = fm_config_rx_mute_reg(fmdev);
-	if (ret < 0) {
-		fmdev->rx.mute_mode = org_state;
-		return ret;
-	}
-
-	return 0;
-}
-
-/* Gets RF dependent soft mute mode enable/disable status */
-int fm_rx_get_rfdepend_softmute(struct fmdev *fmdev, u8 *curr_mute_mode)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_mute_mode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_mute_mode = fmdev->rx.rf_depend_mute;
-
-	return 0;
-}
-
-/* Sets RF dependent soft mute mode */
-int fm_rx_set_rfdepend_softmute(struct fmdev *fmdev, u8 rfdepend_mute)
-{
-	u8 org_state;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (rfdepend_mute != FM_RX_RF_DEPENDENT_MUTE_ON &&
-	    rfdepend_mute != FM_RX_RF_DEPENDENT_MUTE_OFF) {
-		fmerr("Invalid RF dependent soft mute\n");
-		return -EINVAL;
-	}
-	if (fmdev->rx.rf_depend_mute == rfdepend_mute)
-		return 0;
-
-	org_state = fmdev->rx.rf_depend_mute;
-	fmdev->rx.rf_depend_mute = rfdepend_mute;
-
-	ret = fm_config_rx_mute_reg(fmdev);
-	if (ret < 0) {
-		fmdev->rx.rf_depend_mute = org_state;
-		return ret;
-	}
-
-	return 0;
-}
-
-/* Returns the signal strength level of current channel */
-int fm_rx_get_rssi_level(struct fmdev *fmdev, u16 *rssilvl)
-{
-	__be16 curr_rssi_lel;
-	u32 resp_len;
-	int ret;
-
-	if (rssilvl == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-	/* Read current RSSI level */
-	ret = fmc_send_cmd(fmdev, RSSI_LVL_GET, REG_RD, NULL, 2,
-			&curr_rssi_lel, &resp_len);
-	if (ret < 0)
-		return ret;
-
-	*rssilvl = be16_to_cpu(curr_rssi_lel);
-
-	return 0;
-}
-
-/*
- * Sets the signal strength level that once reached
- * will stop the auto search process
- */
-int fm_rx_set_rssi_threshold(struct fmdev *fmdev, short rssi_lvl_toset)
-{
-	u16 payload;
-	int ret;
-
-	if (rssi_lvl_toset < FM_RX_RSSI_THRESHOLD_MIN ||
-			rssi_lvl_toset > FM_RX_RSSI_THRESHOLD_MAX) {
-		fmerr("Invalid RSSI threshold level\n");
-		return -EINVAL;
-	}
-	payload = (u16)rssi_lvl_toset;
-	ret = fmc_send_cmd(fmdev, SEARCH_LVL_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.rssi_threshold = rssi_lvl_toset;
-
-	return 0;
-}
-
-/* Returns current RX RSSI threshold value */
-int fm_rx_get_rssi_threshold(struct fmdev *fmdev, short *curr_rssi_lvl)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_rssi_lvl == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_rssi_lvl = fmdev->rx.rssi_threshold;
-
-	return 0;
-}
-
-/* Sets RX stereo/mono modes */
-int fm_rx_set_stereo_mono(struct fmdev *fmdev, u16 mode)
-{
-	u16 payload;
-	int ret;
-
-	if (mode != FM_STEREO_MODE && mode != FM_MONO_MODE) {
-		fmerr("Invalid mode\n");
-		return -EINVAL;
-	}
-
-	/* Set stereo/mono mode */
-	payload = (u16)mode;
-	ret = fmc_send_cmd(fmdev, MOST_MODE_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Set stereo blending mode */
-	payload = FM_STEREO_SOFT_BLEND;
-	ret = fmc_send_cmd(fmdev, MOST_BLEND_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-/* Gets current RX stereo/mono mode */
-int fm_rx_get_stereo_mono(struct fmdev *fmdev, u16 *mode)
-{
-	__be16 curr_mode;
-	u32 resp_len;
-	int ret;
-
-	if (mode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	ret = fmc_send_cmd(fmdev, MOST_MODE_SET, REG_RD, NULL, 2,
-			&curr_mode, &resp_len);
-	if (ret < 0)
-		return ret;
-
-	*mode = be16_to_cpu(curr_mode);
-
-	return 0;
-}
-
-/* Choose RX de-emphasis filter mode (50us/75us) */
-int fm_rx_set_deemphasis_mode(struct fmdev *fmdev, u16 mode)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (mode != FM_RX_EMPHASIS_FILTER_50_USEC &&
-			mode != FM_RX_EMPHASIS_FILTER_75_USEC) {
-		fmerr("Invalid rx de-emphasis mode (%d)\n", mode);
-		return -EINVAL;
-	}
-
-	payload = mode;
-	ret = fmc_send_cmd(fmdev, DEMPH_MODE_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.deemphasis_mode = mode;
-
-	return 0;
-}
-
-/* Gets current RX de-emphasis filter mode */
-int fm_rx_get_deemph_mode(struct fmdev *fmdev, u16 *curr_deemphasis_mode)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_deemphasis_mode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_deemphasis_mode = fmdev->rx.deemphasis_mode;
-
-	return 0;
-}
-
-/* Enable/Disable RX RDS */
-int fm_rx_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
-{
-	u16 payload;
-	int ret;
-
-	if (rds_en_dis != FM_RDS_ENABLE && rds_en_dis != FM_RDS_DISABLE) {
-		fmerr("Invalid rds option\n");
-		return -EINVAL;
-	}
-
-	if (rds_en_dis == FM_RDS_ENABLE
-	    && fmdev->rx.rds.flag == FM_RDS_DISABLE) {
-		/* Turn on RX RDS and RDS circuit */
-		payload = FM_RX_PWR_SET_FM_AND_RDS_BLK_ON;
-		ret = fmc_send_cmd(fmdev, POWER_SET, REG_WR, &payload,
-				sizeof(payload), NULL, NULL);
-		if (ret < 0)
-			return ret;
-
-		/* Clear and reset RDS FIFO */
-		payload = FM_RX_RDS_FLUSH_FIFO;
-		ret = fmc_send_cmd(fmdev, RDS_CNTRL_SET, REG_WR, &payload,
-		sizeof(payload), NULL, NULL);
-		if (ret < 0)
-			return ret;
-
-		/* Read flags - just to clear any pending interrupts. */
-		ret = fmc_send_cmd(fmdev, FLAG_GET, REG_RD, NULL, 2,
-				NULL, NULL);
-		if (ret < 0)
-			return ret;
-
-		/* Set RDS FIFO threshold value */
-		payload = FM_RX_RDS_FIFO_THRESHOLD;
-		ret = fmc_send_cmd(fmdev, RDS_MEM_SET, REG_WR, &payload,
-		sizeof(payload), NULL, NULL);
-		if (ret < 0)
-			return ret;
-
-		/* Enable RDS interrupt */
-		fmdev->irq_info.mask |= FM_RDS_EVENT;
-		payload = fmdev->irq_info.mask;
-		ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-				sizeof(payload), NULL, NULL);
-		if (ret < 0) {
-			fmdev->irq_info.mask &= ~FM_RDS_EVENT;
-			return ret;
-		}
-
-		/* Update our local flag */
-		fmdev->rx.rds.flag = FM_RDS_ENABLE;
-	} else if (rds_en_dis == FM_RDS_DISABLE
-		   && fmdev->rx.rds.flag == FM_RDS_ENABLE) {
-		/* Turn off RX RDS */
-		payload = FM_RX_PWR_SET_FM_ON_RDS_OFF;
-		ret = fmc_send_cmd(fmdev, POWER_SET, REG_WR, &payload,
-				sizeof(payload), NULL, NULL);
-		if (ret < 0)
-			return ret;
-
-		/* Reset RDS pointers */
-		fmdev->rx.rds.last_blk_idx = 0;
-		fmdev->rx.rds.wr_idx = 0;
-		fmdev->rx.rds.rd_idx = 0;
-		fm_rx_reset_station_info(fmdev);
-
-		/* Update RDS local cache */
-		fmdev->irq_info.mask &= ~(FM_RDS_EVENT);
-		fmdev->rx.rds.flag = FM_RDS_DISABLE;
-	}
-
-	return 0;
-}
-
-/* Returns current RX RDS enable/disable status */
-int fm_rx_get_rds_mode(struct fmdev *fmdev, u8 *curr_rds_en_dis)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (curr_rds_en_dis == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*curr_rds_en_dis = fmdev->rx.rds.flag;
-
-	return 0;
-}
-
-/* Sets RDS operation mode (RDS/RDBS) */
-int fm_rx_set_rds_system(struct fmdev *fmdev, u8 rds_mode)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (rds_mode != FM_RDS_SYSTEM_RDS && rds_mode != FM_RDS_SYSTEM_RBDS) {
-		fmerr("Invalid rds mode\n");
-		return -EINVAL;
-	}
-	/* Set RDS operation mode */
-	payload = (u16)rds_mode;
-	ret = fmc_send_cmd(fmdev, RDS_SYSTEM_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.rds_mode = rds_mode;
-
-	return 0;
-}
-
-/* Configures Alternate Frequency switch mode */
-int fm_rx_set_af_switch(struct fmdev *fmdev, u8 af_mode)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (af_mode != FM_RX_RDS_AF_SWITCH_MODE_ON &&
-	    af_mode != FM_RX_RDS_AF_SWITCH_MODE_OFF) {
-		fmerr("Invalid af mode\n");
-		return -EINVAL;
-	}
-	/* Enable/disable low RSSI interrupt based on af_mode */
-	if (af_mode == FM_RX_RDS_AF_SWITCH_MODE_ON)
-		fmdev->irq_info.mask |= FM_LEV_EVENT;
-	else
-		fmdev->irq_info.mask &= ~FM_LEV_EVENT;
-
-	payload = fmdev->irq_info.mask;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->rx.af_mode = af_mode;
-
-	return 0;
-}
-
-/* Returns Alternate Frequency switch status */
-int fm_rx_get_af_switch(struct fmdev *fmdev, u8 *af_mode)
-{
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	if (af_mode == NULL) {
-		fmerr("Invalid memory\n");
-		return -ENOMEM;
-	}
-
-	*af_mode = fmdev->rx.af_mode;
-
-	return 0;
-}
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.h b/drivers/media/radio/wl128x/fmdrv_rx.h
deleted file mode 100644
index 2748e99662c3..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_rx.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  FM RX module header.
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#ifndef _FMDRV_RX_H
-#define _FMDRV_RX_H
-
-int fm_rx_set_freq(struct fmdev *, u32);
-int fm_rx_set_mute_mode(struct fmdev *, u8);
-int fm_rx_set_stereo_mono(struct fmdev *, u16);
-int fm_rx_set_rds_mode(struct fmdev *, u8);
-int fm_rx_set_rds_system(struct fmdev *, u8);
-int fm_rx_set_volume(struct fmdev *, u16);
-int fm_rx_set_rssi_threshold(struct fmdev *, short);
-int fm_rx_set_region(struct fmdev *, u8);
-int fm_rx_set_rfdepend_softmute(struct fmdev *, u8);
-int fm_rx_set_deemphasis_mode(struct fmdev *, u16);
-int fm_rx_set_af_switch(struct fmdev *, u8);
-
-void fm_rx_reset_rds_cache(struct fmdev *);
-void fm_rx_reset_station_info(struct fmdev *);
-
-int fm_rx_seek(struct fmdev *, u32, u32, u32);
-
-int fm_rx_get_rds_mode(struct fmdev *, u8 *);
-int fm_rx_get_mute_mode(struct fmdev *, u8 *);
-int fm_rx_get_volume(struct fmdev *, u16 *);
-int fm_rx_get_band_freq_range(struct fmdev *,
-					u32 *, u32 *);
-int fm_rx_get_stereo_mono(struct fmdev *, u16 *);
-int fm_rx_get_rssi_level(struct fmdev *, u16 *);
-int fm_rx_get_rssi_threshold(struct fmdev *, short *);
-int fm_rx_get_rfdepend_softmute(struct fmdev *, u8 *);
-int fm_rx_get_deemph_mode(struct fmdev *, u16 *);
-int fm_rx_get_af_switch(struct fmdev *, u8 *);
-void fm_rx_get_region(struct fmdev *, u8 *);
-
-int fm_rx_set_chanl_spacing(struct fmdev *, u8);
-int fm_rx_get_chanl_spacing(struct fmdev *, u8 *);
-#endif
-
diff --git a/drivers/media/radio/wl128x/fmdrv_tx.c b/drivers/media/radio/wl128x/fmdrv_tx.c
deleted file mode 100644
index c589de02f4f5..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_tx.c
+++ /dev/null
@@ -1,413 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  This sub-module of FM driver implements FM TX functionality.
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#include <linux/delay.h>
-#include "fmdrv.h"
-#include "fmdrv_common.h"
-#include "fmdrv_tx.h"
-
-int fm_tx_set_stereo_mono(struct fmdev *fmdev, u16 mode)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->tx_data.aud_mode == mode)
-		return 0;
-
-	fmdbg("stereo mode: %d\n", mode);
-
-	/* Set Stereo/Mono mode */
-	payload = (1 - mode);
-	ret = fmc_send_cmd(fmdev, MONO_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fmdev->tx_data.aud_mode = mode;
-
-	return ret;
-}
-
-static int set_rds_text(struct fmdev *fmdev, u8 *rds_text)
-{
-	u16 payload;
-	int ret;
-
-	ret = fmc_send_cmd(fmdev, RDS_DATA_SET, REG_WR, rds_text,
-			strlen(rds_text), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Scroll mode */
-	payload = (u16)0x1;
-	ret = fmc_send_cmd(fmdev, DISPLAY_MODE, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-static int set_rds_data_mode(struct fmdev *fmdev, u8 mode)
-{
-	u16 payload;
-	int ret;
-
-	/* Setting unique PI TODO: how unique? */
-	payload = (u16)0xcafe;
-	ret = fmc_send_cmd(fmdev, PI_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Set decoder id */
-	payload = (u16)0xa;
-	ret = fmc_send_cmd(fmdev, DI_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* TODO: RDS_MODE_GET? */
-	return 0;
-}
-
-static int set_rds_len(struct fmdev *fmdev, u8 type, u16 len)
-{
-	u16 payload;
-	int ret;
-
-	len |= type << 8;
-	payload = len;
-	ret = fmc_send_cmd(fmdev, RDS_CONFIG_DATA_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* TODO: LENGTH_GET? */
-	return 0;
-}
-
-int fm_tx_set_rds_mode(struct fmdev *fmdev, u8 rds_en_dis)
-{
-	u16 payload;
-	int ret;
-	u8 rds_text[] = "Zoom2\n";
-
-	fmdbg("rds_en_dis:%d(E:%d, D:%d)\n", rds_en_dis,
-		   FM_RDS_ENABLE, FM_RDS_DISABLE);
-
-	if (rds_en_dis == FM_RDS_ENABLE) {
-		/* Set RDS length */
-		set_rds_len(fmdev, 0, strlen(rds_text));
-
-		/* Set RDS text */
-		set_rds_text(fmdev, rds_text);
-
-		/* Set RDS mode */
-		set_rds_data_mode(fmdev, 0x0);
-	}
-
-	/* Send command to enable RDS */
-	if (rds_en_dis == FM_RDS_ENABLE)
-		payload = 0x01;
-	else
-		payload = 0x00;
-
-	ret = fmc_send_cmd(fmdev, RDS_DATA_ENB, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	if (rds_en_dis == FM_RDS_ENABLE) {
-		/* Set RDS length */
-		set_rds_len(fmdev, 0, strlen(rds_text));
-
-		/* Set RDS text */
-		set_rds_text(fmdev, rds_text);
-	}
-	fmdev->tx_data.rds.flag = rds_en_dis;
-
-	return 0;
-}
-
-int fm_tx_set_radio_text(struct fmdev *fmdev, u8 *rds_text, u8 rds_type)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-
-	fm_tx_set_rds_mode(fmdev, 0);
-
-	/* Set RDS length */
-	set_rds_len(fmdev, rds_type, strlen(rds_text));
-
-	/* Set RDS text */
-	set_rds_text(fmdev, rds_text);
-
-	/* Set RDS mode */
-	set_rds_data_mode(fmdev, 0x0);
-
-	payload = 1;
-	ret = fmc_send_cmd(fmdev, RDS_DATA_ENB, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-int fm_tx_set_af(struct fmdev *fmdev, u32 af)
-{
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-
-	fmdbg("AF: %d\n", af);
-
-	af = (af - 87500) / 100;
-	payload = (u16)af;
-	ret = fmc_send_cmd(fmdev, TA_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-int fm_tx_set_region(struct fmdev *fmdev, u8 region)
-{
-	u16 payload;
-	int ret;
-
-	if (region != FM_BAND_EUROPE_US && region != FM_BAND_JAPAN) {
-		fmerr("Invalid band\n");
-		return -EINVAL;
-	}
-
-	/* Send command to set the band */
-	payload = (u16)region;
-	ret = fmc_send_cmd(fmdev, TX_BAND_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-int fm_tx_set_mute_mode(struct fmdev *fmdev, u8 mute_mode_toset)
-{
-	u16 payload;
-	int ret;
-
-	fmdbg("tx: mute mode %d\n", mute_mode_toset);
-
-	payload = mute_mode_toset;
-	ret = fmc_send_cmd(fmdev, MUTE, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	return 0;
-}
-
-/* Set TX Audio I/O */
-static int set_audio_io(struct fmdev *fmdev)
-{
-	struct fmtx_data *tx = &fmdev->tx_data;
-	u16 payload;
-	int ret;
-
-	/* Set Audio I/O Enable */
-	payload = tx->audio_io;
-	ret = fmc_send_cmd(fmdev, AUDIO_IO_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* TODO: is audio set? */
-	return 0;
-}
-
-/* Start TX Transmission */
-static int enable_xmit(struct fmdev *fmdev, u8 new_xmit_state)
-{
-	struct fmtx_data *tx = &fmdev->tx_data;
-	unsigned long timeleft;
-	u16 payload;
-	int ret;
-
-	/* Enable POWER_ENB interrupts */
-	payload = FM_POW_ENB_EVENT;
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Set Power Enable */
-	payload = new_xmit_state;
-	ret = fmc_send_cmd(fmdev, POWER_ENB_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* Wait for Power Enabled */
-	init_completion(&fmdev->maintask_comp);
-	timeleft = wait_for_completion_timeout(&fmdev->maintask_comp,
-			FM_DRV_TX_TIMEOUT);
-	if (!timeleft) {
-		fmerr("Timeout(%d sec),didn't get tune ended interrupt\n",
-			   jiffies_to_msecs(FM_DRV_TX_TIMEOUT) / 1000);
-		return -ETIMEDOUT;
-	}
-
-	set_bit(FM_CORE_TX_XMITING, &fmdev->flag);
-	tx->xmit_state = new_xmit_state;
-
-	return 0;
-}
-
-/* Set TX power level */
-int fm_tx_set_pwr_lvl(struct fmdev *fmdev, u8 new_pwr_lvl)
-{
-	u16 payload;
-	struct fmtx_data *tx = &fmdev->tx_data;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-	fmdbg("tx: pwr_level_to_set %ld\n", (long int)new_pwr_lvl);
-
-	/* If the core isn't ready update global variable */
-	if (!test_bit(FM_CORE_READY, &fmdev->flag)) {
-		tx->pwr_lvl = new_pwr_lvl;
-		return 0;
-	}
-
-	/* Set power level: Application will specify power level value in
-	 * units of dB/uV, whereas range and step are specific to FM chip.
-	 * For TI's WL chips, convert application specified power level value
-	 * to chip specific value by subtracting 122 from it. Refer to TI FM
-	 * data sheet for details.
-	 * */
-
-	payload = (FM_PWR_LVL_HIGH - new_pwr_lvl);
-	ret = fmc_send_cmd(fmdev, POWER_LEV_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	/* TODO: is the power level set? */
-	tx->pwr_lvl = new_pwr_lvl;
-
-	return 0;
-}
-
-/*
- * Sets FM TX pre-emphasis filter value (OFF, 50us, or 75us)
- * Convert V4L2 specified filter values to chip specific filter values.
- */
-int fm_tx_set_preemph_filter(struct fmdev *fmdev, u32 preemphasis)
-{
-	struct fmtx_data *tx = &fmdev->tx_data;
-	u16 payload;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-
-	switch (preemphasis) {
-	case V4L2_PREEMPHASIS_DISABLED:
-		payload = FM_TX_PREEMPH_OFF;
-		break;
-	case V4L2_PREEMPHASIS_50_uS:
-		payload = FM_TX_PREEMPH_50US;
-		break;
-	case V4L2_PREEMPHASIS_75_uS:
-		payload = FM_TX_PREEMPH_75US;
-		break;
-	}
-
-	ret = fmc_send_cmd(fmdev, PREMPH_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	tx->preemph = payload;
-
-	return ret;
-}
-
-/* Get the TX tuning capacitor value.*/
-int fm_tx_get_tune_cap_val(struct fmdev *fmdev)
-{
-	u16 curr_val;
-	u32 resp_len;
-	int ret;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-
-	ret = fmc_send_cmd(fmdev, READ_FMANT_TUNE_VALUE, REG_RD,
-			NULL, sizeof(curr_val), &curr_val, &resp_len);
-	if (ret < 0)
-		return ret;
-
-	curr_val = be16_to_cpu((__force __be16)curr_val);
-
-	return curr_val;
-}
-
-/* Set TX Frequency */
-int fm_tx_set_freq(struct fmdev *fmdev, u32 freq_to_set)
-{
-	struct fmtx_data *tx = &fmdev->tx_data;
-	u16 payload, chanl_index;
-	int ret;
-
-	if (test_bit(FM_CORE_TX_XMITING, &fmdev->flag)) {
-		enable_xmit(fmdev, 0);
-		clear_bit(FM_CORE_TX_XMITING, &fmdev->flag);
-	}
-
-	/* Enable FR, BL interrupts */
-	payload = (FM_FR_EVENT | FM_BL_EVENT);
-	ret = fmc_send_cmd(fmdev, INT_MASK_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	tx->tx_frq = (unsigned long)freq_to_set;
-	fmdbg("tx: freq_to_set %ld\n", (long int)tx->tx_frq);
-
-	chanl_index = freq_to_set / 10;
-
-	/* Set current tuner channel */
-	payload = chanl_index;
-	ret = fmc_send_cmd(fmdev, CHANL_SET, REG_WR, &payload,
-			sizeof(payload), NULL, NULL);
-	if (ret < 0)
-		return ret;
-
-	fm_tx_set_pwr_lvl(fmdev, tx->pwr_lvl);
-	fm_tx_set_preemph_filter(fmdev, tx->preemph);
-
-	tx->audio_io = 0x01;	/* I2S */
-	set_audio_io(fmdev);
-
-	enable_xmit(fmdev, 0x01);	/* Enable transmission */
-
-	tx->aud_mode = FM_STEREO_MODE;
-	tx->rds.flag = FM_RDS_DISABLE;
-
-	return 0;
-}
-
diff --git a/drivers/media/radio/wl128x/fmdrv_tx.h b/drivers/media/radio/wl128x/fmdrv_tx.h
deleted file mode 100644
index aebdadf9e99b..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_tx.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  FM TX module header.
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#ifndef _FMDRV_TX_H
-#define _FMDRV_TX_H
-
-int fm_tx_set_freq(struct fmdev *, u32);
-int fm_tx_set_pwr_lvl(struct fmdev *, u8);
-int fm_tx_set_region(struct fmdev *, u8);
-int fm_tx_set_mute_mode(struct fmdev *, u8);
-int fm_tx_set_stereo_mono(struct fmdev *, u16);
-int fm_tx_set_rds_mode(struct fmdev *, u8);
-int fm_tx_set_radio_text(struct fmdev *, u8 *, u8);
-int fm_tx_set_af(struct fmdev *, u32);
-int fm_tx_set_preemph_filter(struct fmdev *, u32);
-int fm_tx_get_tune_cap_val(struct fmdev *);
-
-#endif
-
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.c b/drivers/media/radio/wl128x/fmdrv_v4l2.c
deleted file mode 100644
index 1c146d14dbbd..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.c
+++ /dev/null
@@ -1,604 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0-only
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *  This file provides interfaces to V4L2 subsystem.
- *
- *  This module registers with V4L2 subsystem as Radio
- *  data system interface (/dev/radio). During the registration,
- *  it will expose two set of function pointers.
- *
- *    1) File operation related API (open, close, read, write, poll...etc).
- *    2) Set of V4L2 IOCTL complaint API.
- *
- *  Copyright (C) 2011 Texas Instruments
- *  Author: Raja Mani <raja_mani@ti.com>
- *  Author: Manjunatha Halli <manjunatha_halli@ti.com>
- */
-
-#include <linux/export.h>
-
-#include "fmdrv.h"
-#include "fmdrv_v4l2.h"
-#include "fmdrv_common.h"
-#include "fmdrv_rx.h"
-#include "fmdrv_tx.h"
-
-static struct video_device gradio_dev;
-static u8 radio_disconnected;
-
-/* -- V4L2 RADIO (/dev/radioX) device file operation interfaces --- */
-
-/* Read RX RDS data */
-static ssize_t fm_v4l2_fops_read(struct file *file, char __user * buf,
-					size_t count, loff_t *ppos)
-{
-	u8 rds_mode;
-	int ret;
-	struct fmdev *fmdev;
-
-	fmdev = video_drvdata(file);
-
-	if (!radio_disconnected) {
-		fmerr("FM device is already disconnected\n");
-		return -EIO;
-	}
-
-	if (mutex_lock_interruptible(&fmdev->mutex))
-		return -ERESTARTSYS;
-
-	/* Turn on RDS mode if it is disabled */
-	ret = fm_rx_get_rds_mode(fmdev, &rds_mode);
-	if (ret < 0) {
-		fmerr("Unable to read current rds mode\n");
-		goto read_unlock;
-	}
-
-	if (rds_mode == FM_RDS_DISABLE) {
-		ret = fmc_set_rds_mode(fmdev, FM_RDS_ENABLE);
-		if (ret < 0) {
-			fmerr("Failed to enable rds mode\n");
-			goto read_unlock;
-		}
-	}
-
-	/* Copy RDS data from internal buffer to user buffer */
-	ret = fmc_transfer_rds_from_internal_buff(fmdev, file, buf, count);
-read_unlock:
-	mutex_unlock(&fmdev->mutex);
-	return ret;
-}
-
-/* Write TX RDS data */
-static ssize_t fm_v4l2_fops_write(struct file *file, const char __user * buf,
-		size_t count, loff_t *ppos)
-{
-	struct tx_rds rds;
-	int ret;
-	struct fmdev *fmdev;
-
-	ret = copy_from_user(&rds, buf, sizeof(rds));
-	rds.text[sizeof(rds.text) - 1] = '\0';
-	fmdbg("(%d)type: %d, text %s, af %d\n",
-		   ret, rds.text_type, rds.text, rds.af_freq);
-	if (ret)
-		return -EFAULT;
-
-	fmdev = video_drvdata(file);
-	if (mutex_lock_interruptible(&fmdev->mutex))
-		return -ERESTARTSYS;
-	fm_tx_set_radio_text(fmdev, rds.text, rds.text_type);
-	fm_tx_set_af(fmdev, rds.af_freq);
-	mutex_unlock(&fmdev->mutex);
-
-	return sizeof(rds);
-}
-
-static __poll_t fm_v4l2_fops_poll(struct file *file, struct poll_table_struct *pts)
-{
-	int ret;
-	struct fmdev *fmdev;
-
-	fmdev = video_drvdata(file);
-	mutex_lock(&fmdev->mutex);
-	ret = fmc_is_rds_data_available(fmdev, file, pts);
-	mutex_unlock(&fmdev->mutex);
-	if (ret < 0)
-		return EPOLLIN | EPOLLRDNORM;
-
-	return 0;
-}
-
-/*
- * Handle open request for "/dev/radioX" device.
- * Start with FM RX mode as default.
- */
-static int fm_v4l2_fops_open(struct file *file)
-{
-	int ret;
-	struct fmdev *fmdev = NULL;
-
-	/* Don't allow multiple open */
-	if (radio_disconnected) {
-		fmerr("FM device is already opened\n");
-		return -EBUSY;
-	}
-
-	fmdev = video_drvdata(file);
-
-	if (mutex_lock_interruptible(&fmdev->mutex))
-		return -ERESTARTSYS;
-	ret = fmc_prepare(fmdev);
-	if (ret < 0) {
-		fmerr("Unable to prepare FM CORE\n");
-		goto open_unlock;
-	}
-
-	fmdbg("Load FM RX firmware..\n");
-
-	ret = fmc_set_mode(fmdev, FM_MODE_RX);
-	if (ret < 0) {
-		fmerr("Unable to load FM RX firmware\n");
-		goto open_unlock;
-	}
-	radio_disconnected = 1;
-
-open_unlock:
-	mutex_unlock(&fmdev->mutex);
-	return ret;
-}
-
-static int fm_v4l2_fops_release(struct file *file)
-{
-	int ret;
-	struct fmdev *fmdev;
-
-	fmdev = video_drvdata(file);
-	if (!radio_disconnected) {
-		fmdbg("FM device is already closed\n");
-		return 0;
-	}
-
-	mutex_lock(&fmdev->mutex);
-	ret = fmc_set_mode(fmdev, FM_MODE_OFF);
-	if (ret < 0) {
-		fmerr("Unable to turn off the chip\n");
-		goto release_unlock;
-	}
-
-	ret = fmc_release(fmdev);
-	if (ret < 0) {
-		fmerr("FM CORE release failed\n");
-		goto release_unlock;
-	}
-	radio_disconnected = 0;
-
-release_unlock:
-	mutex_unlock(&fmdev->mutex);
-	return ret;
-}
-
-/* V4L2 RADIO (/dev/radioX) device IOCTL interfaces */
-static int fm_v4l2_vidioc_querycap(struct file *file, void *priv,
-		struct v4l2_capability *capability)
-{
-	strscpy(capability->driver, FM_DRV_NAME, sizeof(capability->driver));
-	strscpy(capability->card, FM_DRV_CARD_SHORT_NAME,
-		sizeof(capability->card));
-	sprintf(capability->bus_info, "UART");
-	return 0;
-}
-
-static int fm_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct fmdev *fmdev = container_of(ctrl->handler,
-			struct fmdev, ctrl_handler);
-
-	switch (ctrl->id) {
-	case  V4L2_CID_TUNE_ANTENNA_CAPACITOR:
-		ctrl->val = fm_tx_get_tune_cap_val(fmdev);
-		break;
-	default:
-		fmwarn("%s: Unknown IOCTL: %d\n", __func__, ctrl->id);
-		break;
-	}
-
-	return 0;
-}
-
-static int fm_v4l2_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct fmdev *fmdev = container_of(ctrl->handler,
-			struct fmdev, ctrl_handler);
-
-	switch (ctrl->id) {
-	case V4L2_CID_AUDIO_VOLUME:	/* set volume */
-		return fm_rx_set_volume(fmdev, (u16)ctrl->val);
-
-	case V4L2_CID_AUDIO_MUTE:	/* set mute */
-		return fmc_set_mute_mode(fmdev, (u8)ctrl->val);
-
-	case V4L2_CID_TUNE_POWER_LEVEL:
-		/* set TX power level - ext control */
-		return fm_tx_set_pwr_lvl(fmdev, (u8)ctrl->val);
-
-	case V4L2_CID_TUNE_PREEMPHASIS:
-		return fm_tx_set_preemph_filter(fmdev, (u8) ctrl->val);
-
-	default:
-		return -EINVAL;
-	}
-}
-
-static int fm_v4l2_vidioc_g_audio(struct file *file, void *priv,
-		struct v4l2_audio *audio)
-{
-	memset(audio, 0, sizeof(*audio));
-	strscpy(audio->name, "Radio", sizeof(audio->name));
-	audio->capability = V4L2_AUDCAP_STEREO;
-
-	return 0;
-}
-
-static int fm_v4l2_vidioc_s_audio(struct file *file, void *priv,
-		const struct v4l2_audio *audio)
-{
-	if (audio->index != 0)
-		return -EINVAL;
-
-	return 0;
-}
-
-/* Get tuner attributes. If current mode is NOT RX, return error */
-static int fm_v4l2_vidioc_g_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-	u32 bottom_freq;
-	u32 top_freq;
-	u16 stereo_mono_mode;
-	u16 rssilvl;
-	int ret;
-
-	if (tuner->index != 0)
-		return -EINVAL;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX)
-		return -EPERM;
-
-	ret = fm_rx_get_band_freq_range(fmdev, &bottom_freq, &top_freq);
-	if (ret != 0)
-		return ret;
-
-	ret = fm_rx_get_stereo_mono(fmdev, &stereo_mono_mode);
-	if (ret != 0)
-		return ret;
-
-	ret = fm_rx_get_rssi_level(fmdev, &rssilvl);
-	if (ret != 0)
-		return ret;
-
-	strscpy(tuner->name, "FM", sizeof(tuner->name));
-	tuner->type = V4L2_TUNER_RADIO;
-	/* Store rangelow and rangehigh freq in unit of 62.5 Hz */
-	tuner->rangelow = bottom_freq * 16;
-	tuner->rangehigh = top_freq * 16;
-	tuner->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_STEREO |
-	((fmdev->rx.rds.flag == FM_RDS_ENABLE) ? V4L2_TUNER_SUB_RDS : 0);
-	tuner->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
-			    V4L2_TUNER_CAP_LOW |
-			    V4L2_TUNER_CAP_HWSEEK_BOUNDED |
-			    V4L2_TUNER_CAP_HWSEEK_WRAP;
-	tuner->audmode = (stereo_mono_mode ?
-			  V4L2_TUNER_MODE_MONO : V4L2_TUNER_MODE_STEREO);
-
-	/*
-	 * Actual rssi value lies in between -128 to +127.
-	 * Convert this range from 0 to 255 by adding +128
-	 */
-	rssilvl += 128;
-
-	/*
-	 * Return signal strength value should be within 0 to 65535.
-	 * Find out correct signal radio by multiplying (65535/255) = 257
-	 */
-	tuner->signal = rssilvl * 257;
-	tuner->afc = 0;
-
-	return ret;
-}
-
-/*
- * Set tuner attributes. If current mode is NOT RX, set to RX.
- * Currently, we set only audio mode (mono/stereo) and RDS state (on/off).
- * Should we set other tuner attributes, too?
- */
-static int fm_v4l2_vidioc_s_tuner(struct file *file, void *priv,
-		const struct v4l2_tuner *tuner)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-	u16 aud_mode;
-	u8 rds_mode;
-	int ret;
-
-	if (tuner->index != 0)
-		return -EINVAL;
-
-	aud_mode = (tuner->audmode == V4L2_TUNER_MODE_STEREO) ?
-			FM_STEREO_MODE : FM_MONO_MODE;
-	rds_mode = (tuner->rxsubchans & V4L2_TUNER_SUB_RDS) ?
-			FM_RDS_ENABLE : FM_RDS_DISABLE;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX) {
-		ret = fmc_set_mode(fmdev, FM_MODE_RX);
-		if (ret < 0) {
-			fmerr("Failed to set RX mode\n");
-			return ret;
-		}
-	}
-
-	ret = fmc_set_stereo_mono(fmdev, aud_mode);
-	if (ret < 0) {
-		fmerr("Failed to set RX stereo/mono mode\n");
-		return ret;
-	}
-
-	ret = fmc_set_rds_mode(fmdev, rds_mode);
-	if (ret < 0)
-		fmerr("Failed to set RX RDS mode\n");
-
-	return ret;
-}
-
-/* Get tuner or modulator radio frequency */
-static int fm_v4l2_vidioc_g_freq(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-	int ret;
-
-	ret = fmc_get_freq(fmdev, &freq->frequency);
-	if (ret < 0) {
-		fmerr("Failed to get frequency\n");
-		return ret;
-	}
-
-	/* Frequency unit of 62.5 Hz*/
-	freq->frequency = (u32) freq->frequency * 16;
-
-	return 0;
-}
-
-/* Set tuner or modulator radio frequency */
-static int fm_v4l2_vidioc_s_freq(struct file *file, void *priv,
-		const struct v4l2_frequency *freq)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-
-	/*
-	 * As V4L2_TUNER_CAP_LOW is set 1 user sends the frequency
-	 * in units of 62.5 Hz.
-	 */
-	return fmc_set_freq(fmdev, freq->frequency / 16);
-}
-
-/* Set hardware frequency seek. If current mode is NOT RX, set it RX. */
-static int fm_v4l2_vidioc_s_hw_freq_seek(struct file *file, void *priv,
-		const struct v4l2_hw_freq_seek *seek)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-	int ret;
-
-	if (file->f_flags & O_NONBLOCK)
-		return -EWOULDBLOCK;
-
-	if (fmdev->curr_fmmode != FM_MODE_RX) {
-		ret = fmc_set_mode(fmdev, FM_MODE_RX);
-		if (ret != 0) {
-			fmerr("Failed to set RX mode\n");
-			return ret;
-		}
-	}
-
-	ret = fm_rx_seek(fmdev, seek->seek_upward, seek->wrap_around,
-			seek->spacing);
-	if (ret < 0)
-		fmerr("RX seek failed - %d\n", ret);
-
-	return ret;
-}
-/* Get modulator attributes. If mode is not TX, return no attributes. */
-static int fm_v4l2_vidioc_g_modulator(struct file *file, void *priv,
-		struct v4l2_modulator *mod)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-
-	if (mod->index != 0)
-		return -EINVAL;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX)
-		return -EPERM;
-
-	mod->txsubchans = ((fmdev->tx_data.aud_mode == FM_STEREO_MODE) ?
-				V4L2_TUNER_SUB_STEREO : V4L2_TUNER_SUB_MONO) |
-				((fmdev->tx_data.rds.flag == FM_RDS_ENABLE) ?
-				V4L2_TUNER_SUB_RDS : 0);
-
-	mod->capability = V4L2_TUNER_CAP_STEREO | V4L2_TUNER_CAP_RDS |
-				V4L2_TUNER_CAP_LOW;
-
-	return 0;
-}
-
-/* Set modulator attributes. If mode is not TX, set to TX. */
-static int fm_v4l2_vidioc_s_modulator(struct file *file, void *priv,
-		const struct v4l2_modulator *mod)
-{
-	struct fmdev *fmdev = video_drvdata(file);
-	u8 rds_mode;
-	u16 aud_mode;
-	int ret;
-
-	if (mod->index != 0)
-		return -EINVAL;
-
-	if (fmdev->curr_fmmode != FM_MODE_TX) {
-		ret = fmc_set_mode(fmdev, FM_MODE_TX);
-		if (ret != 0) {
-			fmerr("Failed to set TX mode\n");
-			return ret;
-		}
-	}
-
-	aud_mode = (mod->txsubchans & V4L2_TUNER_SUB_STEREO) ?
-			FM_STEREO_MODE : FM_MONO_MODE;
-	rds_mode = (mod->txsubchans & V4L2_TUNER_SUB_RDS) ?
-			FM_RDS_ENABLE : FM_RDS_DISABLE;
-	ret = fm_tx_set_stereo_mono(fmdev, aud_mode);
-	if (ret < 0) {
-		fmerr("Failed to set mono/stereo mode for TX\n");
-		return ret;
-	}
-	ret = fm_tx_set_rds_mode(fmdev, rds_mode);
-	if (ret < 0)
-		fmerr("Failed to set rds mode for TX\n");
-
-	return ret;
-}
-
-static const struct v4l2_file_operations fm_drv_fops = {
-	.owner = THIS_MODULE,
-	.read = fm_v4l2_fops_read,
-	.write = fm_v4l2_fops_write,
-	.poll = fm_v4l2_fops_poll,
-	.unlocked_ioctl = video_ioctl2,
-	.open = fm_v4l2_fops_open,
-	.release = fm_v4l2_fops_release,
-};
-
-static const struct v4l2_ctrl_ops fm_ctrl_ops = {
-	.s_ctrl = fm_v4l2_s_ctrl,
-	.g_volatile_ctrl = fm_g_volatile_ctrl,
-};
-static const struct v4l2_ioctl_ops fm_drv_ioctl_ops = {
-	.vidioc_querycap = fm_v4l2_vidioc_querycap,
-	.vidioc_g_audio = fm_v4l2_vidioc_g_audio,
-	.vidioc_s_audio = fm_v4l2_vidioc_s_audio,
-	.vidioc_g_tuner = fm_v4l2_vidioc_g_tuner,
-	.vidioc_s_tuner = fm_v4l2_vidioc_s_tuner,
-	.vidioc_g_frequency = fm_v4l2_vidioc_g_freq,
-	.vidioc_s_frequency = fm_v4l2_vidioc_s_freq,
-	.vidioc_s_hw_freq_seek = fm_v4l2_vidioc_s_hw_freq_seek,
-	.vidioc_g_modulator = fm_v4l2_vidioc_g_modulator,
-	.vidioc_s_modulator = fm_v4l2_vidioc_s_modulator
-};
-
-/* V4L2 RADIO device parent structure */
-static const struct video_device fm_viddev_template = {
-	.fops = &fm_drv_fops,
-	.ioctl_ops = &fm_drv_ioctl_ops,
-	.name = FM_DRV_NAME,
-	.release = video_device_release_empty,
-	/*
-	 * To ensure both the tuner and modulator ioctls are accessible we
-	 * set the vfl_dir to M2M to indicate this.
-	 *
-	 * It is not really a mem2mem device of course, but it can both receive
-	 * and transmit using the same radio device. It's the only radio driver
-	 * that does this and it should really be split in two radio devices,
-	 * but that would affect applications using this driver.
-	 */
-	.vfl_dir = VFL_DIR_M2M,
-	.device_caps = V4L2_CAP_HW_FREQ_SEEK | V4L2_CAP_TUNER | V4L2_CAP_RADIO |
-		       V4L2_CAP_MODULATOR | V4L2_CAP_AUDIO |
-		       V4L2_CAP_READWRITE | V4L2_CAP_RDS_CAPTURE,
-};
-
-int fm_v4l2_init_video_device(struct fmdev *fmdev, int radio_nr)
-{
-	struct v4l2_ctrl *ctrl;
-	int ret;
-
-	strscpy(fmdev->v4l2_dev.name, FM_DRV_NAME,
-		sizeof(fmdev->v4l2_dev.name));
-	ret = v4l2_device_register(NULL, &fmdev->v4l2_dev);
-	if (ret < 0)
-		return ret;
-
-	/* Init mutex for core locking */
-	mutex_init(&fmdev->mutex);
-
-	/* Setup FM driver's V4L2 properties */
-	gradio_dev = fm_viddev_template;
-
-	video_set_drvdata(&gradio_dev, fmdev);
-
-	gradio_dev.lock = &fmdev->mutex;
-	gradio_dev.v4l2_dev = &fmdev->v4l2_dev;
-
-	/* Register with V4L2 subsystem as RADIO device */
-	if (video_register_device(&gradio_dev, VFL_TYPE_RADIO, radio_nr)) {
-		v4l2_device_unregister(&fmdev->v4l2_dev);
-		fmerr("Could not register video device\n");
-		return -ENOMEM;
-	}
-
-	fmdev->radio_dev = &gradio_dev;
-
-	/* Register to v4l2 ctrl handler framework */
-	fmdev->radio_dev->ctrl_handler = &fmdev->ctrl_handler;
-
-	ret = v4l2_ctrl_handler_init(&fmdev->ctrl_handler, 5);
-	if (ret < 0) {
-		fmerr("(fmdev): Can't init ctrl handler\n");
-		v4l2_ctrl_handler_free(&fmdev->ctrl_handler);
-		video_unregister_device(fmdev->radio_dev);
-		v4l2_device_unregister(&fmdev->v4l2_dev);
-		return -EBUSY;
-	}
-
-	/*
-	 * Following controls are handled by V4L2 control framework.
-	 * Added in ascending ID order.
-	 */
-	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
-			V4L2_CID_AUDIO_VOLUME, FM_RX_VOLUME_MIN,
-			FM_RX_VOLUME_MAX, 1, FM_RX_VOLUME_MAX);
-
-	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
-			V4L2_CID_AUDIO_MUTE, 0, 1, 1, 1);
-
-	v4l2_ctrl_new_std_menu(&fmdev->ctrl_handler, &fm_ctrl_ops,
-			V4L2_CID_TUNE_PREEMPHASIS, V4L2_PREEMPHASIS_75_uS,
-			0, V4L2_PREEMPHASIS_75_uS);
-
-	v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
-			V4L2_CID_TUNE_POWER_LEVEL, FM_PWR_LVL_LOW,
-			FM_PWR_LVL_HIGH, 1, FM_PWR_LVL_HIGH);
-
-	ctrl = v4l2_ctrl_new_std(&fmdev->ctrl_handler, &fm_ctrl_ops,
-			V4L2_CID_TUNE_ANTENNA_CAPACITOR, 0,
-			255, 1, 255);
-
-	if (ctrl)
-		ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
-
-	return 0;
-}
-
-void *fm_v4l2_deinit_video_device(void)
-{
-	struct fmdev *fmdev;
-
-
-	fmdev = video_get_drvdata(&gradio_dev);
-
-	/* Unregister to v4l2 ctrl handler framework*/
-	v4l2_ctrl_handler_free(&fmdev->ctrl_handler);
-
-	/* Unregister RADIO device from V4L2 subsystem */
-	video_unregister_device(&gradio_dev);
-
-	v4l2_device_unregister(&fmdev->v4l2_dev);
-
-	return fmdev;
-}
diff --git a/drivers/media/radio/wl128x/fmdrv_v4l2.h b/drivers/media/radio/wl128x/fmdrv_v4l2.h
deleted file mode 100644
index 963214e9d6f2..000000000000
--- a/drivers/media/radio/wl128x/fmdrv_v4l2.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0-only */
-/*
- *  FM Driver for Connectivity chip of Texas Instruments.
- *
- *  FM V4L2 module header.
- *
- *  Copyright (C) 2011 Texas Instruments
- */
-
-#ifndef _FMDRV_V4L2_H
-#define _FMDRV_V4L2_H
-
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-common.h>
-#include <media/v4l2-ctrls.h>
-
-int fm_v4l2_init_video_device(struct fmdev *, int);
-void *fm_v4l2_deinit_video_device(void);
-
-#endif
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index 74d69ce22a33..0a8aeafdb7e0 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -184,6 +184,7 @@ config IR_GPIO_TX
 	tristate "GPIO IR Bit Banging Transmitter"
 	depends on LIRC
 	depends on (OF && GPIOLIB) || COMPILE_TEST
+	depends on !PREEMPT_RT
 	help
 	   Say Y if you want to a GPIO based IR transmitter. This is a
 	   bit banging driver.
diff --git a/drivers/media/rc/ati_remote.c b/drivers/media/rc/ati_remote.c
index d7721e60776e..a733914a2574 100644
--- a/drivers/media/rc/ati_remote.c
+++ b/drivers/media/rc/ati_remote.c
@@ -311,9 +311,9 @@ static void ati_remote_dump(struct device *dev, unsigned char *data,
 		if (data[0] != (unsigned char)0xff && data[0] != 0x00)
 			dev_warn(dev, "Weird byte 0x%02x\n", data[0]);
 	} else if (len == 4)
-		dev_warn(dev, "Weird key %*ph\n", 4, data);
+		dev_warn(dev, "Weird key %4ph\n", data);
 	else
-		dev_warn(dev, "Weird data, len=%d %*ph ...\n", len, 6, data);
+		dev_warn(dev, "Weird data, len=%d %6ph ...\n", len, data);
 }
 
 /*
@@ -502,7 +502,7 @@ static void ati_remote_input_report(struct urb *urb)
 
 	if (data[1] != ((data[2] + data[3] + 0xd5) & 0xff)) {
 		dbginfo(&ati_remote->interface->dev,
-			"wrong checksum in input: %*ph\n", 4, data);
+			"wrong checksum in input: %4ph\n", data);
 		return;
 	}
 
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index 11ee21a7db8f..d6c54a3bccc2 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -451,9 +451,6 @@ select_timeout:
 		dev->rdev->max_timeout = 200000;
 	}
 
-	if (dev->hw_learning_and_tx_capable)
-		dev->rdev->tx_resolution = sample_period;
-
 	if (dev->rdev->timeout > dev->rdev->max_timeout)
 		dev->rdev->timeout = dev->rdev->max_timeout;
 	if (dev->rdev->timeout < dev->rdev->min_timeout)
@@ -662,7 +659,7 @@ exit:
 /* timer to simulate tx done interrupt */
 static void ene_tx_irqsim(struct timer_list *t)
 {
-	struct ene_device *dev = from_timer(dev, t, tx_sim_timer);
+	struct ene_device *dev = timer_container_of(dev, t, tx_sim_timer);
 	unsigned long flags;
 
 	spin_lock_irqsave(&dev->hw_lock, flags);
@@ -1107,7 +1104,7 @@ static void ene_remove(struct pnp_dev *pnp_dev)
 	unsigned long flags;
 
 	rc_unregister_device(dev->rdev);
-	del_timer_sync(&dev->tx_sim_timer);
+	timer_delete_sync(&dev->tx_sim_timer);
 	spin_lock_irqsave(&dev->hw_lock, flags);
 	ene_rx_disable(dev);
 	ene_rx_restore_hw_buffer(dev);
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 41eeec648803..bf6d8fa983bf 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -9,7 +9,6 @@
 #include <linux/gpio/consumer.h>
 #include <linux/slab.h>
 #include <linux/of.h>
-#include <linux/of_gpio.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/pm_qos.h>
@@ -202,7 +201,7 @@ MODULE_DEVICE_TABLE(of, gpio_ir_recv_of_match);
 
 static struct platform_driver gpio_ir_recv_driver = {
 	.probe  = gpio_ir_recv_probe,
-	.remove_new = gpio_ir_recv_remove,
+	.remove = gpio_ir_recv_remove,
 	.driver = {
 		.name   = KBUILD_MODNAME,
 		.of_match_table = gpio_ir_recv_of_match,
diff --git a/drivers/media/rc/gpio-ir-tx.c b/drivers/media/rc/gpio-ir-tx.c
index 1a8fea357f14..e185ead40464 100644
--- a/drivers/media/rc/gpio-ir-tx.c
+++ b/drivers/media/rc/gpio-ir-tx.c
@@ -78,8 +78,6 @@ static void gpio_ir_tx_unmodulated(struct gpio_ir *gpio_ir, uint *txbuf,
 	ktime_t edge;
 	int i;
 
-	local_irq_disable();
-
 	edge = ktime_get();
 
 	for (i = 0; i < count; i++) {
@@ -110,8 +108,6 @@ static void gpio_ir_tx_modulated(struct gpio_ir *gpio_ir, uint *txbuf,
 	space = DIV_ROUND_CLOSEST((100 - gpio_ir->duty_cycle) *
 				  (NSEC_PER_SEC / 100), gpio_ir->carrier);
 
-	local_irq_disable();
-
 	edge = ktime_get();
 
 	for (i = 0; i < count; i++) {
diff --git a/drivers/media/rc/igorplugusb.c b/drivers/media/rc/igorplugusb.c
index 1464ef9c55bc..e034c93d57cf 100644
--- a/drivers/media/rc/igorplugusb.c
+++ b/drivers/media/rc/igorplugusb.c
@@ -131,7 +131,7 @@ static void igorplugusb_cmd(struct igorplugusb *ir, int cmd)
 
 static void igorplugusb_timer(struct timer_list *t)
 {
-	struct igorplugusb *ir = from_timer(ir, t, timer);
+	struct igorplugusb *ir = timer_container_of(ir, t, timer);
 
 	igorplugusb_cmd(ir, GET_INFRACODE);
 }
@@ -223,7 +223,7 @@ static int igorplugusb_probe(struct usb_interface *intf,
 	return 0;
 fail:
 	usb_poison_urb(ir->urb);
-	del_timer(&ir->timer);
+	timer_delete(&ir->timer);
 	usb_unpoison_urb(ir->urb);
 	usb_free_urb(ir->urb);
 	rc_free_device(ir->rc);
@@ -238,7 +238,7 @@ static void igorplugusb_disconnect(struct usb_interface *intf)
 
 	rc_unregister_device(ir->rc);
 	usb_poison_urb(ir->urb);
-	del_timer_sync(&ir->timer);
+	timer_delete_sync(&ir->timer);
 	usb_set_intfdata(intf, NULL);
 	usb_unpoison_urb(ir->urb);
 	usb_free_urb(ir->urb);
diff --git a/drivers/media/rc/iguanair.c b/drivers/media/rc/iguanair.c
index 276bf3c8a8cb..8af94246e591 100644
--- a/drivers/media/rc/iguanair.c
+++ b/drivers/media/rc/iguanair.c
@@ -194,8 +194,10 @@ static int iguanair_send(struct iguanair *ir, unsigned size)
 	if (rc)
 		return rc;
 
-	if (wait_for_completion_timeout(&ir->completion, TIMEOUT) == 0)
+	if (wait_for_completion_timeout(&ir->completion, TIMEOUT) == 0) {
+		usb_kill_urb(ir->urb_out);
 		return -ETIMEDOUT;
+	}
 
 	return rc;
 }
diff --git a/drivers/media/rc/img-ir/img-ir-core.c b/drivers/media/rc/img-ir/img-ir-core.c
index d87d8e14c556..067f4bc7fcc3 100644
--- a/drivers/media/rc/img-ir/img-ir-core.c
+++ b/drivers/media/rc/img-ir/img-ir-core.c
@@ -181,7 +181,7 @@ static struct platform_driver img_ir_driver = {
 		.pm = &img_ir_pmops,
 	},
 	.probe = img_ir_probe,
-	.remove_new = img_ir_remove,
+	.remove = img_ir_remove,
 };
 
 module_platform_driver(img_ir_driver);
diff --git a/drivers/media/rc/img-ir/img-ir-hw.c b/drivers/media/rc/img-ir/img-ir-hw.c
index 5da7479c1793..63f6f5b36838 100644
--- a/drivers/media/rc/img-ir/img-ir-hw.c
+++ b/drivers/media/rc/img-ir/img-ir-hw.c
@@ -556,8 +556,8 @@ static void img_ir_set_decoder(struct img_ir_priv *priv,
 	 * acquires the lock and we don't want to deadlock waiting for it.
 	 */
 	spin_unlock_irq(&priv->lock);
-	del_timer_sync(&hw->end_timer);
-	del_timer_sync(&hw->suspend_timer);
+	timer_delete_sync(&hw->end_timer);
+	timer_delete_sync(&hw->suspend_timer);
 	spin_lock_irq(&priv->lock);
 
 	hw->stopping = false;
@@ -865,7 +865,7 @@ static void img_ir_handle_data(struct img_ir_priv *priv, u32 len, u64 raw)
 /* timer function to end waiting for repeat. */
 static void img_ir_end_timer(struct timer_list *t)
 {
-	struct img_ir_priv *priv = from_timer(priv, t, hw.end_timer);
+	struct img_ir_priv *priv = timer_container_of(priv, t, hw.end_timer);
 
 	spin_lock_irq(&priv->lock);
 	img_ir_end_repeat(priv);
@@ -879,7 +879,8 @@ static void img_ir_end_timer(struct timer_list *t)
  */
 static void img_ir_suspend_timer(struct timer_list *t)
 {
-	struct img_ir_priv *priv = from_timer(priv, t, hw.suspend_timer);
+	struct img_ir_priv *priv = timer_container_of(priv, t,
+						      hw.suspend_timer);
 
 	spin_lock_irq(&priv->lock);
 	/*
diff --git a/drivers/media/rc/img-ir/img-ir-raw.c b/drivers/media/rc/img-ir/img-ir-raw.c
index 8b0bdd9603b3..92fb7b555a0f 100644
--- a/drivers/media/rc/img-ir/img-ir-raw.c
+++ b/drivers/media/rc/img-ir/img-ir-raw.c
@@ -65,7 +65,7 @@ void img_ir_isr_raw(struct img_ir_priv *priv, u32 irq_status)
  */
 static void img_ir_echo_timer(struct timer_list *t)
 {
-	struct img_ir_priv *priv = from_timer(priv, t, raw.timer);
+	struct img_ir_priv *priv = timer_container_of(priv, t, raw.timer);
 
 	spin_lock_irq(&priv->lock);
 
@@ -147,5 +147,5 @@ void img_ir_remove_raw(struct img_ir_priv *priv)
 
 	rc_unregister_device(rdev);
 
-	del_timer_sync(&raw->timer);
+	timer_delete_sync(&raw->timer);
 }
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 5719dda6e0f0..f5221b018808 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -845,13 +845,13 @@ static ssize_t imon_clock_show(struct device *d,
 	mutex_lock(&ictx->lock);
 
 	if (!ictx->display_supported) {
-		len = snprintf(buf, PAGE_SIZE, "Not supported.");
+		len = sysfs_emit(buf, "Not supported.");
 	} else {
-		len = snprintf(buf, PAGE_SIZE,
-			"To set the clock on your iMON display:\n"
-			"# date \"+%%y %%m %%d %%w %%H %%M %%S\" > imon_clock\n"
-			"%s", ictx->display_isopen ?
-			"\nNOTE: imon device must be closed\n" : "");
+		len = sysfs_emit(buf,
+				 "To set the clock on your iMON display:\n"
+				 "# date \"+%%y %%m %%d %%w %%H %%M %%S\" > imon_clock\n"
+				 "%s", ictx->display_isopen ?
+				 "\nNOTE: imon device must be closed\n" : "");
 	}
 
 	mutex_unlock(&ictx->lock);
@@ -1091,7 +1091,7 @@ static void usb_tx_callback(struct urb *urb)
  */
 static void imon_touch_display_timeout(struct timer_list *t)
 {
-	struct imon_context *ictx = from_timer(ictx, t, ttimer);
+	struct imon_context *ictx = timer_container_of(ictx, t, ttimer);
 
 	if (ictx->display_type != IMON_DISPLAY_TYPE_VGA)
 		return;
@@ -1148,10 +1148,7 @@ static int imon_ir_change_protocol(struct rc_dev *rc, u64 *rc_proto)
 
 	memcpy(ictx->usb_tx_buf, &ir_proto_packet, sizeof(ir_proto_packet));
 
-	if (!mutex_is_locked(&ictx->lock)) {
-		unlock = true;
-		mutex_lock(&ictx->lock);
-	}
+	unlock = mutex_trylock(&ictx->lock);
 
 	retval = send_packet(ictx);
 	if (retval)
@@ -2537,7 +2534,7 @@ static void imon_disconnect(struct usb_interface *interface)
 		ictx->dev_present_intf1 = false;
 		usb_kill_urb(ictx->rx_urb_intf1);
 		if (ictx->display_type == IMON_DISPLAY_TYPE_VGA) {
-			del_timer_sync(&ictx->ttimer);
+			timer_delete_sync(&ictx->ttimer);
 			input_unregister_device(ictx->touch);
 		}
 		usb_put_dev(ictx->usbdev_intf1);
diff --git a/drivers/media/rc/imon_raw.c b/drivers/media/rc/imon_raw.c
index b02ded52f19e..3a526dea6532 100644
--- a/drivers/media/rc/imon_raw.c
+++ b/drivers/media/rc/imon_raw.c
@@ -37,7 +37,7 @@ static void imon_ir_data(struct imon *imon)
 	if (packet_no == 0xff)
 		return;
 
-	dev_dbg(imon->dev, "data: %*ph", 8, imon->ir_buf);
+	dev_dbg(imon->dev, "data: %8ph", imon->ir_buf);
 
 	/*
 	 * Only the first 5 bytes contain IR data. Right shift so we move
diff --git a/drivers/media/rc/ir-hix5hd2.c b/drivers/media/rc/ir-hix5hd2.c
index de5bb9a08ea4..afd80d2350c6 100644
--- a/drivers/media/rc/ir-hix5hd2.c
+++ b/drivers/media/rc/ir-hix5hd2.c
@@ -394,7 +394,7 @@ static struct platform_driver hix5hd2_ir_driver = {
 		.pm     = &hix5hd2_ir_pm_ops,
 	},
 	.probe = hix5hd2_ir_probe,
-	.remove_new = hix5hd2_ir_remove,
+	.remove = hix5hd2_ir_remove,
 };
 
 module_platform_driver(hix5hd2_ir_driver);
diff --git a/drivers/media/rc/ir-mce_kbd-decoder.c b/drivers/media/rc/ir-mce_kbd-decoder.c
index 66e8feb9a569..bb2d7c37c263 100644
--- a/drivers/media/rc/ir-mce_kbd-decoder.c
+++ b/drivers/media/rc/ir-mce_kbd-decoder.c
@@ -109,7 +109,8 @@ static unsigned char kbd_keycodes[256] = {
 
 static void mce_kbd_rx_timeout(struct timer_list *t)
 {
-	struct ir_raw_event_ctrl *raw = from_timer(raw, t, mce_kbd.rx_timeout);
+	struct ir_raw_event_ctrl *raw = timer_container_of(raw, t,
+							   mce_kbd.rx_timeout);
 	unsigned char maskcode;
 	unsigned long flags;
 	int i;
@@ -324,7 +325,7 @@ again:
 					msecs_to_jiffies(100);
 				mod_timer(&data->rx_timeout, jiffies + delay);
 			} else {
-				del_timer(&data->rx_timeout);
+				timer_delete(&data->rx_timeout);
 			}
 			/* Pass data to keyboard buffer parser */
 			ir_mce_kbd_process_keyboard_data(dev, scancode);
@@ -372,7 +373,7 @@ static int ir_mce_kbd_unregister(struct rc_dev *dev)
 {
 	struct mce_kbd_dec *mce_kbd = &dev->raw->mce_kbd;
 
-	del_timer_sync(&mce_kbd->rx_timeout);
+	timer_delete_sync(&mce_kbd->rx_timeout);
 
 	return 0;
 }
diff --git a/drivers/media/rc/ir-spi.c b/drivers/media/rc/ir-spi.c
index bbc81bed4f90..8fc8e496e6aa 100644
--- a/drivers/media/rc/ir-spi.c
+++ b/drivers/media/rc/ir-spi.c
@@ -4,13 +4,18 @@
 // Copyright (c) 2016 Samsung Electronics Co., Ltd.
 // Copyright (c) Andi Shyti <andi@etezian.org>
 
-#include <linux/delay.h>
-#include <linux/fs.h>
+#include <linux/bits.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/math.h>
+#include <linux/mod_devicetable.h>
 #include <linux/module.h>
-#include <linux/mutex.h>
-#include <linux/of_gpio.h>
+#include <linux/property.h>
 #include <linux/regulator/consumer.h>
 #include <linux/spi/spi.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
 #include <media/rc-core.h>
 
 #define IR_SPI_DRIVER_NAME		"ir-spi"
@@ -31,8 +36,7 @@ struct ir_spi_data {
 	struct regulator *regulator;
 };
 
-static int ir_spi_tx(struct rc_dev *dev,
-		     unsigned int *buffer, unsigned int count)
+static int ir_spi_tx(struct rc_dev *dev, unsigned int *buffer, unsigned int count)
 {
 	int i;
 	int ret;
@@ -52,7 +56,7 @@ static int ir_spi_tx(struct rc_dev *dev,
 			return -EINVAL;
 
 		/*
-		 * the first value in buffer is a pulse, so that 0, 2, 4, ...
+		 * The first value in buffer is a pulse, so that 0, 2, 4, ...
 		 * contain a pulse duration. On the contrary, 1, 3, 5, ...
 		 * contain a space duration.
 		 */
@@ -111,15 +115,16 @@ static int ir_spi_set_duty_cycle(struct rc_dev *dev, u32 duty_cycle)
 
 static int ir_spi_probe(struct spi_device *spi)
 {
+	struct device *dev = &spi->dev;
 	int ret;
 	u8 dc;
 	struct ir_spi_data *idata;
 
-	idata = devm_kzalloc(&spi->dev, sizeof(*idata), GFP_KERNEL);
+	idata = devm_kzalloc(dev, sizeof(*idata), GFP_KERNEL);
 	if (!idata)
 		return -ENOMEM;
 
-	idata->regulator = devm_regulator_get(&spi->dev, "irda_regulator");
+	idata->regulator = devm_regulator_get(dev, "irda_regulator");
 	if (IS_ERR(idata->regulator))
 		return PTR_ERR(idata->regulator);
 
@@ -135,32 +140,31 @@ static int ir_spi_probe(struct spi_device *spi)
 	idata->rc->priv            = idata;
 	idata->spi                 = spi;
 
-	idata->negated = of_property_read_bool(spi->dev.of_node,
-							"led-active-low");
-	ret = of_property_read_u8(spi->dev.of_node, "duty-cycle", &dc);
+	idata->negated = device_property_read_bool(dev, "led-active-low");
+	ret = device_property_read_u8(dev, "duty-cycle", &dc);
 	if (ret)
 		dc = 50;
 
-	/* ir_spi_set_duty_cycle cannot fail,
-	 * it returns int to be compatible with the
-	 * rc->s_tx_duty_cycle function
+	/*
+	 * ir_spi_set_duty_cycle() cannot fail, it returns int
+	 * to be compatible with the rc->s_tx_duty_cycle function.
 	 */
 	ir_spi_set_duty_cycle(idata->rc, dc);
 
 	idata->freq = IR_SPI_DEFAULT_FREQUENCY;
 
-	return devm_rc_register_device(&spi->dev, idata->rc);
+	return devm_rc_register_device(dev, idata->rc);
 }
 
 static const struct of_device_id ir_spi_of_match[] = {
 	{ .compatible = "ir-spi-led" },
-	{},
+	{}
 };
 MODULE_DEVICE_TABLE(of, ir_spi_of_match);
 
 static const struct spi_device_id ir_spi_ids[] = {
 	{ "ir-spi-led" },
-	{},
+	{}
 };
 MODULE_DEVICE_TABLE(spi, ir_spi_ids);
 
@@ -172,7 +176,6 @@ static struct spi_driver ir_spi_driver = {
 		.of_match_table = ir_spi_of_match,
 	},
 };
-
 module_spi_driver(ir_spi_driver);
 
 MODULE_AUTHOR("Andi Shyti <andi@etezian.org>");
diff --git a/drivers/media/rc/ir_toy.c b/drivers/media/rc/ir_toy.c
index 69e630d85262..533faa117517 100644
--- a/drivers/media/rc/ir_toy.c
+++ b/drivers/media/rc/ir_toy.c
@@ -12,7 +12,7 @@
  * Copyright (C) 2011 Peter Kooiman <pkooiman@gmail.com>
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/completion.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index fcfadd7ea31c..2bacecb02262 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1380,7 +1380,6 @@ static int ite_probe(struct pnp_dev *pdev, const struct pnp_device_id
 	rdev->timeout = IR_DEFAULT_TIMEOUT;
 	rdev->max_timeout = 10 * IR_DEFAULT_TIMEOUT;
 	rdev->rx_resolution = ITE_BAUDRATE_DIVISOR * sample_period / 1000;
-	rdev->tx_resolution = ITE_BAUDRATE_DIVISOR * sample_period / 1000;
 
 	/* set up transmitter related values */
 	rdev->tx_ir = ite_tx_ir;
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index f19558fdab0c..d04572627cdd 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -84,6 +84,7 @@ obj-$(CONFIG_RC_MAP) += \
 			rc-msi-digivox-ii.o \
 			rc-msi-tvanywhere.o \
 			rc-msi-tvanywhere-plus.o \
+			rc-mygica-utv3.o \
 			rc-nebula.o \
 			rc-nec-terratec-cinergy-xs.o \
 			rc-norwood.o \
@@ -105,6 +106,7 @@ obj-$(CONFIG_RC_MAP) += \
 			rc-rc6-mce.o \
 			rc-real-audio-220-32-keys.o \
 			rc-reddo.o \
+			rc-siemens-gigaset-rc20.o \
 			rc-snapstream-firefly.o \
 			rc-streamzap.o \
 			rc-su3000.o \
diff --git a/drivers/media/rc/keymaps/rc-hauppauge.c b/drivers/media/rc/keymaps/rc-hauppauge.c
index d7156774aa0e..9e64c0b2d18e 100644
--- a/drivers/media/rc/keymaps/rc-hauppauge.c
+++ b/drivers/media/rc/keymaps/rc-hauppauge.c
@@ -261,6 +261,48 @@ static struct rc_map_table rc5_hauppauge_new[] = {
 	{ 0x001e, KEY_RED },	/* Reserved */
 	{ 0x0000, KEY_NUMERIC_0 },
 	{ 0x0026, KEY_SLEEP },	/* Minimize */
+
+	/*
+	 * Keycodes for the black Credit Card Remote Control shipped with, for
+	 * example, the WinTV-dualHD tuner.
+	 * Keycodes start with address = 0x19
+	 */
+	{ 0x190a, KEY_LAST },		/* <- */
+	{ 0x192f, KEY_MENU },		/* List */
+	{ 0x1910, KEY_CHANNELUP },
+	{ 0x192e, KEY_CHANNELDOWN },
+	{ 0x192c, KEY_OK },
+
+	{ 0x1911, KEY_TV },
+	{ 0x190c, KEY_POWER },
+
+	{ 0x1900, KEY_NUMERIC_0 },
+	{ 0x1938, KEY_NUMERIC_1 },
+	{ 0x1920, KEY_NUMERIC_2 },
+	{ 0x1901, KEY_NUMERIC_3 },
+	{ 0x1902, KEY_NUMERIC_4 },
+	{ 0x1904, KEY_NUMERIC_5 },
+	{ 0x1905, KEY_NUMERIC_6 },
+	{ 0x1907, KEY_NUMERIC_7 },
+	{ 0x1908, KEY_NUMERIC_8 },
+	{ 0x190f, KEY_NUMERIC_9 },
+
+	{ 0x1921, KEY_VOLUMEUP },
+	{ 0x1903, KEY_VOLUMEDOWN },
+	{ 0x1906, KEY_MUTE },
+
+	{ 0x1909, KEY_CAMERA },		/* Snap */
+	{ 0x1922, KEY_SUBTITLE },	/* CC */
+	{ 0x192b, KEY_INFO },
+
+	{ 0x1929, KEY_END },		/* Skip to live TV */
+	{ 0x190d, KEY_PLAYPAUSE },
+	{ 0x1926, KEY_STOP },
+	{ 0x192a, KEY_RECORD },
+	{ 0x193a, KEY_PREVIOUS },	/* |< */
+	{ 0x193b, KEY_REWIND },		/* << */
+	{ 0x193c, KEY_FASTFORWARD },	/* >> */
+	{ 0x193d, KEY_NEXT },		/* >| */
 };
 
 static struct rc_map_list rc5_hauppauge_new_map = {
diff --git a/drivers/media/rc/keymaps/rc-mygica-utv3.c b/drivers/media/rc/keymaps/rc-mygica-utv3.c
new file mode 100644
index 000000000000..f32b8281459b
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-mygica-utv3.c
@@ -0,0 +1,69 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* rc-mygica-utv3.c - Keytable for the MyGica UTV3 Analog USB2.0 TV Box
+ *
+ * Copyright (c) 2024 by Nils Rothaug
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table mygica_utv3[] = {
+	{ 0x0d, KEY_MUTE },
+	{ 0x38, KEY_VIDEO },  /* Source */
+	{ 0x14, KEY_RADIO },  /* FM Radio */
+	{ 0x0c, KEY_POWER2 },
+
+	{ 0x01, KEY_NUMERIC_1},
+	{ 0x02, KEY_NUMERIC_2},
+	{ 0x03, KEY_NUMERIC_3},
+	{ 0x04, KEY_NUMERIC_4},
+	{ 0x05, KEY_NUMERIC_5},
+	{ 0x06, KEY_NUMERIC_6},
+	{ 0x07, KEY_NUMERIC_7},
+	{ 0x08, KEY_NUMERIC_8},
+	{ 0x09, KEY_NUMERIC_9},
+	{ 0x00, KEY_NUMERIC_0},
+
+	{ 0x0a, KEY_DIGITS }, /* Single/double/triple digit */
+	{ 0x0e, KEY_CAMERA }, /* Snapshot */
+	{ 0x0f, KEY_ZOOM },   /* Full Screen */
+	{ 0x29, KEY_LAST },   /* Recall (return to previous channel) */
+
+	{ 0x17, KEY_PLAY },
+	{ 0x1f, KEY_RECORD },
+	{ 0x0b, KEY_STOP },
+	{ 0x16, KEY_PAUSE },
+
+	{ 0x20, KEY_CHANNELUP },
+	{ 0x21, KEY_CHANNELDOWN },
+	{ 0x10, KEY_VOLUMEUP },
+	{ 0x11, KEY_VOLUMEDOWN },
+	{ 0x26, KEY_REWIND },
+	{ 0x27, KEY_FASTFORWARD },
+};
+
+static struct rc_map_list mygica_utv3_map = {
+	.map = {
+		.scan     = mygica_utv3,
+		.size     = ARRAY_SIZE(mygica_utv3),
+		.rc_proto = RC_PROTO_RC5,
+		.name     = RC_MAP_MYGICA_UTV3,
+	}
+};
+
+static int __init init_rc_map_mygica_utv3(void)
+{
+	return rc_map_register(&mygica_utv3_map);
+}
+
+static void __exit exit_rc_map_mygica_utv3(void)
+{
+	rc_map_unregister(&mygica_utv3_map);
+}
+
+module_init(init_rc_map_mygica_utv3)
+module_exit(exit_rc_map_mygica_utv3)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Nils Rothaug");
+MODULE_DESCRIPTION("MyGica UTV3 Analog USB2.0 TV Box remote keytable");
diff --git a/drivers/media/rc/keymaps/rc-siemens-gigaset-rc20.c b/drivers/media/rc/keymaps/rc-siemens-gigaset-rc20.c
new file mode 100644
index 000000000000..defc77932e10
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-siemens-gigaset-rc20.c
@@ -0,0 +1,71 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/* rc-siemens-gigaset-rc20.c - Keytable for the Siemens Gigaset RC 20 remote
+ *
+ * Copyright (c) 2025 by Michael Klein
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+
+static struct rc_map_table siemens_gigaset_rc20[] = {
+	{ 0x1501, KEY_POWER },
+	{ 0x1502, KEY_MUTE },
+	{ 0x1503, KEY_NUMERIC_1 },
+	{ 0x1504, KEY_NUMERIC_2 },
+	{ 0x1505, KEY_NUMERIC_3 },
+	{ 0x1506, KEY_NUMERIC_4 },
+	{ 0x1507, KEY_NUMERIC_5 },
+	{ 0x1508, KEY_NUMERIC_6 },
+	{ 0x1509, KEY_NUMERIC_7 },
+	{ 0x150a, KEY_NUMERIC_8 },
+	{ 0x150b, KEY_NUMERIC_9 },
+	{ 0x150c, KEY_NUMERIC_0 },
+	{ 0x150d, KEY_UP },
+	{ 0x150e, KEY_LEFT },
+	{ 0x150f, KEY_OK },
+	{ 0x1510, KEY_RIGHT },
+	{ 0x1511, KEY_DOWN },
+	{ 0x1512, KEY_SHUFFLE },        /* double-arrow */
+	{ 0x1513, KEY_EXIT },
+	{ 0x1514, KEY_RED },
+	{ 0x1515, KEY_GREEN },
+	{ 0x1516, KEY_YELLOW },         /* OPT */
+	{ 0x1517, KEY_BLUE },
+	{ 0x1518, KEY_MENU },
+	{ 0x1519, KEY_TEXT },
+	{ 0x151a, KEY_MODE },           /* TV/Radio */
+
+	{ 0x1521, KEY_EPG },
+	{ 0x1522, KEY_FAVORITES },
+	{ 0x1523, KEY_CHANNELUP },
+	{ 0x1524, KEY_CHANNELDOWN },
+	{ 0x1525, KEY_VOLUMEUP },
+	{ 0x1526, KEY_VOLUMEDOWN },
+	{ 0x1527, KEY_INFO },
+};
+
+static struct rc_map_list siemens_gigaset_rc20_map = {
+	.map = {
+		.scan     = siemens_gigaset_rc20,
+		.size     = ARRAY_SIZE(siemens_gigaset_rc20),
+		.rc_proto = RC_PROTO_RC5,
+		.name     = RC_MAP_SIEMENS_GIGASET_RC20,
+	}
+};
+
+static int __init init_rc_map_siemens_gigaset_rc20(void)
+{
+	return rc_map_register(&siemens_gigaset_rc20_map);
+}
+
+static void __exit exit_rc_map_siemens_gigaset_rc20(void)
+{
+	rc_map_unregister(&siemens_gigaset_rc20_map);
+}
+
+module_init(init_rc_map_siemens_gigaset_rc20)
+module_exit(exit_rc_map_siemens_gigaset_rc20)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Michael Klein");
+MODULE_DESCRIPTION("Siemens Gigaset RC20 remote keytable");
diff --git a/drivers/media/rc/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index caad59f76793..a2257dc2f25d 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -27,7 +27,9 @@ static dev_t lirc_base_dev;
 static DEFINE_IDA(lirc_ida);
 
 /* Only used for sysfs but defined to void otherwise */
-static struct class *lirc_class;
+static const struct class lirc_class = {
+	.name = "lirc",
+};
 
 /**
  * lirc_raw_event() - Send raw IR data to lirc to be relayed to userspace
@@ -704,7 +706,6 @@ static const struct file_operations lirc_fops = {
 	.poll		= lirc_poll,
 	.open		= lirc_open,
 	.release	= lirc_close,
-	.llseek		= no_llseek,
 };
 
 static void lirc_release_device(struct device *ld)
@@ -724,7 +725,7 @@ int lirc_register(struct rc_dev *dev)
 		return minor;
 
 	device_initialize(&dev->lirc_dev);
-	dev->lirc_dev.class = lirc_class;
+	dev->lirc_dev.class = &lirc_class;
 	dev->lirc_dev.parent = &dev->dev;
 	dev->lirc_dev.release = lirc_release_device;
 	dev->lirc_dev.devt = MKDEV(MAJOR(lirc_base_dev), minor);
@@ -789,15 +790,13 @@ int __init lirc_dev_init(void)
 {
 	int retval;
 
-	lirc_class = class_create("lirc");
-	if (IS_ERR(lirc_class)) {
-		pr_err("class_create failed\n");
-		return PTR_ERR(lirc_class);
-	}
+	retval = class_register(&lirc_class);
+	if (retval)
+		return retval;
 
 	retval = alloc_chrdev_region(&lirc_base_dev, 0, RC_DEV_MAX, "lirc");
 	if (retval) {
-		class_destroy(lirc_class);
+		class_unregister(&lirc_class);
 		pr_err("alloc_chrdev_region failed\n");
 		return retval;
 	}
@@ -810,32 +809,29 @@ int __init lirc_dev_init(void)
 
 void __exit lirc_dev_exit(void)
 {
-	class_destroy(lirc_class);
+	class_unregister(&lirc_class);
 	unregister_chrdev_region(lirc_base_dev, RC_DEV_MAX);
 }
 
 struct rc_dev *rc_dev_get_from_fd(int fd, bool write)
 {
-	struct fd f = fdget(fd);
+	CLASS(fd, f)(fd);
 	struct lirc_fh *fh;
 	struct rc_dev *dev;
 
-	if (!f.file)
+	if (fd_empty(f))
 		return ERR_PTR(-EBADF);
 
-	if (f.file->f_op != &lirc_fops) {
-		fdput(f);
+	if (fd_file(f)->f_op != &lirc_fops)
 		return ERR_PTR(-EINVAL);
-	}
 
-	if (write && !(f.file->f_mode & FMODE_WRITE))
+	if (write && !(fd_file(f)->f_mode & FMODE_WRITE))
 		return ERR_PTR(-EPERM);
 
-	fh = f.file->private_data;
+	fh = fd_file(f)->private_data;
 	dev = fh->rc;
 
 	get_device(&dev->dev);
-	fdput(f);
 
 	return dev;
 }
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index c76ba24c1f55..044767eb3a38 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -28,7 +28,6 @@
 #include <linux/workqueue.h>
 #include <linux/usb.h>
 #include <linux/usb/input.h>
-#include <linux/pm_wakeup.h>
 #include <media/rc-core.h>
 
 #define DRIVER_VERSION	"1.95"
@@ -494,7 +493,6 @@ struct mceusb_dev {
 	u32 carrier;
 	unsigned char tx_mask;
 
-	char name[128];
 	char phys[64];
 	enum mceusb_model_type model;
 
@@ -659,8 +657,8 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, u8 *buf, int buf_len,
 			if (len == 2)
 				dev_dbg(dev, "Get hw/sw rev?");
 			else
-				dev_dbg(dev, "hw/sw rev %*ph",
-					4, &buf[offset + 2]);
+				dev_dbg(dev, "hw/sw rev %4ph",
+					&buf[offset + 2]);
 			break;
 		case MCE_CMD_RESUME:
 			dev_dbg(dev, "Device resume requested");
@@ -774,7 +772,7 @@ static void mceusb_dev_printdata(struct mceusb_dev *ir, u8 *buf, int buf_len,
 
 /*
  * Schedule work that can't be done in interrupt handlers
- * (mceusb_dev_recv() and mce_write_callback()) nor tasklets.
+ * (mceusb_dev_recv() and mce_write_callback()) nor BH work.
  * Invokes mceusb_deferred_kevent() for recovering from
  * error events specified by the kevent bit field.
  */
@@ -1591,16 +1589,10 @@ static struct rc_dev *mceusb_init_rc_dev(struct mceusb_dev *ir)
 		goto out;
 	}
 
-	snprintf(ir->name, sizeof(ir->name), "%s (%04x:%04x)",
-		 mceusb_model[ir->model].name ?
-			mceusb_model[ir->model].name :
-			"Media Center Ed. eHome Infrared Remote Transceiver",
-		 le16_to_cpu(ir->usbdev->descriptor.idVendor),
-		 le16_to_cpu(ir->usbdev->descriptor.idProduct));
-
 	usb_make_path(ir->usbdev, ir->phys, sizeof(ir->phys));
 
-	rc->device_name = ir->name;
+	rc->device_name = mceusb_model[ir->model].name ? :
+		"Media Center Ed. eHome Infrared Remote Transceiver";
 	rc->input_phys = ir->phys;
 	usb_to_input_id(ir->usbdev, &rc->input_id);
 	rc->dev.parent = dev;
diff --git a/drivers/media/rc/meson-ir.c b/drivers/media/rc/meson-ir.c
index 5303e6da5809..272ebb0d97c8 100644
--- a/drivers/media/rc/meson-ir.c
+++ b/drivers/media/rc/meson-ir.c
@@ -567,6 +567,32 @@ static void meson_ir_shutdown(struct platform_device *pdev)
 	spin_unlock_irqrestore(&ir->lock, flags);
 }
 
+static __maybe_unused int meson_ir_resume(struct device *dev)
+{
+	struct meson_ir *ir = dev_get_drvdata(dev);
+
+	if (ir->param->support_hw_decoder)
+		meson_ir_hw_decoder_init(ir->rc, &ir->rc->enabled_protocols);
+	else
+		meson_ir_sw_decoder_init(ir->rc);
+
+	return 0;
+}
+
+static __maybe_unused int meson_ir_suspend(struct device *dev)
+{
+	struct meson_ir *ir = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	spin_lock_irqsave(&ir->lock, flags);
+	regmap_update_bits(ir->reg, IR_DEC_REG1, IR_DEC_REG1_ENABLE, 0);
+	spin_unlock_irqrestore(&ir->lock, flags);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(meson_ir_pm_ops, meson_ir_suspend, meson_ir_resume);
+
 static const struct meson_ir_param meson6_ir_param = {
 	.support_hw_decoder = false,
 	.max_register = IR_DEC_REG1,
@@ -602,11 +628,12 @@ MODULE_DEVICE_TABLE(of, meson_ir_match);
 
 static struct platform_driver meson_ir_driver = {
 	.probe		= meson_ir_probe,
-	.remove_new	= meson_ir_remove,
+	.remove		= meson_ir_remove,
 	.shutdown	= meson_ir_shutdown,
 	.driver = {
 		.name		= DRIVER_NAME,
 		.of_match_table	= meson_ir_match,
+		.pm = pm_ptr(&meson_ir_pm_ops),
 	},
 };
 
diff --git a/drivers/media/rc/mtk-cir.c b/drivers/media/rc/mtk-cir.c
index 4e294e59d3cb..85c9436b0a20 100644
--- a/drivers/media/rc/mtk-cir.c
+++ b/drivers/media/rc/mtk-cir.c
@@ -8,6 +8,7 @@
 #include <linux/clk.h>
 #include <linux/interrupt.h>
 #include <linux/module.h>
+#include <linux/io.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
@@ -439,7 +440,7 @@ static void mtk_ir_remove(struct platform_device *pdev)
 
 static struct platform_driver mtk_ir_driver = {
 	.probe          = mtk_ir_probe,
-	.remove_new     = mtk_ir_remove,
+	.remove         = mtk_ir_remove,
 	.driver = {
 		.name = MTK_IR_DEV,
 		.of_match_table = mtk_ir_match,
diff --git a/drivers/media/rc/pwm-ir-tx.c b/drivers/media/rc/pwm-ir-tx.c
index fe368aebbc13..84533fdd61aa 100644
--- a/drivers/media/rc/pwm-ir-tx.c
+++ b/drivers/media/rc/pwm-ir-tx.c
@@ -172,8 +172,7 @@ static int pwm_ir_probe(struct platform_device *pdev)
 		rcdev->tx_ir = pwm_ir_tx_sleep;
 	} else {
 		init_completion(&pwm_ir->tx_done);
-		hrtimer_init(&pwm_ir->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
-		pwm_ir->timer.function = pwm_ir_timer;
+		hrtimer_setup(&pwm_ir->timer, pwm_ir_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
 		rcdev->tx_ir = pwm_ir_tx_atomic;
 	}
 
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index 7df949fc65e2..4967d87ec4b7 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -85,8 +85,8 @@ struct ir_raw_event_ctrl {
 	struct rc6_dec {
 		int state;
 		u8 header;
-		u32 body;
 		bool toggle;
+		u32 body;
 		unsigned count;
 		unsigned wanted_bits;
 	} rc6;
@@ -127,8 +127,8 @@ struct ir_raw_event_ctrl {
 	struct mce_kbd_dec {
 		/* locks key up timer */
 		spinlock_t keylock;
-		struct timer_list rx_timeout;
 		int state;
+		struct timer_list rx_timeout;
 		u8 header;
 		u32 body;
 		unsigned count;
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index 16e33d7eaaa2..5dafe11f61c6 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -552,7 +552,8 @@ EXPORT_SYMBOL(ir_raw_encode_scancode);
  */
 static void ir_raw_edge_handle(struct timer_list *t)
 {
-	struct ir_raw_event_ctrl *raw = from_timer(raw, t, edge_handle);
+	struct ir_raw_event_ctrl *raw = timer_container_of(raw, t,
+							   edge_handle);
 	struct rc_dev *dev = raw->dev;
 	unsigned long flags;
 	ktime_t interval;
@@ -662,7 +663,7 @@ void ir_raw_event_unregister(struct rc_dev *dev)
 		return;
 
 	kthread_stop(dev->raw->thread);
-	del_timer_sync(&dev->raw->edge_handle);
+	timer_delete_sync(&dev->raw->edge_handle);
 
 	mutex_lock(&ir_raw_handler_lock);
 	list_del(&dev->raw->list);
diff --git a/drivers/media/rc/rc-loopback.c b/drivers/media/rc/rc-loopback.c
index b356041c5c00..8288366f891f 100644
--- a/drivers/media/rc/rc-loopback.c
+++ b/drivers/media/rc/rc-loopback.c
@@ -230,7 +230,6 @@ static int __init loop_init(void)
 	rc->min_timeout		= 1;
 	rc->max_timeout		= IR_MAX_TIMEOUT;
 	rc->rx_resolution	= 1;
-	rc->tx_resolution	= 1;
 	rc->s_tx_mask		= loop_set_tx_mask;
 	rc->s_tx_carrier	= loop_set_tx_carrier;
 	rc->s_tx_duty_cycle	= loop_set_tx_duty_cycle;
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 6bdad6341844..b9bf5cdcde4a 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -639,7 +639,7 @@ static void ir_do_keyup(struct rc_dev *dev, bool sync)
 		return;
 
 	dev_dbg(&dev->dev, "keyup key 0x%04x\n", dev->last_keycode);
-	del_timer(&dev->timer_repeat);
+	timer_delete(&dev->timer_repeat);
 	input_report_key(dev->input_dev, dev->last_keycode, 0);
 	led_trigger_event(led_feedback, LED_OFF);
 	if (sync)
@@ -674,7 +674,7 @@ EXPORT_SYMBOL_GPL(rc_keyup);
  */
 static void ir_timer_keyup(struct timer_list *t)
 {
-	struct rc_dev *dev = from_timer(dev, t, timer_keyup);
+	struct rc_dev *dev = timer_container_of(dev, t, timer_keyup);
 	unsigned long flags;
 
 	/*
@@ -703,7 +703,7 @@ static void ir_timer_keyup(struct timer_list *t)
  */
 static void ir_timer_repeat(struct timer_list *t)
 {
-	struct rc_dev *dev = from_timer(dev, t, timer_repeat);
+	struct rc_dev *dev = timer_container_of(dev, t, timer_repeat);
 	struct input_dev *input = dev->input_dev;
 	unsigned long flags;
 
@@ -2021,8 +2021,8 @@ void rc_unregister_device(struct rc_dev *dev)
 	if (dev->driver_type == RC_DRIVER_IR_RAW)
 		ir_raw_event_unregister(dev);
 
-	del_timer_sync(&dev->timer_keyup);
-	del_timer_sync(&dev->timer_repeat);
+	timer_delete_sync(&dev->timer_keyup);
+	timer_delete_sync(&dev->timer_repeat);
 
 	mutex_lock(&dev->lock);
 	if (dev->users && dev->close)
@@ -2092,4 +2092,5 @@ subsys_initcall(rc_core_init);
 module_exit(rc_core_exit);
 
 MODULE_AUTHOR("Mauro Carvalho Chehab");
+MODULE_DESCRIPTION("Remote Controller core module");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/rc/redrat3.c b/drivers/media/rc/redrat3.c
index 9f2947af33aa..d89a4cfe3c89 100644
--- a/drivers/media/rc/redrat3.c
+++ b/drivers/media/rc/redrat3.c
@@ -31,7 +31,7 @@
  * --
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/device.h>
 #include <linux/leds.h>
 #include <linux/module.h>
diff --git a/drivers/media/rc/serial_ir.c b/drivers/media/rc/serial_ir.c
index 96ae0294ac10..992fff82b524 100644
--- a/drivers/media/rc/serial_ir.c
+++ b/drivers/media/rc/serial_ir.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/serial_reg.h>
 #include <linux/types.h>
@@ -797,7 +798,7 @@ static int __init serial_ir_init_module(void)
 
 static void __exit serial_ir_exit_module(void)
 {
-	del_timer_sync(&serial_ir.timeout_timer);
+	timer_delete_sync(&serial_ir.timeout_timer);
 	serial_ir_exit();
 }
 
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index 28477aa95563..6539fa0a6e79 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -6,6 +6,7 @@
 #include <linux/kernel.h>
 #include <linux/clk.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
@@ -407,7 +408,7 @@ static struct platform_driver st_rc_driver = {
 		.pm     = &st_rc_pm_ops,
 	},
 	.probe = st_rc_probe,
-	.remove_new = st_rc_remove,
+	.remove = st_rc_remove,
 };
 
 module_platform_driver(st_rc_driver);
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index 9b209e687f25..d3b48a0dd1f4 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -138,39 +138,10 @@ static void sz_push_half_space(struct streamzap_ir *sz,
 	sz_push_full_space(sz, value & SZ_SPACE_MASK);
 }
 
-/*
- * streamzap_callback - usb IRQ handler callback
- *
- * This procedure is invoked on reception of data from
- * the usb remote.
- */
-static void streamzap_callback(struct urb *urb)
+static void sz_process_ir_data(struct streamzap_ir *sz, int len)
 {
-	struct streamzap_ir *sz;
 	unsigned int i;
-	int len;
 
-	if (!urb)
-		return;
-
-	sz = urb->context;
-	len = urb->actual_length;
-
-	switch (urb->status) {
-	case -ECONNRESET:
-	case -ENOENT:
-	case -ESHUTDOWN:
-		/*
-		 * this urb is terminated, clean up.
-		 * sz might already be invalid at this point
-		 */
-		dev_err(sz->dev, "urb terminated, status: %d\n", urb->status);
-		return;
-	default:
-		break;
-	}
-
-	dev_dbg(sz->dev, "%s: received urb, len %d\n", __func__, len);
 	for (i = 0; i < len; i++) {
 		dev_dbg(sz->dev, "sz->buf_in[%d]: %x\n",
 			i, (unsigned char)sz->buf_in[i]);
@@ -219,6 +190,43 @@ static void streamzap_callback(struct urb *urb)
 	}
 
 	ir_raw_event_handle(sz->rdev);
+}
+
+/*
+ * streamzap_callback - usb IRQ handler callback
+ *
+ * This procedure is invoked on reception of data from
+ * the usb remote.
+ */
+static void streamzap_callback(struct urb *urb)
+{
+	struct streamzap_ir *sz;
+	int len;
+
+	if (!urb)
+		return;
+
+	sz = urb->context;
+	len = urb->actual_length;
+
+	switch (urb->status) {
+	case 0:
+		dev_dbg(sz->dev, "%s: received urb, len %d\n", __func__, len);
+		sz_process_ir_data(sz, len);
+		break;
+	case -ECONNRESET:
+	case -ENOENT:
+	case -ESHUTDOWN:
+		/*
+		 * this urb is terminated, clean up.
+		 * sz might already be invalid at this point
+		 */
+		dev_err(sz->dev, "urb terminated, status: %d\n", urb->status);
+		return;
+	default:
+		break;
+	}
+
 	usb_submit_urb(urb, GFP_ATOMIC);
 }
 
@@ -385,8 +393,8 @@ static void streamzap_disconnect(struct usb_interface *interface)
 	if (!sz)
 		return;
 
-	rc_unregister_device(sz->rdev);
 	usb_kill_urb(sz->urb_in);
+	rc_unregister_device(sz->rdev);
 	usb_free_urb(sz->urb_in);
 	usb_free_coherent(usbdev, sz->buf_in_len, sz->buf_in, sz->dma_in);
 
diff --git a/drivers/media/rc/sunxi-cir.c b/drivers/media/rc/sunxi-cir.c
index bf58c965ead8..92ef4e7c6f69 100644
--- a/drivers/media/rc/sunxi-cir.c
+++ b/drivers/media/rc/sunxi-cir.c
@@ -12,6 +12,7 @@
 
 #include <linux/clk.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
@@ -412,7 +413,7 @@ MODULE_DEVICE_TABLE(of, sunxi_ir_match);
 
 static struct platform_driver sunxi_ir_driver = {
 	.probe          = sunxi_ir_probe,
-	.remove_new     = sunxi_ir_remove,
+	.remove         = sunxi_ir_remove,
 	.shutdown       = sunxi_ir_shutdown,
 	.driver = {
 		.name = SUNXI_IR_DEV,
diff --git a/drivers/media/spi/cxd2880-spi.c b/drivers/media/spi/cxd2880-spi.c
index 6be4e5528879..65fa7f857fca 100644
--- a/drivers/media/spi/cxd2880-spi.c
+++ b/drivers/media/spi/cxd2880-spi.c
@@ -388,7 +388,7 @@ static int cxd2880_start_feed(struct dvb_demux_feed *feed)
 
 	if (dvb_spi->feed_count == 0) {
 		dvb_spi->ts_buf =
-			kmalloc(MAX_TRANS_PKT * 188,
+			kzalloc(MAX_TRANS_PKT * 188,
 				GFP_KERNEL | GFP_DMA);
 		if (!dvb_spi->ts_buf) {
 			pr_err("ts buffer allocate failed\n");
diff --git a/drivers/media/spi/gs1662.c b/drivers/media/spi/gs1662.c
index 75c21a93e6d0..7adf55fd0707 100644
--- a/drivers/media/spi/gs1662.c
+++ b/drivers/media/spi/gs1662.c
@@ -55,14 +55,6 @@ struct gs_reg_fmt {
 	struct v4l2_dv_timings format;
 };
 
-struct gs_reg_fmt_custom {
-	u16 reg_value;
-	__u32 width;
-	__u32 height;
-	__u64 pixelclock;
-	__u32 interlaced;
-};
-
 static const struct spi_device_id gs_id[] = {
 	{ "gs1662", 0 },
 	{ }
@@ -259,12 +251,15 @@ static inline struct gs *to_gs(struct v4l2_subdev *sd)
 	return container_of(sd, struct gs, sd);
 }
 
-static int gs_s_dv_timings(struct v4l2_subdev *sd,
-		    struct v4l2_dv_timings *timings)
+static int gs_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+			   struct v4l2_dv_timings *timings)
 {
 	struct gs *gs = to_gs(sd);
 	int reg_value;
 
+	if (pad != 0)
+		return -EINVAL;
+
 	reg_value = get_register_timings(timings);
 	if (reg_value == 0x0)
 		return -EINVAL;
@@ -273,23 +268,29 @@ static int gs_s_dv_timings(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int gs_g_dv_timings(struct v4l2_subdev *sd,
-		    struct v4l2_dv_timings *timings)
+static int gs_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+			   struct v4l2_dv_timings *timings)
 {
 	struct gs *gs = to_gs(sd);
 
+	if (pad != 0)
+		return -EINVAL;
+
 	*timings = gs->current_timings;
 	return 0;
 }
 
-static int gs_query_dv_timings(struct v4l2_subdev *sd,
-			struct v4l2_dv_timings *timings)
+static int gs_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+			       struct v4l2_dv_timings *timings)
 {
 	struct gs *gs = to_gs(sd);
 	struct v4l2_dv_timings fmt;
 	u16 reg_value, i;
 	int ret;
 
+	if (pad != 0)
+		return -EINVAL;
+
 	if (gs->enabled)
 		return -EBUSY;
 
@@ -410,14 +411,14 @@ static const struct v4l2_subdev_core_ops gs_core_ops = {
 };
 
 static const struct v4l2_subdev_video_ops gs_video_ops = {
-	.s_dv_timings = gs_s_dv_timings,
-	.g_dv_timings = gs_g_dv_timings,
 	.s_stream = gs_s_stream,
 	.g_input_status = gs_g_input_status,
-	.query_dv_timings = gs_query_dv_timings,
 };
 
 static const struct v4l2_subdev_pad_ops gs_pad_ops = {
+	.s_dv_timings = gs_s_dv_timings,
+	.g_dv_timings = gs_g_dv_timings,
+	.query_dv_timings = gs_query_dv_timings,
 	.enum_dv_timings = gs_enum_dv_timings,
 	.dv_timings_cap = gs_dv_timings_cap,
 };
diff --git a/drivers/media/test-drivers/vicodec/vicodec-core.c b/drivers/media/test-drivers/vicodec/vicodec-core.c
index e13f5452b927..c45f5cf12ded 100644
--- a/drivers/media/test-drivers/vicodec/vicodec-core.c
+++ b/drivers/media/test-drivers/vicodec/vicodec-core.c
@@ -26,7 +26,7 @@
 #include "codec-v4l2-fwht.h"
 
 MODULE_DESCRIPTION("Virtual codec device");
-MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_AUTHOR("Hans Verkuil <hansverk@cisco.com>");
 MODULE_LICENSE("GPL v2");
 
 static bool multiplanar;
@@ -43,6 +43,8 @@ MODULE_PARM_DESC(debug, " activates debug info");
 #define MIN_WIDTH		640U
 #define MAX_HEIGHT		2160U
 #define MIN_HEIGHT		360U
+/* Recommended number of buffers for the stateful codecs */
+#define VICODEC_REC_BUFS	2
 
 #define dprintk(dev, fmt, arg...) \
 	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
@@ -1215,8 +1217,7 @@ static int vicodec_encoder_cmd(struct file *file, void *fh,
 	if (ret < 0)
 		return ret;
 
-	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
-	    !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
+	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
 		return 0;
 
 	ret = v4l2_m2m_ioctl_encoder_cmd(file, fh, ec);
@@ -1250,8 +1251,7 @@ static int vicodec_decoder_cmd(struct file *file, void *fh,
 	if (ret < 0)
 		return ret;
 
-	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
-	    !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
+	if (!vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
 		return 0;
 
 	ret = v4l2_m2m_ioctl_decoder_cmd(file, fh, dc);
@@ -1345,6 +1345,7 @@ static const struct v4l2_ioctl_ops vicodec_ioctl_ops = {
 	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
+	.vidioc_remove_bufs	= v4l2_m2m_ioctl_remove_bufs,
 
 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
 	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
@@ -1689,8 +1690,6 @@ static const struct vb2_ops vicodec_qops = {
 	.buf_request_complete	= vicodec_buf_request_complete,
 	.start_streaming	= vicodec_start_streaming,
 	.stop_streaming		= vicodec_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static int queue_init(void *priv, struct vb2_queue *src_vq,
@@ -1708,12 +1707,14 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	src_vq->ops = &vicodec_qops;
 	src_vq->mem_ops = &vb2_vmalloc_memops;
 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
-	if (ctx->is_enc)
+	if (ctx->is_enc) {
 		src_vq->lock = &ctx->dev->stateful_enc.mutex;
-	else if (ctx->is_stateless)
+		src_vq->min_reqbufs_allocation = VICODEC_REC_BUFS;
+	} else if (ctx->is_stateless) {
 		src_vq->lock = &ctx->dev->stateless_dec.mutex;
-	else
+	} else {
 		src_vq->lock = &ctx->dev->stateful_dec.mutex;
+	}
 	src_vq->supports_requests = ctx->is_stateless;
 	src_vq->requires_requests = ctx->is_stateless;
 	ret = vb2_queue_init(src_vq);
@@ -1731,6 +1732,8 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	dst_vq->mem_ops = &vb2_vmalloc_memops;
 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
 	dst_vq->lock = src_vq->lock;
+	if (!ctx->is_stateless && !ctx->is_enc)
+		dst_vq->min_reqbufs_allocation = VICODEC_REC_BUFS;
 
 	return vb2_queue_init(dst_vq);
 }
@@ -1855,11 +1858,16 @@ static int vicodec_open(struct file *file)
 			  1, 31, 1, 20);
 	v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_FWHT_P_FRAME_QP,
 			  1, 31, 1, 20);
-	if (ctx->is_enc)
-		v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops,
-				  V4L2_CID_MIN_BUFFERS_FOR_OUTPUT, 1, 1, 1, 1);
+
 	if (ctx->is_stateless)
 		v4l2_ctrl_new_custom(hdl, &vicodec_ctrl_stateless_state, NULL);
+	else
+		v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, ctx->is_enc ?
+				  V4L2_CID_MIN_BUFFERS_FOR_OUTPUT :
+				  V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
+				  VICODEC_REC_BUFS, VICODEC_REC_BUFS, 1,
+				  VICODEC_REC_BUFS);
+
 	if (hdl->error) {
 		rc = hdl->error;
 		v4l2_ctrl_handler_free(hdl);
@@ -2208,7 +2216,7 @@ static void vicodec_remove(struct platform_device *pdev)
 
 static struct platform_driver vicodec_pdrv = {
 	.probe		= vicodec_probe,
-	.remove_new	= vicodec_remove,
+	.remove		= vicodec_remove,
 	.driver		= {
 		.name	= VICODEC_NAME,
 	},
diff --git a/drivers/media/test-drivers/vidtv/vidtv_bridge.c b/drivers/media/test-drivers/vidtv/vidtv_bridge.c
index 613949df897d..438483c62fac 100644
--- a/drivers/media/test-drivers/vidtv/vidtv_bridge.c
+++ b/drivers/media/test-drivers/vidtv/vidtv_bridge.c
@@ -191,10 +191,11 @@ static int vidtv_start_streaming(struct vidtv_dvb *dvb)
 
 	mux_args.mux_buf_sz  = mux_buf_sz;
 
-	dvb->streaming = true;
 	dvb->mux = vidtv_mux_init(dvb->fe[0], dev, &mux_args);
 	if (!dvb->mux)
 		return -ENOMEM;
+
+	dvb->streaming = true;
 	vidtv_mux_start_thread(dvb->mux);
 
 	dev_dbg_ratelimited(dev, "Started streaming\n");
@@ -205,6 +206,11 @@ static int vidtv_stop_streaming(struct vidtv_dvb *dvb)
 {
 	struct device *dev = &dvb->pdev->dev;
 
+	if (!dvb->streaming) {
+		dev_warn_ratelimited(dev, "No streaming. Skipping.\n");
+		return 0;
+	}
+
 	dvb->streaming = false;
 	vidtv_mux_stop_thread(dvb->mux);
 	vidtv_mux_destroy(dvb->mux);
@@ -572,7 +578,7 @@ static struct platform_driver vidtv_bridge_driver = {
 		.name = VIDTV_PDEV_NAME,
 	},
 	.probe    = vidtv_bridge_probe,
-	.remove_new = vidtv_bridge_remove,
+	.remove = vidtv_bridge_remove,
 };
 
 static void __exit vidtv_bridge_exit(void)
diff --git a/drivers/media/test-drivers/vidtv/vidtv_channel.c b/drivers/media/test-drivers/vidtv/vidtv_channel.c
index 7838e6272712..f3023e91b3eb 100644
--- a/drivers/media/test-drivers/vidtv/vidtv_channel.c
+++ b/drivers/media/test-drivers/vidtv/vidtv_channel.c
@@ -497,7 +497,7 @@ free_sdt:
 	vidtv_psi_sdt_table_destroy(m->si.sdt);
 free_pat:
 	vidtv_psi_pat_table_destroy(m->si.pat);
-	return 0;
+	return -EINVAL;
 }
 
 void vidtv_channel_si_destroy(struct vidtv_mux *m)
diff --git a/drivers/media/test-drivers/vidtv/vidtv_demod.c b/drivers/media/test-drivers/vidtv/vidtv_demod.c
index 7a0cd9601917..505f96fccbf3 100644
--- a/drivers/media/test-drivers/vidtv/vidtv_demod.c
+++ b/drivers/media/test-drivers/vidtv/vidtv_demod.c
@@ -407,7 +407,7 @@ static const struct dvb_frontend_ops vidtv_demod_ops = {
 };
 
 static const struct i2c_device_id vidtv_demod_i2c_id_table[] = {
-	{"dvb_vidtv_demod", 0},
+	{ "dvb_vidtv_demod" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, vidtv_demod_i2c_id_table);
diff --git a/drivers/media/test-drivers/vidtv/vidtv_tuner.c b/drivers/media/test-drivers/vidtv/vidtv_tuner.c
index a748737d47f3..4ba302d569d6 100644
--- a/drivers/media/test-drivers/vidtv/vidtv_tuner.c
+++ b/drivers/media/test-drivers/vidtv/vidtv_tuner.c
@@ -385,7 +385,7 @@ static const struct dvb_tuner_ops vidtv_tuner_ops = {
 };
 
 static const struct i2c_device_id vidtv_tuner_i2c_id_table[] = {
-	{"dvb_vidtv_tuner", 0},
+	{ "dvb_vidtv_tuner" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, vidtv_tuner_i2c_id_table);
diff --git a/drivers/media/test-drivers/vim2m.c b/drivers/media/test-drivers/vim2m.c
index 3e3b424b4860..1d1a9e768505 100644
--- a/drivers/media/test-drivers/vim2m.c
+++ b/drivers/media/test-drivers/vim2m.c
@@ -26,6 +26,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-common.h>
 
 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
@@ -42,6 +43,10 @@ static unsigned int default_transtime = 40; /* Max 25 fps */
 module_param(default_transtime, uint, 0644);
 MODULE_PARM_DESC(default_transtime, "default transaction time in ms");
 
+static unsigned int multiplanar = 1;
+module_param(multiplanar, uint, 0644);
+MODULE_PARM_DESC(multiplanar, "1 (default) creates a single planar device, 2 creates multiplanar device.");
+
 #define MIN_W 32
 #define MIN_H 32
 #define MAX_W 640
@@ -134,7 +139,8 @@ static struct vim2m_fmt formats[] = {
 struct vim2m_q_data {
 	unsigned int		width;
 	unsigned int		height;
-	unsigned int		sizeimage;
+	unsigned int            num_mem_planes;
+	unsigned int		sizeimage[VIDEO_MAX_PLANES];
 	unsigned int		sequence;
 	struct vim2m_fmt	*fmt;
 };
@@ -193,6 +199,7 @@ struct vim2m_dev {
 	struct mutex		dev_mutex;
 
 	struct v4l2_m2m_dev	*m2m_dev;
+	bool			multiplanar;
 };
 
 struct vim2m_ctx {
@@ -237,8 +244,10 @@ static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
 {
 	switch (type) {
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 		return &ctx->q_data[V4L2_M2M_SRC];
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 		return &ctx->q_data[V4L2_M2M_DST];
 	default:
 		return NULL;
@@ -249,8 +258,10 @@ static const char *type_name(enum v4l2_buf_type type)
 {
 	switch (type) {
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
 		return "Output";
 	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
 		return "Capture";
 	default:
 		return "Invalid";
@@ -720,6 +731,7 @@ static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 {
 	struct vb2_queue *vq;
 	struct vim2m_q_data *q_data;
+	int ret;
 
 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (!vq)
@@ -729,12 +741,12 @@ static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 	if (!q_data)
 		return -EINVAL;
 
-	f->fmt.pix.width	= q_data->width;
-	f->fmt.pix.height	= q_data->height;
+	ret = v4l2_fill_pixfmt(&f->fmt.pix, q_data->fmt->fourcc,
+			       q_data->width, q_data->height);
+	if (ret)
+		return ret;
+
 	f->fmt.pix.field	= V4L2_FIELD_NONE;
-	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
-	f->fmt.pix.bytesperline	= (q_data->width * q_data->fmt->depth) >> 3;
-	f->fmt.pix.sizeimage	= q_data->sizeimage;
 	f->fmt.pix.colorspace	= ctx->colorspace;
 	f->fmt.pix.xfer_func	= ctx->xfer_func;
 	f->fmt.pix.ycbcr_enc	= ctx->ycbcr_enc;
@@ -743,43 +755,102 @@ static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 	return 0;
 }
 
+static int vidioc_g_fmt_mplane(struct vim2m_ctx *ctx, struct v4l2_format *f)
+{
+	struct vb2_queue *vq;
+	struct vim2m_q_data *q_data;
+	int ret;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	ret = v4l2_fill_pixfmt_mp(&f->fmt.pix_mp, q_data->fmt->fourcc,
+				  q_data->width, q_data->height);
+	if (ret)
+		return ret;
+
+	f->fmt.pix_mp.field	   = V4L2_FIELD_NONE;
+	f->fmt.pix_mp.colorspace   = ctx->colorspace;
+	f->fmt.pix_mp.xfer_func	   = ctx->xfer_func;
+	f->fmt.pix_mp.ycbcr_enc	   = ctx->ycbcr_enc;
+	f->fmt.pix_mp.quantization = ctx->quant;
+
+	return 0;
+}
+
 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
 	return vidioc_g_fmt(file2ctx(file), f);
 }
 
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+
 	return vidioc_g_fmt(file2ctx(file), f);
 }
 
-static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
+static int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv,
+				       struct v4l2_format *f)
 {
-	int walign, halign;
-	/*
-	 * V4L2 specification specifies the driver corrects the
-	 * format struct if any of the dimensions is unsupported
-	 */
-	if (f->fmt.pix.height < MIN_H)
-		f->fmt.pix.height = MIN_H;
-	else if (f->fmt.pix.height > MAX_H)
-		f->fmt.pix.height = MAX_H;
-
-	if (f->fmt.pix.width < MIN_W)
-		f->fmt.pix.width = MIN_W;
-	else if (f->fmt.pix.width > MAX_W)
-		f->fmt.pix.width = MAX_W;
-
-	get_alignment(f->fmt.pix.pixelformat, &walign, &halign);
-	f->fmt.pix.width &= ~(walign - 1);
-	f->fmt.pix.height &= ~(halign - 1);
-	f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
-	f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	return vidioc_g_fmt_mplane(file2ctx(file), f);
+}
+
+static int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv,
+				       struct v4l2_format *f)
+{
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	return vidioc_g_fmt_mplane(file2ctx(file), f);
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f, bool is_mplane)
+{
+	int walign, halign, ret;
+	int width = (is_mplane) ? f->fmt.pix_mp.width : f->fmt.pix.width;
+	int height = (is_mplane) ? f->fmt.pix_mp.height : f->fmt.pix.height;
+	u32 pixfmt = (is_mplane) ? f->fmt.pix_mp.pixelformat :
+		f->fmt.pix.pixelformat;
+
+	width = clamp(width, MIN_W, MAX_W);
+	height = clamp(height, MIN_H, MAX_H);
+
+	get_alignment(pixfmt, &walign, &halign);
+	width = ALIGN(width, walign);
+	height = ALIGN(height, halign);
+
 	f->fmt.pix.field = V4L2_FIELD_NONE;
 
-	return 0;
+	if (is_mplane) {
+		ret = v4l2_fill_pixfmt_mp(&f->fmt.pix_mp, pixfmt, width,
+					  height);
+	} else {
+		ret = v4l2_fill_pixfmt(&f->fmt.pix, pixfmt,  width, height);
+	}
+	return ret;
 }
 
 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
@@ -787,6 +858,10 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
 	struct vim2m_fmt *fmt;
 	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
 
 	fmt = find_format(f->fmt.pix.pixelformat);
 	if (!fmt) {
@@ -804,7 +879,36 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
 	f->fmt.pix.quantization = ctx->quant;
 
-	return vidioc_try_fmt(f, fmt);
+	return vidioc_try_fmt(f, false);
+}
+
+static int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	struct vim2m_fmt *fmt;
+	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	fmt = find_format(f->fmt.pix_mp.pixelformat);
+	if (!fmt) {
+		f->fmt.pix_mp.pixelformat = formats[0].fourcc;
+		fmt = find_format(f->fmt.pix_mp.pixelformat);
+	}
+	if (!(fmt->types & MEM2MEM_CAPTURE)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix.pixelformat);
+		return -EINVAL;
+	}
+	f->fmt.pix_mp.colorspace = ctx->colorspace;
+	f->fmt.pix_mp.xfer_func = ctx->xfer_func;
+	f->fmt.pix_mp.ycbcr_enc = ctx->ycbcr_enc;
+	f->fmt.pix_mp.quantization = ctx->quant;
+
+	return vidioc_try_fmt(f, true);
 }
 
 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
@@ -812,6 +916,10 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 {
 	struct vim2m_fmt *fmt;
 	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
 
 	fmt = find_format(f->fmt.pix.pixelformat);
 	if (!fmt) {
@@ -827,13 +935,45 @@ static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
 	if (!f->fmt.pix.colorspace)
 		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
 
-	return vidioc_try_fmt(f, fmt);
+	return vidioc_try_fmt(f, false);
+}
+
+static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+					 struct v4l2_format *f)
+{
+	struct vim2m_fmt *fmt;
+	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	fmt = find_format(f->fmt.pix_mp.pixelformat);
+	if (!fmt) {
+		f->fmt.pix_mp.pixelformat = formats[0].fourcc;
+		fmt = find_format(f->fmt.pix_mp.pixelformat);
+	}
+	if (!(fmt->types & MEM2MEM_OUTPUT)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "Fourcc format (0x%08x) invalid.\n",
+			 f->fmt.pix_mp.pixelformat);
+		return -EINVAL;
+	}
+	if (!f->fmt.pix_mp.colorspace)
+		f->fmt.pix_mp.colorspace = V4L2_COLORSPACE_REC709;
+
+	return vidioc_try_fmt(f, true);
 }
 
 static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 {
 	struct vim2m_q_data *q_data;
 	struct vb2_queue *vq;
+	unsigned int i;
+	bool is_mplane = ctx->dev->multiplanar;
+	u32 pixfmt = (is_mplane) ? f->fmt.pix_mp.pixelformat : f->fmt.pix.pixelformat;
+	u32 width = (is_mplane) ? f->fmt.pix_mp.width : f->fmt.pix.width;
+	u32 height = (is_mplane) ? f->fmt.pix_mp.height : f->fmt.pix.height;
 
 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 	if (!vq)
@@ -848,11 +988,17 @@ static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
 		return -EBUSY;
 	}
 
-	q_data->fmt		= find_format(f->fmt.pix.pixelformat);
-	q_data->width		= f->fmt.pix.width;
-	q_data->height		= f->fmt.pix.height;
-	q_data->sizeimage	= q_data->width * q_data->height
-				* q_data->fmt->depth >> 3;
+	q_data->fmt		= find_format(pixfmt);
+	q_data->width		= width;
+	q_data->height		= height;
+	if (is_mplane) {
+		q_data->num_mem_planes = f->fmt.pix_mp.num_planes;
+		for (i = 0; i < f->fmt.pix_mp.num_planes; i++)
+			q_data->sizeimage[i] = f->fmt.pix_mp.plane_fmt[i].sizeimage;
+	} else {
+		q_data->sizeimage[0] = f->fmt.pix.sizeimage;
+		q_data->num_mem_planes = 1;
+	}
 
 	dprintk(ctx->dev, 1,
 		"Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",
@@ -870,6 +1016,10 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	int ret;
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
 
 	ret = vidioc_try_fmt_vid_cap(file, priv, f);
 	if (ret)
@@ -878,12 +1028,32 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	return vidioc_s_fmt(file2ctx(file), f);
 }
 
+static int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+				       struct v4l2_format *f)
+{
+	int ret;
+	struct vim2m_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	ret = vidioc_try_fmt_vid_cap_mplane(file, priv, f);
+	if (ret)
+		return ret;
+
+	return vidioc_s_fmt(file2ctx(file), f);
+}
+
 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
 	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
 	int ret;
 
+	if (dev->multiplanar)
+		return -ENOTTY;
+
 	ret = vidioc_try_fmt_vid_out(file, priv, f);
 	if (ret)
 		return ret;
@@ -898,6 +1068,30 @@ static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
 	return ret;
 }
 
+static int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv,
+				       struct v4l2_format *f)
+{
+	struct vim2m_ctx *ctx = file2ctx(file);
+	struct vim2m_dev *dev = video_drvdata(file);
+	int ret;
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+
+	ret = vidioc_try_fmt_vid_out_mplane(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = vidioc_s_fmt(file2ctx(file), f);
+	if (!ret) {
+		ctx->colorspace = f->fmt.pix_mp.colorspace;
+		ctx->xfer_func = f->fmt.pix_mp.xfer_func;
+		ctx->ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
+		ctx->quant = f->fmt.pix_mp.quantization;
+	}
+	return ret;
+}
+
 static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct vim2m_ctx *ctx =
@@ -948,11 +1142,17 @@ static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
 	.vidioc_g_fmt_vid_cap	= vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap	= vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap	= vidioc_s_fmt_vid_cap,
+	.vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= vidioc_s_fmt_vid_cap_mplane,
 
 	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 	.vidioc_g_fmt_vid_out	= vidioc_g_fmt_vid_out,
 	.vidioc_try_fmt_vid_out	= vidioc_try_fmt_vid_out,
 	.vidioc_s_fmt_vid_out	= vidioc_s_fmt_vid_out,
+	.vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt_vid_out_mplane,
+	.vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt_vid_out_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= vidioc_s_fmt_vid_out_mplane,
 
 	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
 	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
@@ -981,23 +1181,32 @@ static int vim2m_queue_setup(struct vb2_queue *vq,
 {
 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
 	struct vim2m_q_data *q_data;
-	unsigned int size, count = *nbuffers;
+	unsigned int size, p, count = *nbuffers;
 
 	q_data = get_q_data(ctx, vq->type);
 	if (!q_data)
 		return -EINVAL;
 
-	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
+	size = 0;
+	for (p = 0; p < q_data->num_mem_planes; p++)
+		size += q_data->sizeimage[p];
 
 	while (size * count > MEM2MEM_VID_MEM_LIMIT)
 		(count)--;
 	*nbuffers = count;
 
-	if (*nplanes)
-		return sizes[0] < size ? -EINVAL : 0;
-
-	*nplanes = 1;
-	sizes[0] = size;
+	if (*nplanes) {
+		if (*nplanes != q_data->num_mem_planes)
+			return -EINVAL;
+		for (p = 0; p < q_data->num_mem_planes; p++) {
+			if (sizes[p] < q_data->sizeimage[p])
+				return -EINVAL;
+		}
+	} else {
+		*nplanes = q_data->num_mem_planes;
+		for (p = 0; p < q_data->num_mem_planes; p++)
+			sizes[p] = q_data->sizeimage[p];
+	}
 
 	dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",
 		type_name(vq->type), count, size);
@@ -1024,21 +1233,24 @@ static int vim2m_buf_prepare(struct vb2_buffer *vb)
 {
 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 	struct vim2m_q_data *q_data;
+	unsigned int p;
 
 	dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));
 
 	q_data = get_q_data(ctx, vb->vb2_queue->type);
 	if (!q_data)
 		return -EINVAL;
-	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
-		dprintk(ctx->dev, 1,
-			"%s data will not fit into plane (%lu < %lu)\n",
-			__func__, vb2_plane_size(vb, 0),
-			(long)q_data->sizeimage);
-		return -EINVAL;
-	}
 
-	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
+	for (p = 0; p < q_data->num_mem_planes; p++) {
+		if (vb2_plane_size(vb, p) < q_data->sizeimage[p]) {
+			dprintk(ctx->dev, 1,
+				"%s data will not fit into plane (%lu < %lu)\n",
+				__func__, vb2_plane_size(vb, p),
+				(long)q_data->sizeimage[p]);
+			return -EINVAL;
+		}
+		vb2_set_plane_payload(vb, p, q_data->sizeimage[p]);
+	}
 
 	return 0;
 }
@@ -1100,8 +1312,6 @@ static const struct vb2_ops vim2m_qops = {
 	.buf_queue	 = vim2m_buf_queue,
 	.start_streaming = vim2m_start_streaming,
 	.stop_streaming  = vim2m_stop_streaming,
-	.wait_prepare	 = vb2_ops_wait_prepare,
-	.wait_finish	 = vb2_ops_wait_finish,
 	.buf_request_complete = vim2m_buf_request_complete,
 };
 
@@ -1111,7 +1321,8 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	struct vim2m_ctx *ctx = priv;
 	int ret;
 
-	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->type = (ctx->dev->multiplanar) ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+		V4L2_BUF_TYPE_VIDEO_OUTPUT;
 	src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	src_vq->drv_priv = ctx;
 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
@@ -1125,7 +1336,8 @@ static int queue_init(void *priv, struct vb2_queue *src_vq,
 	if (ret)
 		return ret;
 
-	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->type = (ctx->dev->multiplanar) ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+		V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	dst_vq->drv_priv = ctx;
 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
@@ -1199,10 +1411,11 @@ static int vim2m_open(struct file *file)
 	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
 	ctx->q_data[V4L2_M2M_SRC].width = 640;
 	ctx->q_data[V4L2_M2M_SRC].height = 480;
-	ctx->q_data[V4L2_M2M_SRC].sizeimage =
+	ctx->q_data[V4L2_M2M_SRC].sizeimage[0] =
 		ctx->q_data[V4L2_M2M_SRC].width *
 		ctx->q_data[V4L2_M2M_SRC].height *
 		(ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
+	ctx->q_data[V4L2_M2M_SRC].num_mem_planes = 1;
 	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
 	ctx->colorspace = V4L2_COLORSPACE_REC709;
 
@@ -1279,7 +1492,7 @@ static const struct video_device vim2m_videodev = {
 	.ioctl_ops	= &vim2m_ioctl_ops,
 	.minor		= -1,
 	.release	= vim2m_device_release,
-	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
+	.device_caps	= V4L2_CAP_STREAMING,
 };
 
 static const struct v4l2_m2m_ops m2m_ops = {
@@ -1310,15 +1523,16 @@ static int vim2m_probe(struct platform_device *pdev)
 	atomic_set(&dev->num_inst, 0);
 	mutex_init(&dev->dev_mutex);
 
+	dev->multiplanar = (multiplanar == 2);
+
 	dev->vfd = vim2m_videodev;
 	vfd = &dev->vfd;
 	vfd->lock = &dev->dev_mutex;
 	vfd->v4l2_dev = &dev->v4l2_dev;
+	vfd->device_caps |= (dev->multiplanar) ? V4L2_CAP_VIDEO_M2M_MPLANE :
+		V4L2_CAP_VIDEO_M2M;
 
 	video_set_drvdata(vfd, dev);
-	v4l2_info(&dev->v4l2_dev,
-		  "Device registered as /dev/video%d\n", vfd->num);
-
 	platform_set_drvdata(pdev, dev);
 
 	dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
@@ -1345,6 +1559,9 @@ static int vim2m_probe(struct platform_device *pdev)
 		goto error_m2m;
 	}
 
+	v4l2_info(&dev->v4l2_dev,
+		  "Device registered as /dev/video%d\n", vfd->num);
+
 #ifdef CONFIG_MEDIA_CONTROLLER
 	ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
 						 MEDIA_ENT_F_PROC_VIDEO_SCALER);
@@ -1394,7 +1611,7 @@ static void vim2m_remove(struct platform_device *pdev)
 
 static struct platform_driver vim2m_pdrv = {
 	.probe		= vim2m_probe,
-	.remove_new	= vim2m_remove,
+	.remove		= vim2m_remove,
 	.driver		= {
 		.name	= MEM2MEM_NAME,
 	},
diff --git a/drivers/media/test-drivers/vimc/vimc-capture.c b/drivers/media/test-drivers/vimc/vimc-capture.c
index 2a2d19d23bab..10df039278e7 100644
--- a/drivers/media/test-drivers/vimc/vimc-capture.c
+++ b/drivers/media/test-drivers/vimc/vimc-capture.c
@@ -221,6 +221,7 @@ static const struct v4l2_ioctl_ops vimc_capture_ioctl_ops = {
 	.vidioc_expbuf = vb2_ioctl_expbuf,
 	.vidioc_streamon = vb2_ioctl_streamon,
 	.vidioc_streamoff = vb2_ioctl_streamoff,
+	.vidioc_remove_bufs = vb2_ioctl_remove_bufs,
 };
 
 static void vimc_capture_return_all_buffers(struct vimc_capture_device *vcapture,
@@ -325,12 +326,6 @@ static const struct vb2_ops vimc_capture_qops = {
 	.buf_queue		= vimc_capture_buf_queue,
 	.queue_setup		= vimc_capture_queue_setup,
 	.buf_prepare		= vimc_capture_buffer_prepare,
-	/*
-	 * Since q->lock is set we can use the standard
-	 * vb2_ops_wait_prepare/finish helper functions.
-	 */
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static const struct media_entity_operations vimc_capture_mops = {
@@ -432,7 +427,7 @@ static struct vimc_ent_device *vimc_capture_add(struct vimc_device *vimc,
 	q->mem_ops = vimc_allocator == VIMC_ALLOCATOR_DMA_CONTIG
 		   ? &vb2_dma_contig_memops : &vb2_vmalloc_memops;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	q->min_queued_buffers = 2;
+	q->min_reqbufs_allocation = 2;
 	q->lock = &vcapture->lock;
 	q->dev = v4l2_dev->dev;
 
@@ -493,7 +488,7 @@ err_free_vcapture:
 	return ERR_PTR(ret);
 }
 
-struct vimc_ent_type vimc_capture_type = {
+const struct vimc_ent_type vimc_capture_type = {
 	.add = vimc_capture_add,
 	.unregister = vimc_capture_unregister,
 	.release = vimc_capture_release
diff --git a/drivers/media/test-drivers/vimc/vimc-common.c b/drivers/media/test-drivers/vimc/vimc-common.c
index 2e72974e35b4..4f4fcb26e236 100644
--- a/drivers/media/test-drivers/vimc/vimc-common.c
+++ b/drivers/media/test-drivers/vimc/vimc-common.c
@@ -8,6 +8,8 @@
 #include <linux/init.h>
 #include <linux/module.h>
 
+#include <media/v4l2-ctrls.h>
+
 #include "vimc-common.h"
 
 /*
@@ -358,6 +360,7 @@ int vimc_ent_sd_register(struct vimc_ent_device *ved,
 			 u32 function,
 			 u16 num_pads,
 			 struct media_pad *pads,
+			 const struct v4l2_subdev_internal_ops *int_ops,
 			 const struct v4l2_subdev_ops *sd_ops)
 {
 	int ret;
@@ -367,6 +370,7 @@ int vimc_ent_sd_register(struct vimc_ent_device *ved,
 
 	/* Initialize the subdev */
 	v4l2_subdev_init(sd, sd_ops);
+	sd->internal_ops = int_ops;
 	sd->entity.function = function;
 	sd->entity.ops = &vimc_ent_sd_mops;
 	sd->owner = THIS_MODULE;
@@ -383,17 +387,36 @@ int vimc_ent_sd_register(struct vimc_ent_device *ved,
 	if (ret)
 		return ret;
 
+	/*
+	 * Finalize the subdev initialization if it supports active states. Use
+	 * the control handler lock as the state lock if available.
+	 */
+	if (int_ops && int_ops->init_state) {
+		if (sd->ctrl_handler)
+			sd->state_lock = sd->ctrl_handler->lock;
+
+		ret = v4l2_subdev_init_finalize(sd);
+		if (ret) {
+			dev_err(v4l2_dev->dev,
+				"%s: subdev initialization failed (err=%d)\n",
+				name, ret);
+			goto err_clean_m_ent;
+		}
+	}
+
 	/* Register the subdev with the v4l2 and the media framework */
 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
 	if (ret) {
 		dev_err(v4l2_dev->dev,
 			"%s: subdev register failed (err=%d)\n",
 			name, ret);
-		goto err_clean_m_ent;
+		goto err_clean_sd;
 	}
 
 	return 0;
 
+err_clean_sd:
+	v4l2_subdev_cleanup(sd);
 err_clean_m_ent:
 	media_entity_cleanup(&sd->entity);
 	return ret;
diff --git a/drivers/media/test-drivers/vimc/vimc-common.h b/drivers/media/test-drivers/vimc/vimc-common.h
index 7641a101a728..7a45a2117748 100644
--- a/drivers/media/test-drivers/vimc/vimc-common.h
+++ b/drivers/media/test-drivers/vimc/vimc-common.h
@@ -156,7 +156,7 @@ struct vimc_ent_type {
  */
 struct vimc_ent_config {
 	const char *name;
-	struct vimc_ent_type *type;
+	const struct vimc_ent_type *type;
 };
 
 /**
@@ -167,11 +167,11 @@ struct vimc_ent_config {
  */
 bool vimc_is_source(struct media_entity *ent);
 
-extern struct vimc_ent_type vimc_sensor_type;
-extern struct vimc_ent_type vimc_debayer_type;
-extern struct vimc_ent_type vimc_scaler_type;
-extern struct vimc_ent_type vimc_capture_type;
-extern struct vimc_ent_type vimc_lens_type;
+extern const struct vimc_ent_type vimc_sensor_type;
+extern const struct vimc_ent_type vimc_debayer_type;
+extern const struct vimc_ent_type vimc_scaler_type;
+extern const struct vimc_ent_type vimc_capture_type;
+extern const struct vimc_ent_type vimc_lens_type;
 
 /**
  * vimc_pix_map_by_index - get vimc_pix_map struct by its index
@@ -215,6 +215,7 @@ const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
  * @num_pads:	number of pads to initialize
  * @pads:	the array of pads of the entity, the caller should set the
  *		flags of the pads
+ * @int_ops:	pointer to &struct v4l2_subdev_internal_ops.
  * @sd_ops:	pointer to &struct v4l2_subdev_ops.
  *
  * Helper function initialize and register the struct vimc_ent_device and struct
@@ -227,6 +228,7 @@ int vimc_ent_sd_register(struct vimc_ent_device *ved,
 			 u32 function,
 			 u16 num_pads,
 			 struct media_pad *pads,
+			 const struct v4l2_subdev_internal_ops *int_ops,
 			 const struct v4l2_subdev_ops *sd_ops);
 
 /**
diff --git a/drivers/media/test-drivers/vimc/vimc-core.c b/drivers/media/test-drivers/vimc/vimc-core.c
index af127476e920..c812fa9f0650 100644
--- a/drivers/media/test-drivers/vimc/vimc-core.c
+++ b/drivers/media/test-drivers/vimc/vimc-core.c
@@ -81,7 +81,7 @@ struct vimc_pipeline_config {
  * Topology Configuration
  */
 
-static struct vimc_ent_config ent_config[] = {
+static const struct vimc_ent_config ent_config[] = {
 	[SENSOR_A] = {
 		.name = "Sensor A",
 		.type = &vimc_sensor_type
@@ -410,7 +410,7 @@ static struct platform_device vimc_pdev = {
 
 static struct platform_driver vimc_pdrv = {
 	.probe		= vimc_probe,
-	.remove_new	= vimc_remove,
+	.remove		= vimc_remove,
 	.driver		= {
 		.name	= VIMC_PDEV_NAME,
 	},
diff --git a/drivers/media/test-drivers/vimc/vimc-debayer.c b/drivers/media/test-drivers/vimc/vimc-debayer.c
index d72ed086e00b..bbb7c7a86df0 100644
--- a/drivers/media/test-drivers/vimc/vimc-debayer.c
+++ b/drivers/media/test-drivers/vimc/vimc-debayer.c
@@ -15,6 +15,9 @@
 
 #include "vimc-common.h"
 
+/* TODO: Add support for more output formats, we only support RGB888 for now. */
+#define VIMC_DEBAYER_SOURCE_MBUS_FMT	MEDIA_BUS_FMT_RGB888_1X24
+
 enum vimc_debayer_rgb_colors {
 	VIMC_DEBAYER_RED = 0,
 	VIMC_DEBAYER_GREEN = 1,
@@ -29,19 +32,26 @@ struct vimc_debayer_pix_map {
 struct vimc_debayer_device {
 	struct vimc_ent_device ved;
 	struct v4l2_subdev sd;
-	/* The active format */
-	struct v4l2_mbus_framefmt sink_fmt;
-	u32 src_code;
+	struct v4l2_ctrl_handler hdl;
+	struct media_pad pads[2];
+
+	u8 *src_frame;
+
 	void (*set_rgb_src)(struct vimc_debayer_device *vdebayer,
 			    unsigned int lin, unsigned int col,
 			    unsigned int rgb[3]);
-	/* Values calculated when the stream starts */
-	u8 *src_frame;
-	const struct vimc_debayer_pix_map *sink_pix_map;
-	unsigned int sink_bpp;
-	unsigned int mean_win_size;
-	struct v4l2_ctrl_handler hdl;
-	struct media_pad pads[2];
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		const struct vimc_debayer_pix_map *sink_pix_map;
+		unsigned int sink_bpp;
+		struct v4l2_area size;
+		unsigned int mean_win_size;
+		u32 src_code;
+	} hw;
 };
 
 static const struct v4l2_mbus_framefmt sink_fmt_default = {
@@ -153,18 +163,14 @@ static bool vimc_debayer_src_code_is_valid(u32 code)
 static int vimc_debayer_init_state(struct v4l2_subdev *sd,
 				   struct v4l2_subdev_state *sd_state)
 {
-	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf;
-	unsigned int i;
 
 	mf = v4l2_subdev_state_get_format(sd_state, 0);
 	*mf = sink_fmt_default;
 
-	for (i = 1; i < sd->entity.num_pads; i++) {
-		mf = v4l2_subdev_state_get_format(sd_state, i);
-		*mf = sink_fmt_default;
-		mf->code = vdebayer->src_code;
-	}
+	mf = v4l2_subdev_state_get_format(sd_state, 1);
+	*mf = sink_fmt_default;
+	mf->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;
 
 	return 0;
 }
@@ -213,24 +219,6 @@ static int vimc_debayer_enum_frame_size(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int vimc_debayer_get_fmt(struct v4l2_subdev *sd,
-				struct v4l2_subdev_state *sd_state,
-				struct v4l2_subdev_format *fmt)
-{
-	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
-
-	/* Get the current sink format */
-	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
-		      *v4l2_subdev_state_get_format(sd_state, 0) :
-		      vdebayer->sink_fmt;
-
-	/* Set the right code for the source pad */
-	if (VIMC_IS_SRC(fmt->pad))
-		fmt->format.code = vdebayer->src_code;
-
-	return 0;
-}
-
 static void vimc_debayer_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
 {
 	const struct vimc_debayer_pix_map *vpix;
@@ -256,52 +244,42 @@ static int vimc_debayer_set_fmt(struct v4l2_subdev *sd,
 				struct v4l2_subdev_format *fmt)
 {
 	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
-	struct v4l2_mbus_framefmt *sink_fmt;
-	u32 *src_code;
+	struct v4l2_mbus_framefmt *format;
 
-	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		/* Do not change the format while stream is on */
-		if (vdebayer->src_frame)
-			return -EBUSY;
-
-		sink_fmt = &vdebayer->sink_fmt;
-		src_code = &vdebayer->src_code;
-	} else {
-		sink_fmt = v4l2_subdev_state_get_format(sd_state, 0);
-		src_code = &v4l2_subdev_state_get_format(sd_state, 1)->code;
-	}
+	/* Do not change the format while stream is on. */
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vdebayer->src_frame)
+		return -EBUSY;
 
 	/*
-	 * Do not change the format of the source pad,
-	 * it is propagated from the sink
+	 * Do not change the format of the source pad, it is propagated from
+	 * the sink.
 	 */
-	if (VIMC_IS_SRC(fmt->pad)) {
-		u32 code = fmt->format.code;
+	if (VIMC_IS_SRC(fmt->pad))
+		return v4l2_subdev_get_fmt(sd, sd_state, fmt);
 
-		fmt->format = *sink_fmt;
+	/* Set the new format in the sink pad. */
+	vimc_debayer_adjust_sink_fmt(&fmt->format);
 
-		if (vimc_debayer_src_code_is_valid(code))
-			*src_code = code;
+	format = v4l2_subdev_state_get_format(sd_state, 0);
 
-		fmt->format.code = *src_code;
-	} else {
-		/* Set the new format in the sink pad */
-		vimc_debayer_adjust_sink_fmt(&fmt->format);
-
-		dev_dbg(vdebayer->ved.dev, "%s: sink format update: "
-			"old:%dx%d (0x%x, %d, %d, %d, %d) "
-			"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdebayer->sd.name,
-			/* old */
-			sink_fmt->width, sink_fmt->height, sink_fmt->code,
-			sink_fmt->colorspace, sink_fmt->quantization,
-			sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
-			/* new */
-			fmt->format.width, fmt->format.height, fmt->format.code,
-			fmt->format.colorspace,	fmt->format.quantization,
-			fmt->format.xfer_func, fmt->format.ycbcr_enc);
-
-		*sink_fmt = fmt->format;
-	}
+	dev_dbg(vdebayer->ved.dev, "%s: sink format update: "
+		"old:%dx%d (0x%x, %d, %d, %d, %d) "
+		"new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdebayer->sd.name,
+		/* old */
+		format->width, format->height, format->code,
+		format->colorspace, format->quantization,
+		format->xfer_func, format->ycbcr_enc,
+		/* new */
+		fmt->format.width, fmt->format.height, fmt->format.code,
+		fmt->format.colorspace,	fmt->format.quantization,
+		fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+	*format = fmt->format;
+
+	/* Propagate the format to the source pad. */
+	format = v4l2_subdev_state_get_format(sd_state, 1);
+	*format = fmt->format;
+	format->code = VIMC_DEBAYER_SOURCE_MBUS_FMT;
 
 	return 0;
 }
@@ -309,7 +287,7 @@ static int vimc_debayer_set_fmt(struct v4l2_subdev *sd,
 static const struct v4l2_subdev_pad_ops vimc_debayer_pad_ops = {
 	.enum_mbus_code		= vimc_debayer_enum_mbus_code,
 	.enum_frame_size	= vimc_debayer_enum_frame_size,
-	.get_fmt		= vimc_debayer_get_fmt,
+	.get_fmt		= v4l2_subdev_get_fmt,
 	.set_fmt		= vimc_debayer_set_fmt,
 };
 
@@ -321,8 +299,8 @@ static void vimc_debayer_process_rgb_frame(struct vimc_debayer_device *vdebayer,
 	const struct vimc_pix_map *vpix;
 	unsigned int i, index;
 
-	vpix = vimc_pix_map_by_code(vdebayer->src_code);
-	index = VIMC_FRAME_INDEX(lin, col, vdebayer->sink_fmt.width, 3);
+	vpix = vimc_pix_map_by_code(vdebayer->hw.src_code);
+	index = VIMC_FRAME_INDEX(lin, col, vdebayer->hw.size.width, 3);
 	for (i = 0; i < 3; i++) {
 		switch (vpix->pixelformat) {
 		case V4L2_PIX_FMT_RGB24:
@@ -340,24 +318,37 @@ static int vimc_debayer_s_stream(struct v4l2_subdev *sd, int enable)
 	struct vimc_debayer_device *vdebayer = v4l2_get_subdevdata(sd);
 
 	if (enable) {
+		const struct v4l2_mbus_framefmt *sink_fmt;
+		const struct v4l2_mbus_framefmt *src_fmt;
+		struct v4l2_subdev_state *state;
 		const struct vimc_pix_map *vpix;
 		unsigned int frame_size;
 
 		if (vdebayer->src_frame)
 			return 0;
 
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+		sink_fmt = v4l2_subdev_state_get_format(state, 0);
+		src_fmt = v4l2_subdev_state_get_format(state, 1);
+
 		/* Calculate the frame size of the source pad */
-		vpix = vimc_pix_map_by_code(vdebayer->src_code);
-		frame_size = vdebayer->sink_fmt.width * vdebayer->sink_fmt.height *
-				vpix->bpp;
+		vpix = vimc_pix_map_by_code(src_fmt->code);
+		frame_size = src_fmt->width * src_fmt->height * vpix->bpp;
 
 		/* Save the bytes per pixel of the sink */
-		vpix = vimc_pix_map_by_code(vdebayer->sink_fmt.code);
-		vdebayer->sink_bpp = vpix->bpp;
+		vpix = vimc_pix_map_by_code(sink_fmt->code);
+		vdebayer->hw.sink_bpp = vpix->bpp;
 
 		/* Get the corresponding pixel map from the table */
-		vdebayer->sink_pix_map =
-			vimc_debayer_pix_map_by_code(vdebayer->sink_fmt.code);
+		vdebayer->hw.sink_pix_map =
+			vimc_debayer_pix_map_by_code(sink_fmt->code);
+
+		vdebayer->hw.size.width = sink_fmt->width;
+		vdebayer->hw.size.height = sink_fmt->height;
+
+		vdebayer->hw.src_code = src_fmt->code;
+
+		v4l2_subdev_unlock_state(state);
 
 		/*
 		 * Allocate the frame buffer. Use vmalloc to be able to
@@ -366,7 +357,6 @@ static int vimc_debayer_s_stream(struct v4l2_subdev *sd, int enable)
 		vdebayer->src_frame = vmalloc(frame_size);
 		if (!vdebayer->src_frame)
 			return -ENOMEM;
-
 	} else {
 		if (!vdebayer->src_frame)
 			return 0;
@@ -427,13 +417,13 @@ static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,
 	 * the top left corner of the mean window (considering the current
 	 * pixel as the center)
 	 */
-	seek = vdebayer->mean_win_size / 2;
+	seek = vdebayer->hw.mean_win_size / 2;
 
 	/* Sum the values of the colors in the mean window */
 
 	dev_dbg(vdebayer->ved.dev,
 		"deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
-		vdebayer->sd.name, lin, col, vdebayer->sink_fmt.height, seek);
+		vdebayer->sd.name, lin, col, vdebayer->hw.size.height, seek);
 
 	/*
 	 * Iterate through all the lines in the mean window, start
@@ -442,7 +432,7 @@ static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,
 	 * frame
 	 */
 	for (wlin = seek > lin ? 0 : lin - seek;
-	     wlin < lin + seek + 1 && wlin < vdebayer->sink_fmt.height;
+	     wlin < lin + seek + 1 && wlin < vdebayer->hw.size.height;
 	     wlin++) {
 
 		/*
@@ -452,17 +442,17 @@ static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,
 		 * frame
 		 */
 		for (wcol = seek > col ? 0 : col - seek;
-		     wcol < col + seek + 1 && wcol < vdebayer->sink_fmt.width;
+		     wcol < col + seek + 1 && wcol < vdebayer->hw.size.width;
 		     wcol++) {
 			enum vimc_debayer_rgb_colors color;
 			unsigned int index;
 
 			/* Check which color this pixel is */
-			color = vdebayer->sink_pix_map->order[wlin % 2][wcol % 2];
+			color = vdebayer->hw.sink_pix_map->order[wlin % 2][wcol % 2];
 
 			index = VIMC_FRAME_INDEX(wlin, wcol,
-						 vdebayer->sink_fmt.width,
-						 vdebayer->sink_bpp);
+						 vdebayer->hw.size.width,
+						 vdebayer->hw.sink_bpp);
 
 			dev_dbg(vdebayer->ved.dev,
 				"deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
@@ -471,7 +461,7 @@ static void vimc_debayer_calc_rgb_sink(struct vimc_debayer_device *vdebayer,
 			/* Get its value */
 			rgb[color] = rgb[color] +
 				vimc_debayer_get_val(&frame[index],
-						     vdebayer->sink_bpp);
+						     vdebayer->hw.sink_bpp);
 
 			/* Save how many values we already added */
 			n_rgb[color]++;
@@ -509,8 +499,8 @@ static void *vimc_debayer_process_frame(struct vimc_ent_device *ved,
 	if (!vdebayer->src_frame)
 		return ERR_PTR(-EINVAL);
 
-	for (i = 0; i < vdebayer->sink_fmt.height; i++)
-		for (j = 0; j < vdebayer->sink_fmt.width; j++) {
+	for (i = 0; i < vdebayer->hw.size.height; i++)
+		for (j = 0; j < vdebayer->hw.size.width; j++) {
 			vimc_debayer_calc_rgb_sink(vdebayer, sink_frame, i, j, rgb);
 			vdebayer->set_rgb_src(vdebayer, i, j, rgb);
 		}
@@ -525,7 +515,7 @@ static int vimc_debayer_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	switch (ctrl->id) {
 	case VIMC_CID_MEAN_WIN_SIZE:
-		vdebayer->mean_win_size = ctrl->val;
+		vdebayer->hw.mean_win_size = ctrl->val;
 		break;
 	default:
 		return -EINVAL;
@@ -543,6 +533,7 @@ static void vimc_debayer_release(struct vimc_ent_device *ved)
 		container_of(ved, struct vimc_debayer_device, ved);
 
 	v4l2_ctrl_handler_free(&vdebayer->hdl);
+	v4l2_subdev_cleanup(&vdebayer->sd);
 	media_entity_cleanup(vdebayer->ved.ent);
 	kfree(vdebayer);
 }
@@ -594,25 +585,15 @@ static struct vimc_ent_device *vimc_debayer_add(struct vimc_device *vimc,
 	ret = vimc_ent_sd_register(&vdebayer->ved, &vdebayer->sd, v4l2_dev,
 				   vcfg_name,
 				   MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV, 2,
-				   vdebayer->pads, &vimc_debayer_ops);
+				   vdebayer->pads, &vimc_debayer_internal_ops,
+				   &vimc_debayer_ops);
 	if (ret)
 		goto err_free_hdl;
 
-	vdebayer->sd.internal_ops = &vimc_debayer_internal_ops;
-
 	vdebayer->ved.process_frame = vimc_debayer_process_frame;
 	vdebayer->ved.dev = vimc->mdev.dev;
-	vdebayer->mean_win_size = vimc_debayer_ctrl_mean_win_size.def;
+	vdebayer->hw.mean_win_size = vimc_debayer_ctrl_mean_win_size.def;
 
-	/* Initialize the frame format */
-	vdebayer->sink_fmt = sink_fmt_default;
-	/*
-	 * TODO: Add support for more output formats, we only support
-	 * RGB888 for now
-	 * NOTE: the src format is always the same as the sink, except
-	 * for the code
-	 */
-	vdebayer->src_code = MEDIA_BUS_FMT_RGB888_1X24;
 	vdebayer->set_rgb_src = vimc_debayer_process_rgb_frame;
 
 	return &vdebayer->ved;
@@ -625,7 +606,7 @@ err_free_vdebayer:
 	return ERR_PTR(ret);
 }
 
-struct vimc_ent_type vimc_debayer_type = {
+const struct vimc_ent_type vimc_debayer_type = {
 	.add = vimc_debayer_add,
 	.release = vimc_debayer_release
 };
diff --git a/drivers/media/test-drivers/vimc/vimc-lens.c b/drivers/media/test-drivers/vimc/vimc-lens.c
index 3ce7f4b4d2cc..96399057a2b5 100644
--- a/drivers/media/test-drivers/vimc/vimc-lens.c
+++ b/drivers/media/test-drivers/vimc/vimc-lens.c
@@ -72,7 +72,7 @@ static struct vimc_ent_device *vimc_lens_add(struct vimc_device *vimc,
 
 	ret = vimc_ent_sd_register(&vlens->ved, &vlens->sd, v4l2_dev,
 				   vcfg_name, MEDIA_ENT_F_LENS, 0,
-				   NULL, &vimc_lens_ops);
+				   NULL, NULL, &vimc_lens_ops);
 	if (ret)
 		goto err_free_hdl;
 
@@ -92,11 +92,12 @@ static void vimc_lens_release(struct vimc_ent_device *ved)
 		container_of(ved, struct vimc_lens_device, ved);
 
 	v4l2_ctrl_handler_free(&vlens->hdl);
+	v4l2_subdev_cleanup(&vlens->sd);
 	media_entity_cleanup(vlens->ved.ent);
 	kfree(vlens);
 }
 
-struct vimc_ent_type vimc_lens_type = {
+const struct vimc_ent_type vimc_lens_type = {
 	.add = vimc_lens_add,
 	.release = vimc_lens_release
 };
diff --git a/drivers/media/test-drivers/vimc/vimc-scaler.c b/drivers/media/test-drivers/vimc/vimc-scaler.c
index afe13d6af321..47d0d63865a0 100644
--- a/drivers/media/test-drivers/vimc/vimc-scaler.c
+++ b/drivers/media/test-drivers/vimc/vimc-scaler.c
@@ -15,7 +15,7 @@
 #include "vimc-common.h"
 
 /* Pad identifier */
-enum vic_sca_pad {
+enum vimc_scaler_pad {
 	VIMC_SCALER_SINK = 0,
 	VIMC_SCALER_SRC = 1,
 };
@@ -26,13 +26,20 @@ enum vic_sca_pad {
 struct vimc_scaler_device {
 	struct vimc_ent_device ved;
 	struct v4l2_subdev sd;
-	struct v4l2_rect crop_rect;
-	/* Frame format for both sink and src pad */
-	struct v4l2_mbus_framefmt fmt[2];
-	/* Values calculated when the stream starts */
-	u8 *src_frame;
-	unsigned int bpp;
 	struct media_pad pads[2];
+
+	u8 *src_frame;
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		struct v4l2_mbus_framefmt sink_fmt;
+		struct v4l2_mbus_framefmt src_fmt;
+		struct v4l2_rect sink_crop;
+		unsigned int bpp;
+	} hw;
 };
 
 static const struct v4l2_mbus_framefmt fmt_default = {
@@ -132,39 +139,6 @@ static int vimc_scaler_enum_frame_size(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static struct v4l2_mbus_framefmt *
-vimc_scaler_pad_format(struct vimc_scaler_device *vscaler,
-		    struct v4l2_subdev_state *sd_state, u32 pad,
-		    enum v4l2_subdev_format_whence which)
-{
-	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return v4l2_subdev_state_get_format(sd_state, pad);
-	else
-		return &vscaler->fmt[pad];
-}
-
-static struct v4l2_rect *
-vimc_scaler_pad_crop(struct vimc_scaler_device *vscaler,
-		  struct v4l2_subdev_state *sd_state,
-		  enum v4l2_subdev_format_whence which)
-{
-	if (which == V4L2_SUBDEV_FORMAT_TRY)
-		return v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
-	else
-		return &vscaler->crop_rect;
-}
-
-static int vimc_scaler_get_fmt(struct v4l2_subdev *sd,
-			    struct v4l2_subdev_state *sd_state,
-			    struct v4l2_subdev_format *format)
-{
-	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
-
-	format->format = *vimc_scaler_pad_format(vscaler, sd_state, format->pad,
-					      format->which);
-	return 0;
-}
-
 static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
 			    struct v4l2_subdev_state *sd_state,
 			    struct v4l2_subdev_format *format)
@@ -176,7 +150,7 @@ static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
 	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
 		return -EBUSY;
 
-	fmt = vimc_scaler_pad_format(vscaler, sd_state, format->pad, format->which);
+	fmt = v4l2_subdev_state_get_format(sd_state, format->pad);
 
 	/*
 	 * The media bus code and colorspace can only be changed on the sink
@@ -214,14 +188,13 @@ static int vimc_scaler_set_fmt(struct v4l2_subdev *sd,
 		struct v4l2_mbus_framefmt *src_fmt;
 		struct v4l2_rect *crop;
 
-		crop = vimc_scaler_pad_crop(vscaler, sd_state, format->which);
+		crop = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
 		crop->width = fmt->width;
 		crop->height = fmt->height;
 		crop->top = 0;
 		crop->left = 0;
 
-		src_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SRC,
-					      format->which);
+		src_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SRC);
 		*src_fmt = *fmt;
 	}
 
@@ -234,7 +207,6 @@ static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_state *sd_state,
 				  struct v4l2_subdev_selection *sel)
 {
-	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *sink_fmt;
 
 	if (VIMC_IS_SRC(sel->pad))
@@ -242,11 +214,10 @@ static int vimc_scaler_get_selection(struct v4l2_subdev *sd,
 
 	switch (sel->target) {
 	case V4L2_SEL_TGT_CROP:
-		sel->r = *vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
+		sel->r = *v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
 		break;
 	case V4L2_SEL_TGT_CROP_BOUNDS:
-		sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
-					       sel->which);
+		sink_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SINK);
 		sel->r = vimc_scaler_get_crop_bound_sink(sink_fmt);
 		break;
 	default:
@@ -282,9 +253,8 @@ static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
 	if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE && vscaler->src_frame)
 		return -EBUSY;
 
-	crop_rect = vimc_scaler_pad_crop(vscaler, sd_state, sel->which);
-	sink_fmt = vimc_scaler_pad_format(vscaler, sd_state, VIMC_SCALER_SINK,
-				       sel->which);
+	crop_rect = v4l2_subdev_state_get_crop(sd_state, VIMC_SCALER_SINK);
+	sink_fmt = v4l2_subdev_state_get_format(sd_state, VIMC_SCALER_SINK);
 	vimc_scaler_adjust_sink_crop(&sel->r, sink_fmt);
 	*crop_rect = sel->r;
 
@@ -294,7 +264,7 @@ static int vimc_scaler_set_selection(struct v4l2_subdev *sd,
 static const struct v4l2_subdev_pad_ops vimc_scaler_pad_ops = {
 	.enum_mbus_code		= vimc_scaler_enum_mbus_code,
 	.enum_frame_size	= vimc_scaler_enum_frame_size,
-	.get_fmt		= vimc_scaler_get_fmt,
+	.get_fmt		= v4l2_subdev_get_fmt,
 	.set_fmt		= vimc_scaler_set_fmt,
 	.get_selection		= vimc_scaler_get_selection,
 	.set_selection		= vimc_scaler_set_selection,
@@ -305,27 +275,38 @@ static int vimc_scaler_s_stream(struct v4l2_subdev *sd, int enable)
 	struct vimc_scaler_device *vscaler = v4l2_get_subdevdata(sd);
 
 	if (enable) {
-		const struct vimc_pix_map *vpix;
+		struct v4l2_subdev_state *state;
+		const struct v4l2_mbus_framefmt *format;
+		const struct v4l2_rect *rect;
 		unsigned int frame_size;
 
 		if (vscaler->src_frame)
 			return 0;
 
-		/* Save the bytes per pixel of the sink */
-		vpix = vimc_pix_map_by_code(vscaler->fmt[VIMC_SCALER_SINK].code);
-		vscaler->bpp = vpix->bpp;
+		state = v4l2_subdev_lock_and_get_active_state(sd);
 
-		/* Calculate the frame size of the source pad */
-		frame_size = vscaler->fmt[VIMC_SCALER_SRC].width
-			   * vscaler->fmt[VIMC_SCALER_SRC].height * vscaler->bpp;
+		/* Save the bytes per pixel of the sink. */
+		format = v4l2_subdev_state_get_format(state, VIMC_SCALER_SINK);
+		vscaler->hw.sink_fmt = *format;
+		vscaler->hw.bpp = vimc_pix_map_by_code(format->code)->bpp;
 
-		/* Allocate the frame buffer. Use vmalloc to be able to
-		 * allocate a large amount of memory
+		/* Calculate the frame size of the source pad. */
+		format = v4l2_subdev_state_get_format(state, VIMC_SCALER_SRC);
+		vscaler->hw.src_fmt = *format;
+		frame_size = format->width * format->height * vscaler->hw.bpp;
+
+		rect = v4l2_subdev_state_get_crop(state, VIMC_SCALER_SINK);
+		vscaler->hw.sink_crop = *rect;
+
+		v4l2_subdev_unlock_state(state);
+
+		/*
+		 * Allocate the frame buffer. Use vmalloc to be able to allocate
+		 * a large amount of memory.
 		 */
 		vscaler->src_frame = vmalloc(frame_size);
 		if (!vscaler->src_frame)
 			return -ENOMEM;
-
 	} else {
 		if (!vscaler->src_frame)
 			return 0;
@@ -353,9 +334,9 @@ static const struct v4l2_subdev_internal_ops vimc_scaler_internal_ops = {
 static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vscaler,
 				    const u8 *const sink_frame)
 {
-	const struct v4l2_mbus_framefmt *src_fmt = &vscaler->fmt[VIMC_SCALER_SRC];
-	const struct v4l2_rect *r = &vscaler->crop_rect;
-	unsigned int snk_width = vscaler->fmt[VIMC_SCALER_SINK].width;
+	const struct v4l2_mbus_framefmt *sink_fmt = &vscaler->hw.sink_fmt;
+	const struct v4l2_mbus_framefmt *src_fmt = &vscaler->hw.src_fmt;
+	const struct v4l2_rect *r = &vscaler->hw.sink_crop;
 	unsigned int src_x, src_y;
 	u8 *walker = vscaler->src_frame;
 
@@ -364,16 +345,16 @@ static void vimc_scaler_fill_src_frame(const struct vimc_scaler_device *const vs
 		unsigned int snk_y, y_offset;
 
 		snk_y = (src_y * r->height) / src_fmt->height + r->top;
-		y_offset = snk_y * snk_width * vscaler->bpp;
+		y_offset = snk_y * sink_fmt->width * vscaler->hw.bpp;
 
 		for (src_x = 0; src_x < src_fmt->width; src_x++) {
 			unsigned int snk_x, x_offset, index;
 
 			snk_x = (src_x * r->width) / src_fmt->width + r->left;
-			x_offset = snk_x * vscaler->bpp;
+			x_offset = snk_x * vscaler->hw.bpp;
 			index = y_offset + x_offset;
-			memcpy(walker, &sink_frame[index], vscaler->bpp);
-			walker += vscaler->bpp;
+			memcpy(walker, &sink_frame[index], vscaler->hw.bpp);
+			walker += vscaler->hw.bpp;
 		}
 	}
 }
@@ -398,6 +379,7 @@ static void vimc_scaler_release(struct vimc_ent_device *ved)
 	struct vimc_scaler_device *vscaler =
 		container_of(ved, struct vimc_scaler_device, ved);
 
+	v4l2_subdev_cleanup(&vscaler->sd);
 	media_entity_cleanup(vscaler->ved.ent);
 	kfree(vscaler);
 }
@@ -421,28 +403,20 @@ static struct vimc_ent_device *vimc_scaler_add(struct vimc_device *vimc,
 	ret = vimc_ent_sd_register(&vscaler->ved, &vscaler->sd, v4l2_dev,
 				   vcfg_name,
 				   MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
-				   vscaler->pads, &vimc_scaler_ops);
+				   vscaler->pads, &vimc_scaler_internal_ops,
+				   &vimc_scaler_ops);
 	if (ret) {
 		kfree(vscaler);
 		return ERR_PTR(ret);
 	}
 
-	vscaler->sd.internal_ops = &vimc_scaler_internal_ops;
-
 	vscaler->ved.process_frame = vimc_scaler_process_frame;
 	vscaler->ved.dev = vimc->mdev.dev;
 
-	/* Initialize the frame format */
-	vscaler->fmt[VIMC_SCALER_SINK] = fmt_default;
-	vscaler->fmt[VIMC_SCALER_SRC] = fmt_default;
-
-	/* Initialize the crop selection */
-	vscaler->crop_rect = crop_rect_default;
-
 	return &vscaler->ved;
 }
 
-struct vimc_ent_type vimc_scaler_type = {
+const struct vimc_ent_type vimc_scaler_type = {
 	.add = vimc_scaler_add,
 	.release = vimc_scaler_release
 };
diff --git a/drivers/media/test-drivers/vimc/vimc-sensor.c b/drivers/media/test-drivers/vimc/vimc-sensor.c
index 5e34b1aed95e..027767777763 100644
--- a/drivers/media/test-drivers/vimc/vimc-sensor.c
+++ b/drivers/media/test-drivers/vimc/vimc-sensor.c
@@ -24,13 +24,20 @@ struct vimc_sensor_device {
 	struct vimc_ent_device ved;
 	struct v4l2_subdev sd;
 	struct tpg_data tpg;
-	u8 *frame;
-	enum vimc_sensor_osd_mode osd_value;
-	u64 start_stream_ts;
-	/* The active format */
-	struct v4l2_mbus_framefmt mbus_format;
 	struct v4l2_ctrl_handler hdl;
 	struct media_pad pad;
+
+	u8 *frame;
+
+	/*
+	 * Virtual "hardware" configuration, filled when the stream starts or
+	 * when controls are set.
+	 */
+	struct {
+		struct v4l2_area size;
+		enum vimc_sensor_osd_mode osd_value;
+		u64 start_stream_ts;
+	} hw;
 };
 
 static const struct v4l2_mbus_framefmt fmt_default = {
@@ -44,14 +51,10 @@ static const struct v4l2_mbus_framefmt fmt_default = {
 static int vimc_sensor_init_state(struct v4l2_subdev *sd,
 				  struct v4l2_subdev_state *sd_state)
 {
-	unsigned int i;
-
-	for (i = 0; i < sd->entity.num_pads; i++) {
-		struct v4l2_mbus_framefmt *mf;
+	struct v4l2_mbus_framefmt *mf;
 
-		mf = v4l2_subdev_state_get_format(sd_state, i);
-		*mf = fmt_default;
-	}
+	mf = v4l2_subdev_state_get_format(sd_state, 0);
+	*mf = fmt_default;
 
 	return 0;
 }
@@ -92,36 +95,22 @@ static int vimc_sensor_enum_frame_size(struct v4l2_subdev *sd,
 	return 0;
 }
 
-static int vimc_sensor_get_fmt(struct v4l2_subdev *sd,
-			       struct v4l2_subdev_state *sd_state,
-			       struct v4l2_subdev_format *fmt)
+static void vimc_sensor_tpg_s_format(struct vimc_sensor_device *vsensor,
+				     const struct v4l2_mbus_framefmt *format)
 {
-	struct vimc_sensor_device *vsensor =
-				container_of(sd, struct vimc_sensor_device, sd);
-
-	fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
-		      *v4l2_subdev_state_get_format(sd_state, fmt->pad) :
-		      vsensor->mbus_format;
+	const struct vimc_pix_map *vpix = vimc_pix_map_by_code(format->code);
 
-	return 0;
-}
-
-static void vimc_sensor_tpg_s_format(struct vimc_sensor_device *vsensor)
-{
-	const struct vimc_pix_map *vpix =
-				vimc_pix_map_by_code(vsensor->mbus_format.code);
-
-	tpg_reset_source(&vsensor->tpg, vsensor->mbus_format.width,
-			 vsensor->mbus_format.height, vsensor->mbus_format.field);
-	tpg_s_bytesperline(&vsensor->tpg, 0, vsensor->mbus_format.width * vpix->bpp);
-	tpg_s_buf_height(&vsensor->tpg, vsensor->mbus_format.height);
+	tpg_reset_source(&vsensor->tpg, format->width, format->height,
+			 format->field);
+	tpg_s_bytesperline(&vsensor->tpg, 0, format->width * vpix->bpp);
+	tpg_s_buf_height(&vsensor->tpg, format->height);
 	tpg_s_fourcc(&vsensor->tpg, vpix->pixelformat);
 	/* TODO: add support for V4L2_FIELD_ALTERNATE */
-	tpg_s_field(&vsensor->tpg, vsensor->mbus_format.field, false);
-	tpg_s_colorspace(&vsensor->tpg, vsensor->mbus_format.colorspace);
-	tpg_s_ycbcr_enc(&vsensor->tpg, vsensor->mbus_format.ycbcr_enc);
-	tpg_s_quantization(&vsensor->tpg, vsensor->mbus_format.quantization);
-	tpg_s_xfer_func(&vsensor->tpg, vsensor->mbus_format.xfer_func);
+	tpg_s_field(&vsensor->tpg, format->field, false);
+	tpg_s_colorspace(&vsensor->tpg, format->colorspace);
+	tpg_s_ycbcr_enc(&vsensor->tpg, format->ycbcr_enc);
+	tpg_s_quantization(&vsensor->tpg, format->quantization);
+	tpg_s_xfer_func(&vsensor->tpg, format->xfer_func);
 }
 
 static void vimc_sensor_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
@@ -152,15 +141,11 @@ static int vimc_sensor_set_fmt(struct v4l2_subdev *sd,
 	struct vimc_sensor_device *vsensor = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt *mf;
 
-	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
-		/* Do not change the format while stream is on */
-		if (vsensor->frame)
-			return -EBUSY;
+	/* Do not change the format while stream is on */
+	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE && vsensor->frame)
+		return -EBUSY;
 
-		mf = &vsensor->mbus_format;
-	} else {
-		mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
-	}
+	mf = v4l2_subdev_state_get_format(sd_state, fmt->pad);
 
 	/* Set the new format */
 	vimc_sensor_adjust_fmt(&fmt->format);
@@ -185,7 +170,7 @@ static int vimc_sensor_set_fmt(struct v4l2_subdev *sd,
 static const struct v4l2_subdev_pad_ops vimc_sensor_pad_ops = {
 	.enum_mbus_code		= vimc_sensor_enum_mbus_code,
 	.enum_frame_size	= vimc_sensor_enum_frame_size,
-	.get_fmt		= vimc_sensor_get_fmt,
+	.get_fmt		= v4l2_subdev_get_fmt,
 	.set_fmt		= vimc_sensor_set_fmt,
 };
 
@@ -202,7 +187,7 @@ static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
 
 	tpg_fill_plane_buffer(&vsensor->tpg, 0, 0, vsensor->frame);
 	tpg_calc_text_basep(&vsensor->tpg, basep, 0, vsensor->frame);
-	switch (vsensor->osd_value) {
+	switch (vsensor->hw.osd_value) {
 	case VIMC_SENSOR_OSD_SHOW_ALL: {
 		const char *order = tpg_g_color_order(&vsensor->tpg);
 
@@ -216,15 +201,14 @@ static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
 			 vsensor->tpg.hue);
 		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 		snprintf(str, sizeof(str), "sensor size: %dx%d",
-			 vsensor->mbus_format.width,
-			 vsensor->mbus_format.height);
+			 vsensor->hw.size.width, vsensor->hw.size.height);
 		tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 		fallthrough;
 	}
 	case VIMC_SENSOR_OSD_SHOW_COUNTERS: {
 		unsigned int ms;
 
-		ms = div_u64(ktime_get_ns() - vsensor->start_stream_ts, 1000000);
+		ms = div_u64(ktime_get_ns() - vsensor->hw.start_stream_ts, 1000000);
 		snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
 			 (ms / (60 * 60 * 1000)) % 24,
 			 (ms / (60 * 1000)) % 60,
@@ -247,15 +231,25 @@ static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
 				container_of(sd, struct vimc_sensor_device, sd);
 
 	if (enable) {
+		const struct v4l2_mbus_framefmt *format;
+		struct v4l2_subdev_state *state;
 		const struct vimc_pix_map *vpix;
 		unsigned int frame_size;
 
-		vsensor->start_stream_ts = ktime_get_ns();
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+		format = v4l2_subdev_state_get_format(state, 0);
 
-		/* Calculate the frame size */
-		vpix = vimc_pix_map_by_code(vsensor->mbus_format.code);
-		frame_size = vsensor->mbus_format.width * vpix->bpp *
-			     vsensor->mbus_format.height;
+		/* Configure the test pattern generator. */
+		vimc_sensor_tpg_s_format(vsensor, format);
+
+		/* Calculate the frame size. */
+		vpix = vimc_pix_map_by_code(format->code);
+		frame_size = format->width * vpix->bpp * format->height;
+
+		vsensor->hw.size.width = format->width;
+		vsensor->hw.size.height = format->height;
+
+		v4l2_subdev_unlock_state(state);
 
 		/*
 		 * Allocate the frame buffer. Use vmalloc to be able to
@@ -265,9 +259,7 @@ static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
 		if (!vsensor->frame)
 			return -ENOMEM;
 
-		/* configure the test pattern generator */
-		vimc_sensor_tpg_s_format(vsensor);
-
+		vsensor->hw.start_stream_ts = ktime_get_ns();
 	} else {
 
 		vfree(vsensor->frame);
@@ -325,7 +317,7 @@ static int vimc_sensor_s_ctrl(struct v4l2_ctrl *ctrl)
 		tpg_s_saturation(&vsensor->tpg, ctrl->val);
 		break;
 	case VIMC_CID_OSD_TEXT_MODE:
-		vsensor->osd_value = ctrl->val;
+		vsensor->hw.osd_value = ctrl->val;
 		break;
 	default:
 		return -EINVAL;
@@ -344,6 +336,7 @@ static void vimc_sensor_release(struct vimc_ent_device *ved)
 
 	v4l2_ctrl_handler_free(&vsensor->hdl);
 	tpg_free(&vsensor->tpg);
+	v4l2_subdev_cleanup(&vsensor->sd);
 	media_entity_cleanup(vsensor->ved.ent);
 	kfree(vsensor);
 }
@@ -417,8 +410,7 @@ static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
 	}
 
 	/* Initialize the test pattern generator */
-	tpg_init(&vsensor->tpg, vsensor->mbus_format.width,
-		 vsensor->mbus_format.height);
+	tpg_init(&vsensor->tpg, fmt_default.width, fmt_default.height);
 	ret = tpg_alloc(&vsensor->tpg, VIMC_FRAME_MAX_WIDTH);
 	if (ret)
 		goto err_free_hdl;
@@ -428,18 +420,13 @@ static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
 	ret = vimc_ent_sd_register(&vsensor->ved, &vsensor->sd, v4l2_dev,
 				   vcfg_name,
 				   MEDIA_ENT_F_CAM_SENSOR, 1, &vsensor->pad,
-				   &vimc_sensor_ops);
+				   &vimc_sensor_internal_ops, &vimc_sensor_ops);
 	if (ret)
 		goto err_free_tpg;
 
-	vsensor->sd.internal_ops = &vimc_sensor_internal_ops;
-
 	vsensor->ved.process_frame = vimc_sensor_process_frame;
 	vsensor->ved.dev = vimc->mdev.dev;
 
-	/* Initialize the frame format */
-	vsensor->mbus_format = fmt_default;
-
 	return &vsensor->ved;
 
 err_free_tpg:
@@ -452,7 +439,7 @@ err_free_vsensor:
 	return ERR_PTR(ret);
 }
 
-struct vimc_ent_type vimc_sensor_type = {
+const struct vimc_ent_type vimc_sensor_type = {
 	.add = vimc_sensor_add,
 	.release = vimc_sensor_release
 };
diff --git a/drivers/media/test-drivers/vimc/vimc-streamer.c b/drivers/media/test-drivers/vimc/vimc-streamer.c
index 807551a5143b..15d863f97cbf 100644
--- a/drivers/media/test-drivers/vimc/vimc-streamer.c
+++ b/drivers/media/test-drivers/vimc/vimc-streamer.c
@@ -59,6 +59,12 @@ static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
 			continue;
 
 		sd = media_entity_to_v4l2_subdev(ved->ent);
+		/*
+		 * Do not call .s_stream() to stop an already
+		 * stopped/unstarted subdev.
+		 */
+		if (!v4l2_subdev_is_streaming(sd))
+			continue;
 		v4l2_subdev_call(sd, video, s_stream, 0);
 	}
 }
diff --git a/drivers/media/test-drivers/visl/visl-core.c b/drivers/media/test-drivers/visl/visl-core.c
index c46464bcaf2e..5bf3136b36eb 100644
--- a/drivers/media/test-drivers/visl/visl-core.c
+++ b/drivers/media/test-drivers/visl/visl-core.c
@@ -161,9 +161,15 @@ static const struct visl_ctrl_desc visl_h264_ctrl_descs[] = {
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_H264_DECODE_MODE,
+		.cfg.min = V4L2_STATELESS_H264_DECODE_MODE_SLICE_BASED,
+		.cfg.max = V4L2_STATELESS_H264_DECODE_MODE_FRAME_BASED,
+		.cfg.def = V4L2_STATELESS_H264_DECODE_MODE_SLICE_BASED,
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_H264_START_CODE,
+		.cfg.min = V4L2_STATELESS_H264_START_CODE_NONE,
+		.cfg.max = V4L2_STATELESS_H264_START_CODE_ANNEX_B,
+		.cfg.def = V4L2_STATELESS_H264_START_CODE_NONE,
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_H264_SLICE_PARAMS,
@@ -198,9 +204,15 @@ static const struct visl_ctrl_desc visl_hevc_ctrl_descs[] = {
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_HEVC_DECODE_MODE,
+		.cfg.min = V4L2_STATELESS_HEVC_DECODE_MODE_SLICE_BASED,
+		.cfg.max = V4L2_STATELESS_HEVC_DECODE_MODE_FRAME_BASED,
+		.cfg.def = V4L2_STATELESS_HEVC_DECODE_MODE_SLICE_BASED,
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_HEVC_START_CODE,
+		.cfg.min = V4L2_STATELESS_HEVC_START_CODE_NONE,
+		.cfg.max = V4L2_STATELESS_HEVC_START_CODE_ANNEX_B,
+		.cfg.def = V4L2_STATELESS_HEVC_START_CODE_NONE,
 	},
 	{
 		.cfg.id = V4L2_CID_STATELESS_HEVC_ENTRY_POINT_OFFSETS,
@@ -523,7 +535,7 @@ static void visl_remove(struct platform_device *pdev)
 
 static struct platform_driver visl_pdrv = {
 	.probe		= visl_probe,
-	.remove_new	= visl_remove,
+	.remove		= visl_remove,
 	.driver		= {
 		.name	= VISL_NAME,
 	},
diff --git a/drivers/media/test-drivers/visl/visl-video.c b/drivers/media/test-drivers/visl/visl-video.c
index b9a4b44bd0ed..8be505d8908c 100644
--- a/drivers/media/test-drivers/visl/visl-video.c
+++ b/drivers/media/test-drivers/visl/visl-video.c
@@ -136,6 +136,12 @@ static const u32 visl_decoded_fmts[] = {
 	V4L2_PIX_FMT_YUV420,
 };
 
+static const u32 visl_extended_decoded_fmts[] = {
+	V4L2_PIX_FMT_NV12,
+	V4L2_PIX_FMT_YUV420,
+	V4L2_PIX_FMT_P010,
+};
+
 const struct visl_coded_format_desc visl_coded_fmts[] = {
 	{
 		.pixelformat = V4L2_PIX_FMT_FWHT_STATELESS,
@@ -341,11 +347,21 @@ static int visl_enum_fmt_vid_cap(struct file *file, void *priv,
 				 struct v4l2_fmtdesc *f)
 {
 	struct visl_ctx *ctx = visl_file_to_ctx(file);
+	u32 index = f->index & ~V4L2_FMTDESC_FLAG_ENUM_ALL;
+	int max_fmts = ctx->coded_format_desc->num_decoded_fmts;
+	const u32 *decoded_fmts = ctx->coded_format_desc->decoded_fmts;
+
+	if (f->index & V4L2_FMTDESC_FLAG_ENUM_ALL) {
+		max_fmts = ARRAY_SIZE(visl_extended_decoded_fmts);
+		decoded_fmts = visl_extended_decoded_fmts;
+	}
+
+	f->index = index;
 
-	if (f->index >= ctx->coded_format_desc->num_decoded_fmts)
+	if (index >= max_fmts)
 		return -EINVAL;
 
-	f->pixelformat = ctx->coded_format_desc->decoded_fmts[f->index];
+	f->pixelformat = decoded_fmts[index];
 	return 0;
 }
 
@@ -539,6 +555,7 @@ const struct v4l2_ioctl_ops visl_ioctl_ops = {
 	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
 	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
 	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
+	.vidioc_remove_bufs		= v4l2_m2m_ioctl_remove_bufs,
 
 	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
 	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
@@ -715,8 +732,6 @@ static const struct vb2_ops visl_qops = {
 	.buf_queue            = visl_buf_queue,
 	.start_streaming      = visl_start_streaming,
 	.stop_streaming       = visl_stop_streaming,
-	.wait_prepare         = vb2_ops_wait_prepare,
-	.wait_finish          = vb2_ops_wait_finish,
 	.buf_request_complete = visl_buf_request_complete,
 };
 
diff --git a/drivers/media/test-drivers/vivid/Kconfig b/drivers/media/test-drivers/vivid/Kconfig
index ec2e71d76965..cc470070a7a5 100644
--- a/drivers/media/test-drivers/vivid/Kconfig
+++ b/drivers/media/test-drivers/vivid/Kconfig
@@ -1,9 +1,8 @@
 # SPDX-License-Identifier: GPL-2.0-only
 config VIDEO_VIVID
 	tristate "Virtual Video Test Driver"
-	depends on VIDEO_DEV && !SPARC32 && !SPARC64 && FB
+	depends on VIDEO_DEV && !SPARC32 && !SPARC64
 	depends on HAS_DMA
-	select FB_IOMEM_HELPERS
 	select FONT_SUPPORT
 	select FONT_8x16
 	select VIDEOBUF2_VMALLOC
@@ -31,6 +30,16 @@ config VIDEO_VIVID_CEC
 	  When selected the vivid module will emulate the optional
 	  HDMI CEC feature.
 
+config VIDEO_VIVID_OSD
+	bool "Enable Framebuffer for testing Output Overlay"
+	depends on VIDEO_VIVID && FB_CORE
+	depends on VIDEO_VIVID=m || FB_CORE=y
+	default y
+	select FB_IOMEM_HELPERS
+	help
+	  When selected the vivid module will emulate a Framebuffer for
+	  testing Output Overlay.
+
 config VIDEO_VIVID_MAX_DEVS
 	int "Maximum number of devices"
 	depends on VIDEO_VIVID
diff --git a/drivers/media/test-drivers/vivid/Makefile b/drivers/media/test-drivers/vivid/Makefile
index b12ad0152a3e..284a59e97335 100644
--- a/drivers/media/test-drivers/vivid/Makefile
+++ b/drivers/media/test-drivers/vivid/Makefile
@@ -3,10 +3,13 @@ vivid-objs := vivid-core.o vivid-ctrls.o vivid-vid-common.o vivid-vbi-gen.o \
 		vivid-vid-cap.o vivid-vid-out.o vivid-kthread-cap.o vivid-kthread-out.o \
 		vivid-radio-rx.o vivid-radio-tx.o vivid-radio-common.o \
 		vivid-rds-gen.o vivid-sdr-cap.o vivid-vbi-cap.o vivid-vbi-out.o \
-		vivid-osd.o vivid-meta-cap.o vivid-meta-out.o \
+		vivid-meta-cap.o vivid-meta-out.o \
 		vivid-kthread-touch.o vivid-touch-cap.o
 ifeq ($(CONFIG_VIDEO_VIVID_CEC),y)
   vivid-objs += vivid-cec.o
 endif
+ifeq ($(CONFIG_VIDEO_VIVID_OSD),y)
+  vivid-objs += vivid-osd.o
+endif
 
 obj-$(CONFIG_VIDEO_VIVID) += vivid.o
diff --git a/drivers/media/test-drivers/vivid/vivid-cec.c b/drivers/media/test-drivers/vivid/vivid-cec.c
index 1f7469ff04d5..356a988dd6a1 100644
--- a/drivers/media/test-drivers/vivid/vivid-cec.c
+++ b/drivers/media/test-drivers/vivid/vivid-cec.c
@@ -23,7 +23,7 @@ struct xfer_on_bus {
 static bool find_dest_adap(struct vivid_dev *dev,
 			   struct cec_adapter *adap, u8 dest)
 {
-	unsigned int i;
+	unsigned int i, j;
 
 	if (dest >= 0xf)
 		return false;
@@ -33,12 +33,29 @@ static bool find_dest_adap(struct vivid_dev *dev,
 	    cec_has_log_addr(dev->cec_rx_adap, dest))
 		return true;
 
-	for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++) {
-		if (adap == dev->cec_tx_adap[i])
+	for (i = 0, j = 0; i < dev->num_inputs; i++) {
+		unsigned int menu_idx =
+			dev->input_is_connected_to_output[i];
+
+		if (dev->input_type[i] != HDMI)
+			continue;
+		j++;
+		if (menu_idx < FIXED_MENU_ITEMS)
+			continue;
+
+		struct vivid_dev *dev_tx = vivid_ctrl_hdmi_to_output_instance[menu_idx];
+		unsigned int output = vivid_ctrl_hdmi_to_output_index[menu_idx];
+
+		if (!dev_tx)
 			continue;
-		if (!dev->cec_tx_adap[i]->is_configured)
+
+		unsigned int hdmi_output = dev_tx->output_to_iface_index[output];
+
+		if (adap == dev_tx->cec_tx_adap[hdmi_output])
 			continue;
-		if (cec_has_log_addr(dev->cec_tx_adap[i], dest))
+		if (!dev_tx->cec_tx_adap[hdmi_output]->is_configured)
+			continue;
+		if (cec_has_log_addr(dev_tx->cec_tx_adap[hdmi_output], dest))
 			return true;
 	}
 	return false;
@@ -96,7 +113,7 @@ static void adjust_sfts(struct vivid_dev *dev)
 int vivid_cec_bus_thread(void *_dev)
 {
 	u32 last_sft;
-	unsigned int i;
+	unsigned int i, j;
 	unsigned int dest;
 	ktime_t start, end;
 	s64 delta_us, retry_us;
@@ -193,9 +210,27 @@ int vivid_cec_bus_thread(void *_dev)
 		if (first_status == CEC_TX_STATUS_OK) {
 			if (xfers_on_bus[first_idx].adap != dev->cec_rx_adap)
 				cec_received_msg(dev->cec_rx_adap, &first_msg);
-			for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++)
-				if (xfers_on_bus[first_idx].adap != dev->cec_tx_adap[i])
-					cec_received_msg(dev->cec_tx_adap[i], &first_msg);
+			for (i = 0, j = 0; i < dev->num_inputs; i++) {
+				unsigned int menu_idx =
+					dev->input_is_connected_to_output[i];
+
+				if (dev->input_type[i] != HDMI)
+					continue;
+				j++;
+				if (menu_idx < FIXED_MENU_ITEMS)
+					continue;
+
+				struct vivid_dev *dev_tx = vivid_ctrl_hdmi_to_output_instance[menu_idx];
+				unsigned int output = vivid_ctrl_hdmi_to_output_index[menu_idx];
+
+				if (!dev_tx)
+					continue;
+
+				unsigned int hdmi_output = dev_tx->output_to_iface_index[output];
+
+				if (xfers_on_bus[first_idx].adap != dev_tx->cec_tx_adap[hdmi_output])
+					cec_received_msg(dev_tx->cec_tx_adap[hdmi_output], &first_msg);
+			}
 		}
 		end = ktime_get();
 		/*
@@ -242,21 +277,36 @@ static int vivid_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
 				   u32 signal_free_time, struct cec_msg *msg)
 {
 	struct vivid_dev *dev = cec_get_drvdata(adap);
+	struct vivid_dev *dev_rx = dev;
 	u8 idx = cec_msg_initiator(msg);
+	u8 output = 0;
 
-	spin_lock(&dev->cec_xfers_slock);
-	dev->xfers[idx].adap = adap;
-	memcpy(dev->xfers[idx].msg, msg->msg, CEC_MAX_MSG_SIZE);
-	dev->xfers[idx].len = msg->len;
-	dev->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_RETRY;
+	if (dev->cec_rx_adap != adap) {
+		int i;
+
+		for (i = 0; i < dev->num_hdmi_outputs; i++)
+			if (dev->cec_tx_adap[i] == adap)
+				break;
+		if (i == dev->num_hdmi_outputs)
+			return -ENONET;
+		output = dev->hdmi_index_to_output_index[i];
+		dev_rx = dev->output_to_input_instance[output];
+		if (!dev_rx)
+			return -ENONET;
+	}
+	spin_lock(&dev_rx->cec_xfers_slock);
+	dev_rx->xfers[idx].adap = adap;
+	memcpy(dev_rx->xfers[idx].msg, msg->msg, CEC_MAX_MSG_SIZE);
+	dev_rx->xfers[idx].len = msg->len;
+	dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_RETRY;
 	if (signal_free_time > CEC_SIGNAL_FREE_TIME_RETRY) {
-		if (idx == dev->last_initiator)
-			dev->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEXT_XFER;
+		if (idx == dev_rx->last_initiator)
+			dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEXT_XFER;
 		else
-			dev->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEW_INITIATOR;
+			dev_rx->xfers[idx].sft = CEC_SIGNAL_FREE_TIME_NEW_INITIATOR;
 	}
-	spin_unlock(&dev->cec_xfers_slock);
-	wake_up_interruptible(&dev->kthread_waitq_cec);
+	spin_unlock(&dev_rx->cec_xfers_slock);
+	wake_up_interruptible(&dev_rx->kthread_waitq_cec);
 
 	return 0;
 }
@@ -266,15 +316,16 @@ static int vivid_received(struct cec_adapter *adap, struct cec_msg *msg)
 	struct vivid_dev *dev = cec_get_drvdata(adap);
 	struct cec_msg reply;
 	u8 dest = cec_msg_destination(msg);
-	u8 disp_ctl;
-	char osd[14];
 
 	if (cec_msg_is_broadcast(msg))
 		dest = adap->log_addrs.log_addr[0];
 	cec_msg_init(&reply, dest, cec_msg_initiator(msg));
 
 	switch (cec_msg_opcode(msg)) {
-	case CEC_MSG_SET_OSD_STRING:
+	case CEC_MSG_SET_OSD_STRING: {
+		u8 disp_ctl;
+		char osd[14];
+
 		if (!cec_is_sink(adap))
 			return -ENOMSG;
 		cec_ops_set_osd_string(msg, &disp_ctl, osd);
@@ -298,6 +349,47 @@ static int vivid_received(struct cec_adapter *adap, struct cec_msg *msg)
 			break;
 		}
 		break;
+	}
+	case CEC_MSG_VENDOR_COMMAND_WITH_ID: {
+		u32 vendor_id;
+		u8 size;
+		const u8 *vendor_cmd;
+
+		/*
+		 * If we receive <Vendor Command With ID> with our vendor ID
+		 * and with a payload of size 1, and the payload value is odd,
+		 * then we reply with the same message, but with the payload
+		 * byte incremented by 1.
+		 *
+		 * If the size is 1 and the payload value is even, then we
+		 * ignore the message.
+		 *
+		 * The reason we reply to odd instead of even payload values
+		 * is that it allows for testing of the corner case where the
+		 * reply value is 0 (0xff + 1 % 256).
+		 *
+		 * For other sizes we Feature Abort.
+		 *
+		 * This is added for the specific purpose of testing the
+		 * CEC_MSG_FL_REPLY_VENDOR_ID flag using vivid.
+		 */
+		cec_ops_vendor_command_with_id(msg, &vendor_id, &size, &vendor_cmd);
+		if (vendor_id != adap->log_addrs.vendor_id)
+			break;
+		if (size == 1) {
+			// Ignore even op values
+			if (!(vendor_cmd[0] & 1))
+				break;
+			reply.len = msg->len;
+			memcpy(reply.msg + 1, msg->msg + 1, msg->len - 1);
+			reply.msg[msg->len - 1]++;
+		} else {
+			cec_msg_feature_abort(&reply, cec_msg_opcode(msg),
+					      CEC_OP_ABORT_INVALID_OP);
+		}
+		cec_transmit_msg(adap, &reply, false);
+		break;
+	}
 	default:
 		return -ENOMSG;
 	}
diff --git a/drivers/media/test-drivers/vivid/vivid-core.c b/drivers/media/test-drivers/vivid/vivid-core.c
index 159c72cbb5bf..8d56168c72aa 100644
--- a/drivers/media/test-drivers/vivid/vivid-core.c
+++ b/drivers/media/test-drivers/vivid/vivid-core.c
@@ -1,4 +1,5 @@
 // SPDX-License-Identifier: GPL-2.0-only
+
 /*
  * vivid-core.c - A Virtual Video Test Driver, core initialization
  *
@@ -42,15 +43,13 @@
 #include "vivid-touch-cap.h"
 
 #define VIVID_MODULE_NAME "vivid"
-
-/* The maximum number of vivid devices */
-#define VIVID_MAX_DEVS CONFIG_VIDEO_VIVID_MAX_DEVS
+#define MAX_STRING_LENGTH 23
 
 MODULE_DESCRIPTION("Virtual Video Test Driver");
 MODULE_AUTHOR("Hans Verkuil");
 MODULE_LICENSE("GPL");
 
-static unsigned n_devs = 1;
+unsigned int n_devs = 1;
 module_param(n_devs, uint, 0444);
 MODULE_PARM_DESC(n_devs, " number of driver instances to create");
 
@@ -126,7 +125,9 @@ MODULE_PARM_DESC(node_types, " node types, default is 0xe1d3d. Bitmask with the
 			     "\t\t    bit 8: Video Output node\n"
 			     "\t\t    bit 10-11: VBI Output node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
 			     "\t\t    bit 12: Radio Transmitter node\n"
+#ifdef CONFIG_VIDEO_VIVID_OSD
 			     "\t\t    bit 16: Framebuffer for testing output overlays\n"
+#endif
 			     "\t\t    bit 17: Metadata Capture node\n"
 			     "\t\t    bit 18: Metadata Output node\n"
 			     "\t\t    bit 19: Touch Capture node\n");
@@ -186,7 +187,32 @@ MODULE_PARM_DESC(supports_requests, " support for requests, default is 1.\n"
 			     "\t\t    1 == supports requests\n"
 			     "\t\t    2 == requires requests");
 
-static struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+
+DEFINE_SPINLOCK(hdmi_output_skip_mask_lock);
+struct workqueue_struct *update_hdmi_ctrls_workqueue;
+u64 hdmi_to_output_menu_skip_mask;
+u64 hdmi_input_update_outputs_mask;
+
+struct vivid_dev *vivid_ctrl_hdmi_to_output_instance[MAX_MENU_ITEMS];
+unsigned int vivid_ctrl_hdmi_to_output_index[MAX_MENU_ITEMS];
+
+char *vivid_ctrl_hdmi_to_output_strings[MAX_MENU_ITEMS + 1] = {
+	"Test Pattern Generator",
+	"None"
+};
+
+DEFINE_SPINLOCK(svid_output_skip_mask_lock);
+struct workqueue_struct *update_svid_ctrls_workqueue;
+u64 svid_to_output_menu_skip_mask;
+
+struct vivid_dev *vivid_ctrl_svid_to_output_instance[MAX_MENU_ITEMS];
+unsigned int vivid_ctrl_svid_to_output_index[MAX_MENU_ITEMS];
+
+char *vivid_ctrl_svid_to_output_strings[MAX_MENU_ITEMS + 1] = {
+	"Test Pattern Generator",
+	"None"
+};
 
 const struct v4l2_rect vivid_min_rect = {
 	0, 0, MIN_WIDTH, MIN_HEIGHT
@@ -218,7 +244,7 @@ static const u8 vivid_hdmi_edid[256] = {
 	0x5e, 0x5d, 0x10, 0x1f, 0x04, 0x13, 0x22, 0x21,
 	0x20, 0x05, 0x14, 0x02, 0x11, 0x01, 0x23, 0x09,
 	0x07, 0x07, 0x83, 0x01, 0x00, 0x00, 0x6d, 0x03,
-	0x0c, 0x00, 0x10, 0x00, 0x00, 0x3c, 0x21, 0x00,
+	0x0c, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x21, 0x00,
 	0x60, 0x01, 0x02, 0x03, 0x67, 0xd8, 0x5d, 0xc4,
 	0x01, 0x78, 0x00, 0x00, 0xe2, 0x00, 0xca, 0xe3,
 	0x05, 0x00, 0x00, 0xe3, 0x06, 0x01, 0x00, 0x4d,
@@ -229,7 +255,7 @@ static const u8 vivid_hdmi_edid[256] = {
 	0x00, 0x00, 0x1a, 0x1a, 0x1d, 0x00, 0x80, 0x51,
 	0xd0, 0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0xc0,
 	0x1c, 0x32, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92,
 };
 
 static int vidioc_querycap(struct file *file, void  *priv,
@@ -769,6 +795,7 @@ static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
 	.vidioc_expbuf			= vb2_ioctl_expbuf,
 	.vidioc_streamon		= vb2_ioctl_streamon,
 	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_remove_bufs		= vb2_ioctl_remove_bufs,
 
 	.vidioc_enum_input		= vivid_enum_input,
 	.vidioc_g_input			= vivid_g_input,
@@ -826,6 +853,7 @@ static void vivid_dev_release(struct v4l2_device *v4l2_dev)
 {
 	struct vivid_dev *dev = container_of(v4l2_dev, struct vivid_dev, v4l2_dev);
 
+	cancel_work_sync(&dev->update_hdmi_ctrl_work);
 	vivid_free_controls(dev);
 	v4l2_device_unregister(&dev->v4l2_dev);
 #ifdef CONFIG_MEDIA_CONTROLLER
@@ -861,7 +889,7 @@ static const struct media_device_ops vivid_media_ops = {
 static int vivid_create_queue(struct vivid_dev *dev,
 			      struct vb2_queue *q,
 			      u32 buf_type,
-			      unsigned int min_queued_buffers,
+			      unsigned int min_reqbufs_allocation,
 			      const struct vb2_ops *ops)
 {
 	if (buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->multiplanar)
@@ -884,7 +912,7 @@ static int vivid_create_queue(struct vivid_dev *dev,
 	 * videobuf2-core.c to MAX_BUFFER_INDEX.
 	 */
 	if (buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		q->max_num_buffers = 64;
+		q->max_num_buffers = MAX_VID_CAP_BUFFERS;
 	if (buf_type == V4L2_BUF_TYPE_SDR_CAPTURE)
 		q->max_num_buffers = 1024;
 	if (buf_type == V4L2_BUF_TYPE_VBI_CAPTURE)
@@ -898,7 +926,7 @@ static int vivid_create_queue(struct vivid_dev *dev,
 	q->mem_ops = allocators[dev->inst] == 1 ? &vb2_dma_contig_memops :
 						  &vb2_vmalloc_memops;
 	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-	q->min_queued_buffers = supports_requests[dev->inst] ? 0 : min_queued_buffers;
+	q->min_reqbufs_allocation = min_reqbufs_allocation;
 	q->lock = &dev->mutex;
 	q->dev = dev->v4l2_dev.dev;
 	q->supports_requests = supports_requests[dev->inst];
@@ -945,6 +973,7 @@ static int vivid_detect_feature_set(struct vivid_dev *dev, int inst,
 		dev->num_inputs--;
 	}
 	dev->num_hdmi_inputs = in_type_counter[HDMI];
+	dev->num_svid_inputs = in_type_counter[SVID];
 
 	/* how many outputs do we have and of what type? */
 	dev->num_outputs = num_outputs[inst];
@@ -959,7 +988,6 @@ static int vivid_detect_feature_set(struct vivid_dev *dev, int inst,
 	for (i = 0; i < dev->num_outputs; i++) {
 		dev->output_type[i] = ((output_types[inst] >> i) & 1) ? HDMI : SVID;
 		dev->output_name_counter[i] = out_type_counter[dev->output_type[i]]++;
-		dev->display_present[i] = true;
 	}
 	dev->has_audio_outputs = out_type_counter[SVID];
 	if (out_type_counter[HDMI] == 16) {
@@ -1045,9 +1073,11 @@ static int vivid_detect_feature_set(struct vivid_dev *dev, int inst,
 	/* do we have a modulator? */
 	*has_modulator = dev->has_radio_tx;
 
+#ifdef CONFIG_VIDEO_VIVID_OSD
 	if (dev->has_vid_cap)
 		/* do we have a framebuffer for overlay testing? */
 		dev->has_fb = node_type & 0x10000;
+#endif
 
 	/* can we do crop/compose/scaling while capturing? */
 	if (no_error_inj && *ccs_cap == -1)
@@ -1364,7 +1394,7 @@ static int vivid_create_queues(struct vivid_dev *dev)
 	if (dev->has_meta_out) {
 		/* initialize meta_out queue */
 		ret = vivid_create_queue(dev, &dev->vb_meta_out_q,
-					 V4L2_BUF_TYPE_META_OUTPUT, 1,
+					 V4L2_BUF_TYPE_META_OUTPUT, 2,
 					 &vivid_meta_out_qops);
 		if (ret)
 			return ret;
@@ -1373,7 +1403,7 @@ static int vivid_create_queues(struct vivid_dev *dev)
 	if (dev->has_touch_cap) {
 		/* initialize touch_cap queue */
 		ret = vivid_create_queue(dev, &dev->vb_touch_cap_q,
-					 V4L2_BUF_TYPE_VIDEO_CAPTURE, 1,
+					 V4L2_BUF_TYPE_VIDEO_CAPTURE, 2,
 					 &vivid_touch_cap_qops);
 		if (ret)
 			return ret;
@@ -1384,15 +1414,12 @@ static int vivid_create_queues(struct vivid_dev *dev)
 		ret = vivid_fb_init(dev);
 		if (ret)
 			return ret;
-		v4l2_info(&dev->v4l2_dev, "Framebuffer device registered as fb%d\n",
-			  dev->fb_info.node);
 	}
 	return 0;
 }
 
 static int vivid_create_devnodes(struct platform_device *pdev,
 				 struct vivid_dev *dev, int inst,
-				 unsigned int cec_tx_bus_cnt,
 				 v4l2_std_id tvnorms_cap,
 				 v4l2_std_id tvnorms_out,
 				 unsigned in_type_counter[4],
@@ -1436,7 +1463,7 @@ static int vivid_create_devnodes(struct platform_device *pdev,
 				return ret;
 			}
 			cec_s_phys_addr(dev->cec_rx_adap, 0, false);
-			v4l2_info(&dev->v4l2_dev, "CEC adapter %s registered for HDMI input 0\n",
+			v4l2_info(&dev->v4l2_dev, "CEC adapter %s registered for HDMI input\n",
 				  dev_name(&dev->cec_rx_adap->devnode.dev));
 		}
 #endif
@@ -1479,10 +1506,10 @@ static int vivid_create_devnodes(struct platform_device *pdev,
 #endif
 
 #ifdef CONFIG_VIDEO_VIVID_CEC
-		for (i = 0; i < cec_tx_bus_cnt; i++) {
+		for (i = 0; i < dev->num_hdmi_outputs; i++) {
 			ret = cec_register_adapter(dev->cec_tx_adap[i], &pdev->dev);
 			if (ret < 0) {
-				for (; i < cec_tx_bus_cnt; i++) {
+				for (; i >= 0; i--) {
 					cec_delete_adapter(dev->cec_tx_adap[i]);
 					dev->cec_tx_adap[i] = NULL;
 				}
@@ -1490,10 +1517,6 @@ static int vivid_create_devnodes(struct platform_device *pdev,
 			}
 			v4l2_info(&dev->v4l2_dev, "CEC adapter %s registered for HDMI output %d\n",
 				  dev_name(&dev->cec_tx_adap[i]->devnode.dev), i);
-			if (i < out_type_counter[HDMI])
-				cec_s_phys_addr(dev->cec_tx_adap[i], (i + 1) << 12, false);
-			else
-				cec_s_phys_addr(dev->cec_tx_adap[i], 0x1000, false);
 		}
 #endif
 
@@ -1733,6 +1756,47 @@ static int vivid_create_devnodes(struct platform_device *pdev,
 	return 0;
 }
 
+static void update_hdmi_ctrls_work_handler(struct work_struct *work)
+{
+	u64 skip_mask;
+	u64 update_mask;
+
+	spin_lock(&hdmi_output_skip_mask_lock);
+	skip_mask = hdmi_to_output_menu_skip_mask;
+	update_mask = hdmi_input_update_outputs_mask;
+	hdmi_input_update_outputs_mask = 0;
+	spin_unlock(&hdmi_output_skip_mask_lock);
+	for (int i = 0; i < n_devs && vivid_devs[i]; i++) {
+		if (update_mask & (1 << i))
+			vivid_update_connected_outputs(vivid_devs[i]);
+		for (int j = 0; j < vivid_devs[i]->num_hdmi_inputs; j++) {
+			struct v4l2_ctrl *c = vivid_devs[i]->ctrl_hdmi_to_output[j];
+
+			v4l2_ctrl_modify_range(c, c->minimum, c->maximum,
+					       skip_mask & ~(1ULL << c->cur.val),
+					       c->default_value);
+		}
+	}
+}
+
+static void update_svid_ctrls_work_handler(struct work_struct *work)
+{
+	u64 skip_mask;
+
+	spin_lock(&svid_output_skip_mask_lock);
+	skip_mask = svid_to_output_menu_skip_mask;
+	spin_unlock(&svid_output_skip_mask_lock);
+	for (int i = 0; i < n_devs && vivid_devs[i]; i++) {
+		for (int j = 0; j < vivid_devs[i]->num_svid_inputs; j++) {
+			struct v4l2_ctrl *c = vivid_devs[i]->ctrl_svid_to_output[j];
+
+			v4l2_ctrl_modify_range(c, c->minimum, c->maximum,
+					       skip_mask & ~(1ULL << c->cur.val),
+					       c->default_value);
+		}
+	}
+}
+
 static int vivid_create_instance(struct platform_device *pdev, int inst)
 {
 	static const struct v4l2_dv_timings def_dv_timings =
@@ -1746,7 +1810,6 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	struct vivid_dev *dev;
 	unsigned node_type = node_types[inst];
 	v4l2_std_id tvnorms_cap = 0, tvnorms_out = 0;
-	unsigned int cec_tx_bus_cnt = 0;
 	int ret;
 	int i;
 
@@ -1849,6 +1912,22 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	dev->edid_max_blocks = dev->edid_blocks = 2;
 	memcpy(dev->edid, vivid_hdmi_edid, sizeof(vivid_hdmi_edid));
 	dev->radio_rds_init_time = ktime_get();
+	INIT_WORK(&dev->update_hdmi_ctrl_work, update_hdmi_ctrls_work_handler);
+	INIT_WORK(&dev->update_svid_ctrl_work, update_svid_ctrls_work_handler);
+	for (int j = 0, k = 0; j < dev->num_inputs; ++j)
+		if (dev->input_type[j] == HDMI)
+			dev->hdmi_index_to_input_index[k++] = j;
+	for (int j = 0, k = 0; j < dev->num_outputs; ++j)
+		if (dev->output_type[j] == HDMI) {
+			dev->output_to_iface_index[j] = k;
+			dev->hdmi_index_to_output_index[k++] = j;
+		}
+	for (int j = 0, k = 0; j < dev->num_inputs; ++j)
+		if (dev->input_type[j] == SVID)
+			dev->svid_index_to_input_index[k++] = j;
+	for (int j = 0, k = 0; j < dev->num_outputs; ++j)
+		if (dev->output_type[j] == SVID)
+			dev->output_to_iface_index[j] = k++;
 
 	/* create all controls */
 	ret = vivid_create_controls(dev, ccs_cap == -1, ccs_out == -1, no_error_inj,
@@ -1859,8 +1938,6 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 		goto unreg_dev;
 
 	/* enable/disable interface specific controls */
-	if (dev->num_outputs && dev->output_type[0] != HDMI)
-		v4l2_ctrl_activate(dev->ctrl_display_present, false);
 	if (dev->num_inputs && dev->input_type[0] != HDMI) {
 		v4l2_ctrl_activate(dev->ctrl_dv_timings_signal_mode, false);
 		v4l2_ctrl_activate(dev->ctrl_dv_timings, false);
@@ -1916,27 +1993,27 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	}
 
 	if (dev->has_vid_out) {
-		for (i = 0; i < dev->num_outputs; i++) {
+		int j;
+
+		for (i = j = 0; i < dev->num_outputs; i++) {
 			struct cec_adapter *adap;
 
 			if (dev->output_type[i] != HDMI)
 				continue;
 
-			dev->cec_output2bus_map[i] = cec_tx_bus_cnt;
-			adap = vivid_cec_alloc_adap(dev, cec_tx_bus_cnt, true);
+			adap = vivid_cec_alloc_adap(dev, j, true);
 			ret = PTR_ERR_OR_ZERO(adap);
 			if (ret < 0) {
-				for (i = 0; i < dev->num_outputs; i++)
-					cec_delete_adapter(dev->cec_tx_adap[i]);
+				while (j--)
+					cec_delete_adapter(dev->cec_tx_adap[j]);
 				goto unreg_dev;
 			}
 
-			dev->cec_tx_adap[cec_tx_bus_cnt] = adap;
-			cec_tx_bus_cnt++;
+			dev->cec_tx_adap[j++] = adap;
 		}
 	}
 
-	if (dev->cec_rx_adap || cec_tx_bus_cnt) {
+	if (dev->cec_rx_adap || dev->num_hdmi_outputs) {
 		init_waitqueue_head(&dev->kthread_waitq_cec);
 		dev->kthread_cec = kthread_run(vivid_cec_bus_thread, dev,
 					       "vivid_cec-%s", dev->v4l2_dev.name);
@@ -1962,7 +2039,7 @@ static int vivid_create_instance(struct platform_device *pdev, int inst)
 	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_touch_cap);
 
 	/* finally start creating the device nodes */
-	ret = vivid_create_devnodes(pdev, dev, inst, cec_tx_bus_cnt,
+	ret = vivid_create_devnodes(pdev, dev, inst,
 				    tvnorms_cap, tvnorms_out,
 				    in_type_counter, out_type_counter);
 	if (ret)
@@ -1985,7 +2062,7 @@ unreg_dev:
 	vb2_video_unregister_device(&dev->vid_out_dev);
 	vb2_video_unregister_device(&dev->vid_cap_dev);
 	cec_unregister_adapter(dev->cec_rx_adap);
-	for (i = 0; i < MAX_OUTPUTS; i++)
+	for (i = 0; i < MAX_HDMI_OUTPUTS; i++)
 		cec_unregister_adapter(dev->cec_tx_adap[i]);
 	if (dev->kthread_cec)
 		kthread_stop(dev->kthread_cec);
@@ -2032,6 +2109,42 @@ static int vivid_probe(struct platform_device *pdev)
 	/* n_devs will reflect the actual number of allocated devices */
 	n_devs = i;
 
+	/* Determine qmenu items actually in use */
+	int hdmi_count = FIXED_MENU_ITEMS;
+	int svid_count = FIXED_MENU_ITEMS;
+
+	for (int i = 0; i < n_devs; i++) {
+		struct vivid_dev *dev = vivid_devs[i];
+
+		if (!dev->has_vid_out)
+			continue;
+		for (int j = 0; j < dev->num_outputs && hdmi_count < MAX_MENU_ITEMS; ++j) {
+			if (dev->output_type[j] == HDMI) {
+				vivid_ctrl_hdmi_to_output_instance[hdmi_count] = vivid_devs[i];
+				vivid_ctrl_hdmi_to_output_index[hdmi_count++] = j;
+			}
+		}
+		for (int j = 0; j < dev->num_outputs && svid_count < MAX_MENU_ITEMS; ++j) {
+			if (dev->output_type[j] == SVID) {
+				vivid_ctrl_svid_to_output_instance[svid_count] = vivid_devs[i];
+				vivid_ctrl_svid_to_output_index[svid_count++] = j;
+			}
+		}
+	}
+	hdmi_count = min(hdmi_count, MAX_MENU_ITEMS);
+	svid_count = min(svid_count, MAX_MENU_ITEMS);
+	for (int i = 0; i < n_devs; i++) {
+		for (int j = 0; j < vivid_devs[i]->num_hdmi_inputs; j++) {
+			struct v4l2_ctrl *c = vivid_devs[i]->ctrl_hdmi_to_output[j];
+
+			v4l2_ctrl_modify_range(c, c->minimum, hdmi_count - 1, 0, c->default_value);
+		}
+		for (int j = 0; j < vivid_devs[i]->num_svid_inputs; j++) {
+			struct v4l2_ctrl *c = vivid_devs[i]->ctrl_svid_to_output[j];
+
+			v4l2_ctrl_modify_range(c, c->minimum, svid_count - 1, 0, c->default_value);
+		}
+	}
 	return ret;
 }
 
@@ -2086,12 +2199,8 @@ static void vivid_remove(struct platform_device *pdev)
 				video_device_node_name(&dev->radio_tx_dev));
 			video_unregister_device(&dev->radio_tx_dev);
 		}
-		if (dev->has_fb) {
-			v4l2_info(&dev->v4l2_dev, "unregistering fb%d\n",
-				dev->fb_info.node);
-			unregister_framebuffer(&dev->fb_info);
-			vivid_fb_release_buffers(dev);
-		}
+		if (dev->has_fb)
+			vivid_fb_deinit(dev);
 		if (dev->has_meta_cap) {
 			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
 				  video_device_node_name(&dev->meta_cap_dev));
@@ -2108,7 +2217,7 @@ static void vivid_remove(struct platform_device *pdev)
 			vb2_video_unregister_device(&dev->touch_cap_dev);
 		}
 		cec_unregister_adapter(dev->cec_rx_adap);
-		for (j = 0; j < MAX_OUTPUTS; j++)
+		for (j = 0; j < MAX_HDMI_OUTPUTS; j++)
 			cec_unregister_adapter(dev->cec_tx_adap[j]);
 		if (dev->kthread_cec)
 			kthread_stop(dev->kthread_cec);
@@ -2128,7 +2237,7 @@ static struct platform_device vivid_pdev = {
 
 static struct platform_driver vivid_pdrv = {
 	.probe		= vivid_probe,
-	.remove_new	= vivid_remove,
+	.remove		= vivid_remove,
 	.driver		= {
 		.name	= "vivid",
 	},
@@ -2136,21 +2245,91 @@ static struct platform_driver vivid_pdrv = {
 
 static int __init vivid_init(void)
 {
-	int ret;
-
+	int hdmi_count = FIXED_MENU_ITEMS;
+	int svid_count = FIXED_MENU_ITEMS;
+	int ret = -ENOMEM;
+	unsigned int ndevs;
+
+	/* Sanity check, prevent insane number of vivid instances */
+	if (n_devs > 64)
+		n_devs = 64;
+	ndevs = clamp_t(unsigned int, n_devs, 1, VIVID_MAX_DEVS);
+
+	for (unsigned int i = 0; i < ndevs; i++) {
+		if (!(node_types[i] & (1 << 8)))
+			continue;
+		unsigned int n_outputs = min(num_outputs[i], MAX_OUTPUTS);
+
+		for (u8 j = 0, k = 0; j < n_outputs && hdmi_count < MAX_MENU_ITEMS &&
+		     k < MAX_HDMI_OUTPUTS; ++j) {
+			if (output_types[i] & BIT(j)) {
+				vivid_ctrl_hdmi_to_output_strings[hdmi_count] =
+					kmalloc(MAX_STRING_LENGTH, GFP_KERNEL);
+				if (!vivid_ctrl_hdmi_to_output_strings[hdmi_count])
+					goto free_output_strings;
+				snprintf(vivid_ctrl_hdmi_to_output_strings[hdmi_count],
+					 MAX_STRING_LENGTH, "Output HDMI %03d-%d",
+					 i & 0xff, k);
+				k++;
+				hdmi_count++;
+			}
+		}
+		for (u8 j = 0, k = 0; j < n_outputs && svid_count < MAX_MENU_ITEMS; ++j) {
+			if (!(output_types[i] & BIT(j))) {
+				vivid_ctrl_svid_to_output_strings[svid_count] =
+					kmalloc(MAX_STRING_LENGTH, GFP_KERNEL);
+				if (!vivid_ctrl_svid_to_output_strings[svid_count])
+					goto free_output_strings;
+				snprintf(vivid_ctrl_svid_to_output_strings[svid_count],
+					 MAX_STRING_LENGTH, "Output S-Video %03d-%d",
+					 i & 0xff, k);
+				k++;
+				svid_count++;
+			}
+		}
+	}
 	ret = platform_device_register(&vivid_pdev);
 	if (ret)
-		return ret;
-
+		goto free_output_strings;
 	ret = platform_driver_register(&vivid_pdrv);
 	if (ret)
-		platform_device_unregister(&vivid_pdev);
+		goto unreg_device;
 
+	/* Initialize workqueue before module is loaded */
+	update_hdmi_ctrls_workqueue = create_workqueue("update_hdmi_ctrls_wq");
+	if (!update_hdmi_ctrls_workqueue) {
+		ret = -ENOMEM;
+		goto unreg_driver;
+	}
+	update_svid_ctrls_workqueue = create_workqueue("update_svid_ctrls_wq");
+	if (!update_svid_ctrls_workqueue) {
+		ret = -ENOMEM;
+		goto destroy_hdmi_wq;
+	}
+	return ret;
+
+destroy_hdmi_wq:
+	destroy_workqueue(update_hdmi_ctrls_workqueue);
+unreg_driver:
+	platform_driver_register(&vivid_pdrv);
+unreg_device:
+	platform_device_unregister(&vivid_pdev);
+free_output_strings:
+	for (int i = FIXED_MENU_ITEMS; i < MAX_MENU_ITEMS; i++) {
+		kfree(vivid_ctrl_hdmi_to_output_strings[i]);
+		kfree(vivid_ctrl_svid_to_output_strings[i]);
+	}
 	return ret;
 }
 
 static void __exit vivid_exit(void)
 {
+	for (int i = FIXED_MENU_ITEMS; i < MAX_MENU_ITEMS; i++) {
+		kfree(vivid_ctrl_hdmi_to_output_strings[i]);
+		kfree(vivid_ctrl_svid_to_output_strings[i]);
+	}
+	destroy_workqueue(update_svid_ctrls_workqueue);
+	destroy_workqueue(update_hdmi_ctrls_workqueue);
 	platform_driver_unregister(&vivid_pdrv);
 	platform_device_unregister(&vivid_pdev);
 }
diff --git a/drivers/media/test-drivers/vivid/vivid-core.h b/drivers/media/test-drivers/vivid/vivid-core.h
index cfb8e66083f6..571a6c222969 100644
--- a/drivers/media/test-drivers/vivid/vivid-core.h
+++ b/drivers/media/test-drivers/vivid/vivid-core.h
@@ -26,6 +26,8 @@
 #define MAX_INPUTS 16
 /* The maximum number of outputs */
 #define MAX_OUTPUTS 16
+/* The maximum number of video capture buffers */
+#define MAX_VID_CAP_BUFFERS 64
 /* The maximum up or down scaling factor is 4 */
 #define MAX_ZOOM  4
 /* The maximum image width/height are set to 4K DMT */
@@ -50,10 +52,95 @@
 #define JIFFIES_PER_DAY (3600U * 24U * HZ)
 #define JIFFIES_RESYNC (JIFFIES_PER_DAY * (0xf0000000U / JIFFIES_PER_DAY))
 
+/*
+ * Maximum number of HDMI inputs allowed by vivid, due to limitations
+ * of the Physical Address in the EDID and used by CEC we stop at 15
+ * inputs and outputs.
+ */
+#define MAX_HDMI_INPUTS 15
+#define MAX_HDMI_OUTPUTS 15
+
+/* Maximum number of S-Video inputs allowed by vivid */
+#define MAX_SVID_INPUTS 16
+
+/* The maximum number of items in a menu control */
+#define MAX_MENU_ITEMS BITS_PER_LONG_LONG
+
+/* Number of fixed menu items in the 'Connected To' menu controls */
+#define FIXED_MENU_ITEMS 2
+
+/* The maximum number of vivid devices */
+#define VIVID_MAX_DEVS CONFIG_VIDEO_VIVID_MAX_DEVS
+
 extern const struct v4l2_rect vivid_min_rect;
 extern const struct v4l2_rect vivid_max_rect;
 extern unsigned vivid_debug;
 
+/*
+ * NULL-terminated string array for the HDMI 'Connected To' menu controls
+ * with the list of possible HDMI outputs.
+ *
+ * The first two items are fixed ("TPG" and "None").
+ */
+extern char *vivid_ctrl_hdmi_to_output_strings[1 + MAX_MENU_ITEMS];
+/* Menu control skip mask of all HDMI outputs that are in use */
+extern u64 hdmi_to_output_menu_skip_mask;
+/*
+ * Bitmask of which vivid instances need to update any connected
+ * HDMI outputs.
+ */
+extern u64 hdmi_input_update_outputs_mask;
+/*
+ * Spinlock for access to hdmi_to_output_menu_skip_mask and
+ * hdmi_input_update_outputs_mask.
+ */
+extern spinlock_t hdmi_output_skip_mask_lock;
+/*
+ * Workqueue that updates the menu controls whenever the HDMI menu skip mask
+ * changes.
+ */
+extern struct workqueue_struct *update_hdmi_ctrls_workqueue;
+
+/*
+ * The HDMI menu control value (index in the menu list) maps to an HDMI
+ * output that is part of the given vivid_dev instance and has the given
+ * output index (as returned by VIDIOC_G_OUTPUT).
+ *
+ * NULL/0 if not available.
+ */
+extern struct vivid_dev *vivid_ctrl_hdmi_to_output_instance[MAX_MENU_ITEMS];
+extern unsigned int vivid_ctrl_hdmi_to_output_index[MAX_MENU_ITEMS];
+
+/*
+ * NULL-terminated string array for the S-Video 'Connected To' menu controls
+ * with the list of possible S-Video outputs.
+ *
+ * The first two items are fixed ("TPG" and "None").
+ */
+extern char *vivid_ctrl_svid_to_output_strings[1 + MAX_MENU_ITEMS];
+/* Menu control skip mask of all S-Video outputs that are in use */
+extern u64 svid_to_output_menu_skip_mask;
+/* Spinlock for access to svid_to_output_menu_skip_mask */
+extern spinlock_t svid_output_skip_mask_lock;
+/*
+ * Workqueue that updates the menu controls whenever the S-Video menu skip mask
+ * changes.
+ */
+extern struct workqueue_struct *update_svid_ctrls_workqueue;
+
+/*
+ * The S-Video menu control value (index in the menu list) maps to an S-Video
+ * output that is part of the given vivid_dev instance and has the given
+ * output index (as returned by VIDIOC_G_OUTPUT).
+ *
+ * NULL/0 if not available.
+ */
+extern struct vivid_dev *vivid_ctrl_svid_to_output_instance[MAX_MENU_ITEMS];
+extern unsigned int vivid_ctrl_svid_to_output_index[MAX_MENU_ITEMS];
+
+extern struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+extern unsigned int n_devs;
+
 struct vivid_fmt {
 	u32	fourcc;          /* v4l2 format id */
 	enum	tgp_color_enc color_enc;
@@ -118,7 +205,7 @@ struct vivid_cec_xfer {
 };
 
 struct vivid_dev {
-	unsigned			inst;
+	u8				inst;
 	struct v4l2_device		v4l2_dev;
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct media_device		mdev;
@@ -161,6 +248,8 @@ struct vivid_dev {
 
 	spinlock_t			slock;
 	struct mutex			mutex;
+	struct work_struct		update_hdmi_ctrl_work;
+	struct work_struct		update_svid_ctrl_work;
 
 	/* capabilities */
 	u32				vid_cap_caps;
@@ -176,12 +265,13 @@ struct vivid_dev {
 
 	/* supported features */
 	bool				multiplanar;
-	unsigned			num_inputs;
-	unsigned int			num_hdmi_inputs;
+	u8				num_inputs;
+	u8				num_hdmi_inputs;
+	u8				num_svid_inputs;
 	u8				input_type[MAX_INPUTS];
 	u8				input_name_counter[MAX_INPUTS];
-	unsigned			num_outputs;
-	unsigned int			num_hdmi_outputs;
+	u8				num_outputs;
+	u8				num_hdmi_outputs;
 	u8				output_type[MAX_OUTPUTS];
 	u8				output_name_counter[MAX_OUTPUTS];
 	bool				has_audio_inputs;
@@ -203,7 +293,20 @@ struct vivid_dev {
 	bool				has_tv_tuner;
 	bool				has_touch_cap;
 
-	bool				can_loop_video;
+	/* Output index (0-MAX_OUTPUTS) to vivid instance of connected input */
+	struct vivid_dev		*output_to_input_instance[MAX_OUTPUTS];
+	/* Output index (0-MAX_OUTPUTS) to input index (0-MAX_INPUTS) of connected input */
+	u8				output_to_input_index[MAX_OUTPUTS];
+	/* Output index (0-MAX_OUTPUTS) to HDMI or S-Video output index (0-MAX_HDMI/SVID_OUTPUTS) */
+	u8				output_to_iface_index[MAX_OUTPUTS];
+	/* ctrl_hdmi_to_output or ctrl_svid_to_output control value for each input */
+	s32                             input_is_connected_to_output[MAX_INPUTS];
+	/* HDMI index (0-MAX_HDMI_OUTPUTS) to output index (0-MAX_OUTPUTS) */
+	u8				hdmi_index_to_output_index[MAX_HDMI_OUTPUTS];
+	/* HDMI index (0-MAX_HDMI_INPUTS) to input index (0-MAX_INPUTS) */
+	u8				hdmi_index_to_input_index[MAX_HDMI_INPUTS];
+	/* S-Video index (0-MAX_SVID_INPUTS) to input index (0-MAX_INPUTS) */
+	u8				svid_index_to_input_index[MAX_SVID_INPUTS];
 
 	/* controls */
 	struct v4l2_ctrl		*brightness;
@@ -242,7 +345,6 @@ struct vivid_dev {
 		struct v4l2_ctrl	*ctrl_dv_timings_signal_mode;
 		struct v4l2_ctrl	*ctrl_dv_timings;
 	};
-	struct v4l2_ctrl		*ctrl_display_present;
 	struct v4l2_ctrl		*ctrl_has_crop_cap;
 	struct v4l2_ctrl		*ctrl_has_compose_cap;
 	struct v4l2_ctrl		*ctrl_has_scaler_cap;
@@ -276,6 +378,11 @@ struct vivid_dev {
 	struct v4l2_ctrl		*radio_rx_rds_psname;
 	struct v4l2_ctrl		*radio_rx_rds_radiotext;
 
+	struct v4l2_ctrl                *ctrl_hdmi_to_output[MAX_HDMI_INPUTS];
+	char				ctrl_hdmi_to_output_names[MAX_HDMI_INPUTS][32];
+	struct v4l2_ctrl		*ctrl_svid_to_output[MAX_SVID_INPUTS];
+	char				ctrl_svid_to_output_names[MAX_SVID_INPUTS][32];
+
 	unsigned			input_brightness[MAX_INPUTS];
 	unsigned			osd_mode;
 	unsigned			button_pressed;
@@ -296,9 +403,11 @@ struct vivid_dev {
 	int				display_byte_stride;
 	int				bits_per_pixel;
 	int				bytes_per_pixel;
+#ifdef CONFIG_VIDEO_VIVID_OSD
 	struct fb_info			fb_info;
 	struct fb_var_screeninfo	fb_defined;
 	struct fb_fix_screeninfo	fb_fix;
+#endif
 
 	/* Error injection */
 	bool				disconnect_error;
@@ -364,7 +473,6 @@ struct vivid_dev {
 	u8				*scaled_line;
 	u8				*blended_line;
 	unsigned			cur_scaled_line;
-	bool				display_present[MAX_OUTPUTS];
 
 	/* Output Overlay */
 	void				*fb_vbase_out;
@@ -377,7 +485,7 @@ struct vivid_dev {
 	/* video capture */
 	struct tpg_data			tpg;
 	unsigned			ms_vid_cap;
-	bool				must_blank[VIDEO_MAX_FRAME];
+	bool				must_blank[MAX_VID_CAP_BUFFERS];
 
 	const struct vivid_fmt		*fmt_cap;
 	struct v4l2_fract		timeperframe_vid_cap;
@@ -544,11 +652,10 @@ struct vivid_dev {
 
 	/* CEC */
 	struct cec_adapter		*cec_rx_adap;
-	struct cec_adapter		*cec_tx_adap[MAX_OUTPUTS];
-	u8				cec_output2bus_map[MAX_OUTPUTS];
+	struct cec_adapter		*cec_tx_adap[MAX_HDMI_OUTPUTS];
 	struct task_struct		*kthread_cec;
 	wait_queue_head_t		kthread_waitq_cec;
-	struct vivid_cec_xfer	xfers[MAX_OUTPUTS];
+	struct vivid_cec_xfer		xfers[MAX_OUTPUTS];
 	spinlock_t			cec_xfers_slock; /* read and write cec messages */
 	u32				cec_sft; /* bus signal free time, in bit periods */
 	u8				last_initiator;
diff --git a/drivers/media/test-drivers/vivid/vivid-ctrls.c b/drivers/media/test-drivers/vivid/vivid-ctrls.c
index f2b20e25a7a4..e340df0b6261 100644
--- a/drivers/media/test-drivers/vivid/vivid-ctrls.c
+++ b/drivers/media/test-drivers/vivid/vivid-ctrls.c
@@ -18,7 +18,6 @@
 #include "vivid-radio-common.h"
 #include "vivid-osd.h"
 #include "vivid-ctrls.h"
-#include "vivid-cec.h"
 
 #define VIVID_CID_CUSTOM_BASE		(V4L2_CID_USER_BASE | 0xf000)
 #define VIVID_CID_BUTTON		(VIVID_CID_CUSTOM_BASE + 0)
@@ -38,6 +37,7 @@
 #define VIVID_CID_U8_PIXEL_ARRAY	(VIVID_CID_CUSTOM_BASE + 14)
 #define VIVID_CID_S32_ARRAY		(VIVID_CID_CUSTOM_BASE + 15)
 #define VIVID_CID_S64_ARRAY		(VIVID_CID_CUSTOM_BASE + 16)
+#define VIVID_CID_RECT			(VIVID_CID_CUSTOM_BASE + 17)
 
 #define VIVID_CID_VIVID_BASE		(0x00f00000 | 0xf000)
 #define VIVID_CID_VIVID_CLASS		(0x00f00000 | 1)
@@ -69,14 +69,12 @@
 #define VIVID_CID_HAS_CROP_OUT		(VIVID_CID_VIVID_BASE + 34)
 #define VIVID_CID_HAS_COMPOSE_OUT	(VIVID_CID_VIVID_BASE + 35)
 #define VIVID_CID_HAS_SCALER_OUT	(VIVID_CID_VIVID_BASE + 36)
-#define VIVID_CID_LOOP_VIDEO		(VIVID_CID_VIVID_BASE + 37)
 #define VIVID_CID_SEQ_WRAP		(VIVID_CID_VIVID_BASE + 38)
 #define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 39)
 #define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 40)
 #define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 41)
 #define VIVID_CID_REDUCED_FPS		(VIVID_CID_VIVID_BASE + 42)
 #define VIVID_CID_HSV_ENC		(VIVID_CID_VIVID_BASE + 43)
-#define VIVID_CID_DISPLAY_PRESENT	(VIVID_CID_VIVID_BASE + 44)
 
 #define VIVID_CID_STD_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 60)
 #define VIVID_CID_STANDARD		(VIVID_CID_VIVID_BASE + 61)
@@ -104,6 +102,12 @@
 #define VIVID_CID_META_CAP_GENERATE_PTS	(VIVID_CID_VIVID_BASE + 111)
 #define VIVID_CID_META_CAP_GENERATE_SCR	(VIVID_CID_VIVID_BASE + 112)
 
+/* HDMI inputs are in the range 0-14. The next available CID is VIVID_CID_VIVID_BASE + 128 */
+#define VIVID_CID_HDMI_IS_CONNECTED_TO_OUTPUT(input) (VIVID_CID_VIVID_BASE + 113 + (input))
+
+/* S-Video inputs are in the range 0-15. The next available CID is VIVID_CID_VIVID_BASE + 144 */
+#define VIVID_CID_SVID_IS_CONNECTED_TO_OUTPUT(input) (VIVID_CID_VIVID_BASE + 128 + (input))
+
 /* General User Controls */
 
 static void vivid_unregister_dev(bool valid, struct video_device *vdev)
@@ -357,6 +361,38 @@ static const struct v4l2_ctrl_config vivid_ctrl_ro_int32 = {
 	.step = 1,
 };
 
+static const struct v4l2_rect rect_def = {
+	.top = 100,
+	.left = 200,
+	.width = 300,
+	.height = 400,
+};
+
+static const struct v4l2_rect rect_min = {
+	.top = 0,
+	.left = 0,
+	.width = 1,
+	.height = 1,
+};
+
+static const struct v4l2_rect rect_max = {
+	.top = 0,
+	.left = 0,
+	.width = 1000,
+	.height = 2000,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_rect = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_RECT,
+	.name = "Rect",
+	.type = V4L2_CTRL_TYPE_RECT,
+	.flags = V4L2_CTRL_FLAG_HAS_WHICH_MIN_MAX,
+	.p_def.p_const = &rect_def,
+	.p_min.p_const = &rect_min,
+	.p_max.p_const = &rect_max,
+};
+
 /* Framebuffer Controls */
 
 static int vivid_fb_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -366,7 +402,7 @@ static int vivid_fb_s_ctrl(struct v4l2_ctrl *ctrl)
 
 	switch (ctrl->id) {
 	case VIVID_CID_CLEAR_FB:
-		vivid_clear_fb(dev);
+		vivid_fb_clear(dev);
 		break;
 	}
 	return 0;
@@ -439,6 +475,33 @@ static const struct v4l2_ctrl_ops vivid_user_vid_ctrl_ops = {
 
 /* Video Capture Controls */
 
+static void vivid_update_power_present(struct vivid_dev *dev)
+{
+	unsigned int i, j;
+
+	dev->power_present = 0;
+	for (i = 0, j = 0;
+	     i < ARRAY_SIZE(dev->dv_timings_signal_mode); i++) {
+		if (dev->input_type[i] != HDMI)
+			continue;
+		/*
+		 * If connected to TPG or HDMI output, and the signal
+		 * mode is not NO_SIGNAL, then there is power present.
+		 */
+		if (dev->input_is_connected_to_output[i] != 1 &&
+		    dev->dv_timings_signal_mode[i] != NO_SIGNAL)
+			dev->power_present |= (1 << j);
+		j++;
+	}
+
+	__v4l2_ctrl_s_ctrl(dev->ctrl_rx_power_present,
+			   dev->power_present);
+
+	v4l2_ctrl_activate(dev->ctrl_dv_timings,
+			   dev->dv_timings_signal_mode[dev->input] ==
+			   SELECTED_DV_TIMINGS);
+}
+
 static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	static const u32 colorspaces[] = {
@@ -453,7 +516,11 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		V4L2_COLORSPACE_470_SYSTEM_BG,
 	};
 	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_cap);
-	unsigned int i, j;
+	unsigned int i;
+	struct vivid_dev *output_inst = NULL;
+	int index = 0;
+	int hdmi_index, svid_index;
+	s32 input_index = 0;
 
 	switch (ctrl->id) {
 	case VIVID_CID_TEST_PATTERN:
@@ -519,7 +586,7 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		break;
 	case VIVID_CID_PERCENTAGE_FILL:
 		tpg_s_perc_fill(&dev->tpg, ctrl->val);
-		for (i = 0; i < VIDEO_MAX_FRAME; i++)
+		for (i = 0; i < MAX_VID_CAP_BUFFERS; i++)
 			dev->must_blank[i] = ctrl->val < 100;
 		break;
 	case VIVID_CID_INSERT_SAV:
@@ -569,25 +636,9 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		dev->dv_timings_signal_mode[dev->input] =
 			dev->ctrl_dv_timings_signal_mode->val;
 		dev->query_dv_timings[dev->input] = dev->ctrl_dv_timings->val;
-
-		dev->power_present = 0;
-		for (i = 0, j = 0;
-		     i < ARRAY_SIZE(dev->dv_timings_signal_mode);
-		     i++)
-			if (dev->input_type[i] == HDMI) {
-				if (dev->dv_timings_signal_mode[i] != NO_SIGNAL)
-					dev->power_present |= (1 << j);
-				j++;
-			}
-		__v4l2_ctrl_s_ctrl(dev->ctrl_rx_power_present,
-				   dev->power_present);
-
-		v4l2_ctrl_activate(dev->ctrl_dv_timings,
-			dev->dv_timings_signal_mode[dev->input] ==
-				SELECTED_DV_TIMINGS);
-
+		vivid_update_power_present(dev);
 		vivid_update_quality(dev);
-		vivid_send_source_change(dev, HDMI);
+		vivid_send_input_source_change(dev, dev->input);
 		break;
 	case VIVID_CID_DV_TIMINGS_ASPECT_RATIO:
 		dev->dv_timings_aspect_ratio[dev->input] = ctrl->val;
@@ -604,6 +655,67 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		if (dev->edid_blocks > dev->edid_max_blocks)
 			dev->edid_blocks = dev->edid_max_blocks;
 		break;
+	case VIVID_CID_HDMI_IS_CONNECTED_TO_OUTPUT(0) ... VIVID_CID_HDMI_IS_CONNECTED_TO_OUTPUT(14):
+		hdmi_index = ctrl->id - VIVID_CID_HDMI_IS_CONNECTED_TO_OUTPUT(0);
+		output_inst = vivid_ctrl_hdmi_to_output_instance[ctrl->cur.val];
+		index = vivid_ctrl_hdmi_to_output_index[ctrl->cur.val];
+		input_index = dev->hdmi_index_to_input_index[hdmi_index];
+		dev->input_is_connected_to_output[input_index] = ctrl->val;
+
+		if (output_inst) {
+			output_inst->output_to_input_instance[index] = NULL;
+			vivid_update_outputs(output_inst);
+			cec_phys_addr_invalidate(output_inst->cec_tx_adap[index]);
+		}
+		if (ctrl->val >= FIXED_MENU_ITEMS) {
+			output_inst = vivid_ctrl_hdmi_to_output_instance[ctrl->val];
+			index = vivid_ctrl_hdmi_to_output_index[ctrl->val];
+			output_inst->output_to_input_instance[index] = dev;
+			output_inst->output_to_input_index[index] =
+				dev->hdmi_index_to_input_index[hdmi_index];
+		}
+		spin_lock(&hdmi_output_skip_mask_lock);
+		hdmi_to_output_menu_skip_mask &= ~(1ULL << ctrl->cur.val);
+		if (ctrl->val >= FIXED_MENU_ITEMS)
+			hdmi_to_output_menu_skip_mask |= 1ULL << ctrl->val;
+		spin_unlock(&hdmi_output_skip_mask_lock);
+		vivid_update_power_present(dev);
+		vivid_update_quality(dev);
+		vivid_send_input_source_change(dev, dev->hdmi_index_to_input_index[hdmi_index]);
+		if (ctrl->val < FIXED_MENU_ITEMS && ctrl->cur.val < FIXED_MENU_ITEMS)
+			break;
+		spin_lock(&hdmi_output_skip_mask_lock);
+		hdmi_input_update_outputs_mask |= 1 << dev->inst;
+		spin_unlock(&hdmi_output_skip_mask_lock);
+		queue_work(update_hdmi_ctrls_workqueue, &dev->update_hdmi_ctrl_work);
+		break;
+	case VIVID_CID_SVID_IS_CONNECTED_TO_OUTPUT(0) ... VIVID_CID_SVID_IS_CONNECTED_TO_OUTPUT(15):
+		svid_index = ctrl->id - VIVID_CID_SVID_IS_CONNECTED_TO_OUTPUT(0);
+		output_inst = vivid_ctrl_svid_to_output_instance[ctrl->cur.val];
+		index = vivid_ctrl_svid_to_output_index[ctrl->cur.val];
+		input_index = dev->svid_index_to_input_index[svid_index];
+		dev->input_is_connected_to_output[input_index] = ctrl->val;
+
+		if (output_inst)
+			output_inst->output_to_input_instance[index] = NULL;
+		if (ctrl->val >= FIXED_MENU_ITEMS) {
+			output_inst = vivid_ctrl_svid_to_output_instance[ctrl->val];
+			index = vivid_ctrl_svid_to_output_index[ctrl->val];
+			output_inst->output_to_input_instance[index] = dev;
+			output_inst->output_to_input_index[index] =
+				dev->svid_index_to_input_index[svid_index];
+		}
+		spin_lock(&svid_output_skip_mask_lock);
+		svid_to_output_menu_skip_mask &= ~(1ULL << ctrl->cur.val);
+		if (ctrl->val >= FIXED_MENU_ITEMS)
+			svid_to_output_menu_skip_mask |= 1ULL << ctrl->val;
+		spin_unlock(&svid_output_skip_mask_lock);
+		vivid_update_quality(dev);
+		vivid_send_input_source_change(dev, dev->svid_index_to_input_index[svid_index]);
+		if (ctrl->val < FIXED_MENU_ITEMS && ctrl->cur.val < FIXED_MENU_ITEMS)
+			break;
+		queue_work(update_svid_ctrls_workqueue, &dev->update_svid_ctrl_work);
+		break;
 	}
 	return 0;
 }
@@ -966,37 +1078,6 @@ static const struct v4l2_ctrl_config vivid_ctrl_limited_rgb_range = {
 };
 
 
-/* Video Loop Control */
-
-static int vivid_loop_cap_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_cap);
-
-	switch (ctrl->id) {
-	case VIVID_CID_LOOP_VIDEO:
-		dev->loop_video = ctrl->val;
-		vivid_update_quality(dev);
-		vivid_send_source_change(dev, SVID);
-		vivid_send_source_change(dev, HDMI);
-		break;
-	}
-	return 0;
-}
-
-static const struct v4l2_ctrl_ops vivid_loop_cap_ctrl_ops = {
-	.s_ctrl = vivid_loop_cap_s_ctrl,
-};
-
-static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
-	.ops = &vivid_loop_cap_ctrl_ops,
-	.id = VIVID_CID_LOOP_VIDEO,
-	.name = "Loop Video",
-	.type = V4L2_CTRL_TYPE_BOOLEAN,
-	.max = 1,
-	.step = 1,
-};
-
-
 /* VBI Capture Control */
 
 static int vivid_vbi_cap_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -1031,8 +1112,6 @@ static int vivid_vid_out_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_out);
 	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
-	u32 display_present = 0;
-	unsigned int i, j, bus_idx;
 
 	switch (ctrl->id) {
 	case VIVID_CID_HAS_CROP_OUT:
@@ -1063,39 +1142,11 @@ static int vivid_vid_out_s_ctrl(struct v4l2_ctrl *ctrl)
 					V4L2_QUANTIZATION_LIM_RANGE :
 					V4L2_QUANTIZATION_DEFAULT;
 		}
-		if (dev->loop_video)
-			vivid_send_source_change(dev, HDMI);
-		break;
-	case VIVID_CID_DISPLAY_PRESENT:
-		if (dev->output_type[dev->output] != HDMI)
-			break;
-
-		dev->display_present[dev->output] = ctrl->val;
-		for (i = 0, j = 0; i < dev->num_outputs; i++)
-			if (dev->output_type[i] == HDMI)
-				display_present |=
-					dev->display_present[i] << j++;
+		if (vivid_output_is_connected_to(dev)) {
+			struct vivid_dev *dev_rx = vivid_output_is_connected_to(dev);
 
-		__v4l2_ctrl_s_ctrl(dev->ctrl_tx_rxsense, display_present);
-
-		if (dev->edid_blocks) {
-			__v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present,
-					   display_present);
-			__v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug,
-					   display_present);
+			vivid_send_source_change(dev_rx, HDMI);
 		}
-
-		bus_idx = dev->cec_output2bus_map[dev->output];
-		if (!dev->cec_tx_adap[bus_idx])
-			break;
-
-		if (ctrl->val && dev->edid_blocks)
-			cec_s_phys_addr(dev->cec_tx_adap[bus_idx],
-					dev->cec_tx_adap[bus_idx]->phys_addr,
-					false);
-		else
-			cec_phys_addr_invalidate(dev->cec_tx_adap[bus_idx]);
-
 		break;
 	}
 	return 0;
@@ -1135,16 +1186,6 @@ static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_out = {
 	.step = 1,
 };
 
-static const struct v4l2_ctrl_config vivid_ctrl_display_present = {
-	.ops = &vivid_vid_out_ctrl_ops,
-	.id = VIVID_CID_DISPLAY_PRESENT,
-	.name = "Display Present",
-	.type = V4L2_CTRL_TYPE_BOOLEAN,
-	.max = 1,
-	.def = 1,
-	.step = 1,
-};
-
 /* Streaming Controls */
 
 static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
@@ -1677,6 +1718,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	dev->int_menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int_menu, NULL);
 	dev->ro_int32 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_ro_int32, NULL);
 	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_area, NULL);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_rect, NULL);
 	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u32_array, NULL);
 	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u32_dyn_array, NULL);
 	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u16_matrix, NULL);
@@ -1710,6 +1752,48 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_eav, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_hdmi_video_guard_band, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_reduced_fps, NULL);
+
+		WARN_ON(dev->num_hdmi_inputs > MAX_HDMI_INPUTS);
+		WARN_ON(dev->num_svid_inputs > MAX_SVID_INPUTS);
+
+		for (u8 i = 0; i < dev->num_hdmi_inputs; i++) {
+			snprintf(dev->ctrl_hdmi_to_output_names[i],
+				 sizeof(dev->ctrl_hdmi_to_output_names[i]),
+				 "HDMI %03u-%u Is Connected To", dev->inst, i);
+		}
+
+		for (u8 i = 0; i < dev->num_hdmi_inputs; i++) {
+			struct v4l2_ctrl_config ctrl_config = {
+				.ops = &vivid_vid_cap_ctrl_ops,
+				.id = VIVID_CID_HDMI_IS_CONNECTED_TO_OUTPUT(i),
+				.name = dev->ctrl_hdmi_to_output_names[i],
+				.type = V4L2_CTRL_TYPE_MENU,
+				.max = 1,
+				.qmenu = (const char * const *)vivid_ctrl_hdmi_to_output_strings,
+			};
+			dev->ctrl_hdmi_to_output[i] = v4l2_ctrl_new_custom(hdl_vid_cap,
+									   &ctrl_config, NULL);
+		}
+
+		for (u8 i = 0; i < dev->num_svid_inputs; i++) {
+			snprintf(dev->ctrl_svid_to_output_names[i],
+				 sizeof(dev->ctrl_svid_to_output_names[i]),
+				 "S-Video %03u-%u Is Connected To", dev->inst, i);
+		}
+
+		for (u8 i = 0; i < dev->num_svid_inputs; i++) {
+			struct v4l2_ctrl_config ctrl_config = {
+				.ops = &vivid_vid_cap_ctrl_ops,
+				.id = VIVID_CID_SVID_IS_CONNECTED_TO_OUTPUT(i),
+				.name = dev->ctrl_svid_to_output_names[i],
+				.type = V4L2_CTRL_TYPE_MENU,
+				.max = 1,
+				.qmenu = (const char * const *)vivid_ctrl_svid_to_output_strings,
+			};
+			dev->ctrl_svid_to_output[i] = v4l2_ctrl_new_custom(hdl_vid_cap,
+									   &ctrl_config, NULL);
+		}
+
 		if (show_ccs_cap) {
 			dev->ctrl_has_crop_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
 				&vivid_ctrl_has_crop_cap, NULL);
@@ -1812,21 +1896,13 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		dev->ctrl_tx_mode = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
 			V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
 			0, V4L2_DV_TX_MODE_HDMI);
-		dev->ctrl_display_present = v4l2_ctrl_new_custom(hdl_vid_out,
-			&vivid_ctrl_display_present, NULL);
-		dev->ctrl_tx_hotplug = v4l2_ctrl_new_std(hdl_vid_out,
-			NULL, V4L2_CID_DV_TX_HOTPLUG, 0, hdmi_output_mask,
-			0, hdmi_output_mask);
-		dev->ctrl_tx_rxsense = v4l2_ctrl_new_std(hdl_vid_out,
-			NULL, V4L2_CID_DV_TX_RXSENSE, 0, hdmi_output_mask,
-			0, hdmi_output_mask);
-		dev->ctrl_tx_edid_present = v4l2_ctrl_new_std(hdl_vid_out,
-			NULL, V4L2_CID_DV_TX_EDID_PRESENT, 0, hdmi_output_mask,
-			0, hdmi_output_mask);
+		dev->ctrl_tx_hotplug = v4l2_ctrl_new_std(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_HOTPLUG, 0, hdmi_output_mask, 0, 0);
+		dev->ctrl_tx_rxsense = v4l2_ctrl_new_std(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_RXSENSE, 0, hdmi_output_mask, 0, 0);
+		dev->ctrl_tx_edid_present = v4l2_ctrl_new_std(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_EDID_PRESENT, 0, hdmi_output_mask, 0, 0);
 	}
-	if ((dev->has_vid_cap && dev->has_vid_out) ||
-	    (dev->has_vbi_cap && dev->has_vbi_out))
-		v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_loop_video, NULL);
 
 	if (dev->has_fb)
 		v4l2_ctrl_new_custom(hdl_fb, &vivid_ctrl_clear_fb, NULL);
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-cap.c b/drivers/media/test-drivers/vivid/vivid-kthread-cap.c
index 42048727d7ff..d845e1644649 100644
--- a/drivers/media/test-drivers/vivid/vivid-kthread-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-cap.c
@@ -142,7 +142,7 @@ static void scale_line(const u8 *src, u8 *dst, unsigned srcw, unsigned dstw, uns
  * (loop_vid_overlay). Finally calculate the part of the capture buffer that
  * will receive that overlaid video.
  */
-static void vivid_precalc_copy_rects(struct vivid_dev *dev)
+static void vivid_precalc_copy_rects(struct vivid_dev *dev, struct vivid_dev *out_dev)
 {
 	/* Framebuffer rectangle */
 	struct v4l2_rect r_fb = {
@@ -150,53 +150,53 @@ static void vivid_precalc_copy_rects(struct vivid_dev *dev)
 	};
 	/* Overlay window rectangle in framebuffer coordinates */
 	struct v4l2_rect r_overlay = {
-		dev->overlay_out_left, dev->overlay_out_top,
-		dev->compose_out.width, dev->compose_out.height
+		out_dev->overlay_out_left, out_dev->overlay_out_top,
+		out_dev->compose_out.width, out_dev->compose_out.height
 	};
 
-	v4l2_rect_intersect(&dev->loop_vid_copy, &dev->crop_cap, &dev->compose_out);
+	v4l2_rect_intersect(&dev->loop_vid_copy, &dev->crop_cap, &out_dev->compose_out);
 
 	dev->loop_vid_out = dev->loop_vid_copy;
-	v4l2_rect_scale(&dev->loop_vid_out, &dev->compose_out, &dev->crop_out);
-	dev->loop_vid_out.left += dev->crop_out.left;
-	dev->loop_vid_out.top += dev->crop_out.top;
+	v4l2_rect_scale(&dev->loop_vid_out, &out_dev->compose_out, &out_dev->crop_out);
+	dev->loop_vid_out.left += out_dev->crop_out.left;
+	dev->loop_vid_out.top += out_dev->crop_out.top;
 
 	dev->loop_vid_cap = dev->loop_vid_copy;
 	v4l2_rect_scale(&dev->loop_vid_cap, &dev->crop_cap, &dev->compose_cap);
 
 	dprintk(dev, 1,
-		"loop_vid_copy: %dx%d@%dx%d loop_vid_out: %dx%d@%dx%d loop_vid_cap: %dx%d@%dx%d\n",
-		dev->loop_vid_copy.width, dev->loop_vid_copy.height,
+		"loop_vid_copy: (%d,%d)/%ux%u loop_vid_out: (%d,%d)/%ux%u loop_vid_cap: (%d,%d)/%ux%u\n",
 		dev->loop_vid_copy.left, dev->loop_vid_copy.top,
-		dev->loop_vid_out.width, dev->loop_vid_out.height,
+		dev->loop_vid_copy.width, dev->loop_vid_copy.height,
 		dev->loop_vid_out.left, dev->loop_vid_out.top,
-		dev->loop_vid_cap.width, dev->loop_vid_cap.height,
-		dev->loop_vid_cap.left, dev->loop_vid_cap.top);
+		dev->loop_vid_out.width, dev->loop_vid_out.height,
+		dev->loop_vid_cap.left, dev->loop_vid_cap.top,
+		dev->loop_vid_cap.width, dev->loop_vid_cap.height);
 
 	v4l2_rect_intersect(&r_overlay, &r_fb, &r_overlay);
 
 	/* shift r_overlay to the same origin as compose_out */
-	r_overlay.left += dev->compose_out.left - dev->overlay_out_left;
-	r_overlay.top += dev->compose_out.top - dev->overlay_out_top;
+	r_overlay.left += out_dev->compose_out.left - out_dev->overlay_out_left;
+	r_overlay.top += out_dev->compose_out.top - out_dev->overlay_out_top;
 
 	v4l2_rect_intersect(&dev->loop_vid_overlay, &r_overlay, &dev->loop_vid_copy);
 	dev->loop_fb_copy = dev->loop_vid_overlay;
 
 	/* shift dev->loop_fb_copy back again to the fb origin */
-	dev->loop_fb_copy.left -= dev->compose_out.left - dev->overlay_out_left;
-	dev->loop_fb_copy.top -= dev->compose_out.top - dev->overlay_out_top;
+	dev->loop_fb_copy.left -= out_dev->compose_out.left - out_dev->overlay_out_left;
+	dev->loop_fb_copy.top -= out_dev->compose_out.top - out_dev->overlay_out_top;
 
 	dev->loop_vid_overlay_cap = dev->loop_vid_overlay;
 	v4l2_rect_scale(&dev->loop_vid_overlay_cap, &dev->crop_cap, &dev->compose_cap);
 
 	dprintk(dev, 1,
-		"loop_fb_copy: %dx%d@%dx%d loop_vid_overlay: %dx%d@%dx%d loop_vid_overlay_cap: %dx%d@%dx%d\n",
-		dev->loop_fb_copy.width, dev->loop_fb_copy.height,
+		"loop_fb_copy: (%d,%d)/%ux%u loop_vid_overlay: (%d,%d)/%ux%u loop_vid_overlay_cap: (%d,%d)/%ux%u\n",
 		dev->loop_fb_copy.left, dev->loop_fb_copy.top,
-		dev->loop_vid_overlay.width, dev->loop_vid_overlay.height,
+		dev->loop_fb_copy.width, dev->loop_fb_copy.height,
 		dev->loop_vid_overlay.left, dev->loop_vid_overlay.top,
-		dev->loop_vid_overlay_cap.width, dev->loop_vid_overlay_cap.height,
-		dev->loop_vid_overlay_cap.left, dev->loop_vid_overlay_cap.top);
+		dev->loop_vid_overlay.width, dev->loop_vid_overlay.height,
+		dev->loop_vid_overlay_cap.left, dev->loop_vid_overlay_cap.top,
+		dev->loop_vid_overlay_cap.width, dev->loop_vid_overlay_cap.height);
 }
 
 static void *plane_vaddr(struct tpg_data *tpg, struct vivid_buffer *buf,
@@ -213,24 +213,25 @@ static void *plane_vaddr(struct tpg_data *tpg, struct vivid_buffer *buf,
 	return vbuf;
 }
 
-static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev, unsigned p,
-		u8 *vcapbuf, struct vivid_buffer *vid_cap_buf)
+static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev,
+						struct vivid_dev *out_dev, unsigned p,
+						u8 *vcapbuf, struct vivid_buffer *vid_cap_buf)
 {
 	bool blank = dev->must_blank[vid_cap_buf->vb.vb2_buf.index];
 	struct tpg_data *tpg = &dev->tpg;
 	struct vivid_buffer *vid_out_buf = NULL;
-	unsigned vdiv = dev->fmt_out->vdownsampling[p];
+	unsigned vdiv = out_dev->fmt_out->vdownsampling[p];
 	unsigned twopixsize = tpg_g_twopixelsize(tpg, p);
 	unsigned img_width = tpg_hdiv(tpg, p, dev->compose_cap.width);
 	unsigned img_height = dev->compose_cap.height;
 	unsigned stride_cap = tpg->bytesperline[p];
-	unsigned stride_out = dev->bytesperline_out[p];
+	unsigned stride_out = out_dev->bytesperline_out[p];
 	unsigned stride_osd = dev->display_byte_stride;
 	unsigned hmax = (img_height * tpg->perc_fill) / 100;
 	u8 *voutbuf;
 	u8 *vosdbuf = NULL;
 	unsigned y;
-	bool blend = dev->fbuf_out_flags;
+	bool blend = out_dev->fbuf_out_flags;
 	/* Coarse scaling with Bresenham */
 	unsigned vid_out_int_part;
 	unsigned vid_out_fract_part;
@@ -247,8 +248,8 @@ static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev, unsigned
 	vid_out_int_part = dev->loop_vid_out.height / dev->loop_vid_cap.height;
 	vid_out_fract_part = dev->loop_vid_out.height % dev->loop_vid_cap.height;
 
-	if (!list_empty(&dev->vid_out_active))
-		vid_out_buf = list_entry(dev->vid_out_active.next,
+	if (!list_empty(&out_dev->vid_out_active))
+		vid_out_buf = list_entry(out_dev->vid_out_active.next,
 					 struct vivid_buffer, list);
 	if (vid_out_buf == NULL)
 		return -ENODATA;
@@ -256,8 +257,8 @@ static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev, unsigned
 	vid_cap_buf->vb.field = vid_out_buf->vb.field;
 
 	voutbuf = plane_vaddr(tpg, vid_out_buf, p,
-			      dev->bytesperline_out, dev->fmt_out_rect.height);
-	if (p < dev->fmt_out->buffers)
+			      out_dev->bytesperline_out, out_dev->fmt_out_rect.height);
+	if (p < out_dev->fmt_out->buffers)
 		voutbuf += vid_out_buf->vb.vb2_buf.planes[p].data_offset;
 	voutbuf += tpg_hdiv(tpg, p, dev->loop_vid_out.left) +
 		(dev->loop_vid_out.top / vdiv) * stride_out;
@@ -274,7 +275,7 @@ static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev, unsigned
 		return 0;
 	}
 
-	if (dev->overlay_out_enabled &&
+	if (out_dev->overlay_out_enabled &&
 	    dev->loop_vid_overlay.width && dev->loop_vid_overlay.height) {
 		vosdbuf = dev->video_vbase;
 		vosdbuf += (dev->loop_fb_copy.left * twopixsize) / 2 +
@@ -385,6 +386,7 @@ update_vid_out_y:
 
 static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 {
+	struct vivid_dev *out_dev = NULL;
 	struct tpg_data *tpg = &dev->tpg;
 	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
 	unsigned line_height = 16 / factor;
@@ -396,14 +398,6 @@ static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 	unsigned ms;
 	char str[100];
 	s32 gain;
-	bool is_loop = false;
-
-	if (dev->loop_video && dev->can_loop_video &&
-		((vivid_is_svid_cap(dev) &&
-		!VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input])) ||
-		(vivid_is_hdmi_cap(dev) &&
-		!VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode[dev->input]))))
-		is_loop = true;
 
 	buf->vb.sequence = dev->vid_cap_seq_count;
 	v4l2_ctrl_s_ctrl(dev->ro_int32, buf->vb.sequence & 0xff);
@@ -428,7 +422,34 @@ static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 		    dev->field_cap == V4L2_FIELD_ALTERNATE);
 	tpg_s_perc_fill_blank(tpg, dev->must_blank[buf->vb.vb2_buf.index]);
 
-	vivid_precalc_copy_rects(dev);
+	if (vivid_vid_can_loop(dev) &&
+	    ((vivid_is_svid_cap(dev) &&
+	    !VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input])) ||
+	    (vivid_is_hdmi_cap(dev) &&
+	    !VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode[dev->input])))) {
+		out_dev = vivid_input_is_connected_to(dev);
+		/*
+		 * If the vivid instance of the output device is different
+		 * from the vivid instance of this input device, then we
+		 * must take care to properly serialize the output device to
+		 * prevent that the buffer we are copying from is being freed.
+		 *
+		 * If the output device is part of the same instance, then the
+		 * lock is already taken and there is no need to take the mutex.
+		 *
+		 * The problem with taking the mutex is that you can get
+		 * deadlocked if instance A locks instance B and vice versa.
+		 * It is not really worth trying to be very smart about this,
+		 * so just try to take the lock, and if you can't, then just
+		 * set out_dev to NULL and you will end up with a single frame
+		 * of Noise (the default test pattern in this case).
+		 */
+		if (out_dev && dev != out_dev && !mutex_trylock(&out_dev->mutex))
+			out_dev = NULL;
+	}
+
+	if (out_dev)
+		vivid_precalc_copy_rects(dev, out_dev);
 
 	for (p = 0; p < tpg_g_planes(tpg); p++) {
 		void *vbuf = plane_vaddr(tpg, buf, p,
@@ -445,10 +466,13 @@ static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 			vbuf += dev->fmt_cap->data_offset[p];
 		}
 		tpg_calc_text_basep(tpg, basep, p, vbuf);
-		if (!is_loop || vivid_copy_buffer(dev, p, vbuf, buf))
+		if (!out_dev || vivid_copy_buffer(dev, out_dev, p, vbuf, buf))
 			tpg_fill_plane_buffer(tpg, vivid_get_std_cap(dev),
 					p, vbuf);
 	}
+	if (out_dev && dev != out_dev)
+		mutex_unlock(&out_dev->mutex);
+
 	dev->must_blank[buf->vb.vb2_buf.index] = false;
 
 	/* Updates stream time, only update at the start of a new frame. */
@@ -765,9 +789,14 @@ static int vivid_thread_vid_cap(void *data)
 			next_jiffies_since_start = jiffies_since_start;
 
 		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
-		while (time_is_after_jiffies(cur_jiffies + wait_jiffies) &&
-		       !kthread_should_stop())
-			schedule();
+		if (!time_is_after_jiffies(cur_jiffies + wait_jiffies))
+			continue;
+
+		wait_queue_head_t wait;
+
+		init_waitqueue_head(&wait);
+		wait_event_interruptible_timeout(wait, kthread_should_stop(),
+					cur_jiffies + wait_jiffies - jiffies);
 	}
 	dprintk(dev, 1, "Video Capture Thread End\n");
 	return 0;
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-out.c b/drivers/media/test-drivers/vivid/vivid-kthread-out.c
index fac6208b51da..015a7b166a1e 100644
--- a/drivers/media/test-drivers/vivid/vivid-kthread-out.c
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-out.c
@@ -235,9 +235,14 @@ static int vivid_thread_vid_out(void *data)
 			next_jiffies_since_start = jiffies_since_start;
 
 		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
-		while (time_is_after_jiffies(cur_jiffies + wait_jiffies) &&
-		       !kthread_should_stop())
-			schedule();
+		if (!time_is_after_jiffies(cur_jiffies + wait_jiffies))
+			continue;
+
+		wait_queue_head_t wait;
+
+		init_waitqueue_head(&wait);
+		wait_event_interruptible_timeout(wait, kthread_should_stop(),
+					cur_jiffies + wait_jiffies - jiffies);
 	}
 	dprintk(dev, 1, "Video Output Thread End\n");
 	return 0;
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-touch.c b/drivers/media/test-drivers/vivid/vivid-kthread-touch.c
index fa711ee36a3f..c862689786b6 100644
--- a/drivers/media/test-drivers/vivid/vivid-kthread-touch.c
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-touch.c
@@ -135,9 +135,14 @@ static int vivid_thread_touch_cap(void *data)
 			next_jiffies_since_start = jiffies_since_start;
 
 		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
-		while (time_is_after_jiffies(cur_jiffies + wait_jiffies) &&
-		       !kthread_should_stop())
-			schedule();
+		if (!time_is_after_jiffies(cur_jiffies + wait_jiffies))
+			continue;
+
+		wait_queue_head_t wait;
+
+		init_waitqueue_head(&wait);
+		wait_event_interruptible_timeout(wait, kthread_should_stop(),
+					cur_jiffies + wait_jiffies - jiffies);
 	}
 	dprintk(dev, 1, "Touch Capture Thread End\n");
 	return 0;
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-cap.c b/drivers/media/test-drivers/vivid/vivid-meta-cap.c
index 0a718d037e59..c7aaecc0b5a2 100644
--- a/drivers/media/test-drivers/vivid/vivid-meta-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-meta-cap.c
@@ -122,8 +122,6 @@ const struct vb2_ops vivid_meta_cap_qops = {
 	.start_streaming	= meta_cap_start_streaming,
 	.stop_streaming		= meta_cap_stop_streaming,
 	.buf_request_complete	= meta_cap_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 int vidioc_enum_fmt_meta_cap(struct file *file, void  *priv,
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-out.c b/drivers/media/test-drivers/vivid/vivid-meta-out.c
index 4a569a6e58be..55e5e5dec2f2 100644
--- a/drivers/media/test-drivers/vivid/vivid-meta-out.c
+++ b/drivers/media/test-drivers/vivid/vivid-meta-out.c
@@ -18,7 +18,6 @@ static int meta_out_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
 				struct device *alloc_devs[])
 {
 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
-	unsigned int q_num_bufs = vb2_get_num_buffers(vq);
 	unsigned int size =  sizeof(struct vivid_meta_out_buf);
 
 	if (!vivid_is_webcam(dev))
@@ -31,9 +30,6 @@ static int meta_out_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
 		sizes[0] = size;
 	}
 
-	if (q_num_bufs + *nbuffers < 2)
-		*nbuffers = 2 - q_num_bufs;
-
 	*nplanes = 1;
 	return 0;
 }
@@ -126,8 +122,6 @@ const struct vb2_ops vivid_meta_out_qops = {
 	.start_streaming        = meta_out_start_streaming,
 	.stop_streaming         = meta_out_stop_streaming,
 	.buf_request_complete   = meta_out_buf_request_complete,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 int vidioc_enum_fmt_meta_out(struct file *file, void  *priv,
diff --git a/drivers/media/test-drivers/vivid/vivid-osd.c b/drivers/media/test-drivers/vivid/vivid-osd.c
index 5c931b94a7b5..91ad9b314f2e 100644
--- a/drivers/media/test-drivers/vivid/vivid-osd.c
+++ b/drivers/media/test-drivers/vivid/vivid-osd.c
@@ -45,13 +45,18 @@ static const u16 rgb565[16] = {
 	0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000
 };
 
-void vivid_clear_fb(struct vivid_dev *dev)
+unsigned int vivid_fb_green_bits(struct vivid_dev *dev)
+{
+	return dev->fb_defined.green.length;
+}
+
+void vivid_fb_clear(struct vivid_dev *dev)
 {
 	void *p = dev->video_vbase;
 	const u16 *rgb = rgb555;
 	unsigned x, y;
 
-	if (dev->fb_defined.green.length == 6)
+	if (vivid_fb_green_bits(dev) == 6)
 		rgb = rgb565;
 
 	for (y = 0; y < dev->display_height; y++) {
@@ -333,7 +338,7 @@ static int vivid_fb_init_vidmode(struct vivid_dev *dev)
 }
 
 /* Release any memory we've grabbed */
-void vivid_fb_release_buffers(struct vivid_dev *dev)
+static void vivid_fb_release_buffers(struct vivid_dev *dev)
 {
 	if (dev->video_vbase == NULL)
 		return;
@@ -370,7 +375,7 @@ int vivid_fb_init(struct vivid_dev *dev)
 		return ret;
 	}
 
-	vivid_clear_fb(dev);
+	vivid_fb_clear(dev);
 
 	/* Register the framebuffer */
 	if (register_framebuffer(&dev->fb_info) < 0) {
@@ -380,6 +385,17 @@ int vivid_fb_init(struct vivid_dev *dev)
 
 	/* Set the card to the requested mode */
 	vivid_fb_set_par(&dev->fb_info);
+
+	v4l2_info(&dev->v4l2_dev, "Framebuffer device registered as fb%d\n",
+		  dev->fb_info.node);
+
 	return 0;
 
 }
+
+void vivid_fb_deinit(struct vivid_dev *dev)
+{
+	v4l2_info(&dev->v4l2_dev, "unregistering fb%d\n", dev->fb_info.node);
+	unregister_framebuffer(&dev->fb_info);
+	vivid_fb_release_buffers(dev);
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-osd.h b/drivers/media/test-drivers/vivid/vivid-osd.h
index f9ac1af25dd3..c52280ebcd03 100644
--- a/drivers/media/test-drivers/vivid/vivid-osd.h
+++ b/drivers/media/test-drivers/vivid/vivid-osd.h
@@ -8,8 +8,23 @@
 #ifndef _VIVID_OSD_H_
 #define _VIVID_OSD_H_
 
+#ifdef CONFIG_VIDEO_VIVID_OSD
 int vivid_fb_init(struct vivid_dev *dev);
-void vivid_fb_release_buffers(struct vivid_dev *dev);
-void vivid_clear_fb(struct vivid_dev *dev);
+void vivid_fb_deinit(struct vivid_dev *dev);
+void vivid_fb_clear(struct vivid_dev *dev);
+unsigned int vivid_fb_green_bits(struct vivid_dev *dev);
+#else
+static inline int vivid_fb_init(struct vivid_dev *dev)
+{
+	return -ENODEV;
+}
+
+static inline void vivid_fb_deinit(struct vivid_dev *dev) {}
+static inline void vivid_fb_clear(struct vivid_dev *dev) {}
+static inline unsigned int vivid_fb_green_bits(struct vivid_dev *dev)
+{
+	return 5;
+}
+#endif
 
 #endif
diff --git a/drivers/media/test-drivers/vivid/vivid-sdr-cap.c b/drivers/media/test-drivers/vivid/vivid-sdr-cap.c
index a81f26b76988..c633fc2ed664 100644
--- a/drivers/media/test-drivers/vivid/vivid-sdr-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-sdr-cap.c
@@ -206,9 +206,14 @@ static int vivid_thread_sdr_cap(void *data)
 			next_jiffies_since_start = jiffies_since_start;
 
 		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
-		while (time_is_after_jiffies(cur_jiffies + wait_jiffies) &&
-		       !kthread_should_stop())
-			schedule();
+		if (!time_is_after_jiffies(cur_jiffies + wait_jiffies))
+			continue;
+
+		wait_queue_head_t wait;
+
+		init_waitqueue_head(&wait);
+		wait_event_interruptible_timeout(wait, kthread_should_stop(),
+					cur_jiffies + wait_jiffies - jiffies);
 	}
 	dprintk(dev, 1, "SDR Capture Thread End\n");
 	return 0;
@@ -219,8 +224,13 @@ static int sdr_cap_queue_setup(struct vb2_queue *vq,
 		       unsigned sizes[], struct device *alloc_devs[])
 {
 	/* 2 = max 16-bit sample returned */
-	sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2;
+	u32 size = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
+
 	*nplanes = 1;
+	sizes[0] = size;
 	return 0;
 }
 
@@ -332,8 +342,6 @@ const struct vb2_ops vivid_sdr_cap_qops = {
 	.start_streaming	= sdr_cap_start_streaming,
 	.stop_streaming		= sdr_cap_stop_streaming,
 	.buf_request_complete	= sdr_cap_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 int vivid_sdr_enum_freq_bands(struct file *file, void *fh,
diff --git a/drivers/media/test-drivers/vivid/vivid-touch-cap.c b/drivers/media/test-drivers/vivid/vivid-touch-cap.c
index 4b3c6ea0afde..36a781fa17bc 100644
--- a/drivers/media/test-drivers/vivid/vivid-touch-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-touch-cap.c
@@ -13,21 +13,17 @@ static int touch_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
 				 struct device *alloc_devs[])
 {
 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
-	unsigned int q_num_bufs = vb2_get_num_buffers(vq);
 	struct v4l2_pix_format *f = &dev->tch_format;
 	unsigned int size = f->sizeimage;
 
 	if (*nplanes) {
-		if (sizes[0] < size)
+		if (*nplanes != 1)
 			return -EINVAL;
-	} else {
-		sizes[0] = size;
+		return sizes[0] < size ? -EINVAL : 0;
 	}
 
-	if (q_num_bufs + *nbuffers < 2)
-		*nbuffers = 2 - q_num_bufs;
-
 	*nplanes = 1;
+	sizes[0] = size;
 	return 0;
 }
 
@@ -114,8 +110,6 @@ const struct vb2_ops vivid_touch_cap_qops = {
 	.start_streaming	= touch_cap_start_streaming,
 	.stop_streaming		= touch_cap_stop_streaming,
 	.buf_request_complete	= touch_cap_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 int vivid_enum_fmt_tch(struct file *file, void  *priv, struct v4l2_fmtdesc *f)
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-cap.c b/drivers/media/test-drivers/vivid/vivid-vbi-cap.c
index 3840b3a664ac..a09f62c66c33 100644
--- a/drivers/media/test-drivers/vivid/vivid-vbi-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-cap.c
@@ -14,6 +14,7 @@
 #include "vivid-kthread-cap.h"
 #include "vivid-vbi-cap.h"
 #include "vivid-vbi-gen.h"
+#include "vivid-vid-common.h"
 
 static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
 {
@@ -23,7 +24,7 @@ static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
 	vivid_vbi_gen_sliced(vbi_gen, is_60hz, seqnr);
 
 	if (!is_60hz) {
-		if (dev->loop_video) {
+		if (vivid_vid_can_loop(dev)) {
 			if (dev->vbi_out_have_wss) {
 				vbi_gen->data[12].data[0] = dev->vbi_out_wss[0];
 				vbi_gen->data[12].data[1] = dev->vbi_out_wss[1];
@@ -47,7 +48,7 @@ static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
 				break;
 			}
 		}
-	} else if (dev->loop_video && is_60hz) {
+	} else if (vivid_vid_can_loop(dev) && is_60hz) {
 		if (dev->vbi_out_have_cc[0]) {
 			vbi_gen->data[0].data[0] = dev->vbi_out_cc[0][0];
 			vbi_gen->data[0].data[1] = dev->vbi_out_cc[0][1];
@@ -132,6 +133,8 @@ static int vbi_cap_queue_setup(struct vb2_queue *vq,
 	if (!vivid_is_sdtv_cap(dev))
 		return -EINVAL;
 
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	sizes[0] = size;
 
 	*nplanes = 1;
@@ -227,8 +230,6 @@ const struct vb2_ops vivid_vbi_cap_qops = {
 	.start_streaming	= vbi_cap_start_streaming,
 	.stop_streaming		= vbi_cap_stop_streaming,
 	.buf_request_complete	= vbi_cap_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-out.c b/drivers/media/test-drivers/vivid/vivid-vbi-out.c
index 434a10676417..b7a09d2f394e 100644
--- a/drivers/media/test-drivers/vivid/vivid-vbi-out.c
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-out.c
@@ -28,6 +28,8 @@ static int vbi_out_queue_setup(struct vb2_queue *vq,
 	if (!vivid_is_svid_out(dev))
 		return -EINVAL;
 
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	sizes[0] = size;
 
 	*nplanes = 1;
@@ -126,8 +128,6 @@ const struct vb2_ops vivid_vbi_out_qops = {
 	.start_streaming	= vbi_out_start_streaming,
 	.stop_streaming		= vbi_out_stop_streaming,
 	.buf_request_complete	= vbi_out_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-cap.c b/drivers/media/test-drivers/vivid/vivid-vid-cap.c
index 2804975fe278..84e9155b5815 100644
--- a/drivers/media/test-drivers/vivid/vivid-vid-cap.c
+++ b/drivers/media/test-drivers/vivid/vivid-vid-cap.c
@@ -10,6 +10,7 @@
 #include <linux/sched.h>
 #include <linux/vmalloc.h>
 #include <linux/videodev2.h>
+#include <linux/prandom.h>
 #include <linux/v4l2-dv-timings.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-event.h>
@@ -24,16 +25,18 @@
 /* Sizes must be in increasing order */
 static const struct v4l2_frmsize_discrete webcam_sizes[] = {
 	{  320, 180 },
+	{  320, 240 },
 	{  640, 360 },
 	{  640, 480 },
 	{ 1280, 720 },
+	{ 1280, 960 },
+	{ 1600, 1200 },
 	{ 1920, 1080 },
 	{ 3840, 2160 },
 };
 
 /*
- * Intervals must be in increasing order and there must be twice as many
- * elements in this array as there are in webcam_sizes.
+ * Intervals must be in increasing order.
  */
 static const struct v4l2_fract webcam_intervals[] = {
 	{  1, 1 },
@@ -106,8 +109,9 @@ static int vid_cap_queue_setup(struct vb2_queue *vq,
 		if (*nplanes != buffers)
 			return -EINVAL;
 		for (p = 0; p < buffers; p++) {
-			if (sizes[p] < tpg_g_line_width(&dev->tpg, p) * h +
-						dev->fmt_cap->data_offset[p])
+			if (sizes[p] < tpg_g_line_width(&dev->tpg, p) * h /
+					dev->fmt_cap->vdownsampling[p] +
+					dev->fmt_cap->data_offset[p])
 				return -EINVAL;
 		}
 	} else {
@@ -210,12 +214,9 @@ static int vid_cap_start_streaming(struct vb2_queue *vq, unsigned count)
 	unsigned i;
 	int err;
 
-	if (vb2_is_streaming(&dev->vb_vid_out_q))
-		dev->can_loop_video = vivid_vid_can_loop(dev);
-
 	dev->vid_cap_seq_count = 0;
 	dprintk(dev, 1, "%s\n", __func__);
-	for (i = 0; i < VIDEO_MAX_FRAME; i++)
+	for (i = 0; i < MAX_VID_CAP_BUFFERS; i++)
 		dev->must_blank[i] = tpg_g_perc_fill(&dev->tpg) < 100;
 	if (dev->start_streaming_error) {
 		dev->start_streaming_error = false;
@@ -242,7 +243,6 @@ static void vid_cap_stop_streaming(struct vb2_queue *vq)
 
 	dprintk(dev, 1, "%s\n", __func__);
 	vivid_stop_generating_vid_cap(dev, &dev->vid_cap_streaming);
-	dev->can_loop_video = false;
 }
 
 static void vid_cap_buf_request_complete(struct vb2_buffer *vb)
@@ -260,8 +260,6 @@ const struct vb2_ops vivid_vid_cap_qops = {
 	.start_streaming	= vid_cap_start_streaming,
 	.stop_streaming		= vid_cap_stop_streaming,
 	.buf_request_complete	= vid_cap_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /*
@@ -273,7 +271,7 @@ void vivid_update_quality(struct vivid_dev *dev)
 {
 	unsigned freq_modulus;
 
-	if (dev->loop_video && (vivid_is_svid_cap(dev) || vivid_is_hdmi_cap(dev))) {
+	if (dev->input_is_connected_to_output[dev->input]) {
 		/*
 		 * The 'noise' will only be replaced by the actual video
 		 * if the output video matches the input video settings.
@@ -487,35 +485,35 @@ static enum v4l2_field vivid_field_cap(struct vivid_dev *dev, enum v4l2_field fi
 
 static unsigned vivid_colorspace_cap(struct vivid_dev *dev)
 {
-	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+	if (!vivid_input_is_connected_to(dev))
 		return tpg_g_colorspace(&dev->tpg);
 	return dev->colorspace_out;
 }
 
 static unsigned vivid_xfer_func_cap(struct vivid_dev *dev)
 {
-	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+	if (!vivid_input_is_connected_to(dev))
 		return tpg_g_xfer_func(&dev->tpg);
 	return dev->xfer_func_out;
 }
 
 static unsigned vivid_ycbcr_enc_cap(struct vivid_dev *dev)
 {
-	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+	if (!vivid_input_is_connected_to(dev))
 		return tpg_g_ycbcr_enc(&dev->tpg);
 	return dev->ycbcr_enc_out;
 }
 
 static unsigned int vivid_hsv_enc_cap(struct vivid_dev *dev)
 {
-	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+	if (!vivid_input_is_connected_to(dev))
 		return tpg_g_hsv_enc(&dev->tpg);
 	return dev->hsv_enc_out;
 }
 
 static unsigned vivid_quantization_cap(struct vivid_dev *dev)
 {
-	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+	if (!vivid_input_is_connected_to(dev))
 		return tpg_g_quantization(&dev->tpg);
 	return dev->quantization_out;
 }
@@ -950,8 +948,8 @@ int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection
 			if (dev->has_compose_cap) {
 				v4l2_rect_set_min_size(compose, &min_rect);
 				v4l2_rect_set_max_size(compose, &max_rect);
-				v4l2_rect_map_inside(compose, &fmt);
 			}
+			v4l2_rect_map_inside(compose, &fmt);
 			dev->fmt_cap_rect = fmt;
 			tpg_s_buf_height(&dev->tpg, fmt.height);
 		} else if (dev->has_compose_cap) {
@@ -1072,13 +1070,13 @@ int vidioc_enum_input(struct file *file, void *priv,
 	inp->type = V4L2_INPUT_TYPE_CAMERA;
 	switch (dev->input_type[inp->index]) {
 	case WEBCAM:
-		snprintf(inp->name, sizeof(inp->name), "Webcam %u",
-				dev->input_name_counter[inp->index]);
+		snprintf(inp->name, sizeof(inp->name), "Webcam %03u-%u",
+			 dev->inst, dev->input_name_counter[inp->index]);
 		inp->capabilities = 0;
 		break;
 	case TV:
-		snprintf(inp->name, sizeof(inp->name), "TV %u",
-				dev->input_name_counter[inp->index]);
+		snprintf(inp->name, sizeof(inp->name), "TV %03u-%u",
+			 dev->inst, dev->input_name_counter[inp->index]);
 		inp->type = V4L2_INPUT_TYPE_TUNER;
 		inp->std = V4L2_STD_ALL;
 		if (dev->has_audio_inputs)
@@ -1086,16 +1084,16 @@ int vidioc_enum_input(struct file *file, void *priv,
 		inp->capabilities = V4L2_IN_CAP_STD;
 		break;
 	case SVID:
-		snprintf(inp->name, sizeof(inp->name), "S-Video %u",
-				dev->input_name_counter[inp->index]);
+		snprintf(inp->name, sizeof(inp->name), "S-Video %03u-%u",
+			 dev->inst, dev->input_name_counter[inp->index]);
 		inp->std = V4L2_STD_ALL;
 		if (dev->has_audio_inputs)
 			inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
 		inp->capabilities = V4L2_IN_CAP_STD;
 		break;
 	case HDMI:
-		snprintf(inp->name, sizeof(inp->name), "HDMI %u",
-				dev->input_name_counter[inp->index]);
+		snprintf(inp->name, sizeof(inp->name), "HDMI %03u-%u",
+			 dev->inst, dev->input_name_counter[inp->index]);
 		inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
 		if (dev->edid_blocks == 0 ||
 		    dev->dv_timings_signal_mode[dev->input] == NO_SIGNAL)
@@ -1462,12 +1460,19 @@ static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
 	h_freq = (u32)bt->pixelclock / total_h_pixel;
 
 	if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_CVT)) {
+		struct v4l2_dv_timings cvt = {};
+
 		if (v4l2_detect_cvt(total_v_lines, h_freq, bt->vsync, bt->width,
-				    bt->polarities, bt->interlaced, timings))
+				    bt->polarities, bt->interlaced,
+				    &vivid_dv_timings_cap, &cvt) &&
+		    cvt.bt.width == bt->width && cvt.bt.height == bt->height) {
+			*timings = cvt;
 			return true;
+		}
 	}
 
 	if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_GTF)) {
+		struct v4l2_dv_timings gtf = {};
 		struct v4l2_fract aspect_ratio;
 
 		find_aspect_ratio(bt->width, bt->height,
@@ -1475,8 +1480,12 @@ static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
 				  &aspect_ratio.denominator);
 		if (v4l2_detect_gtf(total_v_lines, h_freq, bt->vsync,
 				    bt->polarities, bt->interlaced,
-				    aspect_ratio, timings))
+				    aspect_ratio, &vivid_dv_timings_cap,
+				    &gtf) &&
+		    gtf.bt.width == bt->width && gtf.bt.height == bt->height) {
+			*timings = gtf;
 			return true;
+		}
 	}
 	return false;
 }
@@ -1537,13 +1546,65 @@ int vidioc_query_dv_timings(struct file *file, void *_fh,
 	return 0;
 }
 
+void vivid_update_outputs(struct vivid_dev *dev)
+{
+	u32 edid_present = 0;
+
+	if (!dev || !dev->num_outputs)
+		return;
+	for (unsigned int i = 0, j = 0; i < dev->num_outputs; i++) {
+		if (dev->output_type[i] != HDMI)
+			continue;
+
+		struct vivid_dev *dev_rx = dev->output_to_input_instance[i];
+
+		if (dev_rx && dev_rx->edid_blocks)
+			edid_present |= 1 << j;
+		j++;
+	}
+	v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present, edid_present);
+	v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug, edid_present);
+	v4l2_ctrl_s_ctrl(dev->ctrl_tx_rxsense, edid_present);
+}
+
+void vivid_update_connected_outputs(struct vivid_dev *dev)
+{
+	u16 phys_addr = cec_get_edid_phys_addr(dev->edid, dev->edid_blocks * 128, NULL);
+
+	for (unsigned int i = 0, j = 0; i < dev->num_inputs; i++) {
+		unsigned int menu_idx =
+			dev->input_is_connected_to_output[i];
+
+		if (dev->input_type[i] != HDMI)
+			continue;
+		j++;
+		if (menu_idx < FIXED_MENU_ITEMS)
+			continue;
+
+		struct vivid_dev *dev_tx = vivid_ctrl_hdmi_to_output_instance[menu_idx];
+		unsigned int output = vivid_ctrl_hdmi_to_output_index[menu_idx];
+
+		if (!dev_tx)
+			continue;
+
+		unsigned int hdmi_output = dev_tx->output_to_iface_index[output];
+
+		vivid_update_outputs(dev_tx);
+		if (dev->edid_blocks) {
+			cec_s_phys_addr(dev_tx->cec_tx_adap[hdmi_output],
+					v4l2_phys_addr_for_input(phys_addr, j),
+					false);
+		} else {
+			cec_phys_addr_invalidate(dev_tx->cec_tx_adap[hdmi_output]);
+		}
+	}
+}
+
 int vidioc_s_edid(struct file *file, void *_fh,
 			 struct v4l2_edid *edid)
 {
 	struct vivid_dev *dev = video_drvdata(file);
 	u16 phys_addr;
-	u32 display_present = 0;
-	unsigned int i, j;
 	int ret;
 
 	memset(edid->reserved, 0, sizeof(edid->reserved));
@@ -1552,11 +1613,11 @@ int vidioc_s_edid(struct file *file, void *_fh,
 	if (dev->input_type[edid->pad] != HDMI || edid->start_block)
 		return -EINVAL;
 	if (edid->blocks == 0) {
+		if (vb2_is_busy(&dev->vb_vid_cap_q))
+			return -EBUSY;
 		dev->edid_blocks = 0;
-		v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present, 0);
-		v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug, 0);
-		phys_addr = CEC_PHYS_ADDR_INVALID;
-		goto set_phys_addr;
+		vivid_update_connected_outputs(dev);
+		return 0;
 	}
 	if (edid->blocks > dev->edid_max_blocks) {
 		edid->blocks = dev->edid_max_blocks;
@@ -1573,24 +1634,7 @@ int vidioc_s_edid(struct file *file, void *_fh,
 	dev->edid_blocks = edid->blocks;
 	memcpy(dev->edid, edid->edid, edid->blocks * 128);
 
-	for (i = 0, j = 0; i < dev->num_outputs; i++)
-		if (dev->output_type[i] == HDMI)
-			display_present |=
-				dev->display_present[i] << j++;
-
-	v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present, display_present);
-	v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug, display_present);
-
-set_phys_addr:
-	/* TODO: a proper hotplug detect cycle should be emulated here */
-	cec_s_phys_addr(dev->cec_rx_adap, phys_addr, false);
-
-	for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++)
-		cec_s_phys_addr(dev->cec_tx_adap[i],
-				dev->display_present[i] ?
-				v4l2_phys_addr_for_input(phys_addr, i + 1) :
-				CEC_PHYS_ADDR_INVALID,
-				false);
+	vivid_update_connected_outputs(dev);
 	return 0;
 }
 
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-cap.h b/drivers/media/test-drivers/vivid/vivid-vid-cap.h
index 949768652d38..7a8daf0af2ca 100644
--- a/drivers/media/test-drivers/vivid/vivid-vid-cap.h
+++ b/drivers/media/test-drivers/vivid/vivid-vid-cap.h
@@ -10,6 +10,8 @@
 
 void vivid_update_quality(struct vivid_dev *dev);
 void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls);
+void vivid_update_outputs(struct vivid_dev *dev);
+void vivid_update_connected_outputs(struct vivid_dev *dev);
 enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev);
 
 extern const v4l2_std_id vivid_standard[];
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-common.c b/drivers/media/test-drivers/vivid/vivid-vid-common.c
index 38d788b5cf19..df7678db67fb 100644
--- a/drivers/media/test-drivers/vivid/vivid-vid-common.c
+++ b/drivers/media/test-drivers/vivid/vivid-vid-common.c
@@ -769,14 +769,55 @@ const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat)
 	return NULL;
 }
 
+struct vivid_dev *vivid_output_is_connected_to(struct vivid_dev *dev)
+{
+	struct vivid_dev *input_inst = dev->output_to_input_instance[dev->output];
+
+	if (!input_inst)
+		return NULL;
+	if (input_inst->input != dev->output_to_input_index[dev->output])
+		return NULL;
+	return input_inst;
+}
+
+struct vivid_dev *vivid_input_is_connected_to(struct vivid_dev *dev)
+{
+	s32 connected_output = dev->input_is_connected_to_output[dev->input];
+
+	if (connected_output < FIXED_MENU_ITEMS)
+		return NULL;
+	struct vivid_dev *output_inst = NULL;
+
+	if (vivid_is_hdmi_cap(dev)) {
+		output_inst = vivid_ctrl_hdmi_to_output_instance[connected_output];
+		if (vivid_ctrl_hdmi_to_output_index[connected_output] != output_inst->output)
+			return NULL;
+		return output_inst;
+	} else if (vivid_is_svid_cap(dev)) {
+		output_inst = vivid_ctrl_svid_to_output_instance[connected_output];
+		if (vivid_ctrl_svid_to_output_index[connected_output] != output_inst->output)
+			return NULL;
+		return output_inst;
+	} else {
+		return NULL;
+	}
+	return output_inst;
+}
+
 bool vivid_vid_can_loop(struct vivid_dev *dev)
 {
-	if (dev->src_rect.width != dev->sink_rect.width ||
-	    dev->src_rect.height != dev->sink_rect.height)
+	struct vivid_dev *output_inst = vivid_input_is_connected_to(dev);
+
+	if (!output_inst)
 		return false;
-	if (dev->fmt_cap->fourcc != dev->fmt_out->fourcc)
+	if (!vb2_is_streaming(&output_inst->vb_vid_out_q))
 		return false;
-	if (dev->field_cap != dev->field_out)
+	if (dev->src_rect.width != output_inst->sink_rect.width ||
+	    dev->src_rect.height != output_inst->sink_rect.height)
+		return false;
+	if (dev->fmt_cap->fourcc != output_inst->fmt_out->fourcc)
+		return false;
+	if (dev->field_cap != output_inst->field_out)
 		return false;
 	/*
 	 * While this can be supported, it is just too much work
@@ -785,34 +826,34 @@ bool vivid_vid_can_loop(struct vivid_dev *dev)
 	if (dev->field_cap == V4L2_FIELD_SEQ_TB ||
 	    dev->field_cap == V4L2_FIELD_SEQ_BT)
 		return false;
-	if (vivid_is_svid_cap(dev) && vivid_is_svid_out(dev)) {
-		if (!(dev->std_cap[dev->input] & V4L2_STD_525_60) !=
-		    !(dev->std_out & V4L2_STD_525_60))
-			return false;
+	if (vivid_is_hdmi_cap(dev))
 		return true;
-	}
-	if (vivid_is_hdmi_cap(dev) && vivid_is_hdmi_out(dev))
-		return true;
-	return false;
+	if (!(dev->std_cap[dev->input] & V4L2_STD_525_60) !=
+	    !(output_inst->std_out & V4L2_STD_525_60))
+		return false;
+	return true;
 }
 
-void vivid_send_source_change(struct vivid_dev *dev, unsigned type)
+void vivid_send_input_source_change(struct vivid_dev *dev, unsigned int input_index)
 {
 	struct v4l2_event ev = {
 		.type = V4L2_EVENT_SOURCE_CHANGE,
 		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
 	};
-	unsigned i;
+	ev.id = input_index;
 
-	for (i = 0; i < dev->num_inputs; i++) {
-		ev.id = i;
-		if (dev->input_type[i] == type) {
-			if (video_is_registered(&dev->vid_cap_dev) && dev->has_vid_cap)
-				v4l2_event_queue(&dev->vid_cap_dev, &ev);
-			if (video_is_registered(&dev->vbi_cap_dev) && dev->has_vbi_cap)
-				v4l2_event_queue(&dev->vbi_cap_dev, &ev);
-		}
-	}
+	if (video_is_registered(&dev->vid_cap_dev) && dev->has_vid_cap)
+		v4l2_event_queue(&dev->vid_cap_dev, &ev);
+	if (dev->input_type[input_index] == TV || dev->input_type[input_index] == SVID)
+		if (video_is_registered(&dev->vbi_cap_dev) && dev->has_vbi_cap)
+			v4l2_event_queue(&dev->vbi_cap_dev, &ev);
+}
+
+void vivid_send_source_change(struct vivid_dev *dev, unsigned int type)
+{
+	for (int i = 0; i < dev->num_inputs; i++)
+		if (dev->input_type[i] == type)
+			vivid_send_input_source_change(dev, i);
 }
 
 /*
@@ -1036,8 +1077,10 @@ int vidioc_g_edid(struct file *file, void *_fh,
 			 struct v4l2_edid *edid)
 {
 	struct vivid_dev *dev = video_drvdata(file);
+	struct vivid_dev *dev_rx = dev;
 	struct video_device *vdev = video_devdata(file);
 	struct cec_adapter *adap;
+	unsigned int loc;
 
 	memset(edid->reserved, 0, sizeof(edid->reserved));
 	if (vdev->vfl_dir == VFL_DIR_RX) {
@@ -1047,29 +1090,48 @@ int vidioc_g_edid(struct file *file, void *_fh,
 			return -EINVAL;
 		adap = dev->cec_rx_adap;
 	} else {
-		unsigned int bus_idx;
-
 		if (edid->pad >= dev->num_outputs)
 			return -EINVAL;
 		if (dev->output_type[edid->pad] != HDMI)
 			return -EINVAL;
-		if (!dev->display_present[edid->pad])
+		dev_rx = dev->output_to_input_instance[edid->pad];
+		if (!dev_rx)
 			return -ENODATA;
-		bus_idx = dev->cec_output2bus_map[edid->pad];
-		adap = dev->cec_tx_adap[bus_idx];
+
+		unsigned int hdmi_output = dev->output_to_iface_index[edid->pad];
+
+		adap = dev->cec_tx_adap[hdmi_output];
 	}
 	if (edid->start_block == 0 && edid->blocks == 0) {
-		edid->blocks = dev->edid_blocks;
+		edid->blocks = dev_rx->edid_blocks;
 		return 0;
 	}
-	if (dev->edid_blocks == 0)
+	if (dev_rx->edid_blocks == 0)
 		return -ENODATA;
-	if (edid->start_block >= dev->edid_blocks)
+	if (edid->start_block >= dev_rx->edid_blocks)
 		return -EINVAL;
-	if (edid->blocks > dev->edid_blocks - edid->start_block)
-		edid->blocks = dev->edid_blocks - edid->start_block;
-	if (adap)
-		v4l2_set_edid_phys_addr(dev->edid, dev->edid_blocks * 128, adap->phys_addr);
-	memcpy(edid->edid, dev->edid + edid->start_block * 128, edid->blocks * 128);
+	if (edid->blocks > dev_rx->edid_blocks - edid->start_block)
+		edid->blocks = dev_rx->edid_blocks - edid->start_block;
+
+	memcpy(edid->edid, dev_rx->edid + edid->start_block * 128, edid->blocks * 128);
+
+	loc = cec_get_edid_spa_location(dev_rx->edid,
+					dev_rx->edid_blocks * 128);
+	if (vdev->vfl_dir == VFL_DIR_TX && adap && loc &&
+	    loc >= edid->start_block * 128 &&
+	    loc < (edid->start_block + edid->blocks) * 128) {
+		unsigned int i;
+		u8 sum = 0;
+
+		loc -= edid->start_block * 128;
+		edid->edid[loc] = adap->phys_addr >> 8;
+		edid->edid[loc + 1] = adap->phys_addr & 0xff;
+		loc &= ~0x7f;
+
+		/* update the checksum */
+		for (i = loc; i < loc + 127; i++)
+			sum += edid->edid[i];
+		edid->edid[i] = 256 - sum;
+	}
 	return 0;
 }
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-common.h b/drivers/media/test-drivers/vivid/vivid-vid-common.h
index d908d9725283..c49ac85abaed 100644
--- a/drivers/media/test-drivers/vivid/vivid-vid-common.h
+++ b/drivers/media/test-drivers/vivid/vivid-vid-common.h
@@ -22,8 +22,11 @@ extern const struct v4l2_dv_timings_cap vivid_dv_timings_cap;
 
 const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat);
 
+struct vivid_dev *vivid_input_is_connected_to(struct vivid_dev *dev);
+struct vivid_dev *vivid_output_is_connected_to(struct vivid_dev *dev);
 bool vivid_vid_can_loop(struct vivid_dev *dev);
-void vivid_send_source_change(struct vivid_dev *dev, unsigned type);
+void vivid_send_source_change(struct vivid_dev *dev, unsigned int type);
+void vivid_send_input_source_change(struct vivid_dev *dev, unsigned int input_index);
 
 int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r);
 
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-out.c b/drivers/media/test-drivers/vivid/vivid-vid-out.c
index 1653b2988f7e..c3398bce6c15 100644
--- a/drivers/media/test-drivers/vivid/vivid-vid-out.c
+++ b/drivers/media/test-drivers/vivid/vivid-vid-out.c
@@ -16,6 +16,7 @@
 #include <media/v4l2-rect.h>
 
 #include "vivid-core.h"
+#include "vivid-osd.h"
 #include "vivid-vid-common.h"
 #include "vivid-kthread-out.h"
 #include "vivid-vid-out.h"
@@ -63,14 +64,16 @@ static int vid_out_queue_setup(struct vb2_queue *vq,
 		if (sizes[0] < size)
 			return -EINVAL;
 		for (p = 1; p < planes; p++) {
-			if (sizes[p] < dev->bytesperline_out[p] * h +
-				       vfmt->data_offset[p])
+			if (sizes[p] < dev->bytesperline_out[p] * h /
+					vfmt->vdownsampling[p] +
+					vfmt->data_offset[p])
 				return -EINVAL;
 		}
 	} else {
 		for (p = 0; p < planes; p++)
-			sizes[p] = p ? dev->bytesperline_out[p] * h +
-				       vfmt->data_offset[p] : size;
+			sizes[p] = p ? dev->bytesperline_out[p] * h /
+					vfmt->vdownsampling[p] +
+					vfmt->data_offset[p] : size;
 	}
 
 	*nplanes = planes;
@@ -124,7 +127,7 @@ static int vid_out_buf_prepare(struct vb2_buffer *vb)
 
 	for (p = 0; p < planes; p++) {
 		if (p)
-			size = dev->bytesperline_out[p] * h;
+			size = dev->bytesperline_out[p] * h / vfmt->vdownsampling[p];
 		size += vb->planes[p].data_offset;
 
 		if (vb2_get_plane_payload(vb, p) < size) {
@@ -155,9 +158,6 @@ static int vid_out_start_streaming(struct vb2_queue *vq, unsigned count)
 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
 	int err;
 
-	if (vb2_is_streaming(&dev->vb_vid_cap_q))
-		dev->can_loop_video = vivid_vid_can_loop(dev);
-
 	dev->vid_out_seq_count = 0;
 	dprintk(dev, 1, "%s\n", __func__);
 	if (dev->start_streaming_error) {
@@ -185,7 +185,6 @@ static void vid_out_stop_streaming(struct vb2_queue *vq)
 
 	dprintk(dev, 1, "%s\n", __func__);
 	vivid_stop_generating_vid_out(dev, &dev->vid_out_streaming);
-	dev->can_loop_video = false;
 }
 
 static void vid_out_buf_request_complete(struct vb2_buffer *vb)
@@ -203,8 +202,6 @@ const struct vb2_ops vivid_vid_out_qops = {
 	.start_streaming	= vid_out_start_streaming,
 	.stop_streaming		= vid_out_stop_streaming,
 	.buf_request_complete	= vid_out_buf_request_complete,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /*
@@ -331,8 +328,8 @@ int vivid_g_fmt_vid_out(struct file *file, void *priv,
 	for (p = 0; p < mp->num_planes; p++) {
 		mp->plane_fmt[p].bytesperline = dev->bytesperline_out[p];
 		mp->plane_fmt[p].sizeimage =
-			mp->plane_fmt[p].bytesperline * mp->height +
-			fmt->data_offset[p];
+			mp->plane_fmt[p].bytesperline * mp->height /
+			fmt->vdownsampling[p] + fmt->data_offset[p];
 	}
 	for (p = fmt->buffers; p < fmt->planes; p++) {
 		unsigned stride = dev->bytesperline_out[p];
@@ -562,9 +559,11 @@ set_colorspace:
 	dev->xfer_func_out = mp->xfer_func;
 	dev->ycbcr_enc_out = mp->ycbcr_enc;
 	dev->quantization_out = mp->quantization;
-	if (dev->loop_video) {
-		vivid_send_source_change(dev, SVID);
-		vivid_send_source_change(dev, HDMI);
+	struct vivid_dev *in_dev = vivid_output_is_connected_to(dev);
+
+	if (in_dev) {
+		vivid_send_source_change(in_dev, SVID);
+		vivid_send_source_change(in_dev, HDMI);
 	}
 	return 0;
 }
@@ -909,7 +908,7 @@ int vivid_vid_out_g_fbuf(struct file *file, void *fh,
 	a->base = (void *)dev->video_pbase;
 	a->fmt.width = dev->display_width;
 	a->fmt.height = dev->display_height;
-	if (dev->fb_defined.green.length == 5)
+	if (vivid_fb_green_bits(dev) == 5)
 		a->fmt.pixelformat = V4L2_PIX_FMT_ARGB555;
 	else
 		a->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
@@ -964,16 +963,16 @@ int vidioc_enum_output(struct file *file, void *priv,
 	out->type = V4L2_OUTPUT_TYPE_ANALOG;
 	switch (dev->output_type[out->index]) {
 	case SVID:
-		snprintf(out->name, sizeof(out->name), "S-Video %u",
-				dev->output_name_counter[out->index]);
+		snprintf(out->name, sizeof(out->name), "S-Video %03u-%u",
+			 dev->inst, dev->output_name_counter[out->index]);
 		out->std = V4L2_STD_ALL;
 		if (dev->has_audio_outputs)
 			out->audioset = (1 << ARRAY_SIZE(vivid_audio_outputs)) - 1;
 		out->capabilities = V4L2_OUT_CAP_STD;
 		break;
 	case HDMI:
-		snprintf(out->name, sizeof(out->name), "HDMI %u",
-				dev->output_name_counter[out->index]);
+		snprintf(out->name, sizeof(out->name), "HDMI %03u-%u",
+			 dev->inst, dev->output_name_counter[out->index]);
 		out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
 		break;
 	}
@@ -1014,11 +1013,6 @@ int vidioc_s_output(struct file *file, void *priv, unsigned o)
 	dev->meta_out_dev.tvnorms = dev->vid_out_dev.tvnorms;
 	vivid_update_format_out(dev);
 
-	v4l2_ctrl_activate(dev->ctrl_display_present, vivid_is_hdmi_out(dev));
-	if (vivid_is_hdmi_out(dev))
-		v4l2_ctrl_s_ctrl(dev->ctrl_display_present,
-				 dev->display_present[dev->output]);
-
 	return 0;
 }
 
diff --git a/drivers/media/tuners/e4000.c b/drivers/media/tuners/e4000.c
index 3893a00c18ce..549b2009f974 100644
--- a/drivers/media/tuners/e4000.c
+++ b/drivers/media/tuners/e4000.c
@@ -719,7 +719,7 @@ static void e4000_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id e4000_id_table[] = {
-	{"e4000", 0},
+	{ "e4000" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, e4000_id_table);
diff --git a/drivers/media/tuners/fc0013.c b/drivers/media/tuners/fc0013.c
index 1006a2798eef..90d2ef067594 100644
--- a/drivers/media/tuners/fc0013.c
+++ b/drivers/media/tuners/fc0013.c
@@ -112,70 +112,6 @@ static int fc0013_sleep(struct dvb_frontend *fe)
 	return 0;
 }
 
-int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
-{
-	struct fc0013_priv *priv = fe->tuner_priv;
-	int ret;
-	u8 rc_cal;
-	int val;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
-
-	/* push rc_cal value, get rc_cal value */
-	ret = fc0013_writereg(priv, 0x10, 0x00);
-	if (ret)
-		goto error_out;
-
-	/* get rc_cal value */
-	ret = fc0013_readreg(priv, 0x10, &rc_cal);
-	if (ret)
-		goto error_out;
-
-	rc_cal &= 0x0f;
-
-	val = (int)rc_cal + rc_val;
-
-	/* forcing rc_cal */
-	ret = fc0013_writereg(priv, 0x0d, 0x11);
-	if (ret)
-		goto error_out;
-
-	/* modify rc_cal value */
-	if (val > 15)
-		ret = fc0013_writereg(priv, 0x10, 0x0f);
-	else if (val < 0)
-		ret = fc0013_writereg(priv, 0x10, 0x00);
-	else
-		ret = fc0013_writereg(priv, 0x10, (u8)val);
-
-error_out:
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
-
-	return ret;
-}
-EXPORT_SYMBOL(fc0013_rc_cal_add);
-
-int fc0013_rc_cal_reset(struct dvb_frontend *fe)
-{
-	struct fc0013_priv *priv = fe->tuner_priv;
-	int ret;
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
-
-	ret = fc0013_writereg(priv, 0x0d, 0x01);
-	if (!ret)
-		ret = fc0013_writereg(priv, 0x10, 0x00);
-
-	if (fe->ops.i2c_gate_ctrl)
-		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
-
-	return ret;
-}
-EXPORT_SYMBOL(fc0013_rc_cal_reset);
-
 static int fc0013_set_vhf_track(struct fc0013_priv *priv, u32 freq)
 {
 	int ret;
diff --git a/drivers/media/tuners/fc0013.h b/drivers/media/tuners/fc0013.h
index 74ce5903f199..47ab36342ee8 100644
--- a/drivers/media/tuners/fc0013.h
+++ b/drivers/media/tuners/fc0013.h
@@ -16,8 +16,6 @@ extern struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
 					u8 i2c_address, int dual_master,
 					enum fc001x_xtal_freq xtal_freq);
-extern int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val);
-extern int fc0013_rc_cal_reset(struct dvb_frontend *fe);
 #else
 static inline struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
 					struct i2c_adapter *i2c,
@@ -28,15 +26,6 @@ static inline struct dvb_frontend *fc0013_attach(struct dvb_frontend *fe,
 	return NULL;
 }
 
-static inline int fc0013_rc_cal_add(struct dvb_frontend *fe, int rc_val)
-{
-	return 0;
-}
-
-static inline int fc0013_rc_cal_reset(struct dvb_frontend *fe)
-{
-	return 0;
-}
 #endif
 
 #endif
diff --git a/drivers/media/tuners/fc2580.c b/drivers/media/tuners/fc2580.c
index f6613dcf40a3..046389896dc5 100644
--- a/drivers/media/tuners/fc2580.c
+++ b/drivers/media/tuners/fc2580.c
@@ -600,7 +600,7 @@ static void fc2580_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id fc2580_id_table[] = {
-	{"fc2580", 0},
+	{ "fc2580" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, fc2580_id_table);
diff --git a/drivers/media/tuners/it913x.c b/drivers/media/tuners/it913x.c
index 4d5b1c878028..9186174a46fd 100644
--- a/drivers/media/tuners/it913x.c
+++ b/drivers/media/tuners/it913x.c
@@ -444,7 +444,7 @@ static struct platform_driver it913x_driver = {
 		.suppress_bind_attrs	= true,
 	},
 	.probe		= it913x_probe,
-	.remove_new	= it913x_remove,
+	.remove		= it913x_remove,
 	.id_table	= it913x_id_table,
 };
 
diff --git a/drivers/media/tuners/m88rs6000t.c b/drivers/media/tuners/m88rs6000t.c
index 2cd7f0e0c70d..cc57980ed417 100644
--- a/drivers/media/tuners/m88rs6000t.c
+++ b/drivers/media/tuners/m88rs6000t.c
@@ -709,7 +709,7 @@ static void m88rs6000t_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id m88rs6000t_id[] = {
-	{"m88rs6000t", 0},
+	{ "m88rs6000t" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, m88rs6000t_id);
diff --git a/drivers/media/tuners/mt2060.c b/drivers/media/tuners/mt2060.c
index 4205ed4cf467..4b9dca2f17cc 100644
--- a/drivers/media/tuners/mt2060.c
+++ b/drivers/media/tuners/mt2060.c
@@ -514,7 +514,7 @@ static void mt2060_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id mt2060_id_table[] = {
-	{"mt2060", 0},
+	{ "mt2060" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, mt2060_id_table);
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 6afef11a49cb..2c8ce74ddca4 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -1476,7 +1476,7 @@ static u32 MT2063_CalcLO2Mult(u32 *Div,
 }
 
 /*
- * FindClearTuneFilter() - Calculate the corrrect ClearTune filter to be
+ * FindClearTuneFilter() - Calculate the correct ClearTune filter to be
  *			   used for a given input frequency.
  *
  * @state:	ptr to tuner data structure
diff --git a/drivers/media/tuners/mxl301rf.c b/drivers/media/tuners/mxl301rf.c
index 9b2b237745ae..3b61c3afed18 100644
--- a/drivers/media/tuners/mxl301rf.c
+++ b/drivers/media/tuners/mxl301rf.c
@@ -64,7 +64,7 @@ static int reg_read(struct mxl301rf_state *state, u8 reg, u8 *val)
 
 /* tuner_ops */
 
-/* get RSSI and update propery cache, set to *out in % */
+/* get RSSI and update property cache, set to *out in % */
 static int mxl301rf_get_rf_strength(struct dvb_frontend *fe, u16 *out)
 {
 	struct mxl301rf_state *state;
@@ -317,7 +317,7 @@ static void mxl301rf_remove(struct i2c_client *client)
 
 
 static const struct i2c_device_id mxl301rf_id[] = {
-	{"mxl301rf", 0},
+	{ "mxl301rf" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, mxl301rf_id);
diff --git a/drivers/media/tuners/mxl5005s.c b/drivers/media/tuners/mxl5005s.c
index d9bfa257a005..0e811c5eae6c 100644
--- a/drivers/media/tuners/mxl5005s.c
+++ b/drivers/media/tuners/mxl5005s.c
@@ -2639,7 +2639,7 @@ static u16 MXL_TuneRF(struct dvb_frontend *fe, u32 RF_Freq)
 	E5A = (((Fmax - state->RF_LO)/1000)*4/((Fmax-Fmin)/1000)) + 1 ;
 	status += MXL_ControlWrite(fe, RFSYN_LPF_R, E5A);
 
-	/* Euqation E5B CHCAL_EN_INIT_RF */
+	/* Equation E5B CHCAL_EN_INIT_RF */
 	status += MXL_ControlWrite(fe, CHCAL_EN_INT_RF, ((E5 == 0) ? 1 : 0));
 	/*if (E5 == 0)
 	 *	status += MXL_ControlWrite(fe, CHCAL_EN_INT_RF, 1);
diff --git a/drivers/media/tuners/qm1d1b0004.c b/drivers/media/tuners/qm1d1b0004.c
index af2d3618b9d5..c53aeb558413 100644
--- a/drivers/media/tuners/qm1d1b0004.c
+++ b/drivers/media/tuners/qm1d1b0004.c
@@ -243,7 +243,7 @@ static void qm1d1b0004_remove(struct i2c_client *client)
 
 
 static const struct i2c_device_id qm1d1b0004_id[] = {
-	{"qm1d1b0004", 0},
+	{ "qm1d1b0004" },
 	{}
 };
 
diff --git a/drivers/media/tuners/qm1d1c0042.c b/drivers/media/tuners/qm1d1c0042.c
index ce7223315b0c..c58f5b6526f1 100644
--- a/drivers/media/tuners/qm1d1c0042.c
+++ b/drivers/media/tuners/qm1d1c0042.c
@@ -434,7 +434,7 @@ static void qm1d1c0042_remove(struct i2c_client *client)
 
 
 static const struct i2c_device_id qm1d1c0042_id[] = {
-	{"qm1d1c0042", 0},
+	{ "qm1d1c0042" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, qm1d1c0042_id);
diff --git a/drivers/media/tuners/tda18212.c b/drivers/media/tuners/tda18212.c
index 8d742bd61df0..39f2dc9c2845 100644
--- a/drivers/media/tuners/tda18212.c
+++ b/drivers/media/tuners/tda18212.c
@@ -254,7 +254,7 @@ static void tda18212_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tda18212_id[] = {
-	{"tda18212", 0},
+	{ "tda18212" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, tda18212_id);
diff --git a/drivers/media/tuners/tda18250.c b/drivers/media/tuners/tda18250.c
index 32ea473f3f49..68d0275f29e1 100644
--- a/drivers/media/tuners/tda18250.c
+++ b/drivers/media/tuners/tda18250.c
@@ -868,7 +868,7 @@ static void tda18250_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tda18250_id_table[] = {
-	{"tda18250", 0},
+	{ "tda18250" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, tda18250_id_table);
diff --git a/drivers/media/tuners/tda18271-fe.c b/drivers/media/tuners/tda18271-fe.c
index a7e721baaa99..3a3309bc0151 100644
--- a/drivers/media/tuners/tda18271-fe.c
+++ b/drivers/media/tuners/tda18271-fe.c
@@ -279,7 +279,9 @@ static int tda18271c2_rf_tracking_filters_correction(struct dvb_frontend *fe,
 	if (approx > 255)
 		approx = 255;
 
-	tda18271_lookup_map(fe, RF_CAL_DC_OVER_DT, &freq, &dc_over_dt);
+	ret = tda18271_lookup_map(fe, RF_CAL_DC_OVER_DT, &freq, &dc_over_dt);
+	if (tda_fail(ret))
+		goto fail;
 
 	/* calculate temperature compensation */
 	rfcal_comp = dc_over_dt * (s32)(tm_current - priv->tm_rfcal) / 1000;
diff --git a/drivers/media/tuners/tda9887.c b/drivers/media/tuners/tda9887.c
index dca2fc776e44..b2f7054c1832 100644
--- a/drivers/media/tuners/tda9887.c
+++ b/drivers/media/tuners/tda9887.c
@@ -707,4 +707,5 @@ struct dvb_frontend *tda9887_attach(struct dvb_frontend *fe,
 }
 EXPORT_SYMBOL_GPL(tda9887_attach);
 
+MODULE_DESCRIPTION("NXP TDA9885/6/7 analog IF demodulator driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tea5761.c b/drivers/media/tuners/tea5761.c
index d78a2bdb3e36..425e9fd3f3d4 100644
--- a/drivers/media/tuners/tea5761.c
+++ b/drivers/media/tuners/tea5761.c
@@ -46,7 +46,7 @@ struct tea5761_priv {
 /* FRQSET - Read: bytes 2 and 3 / Write: byte 1 and 2 */
 
 	/* First byte */
-#define TEA5761_FRQSET_SEARCH_UP 0x80		/* 1=Station search from botton to up */
+#define TEA5761_FRQSET_SEARCH_UP 0x80		/* 1=Station search from bottom to up */
 #define TEA5761_FRQSET_SEARCH_MODE 0x40		/* 1=Search mode */
 
 	/* Bits 0-5 for divider MSB */
@@ -132,7 +132,7 @@ static void tea5761_status_dump(unsigned char *buffer)
 	       frq / 1000, frq % 1000, div);
 }
 
-/* Freq should be specifyed at 62.5 Hz */
+/* Freq should be specified at 62.5 Hz */
 static int __set_radio_freq(struct dvb_frontend *fe,
 			    unsigned int freq,
 			    bool mono)
diff --git a/drivers/media/tuners/tea5767.c b/drivers/media/tuners/tea5767.c
index 016d0d5ec50b..ef4acb1f1bfa 100644
--- a/drivers/media/tuners/tea5767.c
+++ b/drivers/media/tuners/tea5767.c
@@ -44,7 +44,7 @@ struct tea5767_priv {
 
 /* Third register */
 
-/* Station search from botton to up */
+/* Station search from bottom to up */
 #define TEA5767_SEARCH_UP	0x80
 
 /* Searches with ADC output = 10 */
@@ -183,7 +183,7 @@ static void tea5767_status_dump(struct tea5767_priv *priv,
 		   (buffer[4] & TEA5767_RESERVED_MASK));
 }
 
-/* Freq should be specifyed at 62.5 Hz */
+/* Freq should be specified at 62.5 Hz */
 static int set_radio_freq(struct dvb_frontend *fe,
 			  struct analog_parameters *params)
 {
diff --git a/drivers/media/tuners/tua9001.c b/drivers/media/tuners/tua9001.c
index 03a3a022b0a8..562a7a5c26f5 100644
--- a/drivers/media/tuners/tua9001.c
+++ b/drivers/media/tuners/tua9001.c
@@ -245,7 +245,7 @@ static void tua9001_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id tua9001_id_table[] = {
-	{"tua9001", 0},
+	{ "tua9001" },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, tua9001_id_table);
diff --git a/drivers/media/tuners/tuner-i2c.h b/drivers/media/tuners/tuner-i2c.h
index 07aeead0644a..724952e001cd 100644
--- a/drivers/media/tuners/tuner-i2c.h
+++ b/drivers/media/tuners/tuner-i2c.h
@@ -133,10 +133,8 @@ static inline int tuner_i2c_xfer_send_recv(struct tuner_i2c_props *props,
 	}								\
 	if (0 == __ret) {						\
 		state = kzalloc(sizeof(type), GFP_KERNEL);		\
-		if (!state) {						\
-			__ret = -ENOMEM;				\
+		if (NULL == state)					\
 			goto __fail;					\
-		}							\
 		state->i2c_props.addr = i2caddr;			\
 		state->i2c_props.adap = i2cadap;			\
 		state->i2c_props.name = devname;			\
diff --git a/drivers/media/tuners/tuner-simple.c b/drivers/media/tuners/tuner-simple.c
index 8fb186b25d6a..b52cd8bd07dd 100644
--- a/drivers/media/tuners/tuner-simple.c
+++ b/drivers/media/tuners/tuner-simple.c
@@ -112,7 +112,7 @@ struct tuner_simple_priv {
 	struct list_head hybrid_tuner_instance_list;
 
 	unsigned int type;
-	struct tunertype *tun;
+	const struct tunertype *tun;
 
 	u32 frequency;
 	u32 bandwidth;
@@ -232,11 +232,11 @@ static inline char *tuner_param_name(enum param_type type)
 	return name;
 }
 
-static struct tuner_params *simple_tuner_params(struct dvb_frontend *fe,
-						enum param_type desired_type)
+static const struct tuner_params *simple_tuner_params(struct dvb_frontend *fe,
+						      enum param_type desired_type)
 {
 	struct tuner_simple_priv *priv = fe->tuner_priv;
-	struct tunertype *tun = priv->tun;
+	const struct tunertype *tun = priv->tun;
 	int i;
 
 	for (i = 0; i < tun->count; i++)
@@ -257,7 +257,7 @@ static struct tuner_params *simple_tuner_params(struct dvb_frontend *fe,
 }
 
 static int simple_config_lookup(struct dvb_frontend *fe,
-				struct tuner_params *t_params,
+				const struct tuner_params *t_params,
 				unsigned *frequency, u8 *config, u8 *cb)
 {
 	struct tuner_simple_priv *priv = fe->tuner_priv;
@@ -549,7 +549,7 @@ static int simple_set_tv_freq(struct dvb_frontend *fe,
 	u8 buffer[4];
 	int rc, IFPCoff, i;
 	enum param_type desired_type;
-	struct tuner_params *t_params;
+	const struct tuner_params *t_params;
 
 	/* IFPCoff = Video Intermediate Frequency - Vif:
 		940  =16*58.75  NTSC/J (Japan)
@@ -664,12 +664,12 @@ static int simple_set_tv_freq(struct dvb_frontend *fe,
 static int simple_set_radio_freq(struct dvb_frontend *fe,
 				 struct analog_parameters *params)
 {
-	struct tunertype *tun;
+	const struct tunertype *tun;
 	struct tuner_simple_priv *priv = fe->tuner_priv;
 	u8 buffer[4];
 	u16 div;
 	int rc, j;
-	struct tuner_params *t_params;
+	const struct tuner_params *t_params;
 	unsigned int freq = params->frequency;
 	bool mono = params->audmode == V4L2_TUNER_MODE_MONO;
 
@@ -848,8 +848,8 @@ static u32 simple_dvb_configure(struct dvb_frontend *fe, u8 *buf,
 {
 	/* This function returns the tuned frequency on success, 0 on error */
 	struct tuner_simple_priv *priv = fe->tuner_priv;
-	struct tunertype *tun = priv->tun;
-	struct tuner_params *t_params;
+	const struct tunertype *tun = priv->tun;
+	const struct tuner_params *t_params;
 	u8 config, cb;
 	u32 div;
 	int ret;
diff --git a/drivers/media/tuners/tuner-types.c b/drivers/media/tuners/tuner-types.c
index ff5a6c0acdd4..0716cc028212 100644
--- a/drivers/media/tuners/tuner-types.c
+++ b/drivers/media/tuners/tuner-types.c
@@ -61,13 +61,13 @@ static u8 tua603x_agc112[] = { 2, 0x80|0x40|0x18|0x04|0x01, 0x80|0x20 };
 /* 0-9 */
 /* ------------ TUNER_TEMIC_PAL - TEMIC PAL ------------ */
 
-static struct tuner_range tuner_temic_pal_ranges[] = {
+static const struct tuner_range tuner_temic_pal_ranges[] = {
 	{ 16 * 140.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0x04, },
 	{ 16 * 999.99        , 0x8e, 0x01, },
 };
 
-static struct tuner_params tuner_temic_pal_params[] = {
+static const struct tuner_params tuner_temic_pal_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_pal_ranges,
@@ -77,13 +77,13 @@ static struct tuner_params tuner_temic_pal_params[] = {
 
 /* ------------ TUNER_PHILIPS_PAL_I - Philips PAL_I ------------ */
 
-static struct tuner_range tuner_philips_pal_i_ranges[] = {
+static const struct tuner_range tuner_philips_pal_i_ranges[] = {
 	{ 16 * 140.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_philips_pal_i_params[] = {
+static const struct tuner_params tuner_philips_pal_i_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_pal_i_ranges,
@@ -93,13 +93,13 @@ static struct tuner_params tuner_philips_pal_i_params[] = {
 
 /* ------------ TUNER_PHILIPS_NTSC - Philips NTSC ------------ */
 
-static struct tuner_range tuner_philips_ntsc_ranges[] = {
+static const struct tuner_range tuner_philips_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 451.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_philips_ntsc_params[] = {
+static const struct tuner_params tuner_philips_ntsc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_philips_ntsc_ranges,
@@ -110,13 +110,13 @@ static struct tuner_params tuner_philips_ntsc_params[] = {
 
 /* ------------ TUNER_PHILIPS_SECAM - Philips SECAM ------------ */
 
-static struct tuner_range tuner_philips_secam_ranges[] = {
+static const struct tuner_range tuner_philips_secam_ranges[] = {
 	{ 16 * 168.25 /*MHz*/, 0x8e, 0xa7, },
 	{ 16 * 447.25 /*MHz*/, 0x8e, 0x97, },
 	{ 16 * 999.99        , 0x8e, 0x37, },
 };
 
-static struct tuner_params tuner_philips_secam_params[] = {
+static const struct tuner_params tuner_philips_secam_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_SECAM,
 		.ranges = tuner_philips_secam_ranges,
@@ -127,13 +127,13 @@ static struct tuner_params tuner_philips_secam_params[] = {
 
 /* ------------ TUNER_PHILIPS_PAL - Philips PAL ------------ */
 
-static struct tuner_range tuner_philips_pal_ranges[] = {
+static const struct tuner_range tuner_philips_pal_ranges[] = {
 	{ 16 * 168.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 447.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_philips_pal_params[] = {
+static const struct tuner_params tuner_philips_pal_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_pal_ranges,
@@ -144,13 +144,13 @@ static struct tuner_params tuner_philips_pal_params[] = {
 
 /* ------------ TUNER_TEMIC_NTSC - TEMIC NTSC ------------ */
 
-static struct tuner_range tuner_temic_ntsc_ranges[] = {
+static const struct tuner_range tuner_temic_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0x04, },
 	{ 16 * 999.99        , 0x8e, 0x01, },
 };
 
-static struct tuner_params tuner_temic_ntsc_params[] = {
+static const struct tuner_params tuner_temic_ntsc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_temic_ntsc_ranges,
@@ -160,13 +160,13 @@ static struct tuner_params tuner_temic_ntsc_params[] = {
 
 /* ------------ TUNER_TEMIC_PAL_I - TEMIC PAL_I ------------ */
 
-static struct tuner_range tuner_temic_pal_i_ranges[] = {
+static const struct tuner_range tuner_temic_pal_i_ranges[] = {
 	{ 16 * 170.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 450.00 /*MHz*/, 0x8e, 0x04, },
 	{ 16 * 999.99        , 0x8e, 0x01, },
 };
 
-static struct tuner_params tuner_temic_pal_i_params[] = {
+static const struct tuner_params tuner_temic_pal_i_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_pal_i_ranges,
@@ -176,13 +176,13 @@ static struct tuner_params tuner_temic_pal_i_params[] = {
 
 /* ------------ TUNER_TEMIC_4036FY5_NTSC - TEMIC NTSC ------------ */
 
-static struct tuner_range tuner_temic_4036fy5_ntsc_ranges[] = {
+static const struct tuner_range tuner_temic_4036fy5_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_4036fy5_ntsc_params[] = {
+static const struct tuner_params tuner_temic_4036fy5_ntsc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_temic_4036fy5_ntsc_ranges,
@@ -192,13 +192,13 @@ static struct tuner_params tuner_temic_4036fy5_ntsc_params[] = {
 
 /* ------------ TUNER_ALPS_TSBH1_NTSC - TEMIC NTSC ------------ */
 
-static struct tuner_range tuner_alps_tsb_1_ranges[] = {
+static const struct tuner_range tuner_alps_tsb_1_ranges[] = {
 	{ 16 * 137.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 385.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_alps_tsbh1_ntsc_params[] = {
+static const struct tuner_params tuner_alps_tsbh1_ntsc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_alps_tsb_1_ranges,
@@ -209,7 +209,7 @@ static struct tuner_params tuner_alps_tsbh1_ntsc_params[] = {
 /* 10-19 */
 /* ------------ TUNER_ALPS_TSBE1_PAL - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_alps_tsb_1_params[] = {
+static const struct tuner_params tuner_alps_tsb_1_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_alps_tsb_1_ranges,
@@ -219,13 +219,13 @@ static struct tuner_params tuner_alps_tsb_1_params[] = {
 
 /* ------------ TUNER_ALPS_TSBB5_PAL_I - Alps PAL_I ------------ */
 
-static struct tuner_range tuner_alps_tsb_5_pal_ranges[] = {
+static const struct tuner_range tuner_alps_tsb_5_pal_ranges[] = {
 	{ 16 * 133.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 351.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_alps_tsbb5_params[] = {
+static const struct tuner_params tuner_alps_tsbb5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_alps_tsb_5_pal_ranges,
@@ -235,7 +235,7 @@ static struct tuner_params tuner_alps_tsbb5_params[] = {
 
 /* ------------ TUNER_ALPS_TSBE5_PAL - Alps PAL ------------ */
 
-static struct tuner_params tuner_alps_tsbe5_params[] = {
+static const struct tuner_params tuner_alps_tsbe5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_alps_tsb_5_pal_ranges,
@@ -245,7 +245,7 @@ static struct tuner_params tuner_alps_tsbe5_params[] = {
 
 /* ------------ TUNER_ALPS_TSBC5_PAL - Alps PAL ------------ */
 
-static struct tuner_params tuner_alps_tsbc5_params[] = {
+static const struct tuner_params tuner_alps_tsbc5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_alps_tsb_5_pal_ranges,
@@ -255,13 +255,13 @@ static struct tuner_params tuner_alps_tsbc5_params[] = {
 
 /* ------------ TUNER_TEMIC_4006FH5_PAL - TEMIC PAL ------------ */
 
-static struct tuner_range tuner_lg_pal_ranges[] = {
+static const struct tuner_range tuner_lg_pal_ranges[] = {
 	{ 16 * 170.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 450.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_4006fh5_params[] = {
+static const struct tuner_params tuner_temic_4006fh5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -271,13 +271,13 @@ static struct tuner_params tuner_temic_4006fh5_params[] = {
 
 /* ------------ TUNER_ALPS_TSHC6_NTSC - Alps NTSC ------------ */
 
-static struct tuner_range tuner_alps_tshc6_ntsc_ranges[] = {
+static const struct tuner_range tuner_alps_tshc6_ntsc_ranges[] = {
 	{ 16 * 137.25 /*MHz*/, 0x8e, 0x14, },
 	{ 16 * 385.25 /*MHz*/, 0x8e, 0x12, },
 	{ 16 * 999.99        , 0x8e, 0x11, },
 };
 
-static struct tuner_params tuner_alps_tshc6_params[] = {
+static const struct tuner_params tuner_alps_tshc6_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_alps_tshc6_ntsc_ranges,
@@ -287,13 +287,13 @@ static struct tuner_params tuner_alps_tshc6_params[] = {
 
 /* ------------ TUNER_TEMIC_PAL_DK - TEMIC PAL ------------ */
 
-static struct tuner_range tuner_temic_pal_dk_ranges[] = {
+static const struct tuner_range tuner_temic_pal_dk_ranges[] = {
 	{ 16 * 168.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 456.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_pal_dk_params[] = {
+static const struct tuner_params tuner_temic_pal_dk_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_pal_dk_ranges,
@@ -303,13 +303,13 @@ static struct tuner_params tuner_temic_pal_dk_params[] = {
 
 /* ------------ TUNER_PHILIPS_NTSC_M - Philips NTSC ------------ */
 
-static struct tuner_range tuner_philips_ntsc_m_ranges[] = {
+static const struct tuner_range tuner_philips_ntsc_m_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 454.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_philips_ntsc_m_params[] = {
+static const struct tuner_params tuner_philips_ntsc_m_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_philips_ntsc_m_ranges,
@@ -319,13 +319,13 @@ static struct tuner_params tuner_philips_ntsc_m_params[] = {
 
 /* ------------ TUNER_TEMIC_4066FY5_PAL_I - TEMIC PAL_I ------------ */
 
-static struct tuner_range tuner_temic_40x6f_5_pal_ranges[] = {
+static const struct tuner_range tuner_temic_40x6f_5_pal_ranges[] = {
 	{ 16 * 169.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 454.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_4066fy5_pal_i_params[] = {
+static const struct tuner_params tuner_temic_4066fy5_pal_i_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_40x6f_5_pal_ranges,
@@ -335,7 +335,7 @@ static struct tuner_params tuner_temic_4066fy5_pal_i_params[] = {
 
 /* ------------ TUNER_TEMIC_4006FN5_MULTI_PAL - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_temic_4006fn5_multi_params[] = {
+static const struct tuner_params tuner_temic_4006fn5_multi_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_40x6f_5_pal_ranges,
@@ -346,13 +346,13 @@ static struct tuner_params tuner_temic_4006fn5_multi_params[] = {
 /* 20-29 */
 /* ------------ TUNER_TEMIC_4009FR5_PAL - TEMIC PAL ------------ */
 
-static struct tuner_range tuner_temic_4009f_5_pal_ranges[] = {
+static const struct tuner_range tuner_temic_4009f_5_pal_ranges[] = {
 	{ 16 * 141.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 464.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_4009f_5_params[] = {
+static const struct tuner_params tuner_temic_4009f_5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_4009f_5_pal_ranges,
@@ -362,13 +362,13 @@ static struct tuner_params tuner_temic_4009f_5_params[] = {
 
 /* ------------ TUNER_TEMIC_4039FR5_NTSC - TEMIC NTSC ------------ */
 
-static struct tuner_range tuner_temic_4x3x_f_5_ntsc_ranges[] = {
+static const struct tuner_range tuner_temic_4x3x_f_5_ntsc_ranges[] = {
 	{ 16 * 158.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 453.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_temic_4039fr5_params[] = {
+static const struct tuner_params tuner_temic_4039fr5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_temic_4x3x_f_5_ntsc_ranges,
@@ -378,7 +378,7 @@ static struct tuner_params tuner_temic_4039fr5_params[] = {
 
 /* ------------ TUNER_TEMIC_4046FM5 - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_temic_4046fm5_params[] = {
+static const struct tuner_params tuner_temic_4046fm5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_40x6f_5_pal_ranges,
@@ -388,7 +388,7 @@ static struct tuner_params tuner_temic_4046fm5_params[] = {
 
 /* ------------ TUNER_PHILIPS_PAL_DK - Philips PAL ------------ */
 
-static struct tuner_params tuner_philips_pal_dk_params[] = {
+static const struct tuner_params tuner_philips_pal_dk_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -398,7 +398,7 @@ static struct tuner_params tuner_philips_pal_dk_params[] = {
 
 /* ------------ TUNER_PHILIPS_FQ1216ME - Philips PAL ------------ */
 
-static struct tuner_params tuner_philips_fq1216me_params[] = {
+static const struct tuner_params tuner_philips_fq1216me_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -412,7 +412,7 @@ static struct tuner_params tuner_philips_fq1216me_params[] = {
 
 /* ------------ TUNER_LG_PAL_I_FM - LGINNOTEK PAL_I ------------ */
 
-static struct tuner_params tuner_lg_pal_i_fm_params[] = {
+static const struct tuner_params tuner_lg_pal_i_fm_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -422,7 +422,7 @@ static struct tuner_params tuner_lg_pal_i_fm_params[] = {
 
 /* ------------ TUNER_LG_PAL_I - LGINNOTEK PAL_I ------------ */
 
-static struct tuner_params tuner_lg_pal_i_params[] = {
+static const struct tuner_params tuner_lg_pal_i_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -432,13 +432,13 @@ static struct tuner_params tuner_lg_pal_i_params[] = {
 
 /* ------------ TUNER_LG_NTSC_FM - LGINNOTEK NTSC ------------ */
 
-static struct tuner_range tuner_lg_ntsc_fm_ranges[] = {
+static const struct tuner_range tuner_lg_ntsc_fm_ranges[] = {
 	{ 16 * 210.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 497.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_lg_ntsc_fm_params[] = {
+static const struct tuner_params tuner_lg_ntsc_fm_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_lg_ntsc_fm_ranges,
@@ -448,7 +448,7 @@ static struct tuner_params tuner_lg_ntsc_fm_params[] = {
 
 /* ------------ TUNER_LG_PAL_FM - LGINNOTEK PAL ------------ */
 
-static struct tuner_params tuner_lg_pal_fm_params[] = {
+static const struct tuner_params tuner_lg_pal_fm_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -458,7 +458,7 @@ static struct tuner_params tuner_lg_pal_fm_params[] = {
 
 /* ------------ TUNER_LG_PAL - LGINNOTEK PAL ------------ */
 
-static struct tuner_params tuner_lg_pal_params[] = {
+static const struct tuner_params tuner_lg_pal_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_pal_ranges,
@@ -469,7 +469,7 @@ static struct tuner_params tuner_lg_pal_params[] = {
 /* 30-39 */
 /* ------------ TUNER_TEMIC_4009FN5_MULTI_PAL_FM - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_temic_4009_fn5_multi_pal_fm_params[] = {
+static const struct tuner_params tuner_temic_4009_fn5_multi_pal_fm_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_4009f_5_pal_ranges,
@@ -479,13 +479,13 @@ static struct tuner_params tuner_temic_4009_fn5_multi_pal_fm_params[] = {
 
 /* ------------ TUNER_SHARP_2U5JF5540_NTSC - SHARP NTSC ------------ */
 
-static struct tuner_range tuner_sharp_2u5jf5540_ntsc_ranges[] = {
+static const struct tuner_range tuner_sharp_2u5jf5540_ntsc_ranges[] = {
 	{ 16 * 137.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 317.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_sharp_2u5jf5540_params[] = {
+static const struct tuner_params tuner_sharp_2u5jf5540_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_sharp_2u5jf5540_ntsc_ranges,
@@ -495,13 +495,13 @@ static struct tuner_params tuner_sharp_2u5jf5540_params[] = {
 
 /* ------------ TUNER_Samsung_PAL_TCPM9091PD27 - Samsung PAL ------------ */
 
-static struct tuner_range tuner_samsung_pal_tcpm9091pd27_ranges[] = {
+static const struct tuner_range tuner_samsung_pal_tcpm9091pd27_ranges[] = {
 	{ 16 * 169 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 464 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99     , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_samsung_pal_tcpm9091pd27_params[] = {
+static const struct tuner_params tuner_samsung_pal_tcpm9091pd27_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_samsung_pal_tcpm9091pd27_ranges,
@@ -511,7 +511,7 @@ static struct tuner_params tuner_samsung_pal_tcpm9091pd27_params[] = {
 
 /* ------------ TUNER_TEMIC_4106FH5 - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_temic_4106fh5_params[] = {
+static const struct tuner_params tuner_temic_4106fh5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_4009f_5_pal_ranges,
@@ -521,7 +521,7 @@ static struct tuner_params tuner_temic_4106fh5_params[] = {
 
 /* ------------ TUNER_TEMIC_4012FY5 - TEMIC PAL ------------ */
 
-static struct tuner_params tuner_temic_4012fy5_params[] = {
+static const struct tuner_params tuner_temic_4012fy5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_pal_ranges,
@@ -531,7 +531,7 @@ static struct tuner_params tuner_temic_4012fy5_params[] = {
 
 /* ------------ TUNER_TEMIC_4136FY5 - TEMIC NTSC ------------ */
 
-static struct tuner_params tuner_temic_4136_fy5_params[] = {
+static const struct tuner_params tuner_temic_4136_fy5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_temic_4x3x_f_5_ntsc_ranges,
@@ -541,13 +541,13 @@ static struct tuner_params tuner_temic_4136_fy5_params[] = {
 
 /* ------------ TUNER_LG_PAL_NEW_TAPC - LGINNOTEK PAL ------------ */
 
-static struct tuner_range tuner_lg_new_tapc_ranges[] = {
+static const struct tuner_range tuner_lg_new_tapc_ranges[] = {
 	{ 16 * 170.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 450.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_lg_pal_new_tapc_params[] = {
+static const struct tuner_params tuner_lg_pal_new_tapc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_lg_new_tapc_ranges,
@@ -557,13 +557,13 @@ static struct tuner_params tuner_lg_pal_new_tapc_params[] = {
 
 /* ------------ TUNER_PHILIPS_FM1216ME_MK3 - Philips PAL ------------ */
 
-static struct tuner_range tuner_fm1216me_mk3_pal_ranges[] = {
+static const struct tuner_range tuner_fm1216me_mk3_pal_ranges[] = {
 	{ 16 * 158.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 442.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_fm1216me_mk3_params[] = {
+static const struct tuner_params tuner_fm1216me_mk3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_fm1216me_mk3_pal_ranges,
@@ -582,13 +582,13 @@ static struct tuner_params tuner_fm1216me_mk3_params[] = {
 
 /* ------------ TUNER_PHILIPS_FM1216MK5 - Philips PAL ------------ */
 
-static struct tuner_range tuner_fm1216mk5_pal_ranges[] = {
+static const struct tuner_range tuner_fm1216mk5_pal_ranges[] = {
 	{ 16 * 158.00 /*MHz*/, 0xce, 0x01, },
 	{ 16 * 441.00 /*MHz*/, 0xce, 0x02, },
 	{ 16 * 864.00        , 0xce, 0x04, },
 };
 
-static struct tuner_params tuner_fm1216mk5_params[] = {
+static const struct tuner_params tuner_fm1216mk5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_fm1216mk5_pal_ranges,
@@ -607,7 +607,7 @@ static struct tuner_params tuner_fm1216mk5_params[] = {
 
 /* ------------ TUNER_LG_NTSC_NEW_TAPC - LGINNOTEK NTSC ------------ */
 
-static struct tuner_params tuner_lg_ntsc_new_tapc_params[] = {
+static const struct tuner_params tuner_lg_ntsc_new_tapc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_lg_new_tapc_ranges,
@@ -618,7 +618,7 @@ static struct tuner_params tuner_lg_ntsc_new_tapc_params[] = {
 /* 40-49 */
 /* ------------ TUNER_HITACHI_NTSC - HITACHI NTSC ------------ */
 
-static struct tuner_params tuner_hitachi_ntsc_params[] = {
+static const struct tuner_params tuner_hitachi_ntsc_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_lg_new_tapc_ranges,
@@ -628,13 +628,13 @@ static struct tuner_params tuner_hitachi_ntsc_params[] = {
 
 /* ------------ TUNER_PHILIPS_PAL_MK - Philips PAL ------------ */
 
-static struct tuner_range tuner_philips_pal_mk_pal_ranges[] = {
+static const struct tuner_range tuner_philips_pal_mk_pal_ranges[] = {
 	{ 16 * 140.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0xc2, },
 	{ 16 * 999.99        , 0x8e, 0xcf, },
 };
 
-static struct tuner_params tuner_philips_pal_mk_params[] = {
+static const struct tuner_params tuner_philips_pal_mk_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_pal_mk_pal_ranges,
@@ -644,19 +644,19 @@ static struct tuner_params tuner_philips_pal_mk_params[] = {
 
 /* ---- TUNER_PHILIPS_FCV1236D - Philips FCV1236D (ATSC/NTSC) ---- */
 
-static struct tuner_range tuner_philips_fcv1236d_ntsc_ranges[] = {
+static const struct tuner_range tuner_philips_fcv1236d_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0x8e, 0xa2, },
 	{ 16 * 451.25 /*MHz*/, 0x8e, 0x92, },
 	{ 16 * 999.99        , 0x8e, 0x32, },
 };
 
-static struct tuner_range tuner_philips_fcv1236d_atsc_ranges[] = {
+static const struct tuner_range tuner_philips_fcv1236d_atsc_ranges[] = {
 	{ 16 * 159.00 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 453.00 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_philips_fcv1236d_params[] = {
+static const struct tuner_params tuner_philips_fcv1236d_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_philips_fcv1236d_ntsc_ranges,
@@ -672,13 +672,13 @@ static struct tuner_params tuner_philips_fcv1236d_params[] = {
 
 /* ------------ TUNER_PHILIPS_FM1236_MK3 - Philips NTSC ------------ */
 
-static struct tuner_range tuner_fm1236_mk3_ntsc_ranges[] = {
+static const struct tuner_range tuner_fm1236_mk3_ntsc_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 442.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_fm1236_mk3_params[] = {
+static const struct tuner_params tuner_fm1236_mk3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_fm1236_mk3_ntsc_ranges,
@@ -693,7 +693,7 @@ static struct tuner_params tuner_fm1236_mk3_params[] = {
 
 /* ------------ TUNER_PHILIPS_4IN1 - Philips NTSC ------------ */
 
-static struct tuner_params tuner_philips_4in1_params[] = {
+static const struct tuner_params tuner_philips_4in1_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_fm1236_mk3_ntsc_ranges,
@@ -703,7 +703,7 @@ static struct tuner_params tuner_philips_4in1_params[] = {
 
 /* ------------ TUNER_MICROTUNE_4049FM5 - Microtune PAL ------------ */
 
-static struct tuner_params tuner_microtune_4049_fm5_params[] = {
+static const struct tuner_params tuner_microtune_4049_fm5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_temic_4009f_5_pal_ranges,
@@ -718,13 +718,13 @@ static struct tuner_params tuner_microtune_4049_fm5_params[] = {
 
 /* ------------ TUNER_PANASONIC_VP27 - Panasonic NTSC ------------ */
 
-static struct tuner_range tuner_panasonic_vp27_ntsc_ranges[] = {
+static const struct tuner_range tuner_panasonic_vp27_ntsc_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0xce, 0x01, },
 	{ 16 * 454.00 /*MHz*/, 0xce, 0x02, },
 	{ 16 * 999.99        , 0xce, 0x08, },
 };
 
-static struct tuner_params tuner_panasonic_vp27_params[] = {
+static const struct tuner_params tuner_panasonic_vp27_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_panasonic_vp27_ntsc_ranges,
@@ -739,13 +739,13 @@ static struct tuner_params tuner_panasonic_vp27_params[] = {
 
 /* ------------ TUNER_TNF_8831BGFF - Philips PAL ------------ */
 
-static struct tuner_range tuner_tnf_8831bgff_pal_ranges[] = {
+static const struct tuner_range tuner_tnf_8831bgff_pal_ranges[] = {
 	{ 16 * 161.25 /*MHz*/, 0x8e, 0xa0, },
 	{ 16 * 463.25 /*MHz*/, 0x8e, 0x90, },
 	{ 16 * 999.99        , 0x8e, 0x30, },
 };
 
-static struct tuner_params tuner_tnf_8831bgff_params[] = {
+static const struct tuner_params tuner_tnf_8831bgff_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_tnf_8831bgff_pal_ranges,
@@ -755,19 +755,19 @@ static struct tuner_params tuner_tnf_8831bgff_params[] = {
 
 /* ------------ TUNER_MICROTUNE_4042FI5 - Microtune NTSC ------------ */
 
-static struct tuner_range tuner_microtune_4042fi5_ntsc_ranges[] = {
+static const struct tuner_range tuner_microtune_4042fi5_ntsc_ranges[] = {
 	{ 16 * 162.00 /*MHz*/, 0x8e, 0xa2, },
 	{ 16 * 457.00 /*MHz*/, 0x8e, 0x94, },
 	{ 16 * 999.99        , 0x8e, 0x31, },
 };
 
-static struct tuner_range tuner_microtune_4042fi5_atsc_ranges[] = {
+static const struct tuner_range tuner_microtune_4042fi5_atsc_ranges[] = {
 	{ 16 * 162.00 /*MHz*/, 0x8e, 0xa1, },
 	{ 16 * 457.00 /*MHz*/, 0x8e, 0x91, },
 	{ 16 * 999.99        , 0x8e, 0x31, },
 };
 
-static struct tuner_params tuner_microtune_4042fi5_params[] = {
+static const struct tuner_params tuner_microtune_4042fi5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_microtune_4042fi5_ntsc_ranges,
@@ -784,13 +784,13 @@ static struct tuner_params tuner_microtune_4042fi5_params[] = {
 /* 50-59 */
 /* ------------ TUNER_TCL_2002N - TCL NTSC ------------ */
 
-static struct tuner_range tuner_tcl_2002n_ntsc_ranges[] = {
+static const struct tuner_range tuner_tcl_2002n_ntsc_ranges[] = {
 	{ 16 * 172.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 448.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_tcl_2002n_params[] = {
+static const struct tuner_params tuner_tcl_2002n_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tcl_2002n_ntsc_ranges,
@@ -801,7 +801,7 @@ static struct tuner_params tuner_tcl_2002n_params[] = {
 
 /* ------------ TUNER_PHILIPS_FM1256_IH3 - Philips PAL ------------ */
 
-static struct tuner_params tuner_philips_fm1256_ih3_params[] = {
+static const struct tuner_params tuner_philips_fm1256_ih3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_fm1236_mk3_ntsc_ranges,
@@ -813,13 +813,13 @@ static struct tuner_params tuner_philips_fm1256_ih3_params[] = {
 /* ------------ TUNER_THOMSON_DTT7610 - THOMSON ATSC ------------ */
 
 /* single range used for both ntsc and atsc */
-static struct tuner_range tuner_thomson_dtt7610_ntsc_ranges[] = {
+static const struct tuner_range tuner_thomson_dtt7610_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0x8e, 0x39, },
 	{ 16 * 454.00 /*MHz*/, 0x8e, 0x3a, },
 	{ 16 * 999.99        , 0x8e, 0x3c, },
 };
 
-static struct tuner_params tuner_thomson_dtt7610_params[] = {
+static const struct tuner_params tuner_thomson_dtt7610_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_thomson_dtt7610_ntsc_ranges,
@@ -835,13 +835,13 @@ static struct tuner_params tuner_thomson_dtt7610_params[] = {
 
 /* ------------ TUNER_PHILIPS_FQ1286 - Philips NTSC ------------ */
 
-static struct tuner_range tuner_philips_fq1286_ntsc_ranges[] = {
+static const struct tuner_range tuner_philips_fq1286_ntsc_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0x8e, 0x41, },
 	{ 16 * 454.00 /*MHz*/, 0x8e, 0x42, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_philips_fq1286_params[] = {
+static const struct tuner_params tuner_philips_fq1286_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_philips_fq1286_ntsc_ranges,
@@ -851,13 +851,13 @@ static struct tuner_params tuner_philips_fq1286_params[] = {
 
 /* ------------ TUNER_TCL_2002MB - TCL PAL ------------ */
 
-static struct tuner_range tuner_tcl_2002mb_pal_ranges[] = {
+static const struct tuner_range tuner_tcl_2002mb_pal_ranges[] = {
 	{ 16 * 170.00 /*MHz*/, 0xce, 0x01, },
 	{ 16 * 450.00 /*MHz*/, 0xce, 0x02, },
 	{ 16 * 999.99        , 0xce, 0x08, },
 };
 
-static struct tuner_params tuner_tcl_2002mb_params[] = {
+static const struct tuner_params tuner_tcl_2002mb_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_tcl_2002mb_pal_ranges,
@@ -867,13 +867,13 @@ static struct tuner_params tuner_tcl_2002mb_params[] = {
 
 /* ------------ TUNER_PHILIPS_FQ1216AME_MK4 - Philips PAL ------------ */
 
-static struct tuner_range tuner_philips_fq12_6a___mk4_pal_ranges[] = {
+static const struct tuner_range tuner_philips_fq12_6a___mk4_pal_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0xce, 0x01, },
 	{ 16 * 442.00 /*MHz*/, 0xce, 0x02, },
 	{ 16 * 999.99        , 0xce, 0x04, },
 };
 
-static struct tuner_params tuner_philips_fq1216ame_mk4_params[] = {
+static const struct tuner_params tuner_philips_fq1216ame_mk4_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_fq12_6a___mk4_pal_ranges,
@@ -890,7 +890,7 @@ static struct tuner_params tuner_philips_fq1216ame_mk4_params[] = {
 
 /* ------------ TUNER_PHILIPS_FQ1236A_MK4 - Philips NTSC ------------ */
 
-static struct tuner_params tuner_philips_fq1236a_mk4_params[] = {
+static const struct tuner_params tuner_philips_fq1236a_mk4_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_fm1236_mk3_ntsc_ranges,
@@ -900,7 +900,7 @@ static struct tuner_params tuner_philips_fq1236a_mk4_params[] = {
 
 /* ------------ TUNER_YMEC_TVF_8531MF - Philips NTSC ------------ */
 
-static struct tuner_params tuner_ymec_tvf_8531mf_params[] = {
+static const struct tuner_params tuner_ymec_tvf_8531mf_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_philips_ntsc_m_ranges,
@@ -910,13 +910,13 @@ static struct tuner_params tuner_ymec_tvf_8531mf_params[] = {
 
 /* ------------ TUNER_YMEC_TVF_5533MF - Philips NTSC ------------ */
 
-static struct tuner_range tuner_ymec_tvf_5533mf_ntsc_ranges[] = {
+static const struct tuner_range tuner_ymec_tvf_5533mf_ntsc_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 454.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_ymec_tvf_5533mf_params[] = {
+static const struct tuner_params tuner_ymec_tvf_5533mf_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_ymec_tvf_5533mf_ntsc_ranges,
@@ -928,19 +928,19 @@ static struct tuner_params tuner_ymec_tvf_5533mf_params[] = {
 /* ------------ TUNER_THOMSON_DTT761X - THOMSON ATSC ------------ */
 /* DTT 7611 7611A 7612 7613 7613A 7614 7615 7615A */
 
-static struct tuner_range tuner_thomson_dtt761x_ntsc_ranges[] = {
+static const struct tuner_range tuner_thomson_dtt761x_ntsc_ranges[] = {
 	{ 16 * 145.25 /*MHz*/, 0x8e, 0x39, },
 	{ 16 * 415.25 /*MHz*/, 0x8e, 0x3a, },
 	{ 16 * 999.99        , 0x8e, 0x3c, },
 };
 
-static struct tuner_range tuner_thomson_dtt761x_atsc_ranges[] = {
+static const struct tuner_range tuner_thomson_dtt761x_atsc_ranges[] = {
 	{ 16 * 147.00 /*MHz*/, 0x8e, 0x39, },
 	{ 16 * 417.00 /*MHz*/, 0x8e, 0x3a, },
 	{ 16 * 999.99        , 0x8e, 0x3c, },
 };
 
-static struct tuner_params tuner_thomson_dtt761x_params[] = {
+static const struct tuner_params tuner_thomson_dtt761x_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_thomson_dtt761x_ntsc_ranges,
@@ -959,13 +959,13 @@ static struct tuner_params tuner_thomson_dtt761x_params[] = {
 
 /* ------------ TUNER_TENA_9533_DI - Philips PAL ------------ */
 
-static struct tuner_range tuner_tena_9533_di_pal_ranges[] = {
+static const struct tuner_range tuner_tena_9533_di_pal_ranges[] = {
 	{ 16 * 160.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 464.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_tena_9533_di_params[] = {
+static const struct tuner_params tuner_tena_9533_di_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_tena_9533_di_pal_ranges,
@@ -975,13 +975,13 @@ static struct tuner_params tuner_tena_9533_di_params[] = {
 
 /* ------------ TUNER_TENA_TNF_5337 - Tena tnf5337MFD STD M/N ------------ */
 
-static struct tuner_range tuner_tena_tnf_5337_ntsc_ranges[] = {
+static const struct tuner_range tuner_tena_tnf_5337_ntsc_ranges[] = {
 	{ 16 * 166.25 /*MHz*/, 0x86, 0x01, },
 	{ 16 * 466.25 /*MHz*/, 0x86, 0x02, },
 	{ 16 * 999.99        , 0x86, 0x08, },
 };
 
-static struct tuner_params tuner_tena_tnf_5337_params[] = {
+static const struct tuner_params tuner_tena_tnf_5337_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tena_tnf_5337_ntsc_ranges,
@@ -991,13 +991,13 @@ static struct tuner_params tuner_tena_tnf_5337_params[] = {
 
 /* ------------ TUNER_PHILIPS_FMD1216ME(X)_MK3 - Philips PAL ------------ */
 
-static struct tuner_range tuner_philips_fmd1216me_mk3_pal_ranges[] = {
+static const struct tuner_range tuner_philips_fmd1216me_mk3_pal_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0x86, 0x51, },
 	{ 16 * 442.00 /*MHz*/, 0x86, 0x52, },
 	{ 16 * 999.99        , 0x86, 0x54, },
 };
 
-static struct tuner_range tuner_philips_fmd1216me_mk3_dvb_ranges[] = {
+static const struct tuner_range tuner_philips_fmd1216me_mk3_dvb_ranges[] = {
 	{ 16 * 143.87 /*MHz*/, 0xbc, 0x41 },
 	{ 16 * 158.87 /*MHz*/, 0xf4, 0x41 },
 	{ 16 * 329.87 /*MHz*/, 0xbc, 0x42 },
@@ -1007,7 +1007,7 @@ static struct tuner_range tuner_philips_fmd1216me_mk3_dvb_ranges[] = {
 	{ 16 * 999.99        , 0xfc, 0x44 },
 };
 
-static struct tuner_params tuner_philips_fmd1216me_mk3_params[] = {
+static const struct tuner_params tuner_philips_fmd1216me_mk3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_fmd1216me_mk3_pal_ranges,
@@ -1027,7 +1027,7 @@ static struct tuner_params tuner_philips_fmd1216me_mk3_params[] = {
 	},
 };
 
-static struct tuner_params tuner_philips_fmd1216mex_mk3_params[] = {
+static const struct tuner_params tuner_philips_fmd1216mex_mk3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_fmd1216me_mk3_pal_ranges,
@@ -1051,19 +1051,19 @@ static struct tuner_params tuner_philips_fmd1216mex_mk3_params[] = {
 
 /* ------ TUNER_LG_TDVS_H06XF - LG INNOTEK / INFINEON ATSC ----- */
 
-static struct tuner_range tuner_tua6034_ntsc_ranges[] = {
+static const struct tuner_range tuner_tua6034_ntsc_ranges[] = {
 	{ 16 * 165.00 /*MHz*/, 0x8e, 0x01 },
 	{ 16 * 450.00 /*MHz*/, 0x8e, 0x02 },
 	{ 16 * 999.99        , 0x8e, 0x04 },
 };
 
-static struct tuner_range tuner_tua6034_atsc_ranges[] = {
+static const struct tuner_range tuner_tua6034_atsc_ranges[] = {
 	{ 16 * 165.00 /*MHz*/, 0xce, 0x01 },
 	{ 16 * 450.00 /*MHz*/, 0xce, 0x02 },
 	{ 16 * 999.99        , 0xce, 0x04 },
 };
 
-static struct tuner_params tuner_lg_tdvs_h06xf_params[] = {
+static const struct tuner_params tuner_lg_tdvs_h06xf_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tua6034_ntsc_ranges,
@@ -1079,13 +1079,13 @@ static struct tuner_params tuner_lg_tdvs_h06xf_params[] = {
 
 /* ------------ TUNER_YMEC_TVF66T5_B_DFF - Philips PAL ------------ */
 
-static struct tuner_range tuner_ymec_tvf66t5_b_dff_pal_ranges[] = {
+static const struct tuner_range tuner_ymec_tvf66t5_b_dff_pal_ranges[] = {
 	{ 16 * 160.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 464.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_ymec_tvf66t5_b_dff_params[] = {
+static const struct tuner_params tuner_ymec_tvf66t5_b_dff_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_ymec_tvf66t5_b_dff_pal_ranges,
@@ -1095,19 +1095,19 @@ static struct tuner_params tuner_ymec_tvf66t5_b_dff_params[] = {
 
 /* ------------ TUNER_LG_NTSC_TALN_MINI - LGINNOTEK NTSC ------------ */
 
-static struct tuner_range tuner_lg_taln_ntsc_ranges[] = {
+static const struct tuner_range tuner_lg_taln_ntsc_ranges[] = {
 	{ 16 * 137.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 373.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_range tuner_lg_taln_pal_secam_ranges[] = {
+static const struct tuner_range tuner_lg_taln_pal_secam_ranges[] = {
 	{ 16 * 150.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 425.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_lg_taln_params[] = {
+static const struct tuner_params tuner_lg_taln_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_lg_taln_ntsc_ranges,
@@ -1121,13 +1121,13 @@ static struct tuner_params tuner_lg_taln_params[] = {
 
 /* ------------ TUNER_PHILIPS_TD1316 - Philips PAL ------------ */
 
-static struct tuner_range tuner_philips_td1316_pal_ranges[] = {
+static const struct tuner_range tuner_philips_td1316_pal_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0xc8, 0xa1, },
 	{ 16 * 442.00 /*MHz*/, 0xc8, 0xa2, },
 	{ 16 * 999.99        , 0xc8, 0xa4, },
 };
 
-static struct tuner_range tuner_philips_td1316_dvb_ranges[] = {
+static const struct tuner_range tuner_philips_td1316_dvb_ranges[] = {
 	{ 16 *  93.834 /*MHz*/, 0xca, 0x60, },
 	{ 16 * 123.834 /*MHz*/, 0xca, 0xa0, },
 	{ 16 * 163.834 /*MHz*/, 0xca, 0xc0, },
@@ -1139,7 +1139,7 @@ static struct tuner_range tuner_philips_td1316_dvb_ranges[] = {
 	{ 16 * 999.999        , 0xca, 0xe0, },
 };
 
-static struct tuner_params tuner_philips_td1316_params[] = {
+static const struct tuner_params tuner_philips_td1316_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_philips_td1316_pal_ranges,
@@ -1155,19 +1155,19 @@ static struct tuner_params tuner_philips_td1316_params[] = {
 
 /* ------------ TUNER_PHILIPS_TUV1236D - Philips ATSC ------------ */
 
-static struct tuner_range tuner_tuv1236d_ntsc_ranges[] = {
+static const struct tuner_range tuner_tuv1236d_ntsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0xce, 0x01, },
 	{ 16 * 454.00 /*MHz*/, 0xce, 0x02, },
 	{ 16 * 999.99        , 0xce, 0x04, },
 };
 
-static struct tuner_range tuner_tuv1236d_atsc_ranges[] = {
+static const struct tuner_range tuner_tuv1236d_atsc_ranges[] = {
 	{ 16 * 157.25 /*MHz*/, 0xc6, 0x41, },
 	{ 16 * 454.00 /*MHz*/, 0xc6, 0x42, },
 	{ 16 * 999.99        , 0xc6, 0x44, },
 };
 
-static struct tuner_params tuner_tuv1236d_params[] = {
+static const struct tuner_params tuner_tuv1236d_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tuv1236d_ntsc_ranges,
@@ -1187,19 +1187,19 @@ static struct tuner_params tuner_tuv1236d_params[] = {
  *	models based on TI SN 761677 chip on both PAL and NTSC
  */
 
-static struct tuner_range tuner_tnf_5335_d_if_pal_ranges[] = {
+static const struct tuner_range tuner_tnf_5335_d_if_pal_ranges[] = {
 	{ 16 * 168.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 471.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_range tuner_tnf_5335mf_ntsc_ranges[] = {
+static const struct tuner_range tuner_tnf_5335mf_ntsc_ranges[] = {
 	{ 16 * 169.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 469.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x08, },
 };
 
-static struct tuner_params tuner_tnf_5335mf_params[] = {
+static const struct tuner_params tuner_tnf_5335mf_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tnf_5335mf_ntsc_ranges,
@@ -1216,13 +1216,13 @@ static struct tuner_params tuner_tnf_5335mf_params[] = {
 /* ------------ TUNER_SAMSUNG_TCPN_2121P30A - Samsung NTSC ------------ */
 
 /* '+ 4' turns on the Low Noise Amplifier */
-static struct tuner_range tuner_samsung_tcpn_2121p30a_ntsc_ranges[] = {
+static const struct tuner_range tuner_samsung_tcpn_2121p30a_ntsc_ranges[] = {
 	{ 16 * 130.00 /*MHz*/, 0xce, 0x01 + 4, },
 	{ 16 * 364.50 /*MHz*/, 0xce, 0x02 + 4, },
 	{ 16 * 999.99        , 0xce, 0x08 + 4, },
 };
 
-static struct tuner_params tuner_samsung_tcpn_2121p30a_params[] = {
+static const struct tuner_params tuner_samsung_tcpn_2121p30a_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_samsung_tcpn_2121p30a_ntsc_ranges,
@@ -1232,20 +1232,20 @@ static struct tuner_params tuner_samsung_tcpn_2121p30a_params[] = {
 
 /* ------------ TUNER_THOMSON_FE6600 - DViCO Hybrid PAL ------------ */
 
-static struct tuner_range tuner_thomson_fe6600_pal_ranges[] = {
+static const struct tuner_range tuner_thomson_fe6600_pal_ranges[] = {
 	{ 16 * 160.00 /*MHz*/, 0xfe, 0x11, },
 	{ 16 * 442.00 /*MHz*/, 0xf6, 0x12, },
 	{ 16 * 999.99        , 0xf6, 0x18, },
 };
 
-static struct tuner_range tuner_thomson_fe6600_dvb_ranges[] = {
+static const struct tuner_range tuner_thomson_fe6600_dvb_ranges[] = {
 	{ 16 * 250.00 /*MHz*/, 0xb4, 0x12, },
 	{ 16 * 455.00 /*MHz*/, 0xfe, 0x11, },
 	{ 16 * 775.50 /*MHz*/, 0xbc, 0x18, },
 	{ 16 * 999.99        , 0xf4, 0x18, },
 };
 
-static struct tuner_params tuner_thomson_fe6600_params[] = {
+static const struct tuner_params tuner_thomson_fe6600_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_thomson_fe6600_pal_ranges,
@@ -1262,13 +1262,13 @@ static struct tuner_params tuner_thomson_fe6600_params[] = {
 /* ------------ TUNER_SAMSUNG_TCPG_6121P30A - Samsung PAL ------------ */
 
 /* '+ 4' turns on the Low Noise Amplifier */
-static struct tuner_range tuner_samsung_tcpg_6121p30a_pal_ranges[] = {
+static const struct tuner_range tuner_samsung_tcpg_6121p30a_pal_ranges[] = {
 	{ 16 * 146.25 /*MHz*/, 0xce, 0x01 + 4, },
 	{ 16 * 428.50 /*MHz*/, 0xce, 0x02 + 4, },
 	{ 16 * 999.99        , 0xce, 0x08 + 4, },
 };
 
-static struct tuner_params tuner_samsung_tcpg_6121p30a_params[] = {
+static const struct tuner_params tuner_samsung_tcpg_6121p30a_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_samsung_tcpg_6121p30a_pal_ranges,
@@ -1282,13 +1282,13 @@ static struct tuner_params tuner_samsung_tcpg_6121p30a_params[] = {
 
 /* ------------ TUNER_TCL_MF02GIP-5N-E - TCL MF02GIP-5N ------------ */
 
-static struct tuner_range tuner_tcl_mf02gip_5n_ntsc_ranges[] = {
+static const struct tuner_range tuner_tcl_mf02gip_5n_ntsc_ranges[] = {
 	{ 16 * 172.00 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 448.00 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_tcl_mf02gip_5n_params[] = {
+static const struct tuner_params tuner_tcl_mf02gip_5n_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_tcl_mf02gip_5n_ntsc_ranges,
@@ -1300,7 +1300,7 @@ static struct tuner_params tuner_tcl_mf02gip_5n_params[] = {
 /* 80-89 */
 /* --------- TUNER_PHILIPS_FQ1216LME_MK3 -- active loopthrough, no FM ------- */
 
-static struct tuner_params tuner_fq1216lme_mk3_params[] = {
+static const struct tuner_params tuner_fq1216lme_mk3_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_fm1216me_mk3_pal_ranges,
@@ -1321,7 +1321,7 @@ static struct tuner_params tuner_fq1216lme_mk3_params[] = {
 
 /* ----- TUNER_PARTSNIC_PTI_5NF05 - Partsnic (Daewoo) PTI-5NF05 NTSC ----- */
 
-static struct tuner_range tuner_partsnic_pti_5nf05_ranges[] = {
+static const struct tuner_range tuner_partsnic_pti_5nf05_ranges[] = {
 	/* The datasheet specified channel ranges and the bandswitch byte */
 	/* The control byte value of 0x8e is just a guess */
 	{ 16 * 133.25 /*MHz*/, 0x8e, 0x01, }, /* Channels    2 -    B */
@@ -1329,7 +1329,7 @@ static struct tuner_range tuner_partsnic_pti_5nf05_ranges[] = {
 	{ 16 * 999.99        , 0x8e, 0x08, }, /* Channels W+12 -   69 */
 };
 
-static struct tuner_params tuner_partsnic_pti_5nf05_params[] = {
+static const struct tuner_params tuner_partsnic_pti_5nf05_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_partsnic_pti_5nf05_ranges,
@@ -1340,13 +1340,13 @@ static struct tuner_params tuner_partsnic_pti_5nf05_params[] = {
 
 /* --------- TUNER_PHILIPS_CU1216L - DVB-C NIM ------------------------- */
 
-static struct tuner_range tuner_cu1216l_ranges[] = {
+static const struct tuner_range tuner_cu1216l_ranges[] = {
 	{ 16 * 160.25 /*MHz*/, 0xce, 0x01 },
 	{ 16 * 444.25 /*MHz*/, 0xce, 0x02 },
 	{ 16 * 999.99        , 0xce, 0x04 },
 };
 
-static struct tuner_params tuner_philips_cu1216l_params[] = {
+static const struct tuner_params tuner_philips_cu1216l_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_DIGITAL,
 		.ranges = tuner_cu1216l_ranges,
@@ -1357,13 +1357,13 @@ static struct tuner_params tuner_philips_cu1216l_params[] = {
 
 /* ---------------------- TUNER_SONY_BTF_PXN01Z ------------------------ */
 
-static struct tuner_range tuner_sony_btf_pxn01z_ranges[] = {
+static const struct tuner_range tuner_sony_btf_pxn01z_ranges[] = {
 	{ 16 * 137.25 /*MHz*/, 0x8e, 0x01, },
 	{ 16 * 367.25 /*MHz*/, 0x8e, 0x02, },
 	{ 16 * 999.99        , 0x8e, 0x04, },
 };
 
-static struct tuner_params tuner_sony_btf_pxn01z_params[] = {
+static const struct tuner_params tuner_sony_btf_pxn01z_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_sony_btf_pxn01z_ranges,
@@ -1373,7 +1373,7 @@ static struct tuner_params tuner_sony_btf_pxn01z_params[] = {
 
 /* ------------ TUNER_PHILIPS_FQ1236_MK5 - Philips NTSC ------------ */
 
-static struct tuner_params tuner_philips_fq1236_mk5_params[] = {
+static const struct tuner_params tuner_philips_fq1236_mk5_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_fm1236_mk3_ntsc_ranges,
@@ -1384,13 +1384,13 @@ static struct tuner_params tuner_philips_fq1236_mk5_params[] = {
 
 /* --------- Sony BTF-PG472Z PAL/SECAM ------- */
 
-static struct tuner_range tuner_sony_btf_pg472z_ranges[] = {
+static const struct tuner_range tuner_sony_btf_pg472z_ranges[] = {
 	{ 16 * 144.25 /*MHz*/, 0xc6, 0x01, },
 	{ 16 * 427.25 /*MHz*/, 0xc6, 0x02, },
 	{ 16 * 999.99        , 0xc6, 0x04, },
 };
 
-static struct tuner_params tuner_sony_btf_pg472z_params[] = {
+static const struct tuner_params tuner_sony_btf_pg472z_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_PAL,
 		.ranges = tuner_sony_btf_pg472z_ranges,
@@ -1404,13 +1404,13 @@ static struct tuner_params tuner_sony_btf_pg472z_params[] = {
 /* 90-99 */
 /* --------- Sony BTF-PG467Z NTSC-M-JP ------- */
 
-static struct tuner_range tuner_sony_btf_pg467z_ranges[] = {
+static const struct tuner_range tuner_sony_btf_pg467z_ranges[] = {
 	{ 16 * 220.25 /*MHz*/, 0xc6, 0x01, },
 	{ 16 * 467.25 /*MHz*/, 0xc6, 0x02, },
 	{ 16 * 999.99        , 0xc6, 0x04, },
 };
 
-static struct tuner_params tuner_sony_btf_pg467z_params[] = {
+static const struct tuner_params tuner_sony_btf_pg467z_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_sony_btf_pg467z_ranges,
@@ -1420,13 +1420,13 @@ static struct tuner_params tuner_sony_btf_pg467z_params[] = {
 
 /* --------- Sony BTF-PG463Z NTSC-M ------- */
 
-static struct tuner_range tuner_sony_btf_pg463z_ranges[] = {
+static const struct tuner_range tuner_sony_btf_pg463z_ranges[] = {
 	{ 16 * 130.25 /*MHz*/, 0xc6, 0x01, },
 	{ 16 * 364.25 /*MHz*/, 0xc6, 0x02, },
 	{ 16 * 999.99        , 0xc6, 0x04, },
 };
 
-static struct tuner_params tuner_sony_btf_pg463z_params[] = {
+static const struct tuner_params tuner_sony_btf_pg463z_params[] = {
 	{
 		.type   = TUNER_PARAM_TYPE_NTSC,
 		.ranges = tuner_sony_btf_pg463z_ranges,
@@ -1434,9 +1434,25 @@ static struct tuner_params tuner_sony_btf_pg463z_params[] = {
 	},
 };
 
+/* ------------- TUNER_TENA_TNF_931D_DFDR1 - NXP TDA6509A ------------- */
+
+static const struct tuner_range tuner_tena_tnf_931d_dfdr1_ranges[] = {
+	{ 16 * 161.15 /*MHz*/, 0x8e, 0x01, },
+	{ 16 * 463.15 /*MHz*/, 0x8e, 0x02, },
+	{ 16 * 999.99        , 0x8e, 0x08, },
+};
+
+static const struct tuner_params tuner_tena_tnf_931d_dfdr1_params[] = {
+	{
+		.type   = TUNER_PARAM_TYPE_PAL,
+		.ranges = tuner_tena_tnf_931d_dfdr1_ranges,
+		.count  = ARRAY_SIZE(tuner_tena_tnf_931d_dfdr1_ranges),
+	},
+};
+
 /* --------------------------------------------------------------------- */
 
-struct tunertype tuners[] = {
+const struct tunertype tuners[] = {
 	/* 0-9 */
 	[TUNER_TEMIC_PAL] = { /* TEMIC PAL */
 		.name   = "Temic PAL (4002 FH5)",
@@ -1946,6 +1962,11 @@ struct tunertype tuners[] = {
 		.name   = "Silicon Labs Si2157 tuner",
 		/* see si2157.c for details */
 	},
+	[TUNER_TENA_TNF_931D_DFDR1] = {
+		.name = "Tena TNF931D-DFDR1",
+		.params = tuner_tena_tnf_931d_dfdr1_params,
+		.count = ARRAY_SIZE(tuner_tena_tnf_931d_dfdr1_params),
+	}
 };
 EXPORT_SYMBOL(tuners);
 
diff --git a/drivers/media/tuners/xc2028.c b/drivers/media/tuners/xc2028.c
index 5a967edceca9..8e6638e5f688 100644
--- a/drivers/media/tuners/xc2028.c
+++ b/drivers/media/tuners/xc2028.c
@@ -14,7 +14,7 @@
 #include <media/tuner.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include "tuner-i2c.h"
 #include "xc2028.h"
 #include "xc2028-types.h"
@@ -1361,9 +1361,16 @@ static void load_firmware_cb(const struct firmware *fw,
 			     void *context)
 {
 	struct dvb_frontend *fe = context;
-	struct xc2028_data *priv = fe->tuner_priv;
+	struct xc2028_data *priv;
 	int rc;
 
+	if (!fe) {
+		pr_warn("xc2028: No frontend in %s\n", __func__);
+		return;
+	}
+
+	priv = fe->tuner_priv;
+
 	tuner_dbg("request_firmware_nowait(): %s\n", fw ? "OK" : "error");
 	if (!fw) {
 		tuner_err("Could not load firmware %s.\n", priv->fname);
diff --git a/drivers/media/tuners/xc4000.c b/drivers/media/tuners/xc4000.c
index 29bc63021c5a..3cf54d776d36 100644
--- a/drivers/media/tuners/xc4000.c
+++ b/drivers/media/tuners/xc4000.c
@@ -16,7 +16,7 @@
 #include <linux/dvb/frontend.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/dvb_frontend.h>
 
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index 2182e5b7b606..30aa4ee958bd 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -58,7 +58,7 @@ struct xc5000_priv {
 	struct dvb_frontend *fe;
 	struct delayed_work timer_sleep;
 
-	const struct firmware   *firmware;
+	bool inited;
 };
 
 /* Misc Defines */
@@ -1110,23 +1110,19 @@ static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force)
 	if (!force && xc5000_is_firmware_loaded(fe) == 0)
 		return 0;
 
-	if (!priv->firmware) {
-		ret = request_firmware(&fw, desired_fw->name,
-					priv->i2c_props.adap->dev.parent);
-		if (ret) {
-			pr_err("xc5000: Upload failed. rc %d\n", ret);
-			return ret;
-		}
-		dprintk(1, "firmware read %zu bytes.\n", fw->size);
+	ret = request_firmware(&fw, desired_fw->name,
+			       priv->i2c_props.adap->dev.parent);
+	if (ret) {
+		pr_err("xc5000: Upload failed. rc %d\n", ret);
+		return ret;
+	}
+	dprintk(1, "firmware read %zu bytes.\n", fw->size);
 
-		if (fw->size != desired_fw->size) {
-			pr_err("xc5000: Firmware file with incorrect size\n");
-			release_firmware(fw);
-			return -EINVAL;
-		}
-		priv->firmware = fw;
-	} else
-		fw = priv->firmware;
+	if (fw->size != desired_fw->size) {
+		pr_err("xc5000: Firmware file with incorrect size\n");
+		release_firmware(fw);
+		return -EINVAL;
+	}
 
 	/* Try up to 5 times to load firmware */
 	for (i = 0; i < 5; i++) {
@@ -1204,6 +1200,7 @@ static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force)
 	}
 
 err:
+	release_firmware(fw);
 	if (!ret)
 		printk(KERN_INFO "xc5000: Firmware %s loaded and running.\n",
 		       desired_fw->name);
@@ -1274,7 +1271,7 @@ static int xc5000_resume(struct dvb_frontend *fe)
 
 	/* suspended before firmware is loaded.
 	   Avoid firmware load in resume path. */
-	if (!priv->firmware)
+	if (!priv->inited)
 		return 0;
 
 	return xc5000_set_params(fe);
@@ -1293,6 +1290,8 @@ static int xc5000_init(struct dvb_frontend *fe)
 	if (debug)
 		xc_debug_dump(priv);
 
+	priv->inited = true;
+
 	return 0;
 }
 
@@ -1306,10 +1305,6 @@ static void xc5000_release(struct dvb_frontend *fe)
 
 	if (priv) {
 		cancel_delayed_work(&priv->timer_sleep);
-		if (priv->firmware) {
-			release_firmware(priv->firmware);
-			priv->firmware = NULL;
-		}
 		hybrid_tuner_release_state(priv);
 	}
 
diff --git a/drivers/media/usb/airspy/airspy.c b/drivers/media/usb/airspy/airspy.c
index e24e655fb1db..08f0920cf6ca 100644
--- a/drivers/media/usb/airspy/airspy.c
+++ b/drivers/media/usb/airspy/airspy.c
@@ -603,8 +603,6 @@ static const struct vb2_ops airspy_vb2_ops = {
 	.buf_queue              = airspy_buf_queue,
 	.start_streaming        = airspy_start_streaming,
 	.stop_streaming         = airspy_stop_streaming,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 static int airspy_querycap(struct file *file, void *fh,
@@ -1017,6 +1015,7 @@ static int airspy_probe(struct usb_interface *intf,
 	s->vb_queue.ops = &airspy_vb2_ops;
 	s->vb_queue.mem_ops = &vb2_vmalloc_memops;
 	s->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	s->vb_queue.lock = &s->vb_queue_lock;
 	ret = vb2_queue_init(&s->vb_queue);
 	if (ret) {
 		dev_err(s->dev, "Could not initialize vb2 queue\n");
@@ -1026,7 +1025,6 @@ static int airspy_probe(struct usb_interface *intf,
 	/* Init video_device structure */
 	s->vdev = airspy_template;
 	s->vdev.queue = &s->vb_queue;
-	s->vdev.queue->lock = &s->vb_queue_lock;
 	video_set_drvdata(&s->vdev, s);
 
 	/* Register the v4l2_device structure */
diff --git a/drivers/media/usb/as102/as102_usb_drv.c b/drivers/media/usb/as102/as102_usb_drv.c
index 6b380144d6c2..e0ef66a522e2 100644
--- a/drivers/media/usb/as102/as102_usb_drv.c
+++ b/drivers/media/usb/as102/as102_usb_drv.c
@@ -259,7 +259,7 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 	for (i = 0; i < MAX_STREAM_URB; i++) {
 		struct urb *urb;
 
-		urb = usb_alloc_urb(0, GFP_ATOMIC);
+		urb = usb_alloc_urb(0, GFP_KERNEL);
 		if (urb == NULL) {
 			as102_free_usb_stream_buffer(dev);
 			return -ENOMEM;
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index 09f9948c6f8e..5d0447ff7d06 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -97,7 +97,7 @@ static void au0828_restart_dvb_streaming(struct work_struct *work);
 
 static void au0828_bulk_timeout(struct timer_list *t)
 {
-	struct au0828_dev *dev = from_timer(dev, t, bulk_timeout);
+	struct au0828_dev *dev = timer_container_of(dev, t, bulk_timeout);
 
 	dprintk(1, "%s called\n", __func__);
 	dev->bulk_timeout_running = 0;
@@ -143,7 +143,7 @@ static void urb_completion(struct urb *purb)
 		 */
 		dprintk(1, "%s cancelling bulk timeout\n", __func__);
 		dev->bulk_timeout_running = 0;
-		del_timer(&dev->bulk_timeout);
+		timer_delete(&dev->bulk_timeout);
 	}
 
 	/* Feed the transport payload into the kernel demux */
@@ -168,7 +168,7 @@ static int stop_urb_transfer(struct au0828_dev *dev)
 
 	if (dev->bulk_timeout_running == 1) {
 		dev->bulk_timeout_running = 0;
-		del_timer(&dev->bulk_timeout);
+		timer_delete(&dev->bulk_timeout);
 	}
 
 	dev->urb_streaming = false;
diff --git a/drivers/media/usb/au0828/au0828-vbi.c b/drivers/media/usb/au0828/au0828-vbi.c
index b0333637b747..11203adf47ea 100644
--- a/drivers/media/usb/au0828/au0828-vbi.c
+++ b/drivers/media/usb/au0828/au0828-vbi.c
@@ -74,6 +74,4 @@ const struct vb2_ops au0828_vbi_qops = {
 	.prepare_streaming = v4l_vb2q_enable_media_source,
 	.start_streaming = au0828_start_analog_streaming,
 	.stop_streaming  = au0828_stop_vbi_streaming,
-	.wait_prepare    = vb2_ops_wait_prepare,
-	.wait_finish     = vb2_ops_wait_finish,
 };
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index fd9fc43d47e0..e5dff969ed57 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -602,10 +602,7 @@ static inline int au0828_isoc_copy(struct au0828_dev *dev, struct urb *urb)
 		vbi_field_size = dev->vbi_width * dev->vbi_height * 2;
 		if (dev->vbi_read < vbi_field_size) {
 			remain  = vbi_field_size - dev->vbi_read;
-			if (len < remain)
-				lencopy = len;
-			else
-				lencopy = remain;
+			lencopy = umin(len, remain);
 
 			if (vbi_buf != NULL)
 				au0828_copy_vbi(dev, vbi_dma_q, vbi_buf, p,
@@ -860,7 +857,7 @@ static void au0828_stop_streaming(struct vb2_queue *vq)
 	}
 
 	dev->vid_timeout_running = 0;
-	del_timer_sync(&dev->vid_timeout);
+	timer_delete_sync(&dev->vid_timeout);
 
 	spin_lock_irqsave(&dev->slock, flags);
 	if (dev->isoc_ctl.buf != NULL) {
@@ -908,7 +905,7 @@ void au0828_stop_vbi_streaming(struct vb2_queue *vq)
 	spin_unlock_irqrestore(&dev->slock, flags);
 
 	dev->vbi_timeout_running = 0;
-	del_timer_sync(&dev->vbi_timeout);
+	timer_delete_sync(&dev->vbi_timeout);
 }
 
 static const struct vb2_ops au0828_video_qops = {
@@ -918,8 +915,6 @@ static const struct vb2_ops au0828_video_qops = {
 	.prepare_streaming = v4l_vb2q_enable_media_source,
 	.start_streaming = au0828_start_analog_streaming,
 	.stop_streaming  = au0828_stop_streaming,
-	.wait_prepare    = vb2_ops_wait_prepare,
-	.wait_finish     = vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------
@@ -953,7 +948,7 @@ int au0828_analog_unregister(struct au0828_dev *dev)
    such as tvtime from hanging) */
 static void au0828_vid_buffer_timeout(struct timer_list *t)
 {
-	struct au0828_dev *dev = from_timer(dev, t, vid_timeout);
+	struct au0828_dev *dev = timer_container_of(dev, t, vid_timeout);
 	struct au0828_dmaqueue *dma_q = &dev->vidq;
 	struct au0828_buffer *buf;
 	unsigned char *vid_data;
@@ -977,7 +972,7 @@ static void au0828_vid_buffer_timeout(struct timer_list *t)
 
 static void au0828_vbi_buffer_timeout(struct timer_list *t)
 {
-	struct au0828_dev *dev = from_timer(dev, t, vbi_timeout);
+	struct au0828_dev *dev = timer_container_of(dev, t, vbi_timeout);
 	struct au0828_dmaqueue *dma_q = &dev->vbiq;
 	struct au0828_buffer *buf;
 	unsigned char *vbi_data;
@@ -1045,12 +1040,12 @@ static int au0828_v4l2_close(struct file *filp)
 	if (vdev->vfl_type == VFL_TYPE_VIDEO && dev->vid_timeout_running) {
 		/* Cancel timeout thread in case they didn't call streamoff */
 		dev->vid_timeout_running = 0;
-		del_timer_sync(&dev->vid_timeout);
+		timer_delete_sync(&dev->vid_timeout);
 	} else if (vdev->vfl_type == VFL_TYPE_VBI &&
 			dev->vbi_timeout_running) {
 		/* Cancel timeout thread in case they didn't call streamoff */
 		dev->vbi_timeout_running = 0;
-		del_timer_sync(&dev->vbi_timeout);
+		timer_delete_sync(&dev->vbi_timeout);
 	}
 
 	if (test_bit(DEV_DISCONNECTED, &dev->dev_state))
@@ -1699,9 +1694,9 @@ void au0828_v4l2_suspend(struct au0828_dev *dev)
 	}
 
 	if (dev->vid_timeout_running)
-		del_timer_sync(&dev->vid_timeout);
+		timer_delete_sync(&dev->vid_timeout);
 	if (dev->vbi_timeout_running)
-		del_timer_sync(&dev->vbi_timeout);
+		timer_delete_sync(&dev->vbi_timeout);
 }
 
 void au0828_v4l2_resume(struct au0828_dev *dev)
diff --git a/drivers/media/usb/b2c2/flexcop-usb.c b/drivers/media/usb/b2c2/flexcop-usb.c
index 790787f0eba8..8033622543f2 100644
--- a/drivers/media/usb/b2c2/flexcop-usb.c
+++ b/drivers/media/usb/b2c2/flexcop-usb.c
@@ -179,7 +179,7 @@ static int flexcop_usb_memory_req(struct flexcop_usb *fc_usb,
 		flexcop_usb_request_t req, flexcop_usb_mem_page_t page_start,
 		u32 addr, int extended, u8 *buf, u32 len)
 {
-	int i, ret = 0;
+	int ret = 0;
 	u16 wMax;
 	u32 pagechunk = 0;
 
@@ -196,11 +196,8 @@ static int flexcop_usb_memory_req(struct flexcop_usb *fc_usb,
 	default:
 		return -EINVAL;
 	}
-	for (i = 0; i < len;) {
-		pagechunk =
-			wMax < bytes_left_to_read_on_page(addr, len) ?
-				wMax :
-				bytes_left_to_read_on_page(addr, len);
+	while (len) {
+		pagechunk = min(wMax, bytes_left_to_read_on_page(addr, len));
 		deb_info("%x\n",
 			(addr & V8_MEMORY_PAGE_MASK) |
 				(V8_MEMORY_EXTENDED*extended));
@@ -209,11 +206,12 @@ static int flexcop_usb_memory_req(struct flexcop_usb *fc_usb,
 			page_start + (addr / V8_MEMORY_PAGE_SIZE),
 			(addr & V8_MEMORY_PAGE_MASK) |
 				(V8_MEMORY_EXTENDED*extended),
-			&buf[i], pagechunk);
+			buf, pagechunk);
 
 		if (ret < 0)
 			return ret;
 		addr += pagechunk;
+		buf += pagechunk;
 		len -= pagechunk;
 	}
 	return 0;
@@ -448,7 +446,7 @@ static int flexcop_usb_transfer_init(struct flexcop_usb *fc_usb)
 	/* creating iso urbs */
 	for (i = 0; i < B2C2_USB_NUM_ISO_URB; i++) {
 		fc_usb->iso_urb[i] = usb_alloc_urb(B2C2_USB_FRAMES_PER_ISO,
-			GFP_ATOMIC);
+			GFP_KERNEL);
 		if (fc_usb->iso_urb[i] == NULL) {
 			ret = -ENOMEM;
 			goto urb_error;
@@ -481,7 +479,7 @@ static int flexcop_usb_transfer_init(struct flexcop_usb *fc_usb)
 			frame_offset += frame_size;
 		}
 
-		if ((ret = usb_submit_urb(fc_usb->iso_urb[i],GFP_ATOMIC))) {
+		if ((ret = usb_submit_urb(fc_usb->iso_urb[i],GFP_KERNEL))) {
 			err("submitting urb %d failed with %d.", i, ret);
 			goto urb_error;
 		}
@@ -515,7 +513,7 @@ static int flexcop_usb_init(struct flexcop_usb *fc_usb)
 
 	alt = fc_usb->uintf->cur_altsetting;
 
-	if (alt->desc.bNumEndpoints < 1)
+	if (alt->desc.bNumEndpoints < 2)
 		return -ENODEV;
 	if (!usb_endpoint_is_isoc_in(&alt->endpoint[0].desc))
 		return -ENODEV;
@@ -531,6 +529,12 @@ static int flexcop_usb_init(struct flexcop_usb *fc_usb)
 	case USB_SPEED_HIGH:
 		info("running at HIGH speed.");
 		break;
+	case USB_SPEED_SUPER:
+		info("running at SUPER speed.");
+		break;
+	case USB_SPEED_SUPER_PLUS:
+		info("running at SUPER+ speed.");
+		break;
 	case USB_SPEED_UNKNOWN:
 	default:
 		err("cannot handle USB speed because it is unknown.");
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 343a4433ed24..e585c8f6e4c5 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -1453,13 +1453,11 @@ static void stop_streaming(struct vb2_queue *vq)
 	return_all_buffers(dev, VB2_BUF_STATE_ERROR);
 }
 
-static struct vb2_ops cx231xx_video_qops = {
+static const struct vb2_ops cx231xx_video_qops = {
 	.queue_setup		= queue_setup,
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------ */
@@ -1540,20 +1538,6 @@ static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 	return 0;
 }
 
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx231xx *dev = video_drvdata(file);
-	struct v4l2_subdev *sd;
-
-	dprintk(3, "enter vidioc_s_ctrl()\n");
-	/* Update the A/V core */
-	v4l2_device_for_each_subdev(sd, &dev->v4l2_dev)
-		v4l2_s_ctrl(NULL, sd->ctrl_handler, ctl);
-	dprintk(3, "exit vidioc_s_ctrl()\n");
-	return 0;
-}
-
 static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 					struct v4l2_fmtdesc *f)
 {
@@ -1629,7 +1613,6 @@ static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_enum_input	 = cx231xx_enum_input,
 	.vidioc_g_input		 = cx231xx_g_input,
 	.vidioc_s_input		 = cx231xx_s_input,
-	.vidioc_s_ctrl		 = vidioc_s_ctrl,
 	.vidioc_g_pixelaspect	 = vidioc_g_pixelaspect,
 	.vidioc_g_selection	 = vidioc_g_selection,
 	.vidioc_querycap	 = cx231xx_querycap,
@@ -1722,6 +1705,8 @@ static void cx231xx_video_dev_init(
 	vfd->lock = &dev->lock;
 	vfd->release = video_device_release_empty;
 	vfd->ctrl_handler = &dev->mpeg_ctrl_handler.hdl;
+	vfd->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING |
+			   V4L2_CAP_VIDEO_CAPTURE;
 	video_set_drvdata(vfd, dev);
 	if (dev->tuner_type == TUNER_ABSENT) {
 		v4l2_disable_ioctl(vfd, VIDIOC_G_FREQUENCY);
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index 3d3c881c8e58..1cfec76b72f3 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -623,7 +623,7 @@ int cx231xx_set_video_input_mux(struct cx231xx *dev, u8 input)
 }
 
 int cx231xx_set_decoder_video_input(struct cx231xx *dev,
-				u8 pin_type, u8 input)
+				u8 pin_type, u32 input)
 {
 	int status = 0;
 	u32 value = 0;
@@ -1338,39 +1338,6 @@ void update_HH_register_after_set_DIF(struct cx231xx *dev)
 */
 }
 
-void cx231xx_dump_HH_reg(struct cx231xx *dev)
-{
-	u32 value = 0;
-	u16  i = 0;
-
-	value = 0x45005390;
-	vid_blk_write_word(dev, 0x104, value);
-
-	for (i = 0x100; i < 0x140; i++) {
-		vid_blk_read_word(dev, i, &value);
-		dev_dbg(dev->dev, "reg0x%x=0x%x\n", i, value);
-		i = i+3;
-	}
-
-	for (i = 0x300; i < 0x400; i++) {
-		vid_blk_read_word(dev, i, &value);
-		dev_dbg(dev->dev, "reg0x%x=0x%x\n", i, value);
-		i = i+3;
-	}
-
-	for (i = 0x400; i < 0x440; i++) {
-		vid_blk_read_word(dev, i,  &value);
-		dev_dbg(dev->dev, "reg0x%x=0x%x\n", i, value);
-		i = i+3;
-	}
-
-	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
-	dev_dbg(dev->dev, "AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
-	vid_blk_write_word(dev, AFE_CTRL_C2HH_SRC_CTRL, 0x4485D390);
-	vid_blk_read_word(dev, AFE_CTRL_C2HH_SRC_CTRL, &value);
-	dev_dbg(dev->dev, "AFE_CTRL_C2HH_SRC_CTRL=0x%x\n", value);
-}
-
 #if 0
 static void cx231xx_dump_SC_reg(struct cx231xx *dev)
 {
@@ -2460,30 +2427,6 @@ int cx231xx_set_power_mode(struct cx231xx *dev, enum AV_MODE mode)
 	return status;
 }
 
-int cx231xx_power_suspend(struct cx231xx *dev)
-{
-	u8 value[4] = { 0, 0, 0, 0 };
-	u32 tmp = 0;
-	int status = 0;
-
-	status = cx231xx_read_ctrl_reg(dev, VRT_GET_REGISTER, PWR_CTL_EN,
-				       value, 4);
-	if (status > 0)
-		return status;
-
-	tmp = le32_to_cpu(*((__le32 *) value));
-	tmp &= (~PWR_MODE_MASK);
-
-	value[0] = (u8) tmp;
-	value[1] = (u8) (tmp >> 8);
-	value[2] = (u8) (tmp >> 16);
-	value[3] = (u8) (tmp >> 24);
-	status = cx231xx_write_ctrl_reg(dev, VRT_SET_REGISTER, PWR_CTL_EN,
-					value, 4);
-
-	return status;
-}
-
 /******************************************************************************
  *                  S T R E A M    C O N T R O L   functions                  *
  ******************************************************************************/
@@ -2761,7 +2704,6 @@ int cx231xx_set_gpio_value(struct cx231xx *dev, int pin_number, int pin_value)
 		dev->gpio_dir = value;
 		status = cx231xx_set_gpio_bit(dev, dev->gpio_dir,
 					      dev->gpio_val);
-		value = 0;
 	}
 
 	if (pin_value == 0)
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 92efe6c1f47b..691f073892b3 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -679,8 +679,7 @@ struct cx231xx_board cx231xx_boards[] = {
 			}, {
 				.type = CX231XX_VMUX_SVIDEO,
 				.vmux = CX231XX_VIN_1_1 |
-					(CX231XX_VIN_1_2 << 8) |
-					CX25840_SVIDEO_ON,
+					(CX231XX_VIN_3_2 << 8),
 				.amux = CX231XX_AMUX_LINE_IN,
 				.gpio = NULL,
 			}
@@ -990,10 +989,11 @@ struct cx231xx_board cx231xx_boards[] = {
 		} },
 	},
 };
-const unsigned int cx231xx_bcount = ARRAY_SIZE(cx231xx_boards);
 
 /* table of devices that work with this driver */
 struct usb_device_id cx231xx_id_table[] = {
+	{USB_DEVICE(0x1D19, 0x6108),
+	.driver_info = CX231XX_BOARD_PV_XCAPTURE_USB},
 	{USB_DEVICE(0x1D19, 0x6109),
 	.driver_info = CX231XX_BOARD_PV_XCAPTURE_USB},
 	{USB_DEVICE(0x0572, 0x5A3C),
diff --git a/drivers/media/usb/cx231xx/cx231xx-i2c.c b/drivers/media/usb/cx231xx/cx231xx-i2c.c
index c6659253c6fb..6da8e7943d94 100644
--- a/drivers/media/usb/cx231xx/cx231xx-i2c.c
+++ b/drivers/media/usb/cx231xx/cx231xx-i2c.c
@@ -567,10 +567,7 @@ int cx231xx_i2c_mux_create(struct cx231xx *dev)
 
 int cx231xx_i2c_mux_register(struct cx231xx *dev, int mux_no)
 {
-	return i2c_mux_add_adapter(dev->muxc,
-				   0,
-				   mux_no /* chan_id */,
-				   0 /* class */);
+	return i2c_mux_add_adapter(dev->muxc, 0, mux_no);
 }
 
 void cx231xx_i2c_mux_unregister(struct cx231xx *dev)
diff --git a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
index 5bc44f194d0a..62ffa16bb82c 100644
--- a/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
+++ b/drivers/media/usb/cx231xx/cx231xx-pcb-cfg.h
@@ -57,19 +57,17 @@ enum USB_SPEED{
 };
 
 #define TS_MASK         0x6
-enum TS_PORT{
-	NO_TS_PORT = 0x0,	/* 2'b00: Neither port used. PCB not a Hybrid,
+#define NO_TS_PORT	0x0	/* 2'b00: Neither port used. PCB not a Hybrid,
 				   only offers Analog TV or Video */
-	TS1_PORT = 0x4,		/* 2'b10: TS1 Input (Hybrid mode :
+#define TS1_PORT	0x4	/* 2'b10: TS1 Input (Hybrid mode :
 				Digital or External Analog/Compressed source) */
-	TS1_TS2_PORT = 0x6,	/* 2'b11: TS1 & TS2 Inputs
+#define TS1_TS2_PORT	0x6	/* 2'b11: TS1 & TS2 Inputs
 				(Dual inputs from Digital and/or
 				External Analog/Compressed sources) */
-	TS1_EXT_CLOCK = 0x6,	/* 2'b11: TS1 & TS2 as selector
+#define TS1_EXT_CLOCK	0x6	/* 2'b11: TS1 & TS2 as selector
 						to external clock */
-	TS1VIP_TS2_PORT = 0x2	/* 2'b01: TS1 used as 656/VIP Output,
+#define TS1VIP_TS2_PORT 0x2	/* 2'b01: TS1 used as 656/VIP Output,
 				   TS2 Input (from Compressor) */
-};
 
 #define EAVP_MASK       0x8
 enum EAV_PRESENT{
@@ -89,10 +87,8 @@ enum AT_MODE{
 };
 
 #define PWR_SEL_MASK    0x40
-enum POWE_TYPE{
-	SELF_POWER = 0x0,	/* 0: self power */
-	BUS_POWER = 0x40	/* 1: bus power */
-};
+#define SELF_POWER	0x0	/* 0: self power */
+#define BUS_POWER	0x40	/* 1: bus power */
 
 enum USB_POWE_TYPE{
 	USB_SELF_POWER = 0,
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index 33431d9f54c2..338e10148465 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -243,8 +243,6 @@ struct vb2_ops cx231xx_vbi_qops = {
 	.buf_queue = vbi_buf_queue,
 	.start_streaming = vbi_start_streaming,
 	.stop_streaming = vbi_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 /* ------------------------------------------------------------------
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index 8f347bbeeb32..2cd4e333bc4b 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -795,13 +795,11 @@ static void stop_streaming(struct vb2_queue *vq)
 	return_all_buffers(dev, VB2_BUF_STATE_ERROR);
 }
 
-static struct vb2_ops cx231xx_video_qops = {
+static const struct vb2_ops cx231xx_video_qops = {
 	.queue_setup		= queue_setup,
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /*********************  v4l2 interface  **************************************/
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index 74339a6a2f71..19f5036a78d7 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -790,7 +790,6 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 					 u8 spectral_invert, u32 mode);
 void cx231xx_Setup_AFE_for_LowIF(struct cx231xx *dev);
 void reset_s5h1432_demod(struct cx231xx *dev);
-void cx231xx_dump_HH_reg(struct cx231xx *dev);
 void update_HH_register_after_set_DIF(struct cx231xx *dev);
 
 
@@ -905,7 +904,6 @@ int cx231xx_initialize_stream_xfer(struct cx231xx *dev, u32 media_type);
 
 /* Power control functions */
 int cx231xx_set_power_mode(struct cx231xx *dev, enum AV_MODE mode);
-int cx231xx_power_suspend(struct cx231xx *dev);
 
 /* chip specific control functions */
 int cx231xx_init_ctrl_pin_status(struct cx231xx *dev);
@@ -916,7 +914,7 @@ int cx231xx_enable_i2c_port_3(struct cx231xx *dev, bool is_port_3);
 /* video audio decoder related functions */
 void video_mux(struct cx231xx *dev, int index);
 int cx231xx_set_video_input_mux(struct cx231xx *dev, u8 input);
-int cx231xx_set_decoder_video_input(struct cx231xx *dev, u8 pin_type, u8 input);
+int cx231xx_set_decoder_video_input(struct cx231xx *dev, u8 pin_type, u32 input);
 int cx231xx_do_mode_ctrl_overrides(struct cx231xx *dev);
 int cx231xx_set_audio_input(struct cx231xx *dev, u8 input);
 
@@ -949,7 +947,6 @@ extern void cx231xx_pre_card_setup(struct cx231xx *dev);
 extern void cx231xx_card_setup(struct cx231xx *dev);
 extern struct cx231xx_board cx231xx_boards[];
 extern struct usb_device_id cx231xx_id_table[];
-extern const unsigned int cx231xx_bcount;
 int cx231xx_tuner_callback(void *ptr, int component, int command, int arg);
 
 /* cx23885-417.c                                               */
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 4014f7d07330..3eddc40377bf 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -260,7 +260,7 @@ static u32 af9015_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm af9015_i2c_algo = {
+static const struct i2c_algorithm af9015_i2c_algo = {
 	.master_xfer = af9015_i2c_xfer,
 	.functionality = af9015_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index 4eb7dd4599b7..17062672ea06 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -322,13 +322,16 @@ static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
 			ret = -EOPNOTSUPP;
 		} else if ((msg[0].addr == state->af9033_i2c_addr[0]) ||
 			   (msg[0].addr == state->af9033_i2c_addr[1])) {
+			/* demod access via firmware interface */
+			u32 reg;
+
 			if (msg[0].len < 3 || msg[1].len < 1) {
 				ret = -EOPNOTSUPP;
 				goto unlock;
 			}
-			/* demod access via firmware interface */
-			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
-					msg[0].buf[2];
+
+			reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+				msg[0].buf[2];
 
 			if (msg[0].addr == state->af9033_i2c_addr[1])
 				reg |= 0x100000;
@@ -385,13 +388,16 @@ static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
 			ret = -EOPNOTSUPP;
 		} else if ((msg[0].addr == state->af9033_i2c_addr[0]) ||
 			   (msg[0].addr == state->af9033_i2c_addr[1])) {
+			/* demod access via firmware interface */
+			u32 reg;
+
 			if (msg[0].len < 3) {
 				ret = -EOPNOTSUPP;
 				goto unlock;
 			}
-			/* demod access via firmware interface */
-			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
-					msg[0].buf[2];
+
+			reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
+				msg[0].buf[2];
 
 			if (msg[0].addr == state->af9033_i2c_addr[1])
 				reg |= 0x100000;
@@ -477,7 +483,7 @@ static u32 af9035_i2c_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm af9035_i2c_algo = {
+static const struct i2c_algorithm af9035_i2c_algo = {
 	.master_xfer = af9035_i2c_master_xfer,
 	.functionality = af9035_i2c_functionality,
 };
@@ -868,6 +874,9 @@ static int af9035_read_config(struct dvb_usb_device *d)
 		if ((le16_to_cpu(d->udev->descriptor.idVendor) == USB_VID_AVERMEDIA) &&
 		    (le16_to_cpu(d->udev->descriptor.idProduct) == USB_PID_AVERMEDIA_TD310)) {
 			state->it930x_addresses = 1;
+			/* TD310 RC works with NEC defaults */
+			state->ir_mode = 0x05;
+			state->ir_type = 0x00;
 		}
 		return 0;
 	}
@@ -2066,6 +2075,11 @@ static const struct dvb_usb_device_properties it930x_props = {
 	.tuner_attach = it930x_tuner_attach,
 	.tuner_detach = it930x_tuner_detach,
 	.init = it930x_init,
+	/*
+	 * dvb_usbv2_remote_init() calls rc_config() only for those devices
+	 * which have non-empty rc_map, so it's safe to enable it for every IT930x
+	 */
+	.get_rc_config = af9035_get_rc_config,
 	.get_stream_config = af9035_get_stream_config,
 
 	.get_adapter_count = af9035_get_adapter_count,
@@ -2157,7 +2171,7 @@ static const struct usb_device_id af9035_id_table[] = {
 	{ DVB_USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9303,
 		&it930x_props, "ITE 9303 Generic", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_AVERMEDIA, USB_PID_AVERMEDIA_TD310,
-		&it930x_props, "AVerMedia TD310 DVB-T2", NULL) },
+		&it930x_props, "AVerMedia TD310 DVB-T2", RC_MAP_AVERMEDIA_RM_KS) },
 	{ DVB_USB_DEVICE(USB_VID_DEXATEK, 0x0100,
 		&it930x_props, "Logilink VG0022A", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, USB_PID_TERRATEC_CINERGY_TC2_STICK,
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.c b/drivers/media/usb/dvb-usb-v2/anysee.c
index a1235d0cce92..64bddca5303c 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.c
+++ b/drivers/media/usb/dvb-usb-v2/anysee.c
@@ -46,24 +46,15 @@ static int anysee_ctrl_msg(struct dvb_usb_device *d,
 
 	dev_dbg(&d->udev->dev, "%s: >>> %*ph\n", __func__, slen, state->buf);
 
-	/* We need receive one message more after dvb_usb_generic_rw due
-	   to weird transaction flow, which is 1 x send + 2 x receive. */
+	/*
+	 * We need receive one message more after dvb_usbv2_generic_rw_locked()
+	 * due to weird transaction flow, which is 1 x send + 2 x receive.
+	 */
 	ret = dvb_usbv2_generic_rw_locked(d, state->buf, sizeof(state->buf),
 			state->buf, sizeof(state->buf));
 	if (ret)
 		goto error_unlock;
 
-	/* TODO FIXME: dvb_usb_generic_rw() fails rarely with error code -32
-	 * (EPIPE, Broken pipe). Function supports currently msleep() as a
-	 * parameter but I would not like to use it, since according to
-	 * Documentation/timers/timers-howto.rst it should not be used such
-	 * short, under < 20ms, sleeps. Repeating failed message would be
-	 * better choice as not to add unwanted delays...
-	 * Fixing that correctly is one of those or both;
-	 * 1) use repeat if possible
-	 * 2) add suitable delay
-	 */
-
 	/* get answer, retry few times if error returned */
 	for (i = 0; i < 3; i++) {
 		/* receive 2nd answer */
@@ -202,14 +193,14 @@ static int anysee_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msg,
 
 	while (i < num) {
 		if (num > i + 1 && (msg[i+1].flags & I2C_M_RD)) {
-			if (msg[i].len != 2 || msg[i + 1].len > 60) {
+			if (msg[i].len < 1 || msg[i].len > 2 || msg[i + 1].len > 60) {
 				ret = -EOPNOTSUPP;
 				break;
 			}
 			buf[0] = CMD_I2C_READ;
 			buf[1] = (msg[i].addr << 1) | 0x01;
 			buf[2] = msg[i].buf[0];
-			buf[3] = msg[i].buf[1];
+			buf[3] = (msg[i].len < 2) ? 0 : msg[i].buf[1];
 			buf[4] = msg[i].len-1;
 			buf[5] = msg[i+1].len;
 			ret = anysee_ctrl_msg(d, buf, 6, msg[i+1].buf,
@@ -244,7 +235,7 @@ static u32 anysee_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm anysee_i2c_algo = {
+static const struct i2c_algorithm anysee_i2c_algo = {
 	.master_xfer   = anysee_master_xfer,
 	.functionality = anysee_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/au6610.c b/drivers/media/usb/dvb-usb-v2/au6610.c
index be223fc8aa14..c20a9469f564 100644
--- a/drivers/media/usb/dvb-usb-v2/au6610.c
+++ b/drivers/media/usb/dvb-usb-v2/au6610.c
@@ -115,7 +115,7 @@ static u32 au6610_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm au6610_i2c_algo = {
+static const struct i2c_algorithm au6610_i2c_algo = {
 	.master_xfer   = au6610_i2c_xfer,
 	.functionality = au6610_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/az6007.c b/drivers/media/usb/dvb-usb-v2/az6007.c
index 2410054ddb2c..65ef045b74ca 100644
--- a/drivers/media/usb/dvb-usb-v2/az6007.c
+++ b/drivers/media/usb/dvb-usb-v2/az6007.c
@@ -838,7 +838,7 @@ static u32 az6007_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm az6007_i2c_algo = {
+static const struct i2c_algorithm az6007_i2c_algo = {
 	.master_xfer = az6007_i2c_xfer,
 	.functionality = az6007_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/ce6230.c b/drivers/media/usb/dvb-usb-v2/ce6230.c
index d3b5cb4a24da..7ebaf3ee4491 100644
--- a/drivers/media/usb/dvb-usb-v2/ce6230.c
+++ b/drivers/media/usb/dvb-usb-v2/ce6230.c
@@ -154,7 +154,7 @@ static u32 ce6230_i2c_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm ce6230_i2c_algorithm = {
+static const struct i2c_algorithm ce6230_i2c_algorithm = {
 	.master_xfer   = ce6230_i2c_master_xfer,
 	.functionality = ce6230_i2c_functionality,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb.h b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
index 288c15a7d72b..ecdc20d45132 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb.h
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
@@ -243,7 +243,7 @@ struct dvb_usb_device_properties {
 	int (*download_firmware) (struct dvb_usb_device *,
 			const struct firmware *);
 
-	struct i2c_algorithm *i2c_algo;
+	const struct i2c_algorithm *i2c_algo;
 
 	unsigned int num_adapters;
 	int (*get_adapter_count) (struct dvb_usb_device *);
diff --git a/drivers/media/usb/dvb-usb-v2/dvbsky.c b/drivers/media/usb/dvb-usb-v2/dvbsky.c
index 1221c924312a..ceac0ea21dab 100644
--- a/drivers/media/usb/dvb-usb-v2/dvbsky.c
+++ b/drivers/media/usb/dvb-usb-v2/dvbsky.c
@@ -169,7 +169,7 @@ static u32 dvbsky_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm dvbsky_i2c_algo = {
+static const struct i2c_algorithm dvbsky_i2c_algo = {
 	.master_xfer   = dvbsky_i2c_xfer,
 	.functionality = dvbsky_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/ec168.c b/drivers/media/usb/dvb-usb-v2/ec168.c
index 0e4773fc025c..973b32356b17 100644
--- a/drivers/media/usb/dvb-usb-v2/ec168.c
+++ b/drivers/media/usb/dvb-usb-v2/ec168.c
@@ -176,7 +176,7 @@ static u32 ec168_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm ec168_i2c_algo = {
+static const struct i2c_algorithm ec168_i2c_algo = {
 	.master_xfer   = ec168_i2c_xfer,
 	.functionality = ec168_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/gl861.c b/drivers/media/usb/dvb-usb-v2/gl861.c
index c71e7b93476d..0538170ccf29 100644
--- a/drivers/media/usb/dvb-usb-v2/gl861.c
+++ b/drivers/media/usb/dvb-usb-v2/gl861.c
@@ -162,7 +162,7 @@ static u32 gl861_i2c_functionality(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm gl861_i2c_algo = {
+static const struct i2c_algorithm gl861_i2c_algo = {
 	.master_xfer   = gl861_i2c_master_xfer,
 	.functionality = gl861_i2c_functionality,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index 8a34e6c0d6a6..0c510035805b 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -373,6 +373,7 @@ static int lme2510_int_read(struct dvb_usb_adapter *adap)
 	struct dvb_usb_device *d = adap_to_d(adap);
 	struct lme2510_state *lme_int = adap_to_priv(adap);
 	struct usb_host_endpoint *ep;
+	int ret;
 
 	lme_int->lme_urb = usb_alloc_urb(0, GFP_KERNEL);
 
@@ -390,11 +391,20 @@ static int lme2510_int_read(struct dvb_usb_adapter *adap)
 
 	/* Quirk of pipe reporting PIPE_BULK but behaves as interrupt */
 	ep = usb_pipe_endpoint(d->udev, lme_int->lme_urb->pipe);
+	if (!ep) {
+		usb_free_urb(lme_int->lme_urb);
+		return -ENODEV;
+	}
 
 	if (usb_endpoint_type(&ep->desc) == USB_ENDPOINT_XFER_BULK)
 		lme_int->lme_urb->pipe = usb_rcvbulkpipe(d->udev, 0xa);
 
-	usb_submit_urb(lme_int->lme_urb, GFP_KERNEL);
+	ret = usb_submit_urb(lme_int->lme_urb, GFP_KERNEL);
+	if (ret) {
+		usb_free_urb(lme_int->lme_urb);
+		return ret;
+	}
+
 	info("INT Interrupt Service Started");
 
 	return 0;
@@ -549,7 +559,7 @@ static u32 lme2510_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm lme2510_i2c_algo = {
+static const struct i2c_algorithm lme2510_i2c_algo = {
 	.master_xfer   = lme2510_i2c_xfer,
 	.functionality = lme2510_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index cd5861a30b6f..870ac3c8b085 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -911,7 +911,7 @@ static u32 mxl111sf_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm mxl111sf_i2c_algo = {
+static const struct i2c_algorithm mxl111sf_i2c_algo = {
 	.master_xfer   = mxl111sf_i2c_xfer,
 	.functionality = mxl111sf_i2c_func,
 #ifdef NEED_ALGO_CONTROL
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index f7884bb56fcc..487c6ab784ab 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -290,7 +290,7 @@ static u32 rtl28xxu_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm rtl28xxu_i2c_algo = {
+static const struct i2c_algorithm rtl28xxu_i2c_algo = {
 	.master_xfer   = rtl28xxu_i2c_xfer,
 	.functionality = rtl28xxu_i2c_func,
 };
diff --git a/drivers/media/usb/dvb-usb/a800.c b/drivers/media/usb/dvb-usb/a800.c
index 5f294784923c..c5f95e48f1d5 100644
--- a/drivers/media/usb/dvb-usb/a800.c
+++ b/drivers/media/usb/dvb-usb/a800.c
@@ -77,7 +77,7 @@ enum {
 	AVERMEDIA_DVBT_USB2_WARM,
 };
 
-static struct usb_device_id a800_table[] = {
+static const struct usb_device_id a800_table[] = {
 	DVB_USB_DEV(AVERMEDIA, AVERMEDIA_DVBT_USB2_COLD),
 	DVB_USB_DEV(AVERMEDIA, AVERMEDIA_DVBT_USB2_WARM),
 	{ }
diff --git a/drivers/media/usb/dvb-usb/af9005.c b/drivers/media/usb/dvb-usb/af9005.c
index 13604e6acdb8..a4bede7e8a1d 100644
--- a/drivers/media/usb/dvb-usb/af9005.c
+++ b/drivers/media/usb/dvb-usb/af9005.c
@@ -445,7 +445,7 @@ static u32 af9005_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm af9005_i2c_algo = {
+static const struct i2c_algorithm af9005_i2c_algo = {
 	.master_xfer = af9005_i2c_xfer,
 	.functionality = af9005_i2c_func,
 };
@@ -1005,7 +1005,7 @@ enum {
 	ANSONIC_DVBT_USB,
 };
 
-static struct usb_device_id af9005_usb_table[] = {
+static const struct usb_device_id af9005_usb_table[] = {
 	DVB_USB_DEV(AFATECH, AFATECH_AF9005),
 	DVB_USB_DEV(TERRATEC, TERRATEC_CINERGY_T_USB_XE),
 	DVB_USB_DEV(ANSONIC, ANSONIC_DVBT_USB),
diff --git a/drivers/media/usb/dvb-usb/az6027.c b/drivers/media/usb/dvb-usb/az6027.c
index 2bc27710427d..056935d3cbd6 100644
--- a/drivers/media/usb/dvb-usb/az6027.c
+++ b/drivers/media/usb/dvb-usb/az6027.c
@@ -1062,7 +1062,7 @@ static u32 az6027_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm az6027_i2c_algo = {
+static const struct i2c_algorithm az6027_i2c_algo = {
 	.master_xfer   = az6027_i2c_xfer,
 	.functionality = az6027_i2c_func,
 };
@@ -1107,7 +1107,7 @@ enum {
 	ELGATO_EYETV_SAT_V3,
 };
 
-static struct usb_device_id az6027_usb_table[] = {
+static const struct usb_device_id az6027_usb_table[] = {
 	DVB_USB_DEV(AZUREWAVE, AZUREWAVE_AZ6027),
 	DVB_USB_DEV(TERRATEC, TERRATEC_DVBS2CI_V1),
 	DVB_USB_DEV(TERRATEC, TERRATEC_DVBS2CI_V2),
diff --git a/drivers/media/usb/dvb-usb/cinergyT2-core.c b/drivers/media/usb/dvb-usb/cinergyT2-core.c
index 4926c954e29a..d86c279e2dce 100644
--- a/drivers/media/usb/dvb-usb/cinergyT2-core.c
+++ b/drivers/media/usb/dvb-usb/cinergyT2-core.c
@@ -197,7 +197,7 @@ enum {
 	TERRATEC_CINERGY_T2,
 };
 
-static struct usb_device_id cinergyt2_usb_table[] = {
+static const struct usb_device_id cinergyt2_usb_table[] = {
 	DVB_USB_DEV(TERRATEC, TERRATEC_CINERGY_T2),
 	{ }
 };
diff --git a/drivers/media/usb/dvb-usb/cxusb-analog.c b/drivers/media/usb/dvb-usb/cxusb-analog.c
index b5d8c6b75ae1..3bbee1fcbc8d 100644
--- a/drivers/media/usb/dvb-usb/cxusb-analog.c
+++ b/drivers/media/usb/dvb-usb/cxusb-analog.c
@@ -817,8 +817,8 @@ static int cxusb_medion_v_start_streaming(struct vb2_queue *q,
 		 * doing a large continuous allocation when (if)
 		 * s-g isochronous USB transfers are supported
 		 */
-		streambuf = kmalloc(npackets * CXUSB_VIDEO_PKT_SIZE,
-				    GFP_KERNEL);
+		streambuf = kmalloc_array(npackets, CXUSB_VIDEO_PKT_SIZE,
+					  GFP_KERNEL);
 		if (!streambuf) {
 			if (i < 2) {
 				ret = -ENOMEM;
@@ -956,8 +956,6 @@ static const struct vb2_ops cxdev_video_qops = {
 	.start_streaming = cxusb_medion_v_start_streaming,
 	.stop_streaming = cxusb_medion_v_stop_streaming,
 	.buf_queue = cxusub_medion_v_buf_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish
 };
 
 static const __u32 videocaps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_TUNER |
diff --git a/drivers/media/usb/dvb-usb/cxusb.c b/drivers/media/usb/dvb-usb/cxusb.c
index 1d98d3465e28..d0501c1e81d6 100644
--- a/drivers/media/usb/dvb-usb/cxusb.c
+++ b/drivers/media/usb/dvb-usb/cxusb.c
@@ -78,7 +78,7 @@ enum cxusb_table_index {
 	NR__cxusb_table_index
 };
 
-static struct usb_device_id cxusb_table[];
+static const struct usb_device_id cxusb_table[];
 
 int cxusb_ctrl_msg(struct dvb_usb_device *d,
 		   u8 cmd, const u8 *wbuf, int wlen, u8 *rbuf, int rlen)
@@ -119,9 +119,8 @@ static void cxusb_gpio_tuner(struct dvb_usb_device *d, int onoff)
 
 	o[0] = GPIO_TUNER;
 	o[1] = onoff;
-	cxusb_ctrl_msg(d, CMD_GPIO_WRITE, o, 2, &i, 1);
 
-	if (i != 0x01)
+	if (!cxusb_ctrl_msg(d, CMD_GPIO_WRITE, o, 2, &i, 1) && i != 0x01)
 		dev_info(&d->udev->dev, "gpio_write failed.\n");
 
 	st->gpio_write_state[GPIO_TUNER] = onoff;
@@ -287,7 +286,7 @@ static u32 cxusb_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
 }
 
-static struct i2c_algorithm cxusb_i2c_algo = {
+static const struct i2c_algorithm cxusb_i2c_algo = {
 	.master_xfer   = cxusb_i2c_xfer,
 	.functionality = cxusb_i2c_func,
 };
@@ -1692,7 +1691,7 @@ static void cxusb_disconnect(struct usb_interface *intf)
 	dvb_usb_device_exit(intf);
 }
 
-static struct usb_device_id cxusb_table[] = {
+static const struct usb_device_id cxusb_table[] = {
 	DVB_USB_DEV(MEDION, MEDION_MD95700),
 	DVB_USB_DEV(DVICO, DVICO_BLUEBIRD_LG064F_COLD),
 	DVB_USB_DEV(DVICO, DVICO_BLUEBIRD_LG064F_WARM),
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index 3af594134a6d..6ddc20513393 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -2412,7 +2412,12 @@ static int stk9090m_frontend_attach(struct dvb_usb_adapter *adap)
 
 	adap->fe_adap[0].fe = dvb_attach(dib9000_attach, &adap->dev->i2c_adap, 0x80, &stk9090m_config);
 
-	return adap->fe_adap[0].fe == NULL ?  -ENODEV : 0;
+	if (!adap->fe_adap[0].fe) {
+		release_firmware(state->frontend_firmware);
+		return -ENODEV;
+	}
+
+	return 0;
 }
 
 static int dib9090_tuner_attach(struct dvb_usb_adapter *adap)
@@ -2485,8 +2490,10 @@ static int nim9090md_frontend_attach(struct dvb_usb_adapter *adap)
 	dib9000_i2c_enumeration(&adap->dev->i2c_adap, 1, 0x20, 0x80);
 	adap->fe_adap[0].fe = dvb_attach(dib9000_attach, &adap->dev->i2c_adap, 0x80, &nim9090md_config[0]);
 
-	if (adap->fe_adap[0].fe == NULL)
+	if (!adap->fe_adap[0].fe) {
+		release_firmware(state->frontend_firmware);
 		return -ENODEV;
+	}
 
 	i2c = dib9000_get_i2c_master(adap->fe_adap[0].fe, DIBX000_I2C_INTERFACE_GPIO_3_4, 0);
 	dib9000_i2c_enumeration(i2c, 1, 0x12, 0x82);
@@ -2494,7 +2501,12 @@ static int nim9090md_frontend_attach(struct dvb_usb_adapter *adap)
 	fe_slave = dvb_attach(dib9000_attach, i2c, 0x82, &nim9090md_config[1]);
 	dib9000_set_slave_frontend(adap->fe_adap[0].fe, fe_slave);
 
-	return fe_slave == NULL ?  -ENODEV : 0;
+	if (!fe_slave) {
+		release_firmware(state->frontend_firmware);
+		return -ENODEV;
+	}
+
+	return 0;
 }
 
 static int nim9090md_tuner_attach(struct dvb_usb_adapter *adap)
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index aff60c10cb0b..20f1ef3393a5 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -14,6 +14,7 @@
 static int debug;
 module_param(debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debugging level (1=info (|-able))." DVB_USB_DEBUG_STATUS);
+MODULE_DESCRIPTION("Common methods for dibusb-based receivers");
 MODULE_LICENSE("GPL");
 
 #define deb_info(args...) dprintk(debug,0x01,args)
diff --git a/drivers/media/usb/dvb-usb/dibusb-mb.c b/drivers/media/usb/dvb-usb/dibusb-mb.c
index 2cd88cab4c98..431766f19931 100644
--- a/drivers/media/usb/dvb-usb/dibusb-mb.c
+++ b/drivers/media/usb/dvb-usb/dibusb-mb.c
@@ -155,7 +155,7 @@ enum {
 	ULTIMA_TVBOX_ANCHOR_COLD,
 };
 
-static struct usb_device_id dibusb_dib3000mb_table[] = {
+static const struct usb_device_id dibusb_dib3000mb_table[] = {
 	DVB_USB_DEV(WIDEVIEW, WIDEVIEW_DVBT_USB_COLD),
 	DVB_USB_DEV(WIDEVIEW, WIDEVIEW_DVBT_USB_WARM),
 	DVB_USB_DEV(COMPRO, COMPRO_DVBU2000_COLD),
diff --git a/drivers/media/usb/dvb-usb/dibusb-mc-common.c b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
index b8cde4cded33..36bc7762acf4 100644
--- a/drivers/media/usb/dvb-usb/dibusb-mc-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-mc-common.c
@@ -8,6 +8,7 @@
 
 #include "dibusb.h"
 
+MODULE_DESCRIPTION("Common methods for DIB3000MC");
 MODULE_LICENSE("GPL");
 
 /* 3000MC/P stuff */
diff --git a/drivers/media/usb/dvb-usb/dibusb-mc.c b/drivers/media/usb/dvb-usb/dibusb-mc.c
index 00cb016f6266..01eece2687d6 100644
--- a/drivers/media/usb/dvb-usb/dibusb-mc.c
+++ b/drivers/media/usb/dvb-usb/dibusb-mc.c
@@ -43,7 +43,7 @@ enum {
 	HUMAX_DVB_T_STICK_HIGH_SPEED_WARM,
 };
 
-static struct usb_device_id dibusb_dib3000mc_table[] = {
+static const struct usb_device_id dibusb_dib3000mc_table[] = {
 	DVB_USB_DEV(DIBCOM, DIBCOM_MOD3001_COLD),
 	DVB_USB_DEV(DIBCOM, DIBCOM_MOD3001_WARM),
 	DVB_USB_DEV(ULTIMA_ELECTRONIC, ULTIMA_TVBOX_USB2_COLD),
diff --git a/drivers/media/usb/dvb-usb/digitv.c b/drivers/media/usb/dvb-usb/digitv.c
index 32134be16914..ab229ab1a858 100644
--- a/drivers/media/usb/dvb-usb/digitv.c
+++ b/drivers/media/usb/dvb-usb/digitv.c
@@ -88,7 +88,7 @@ static u32 digitv_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm digitv_i2c_algo = {
+static const struct i2c_algorithm digitv_i2c_algo = {
 	.master_xfer   = digitv_i2c_xfer,
 	.functionality = digitv_i2c_func,
 };
@@ -299,7 +299,7 @@ enum {
 	ANCHOR_NEBULA_DIGITV,
 };
 
-static struct usb_device_id digitv_table[] = {
+static const struct usb_device_id digitv_table[] = {
 	DVB_USB_DEV(ANCHOR, ANCHOR_NEBULA_DIGITV),
 	{ }
 };
diff --git a/drivers/media/usb/dvb-usb/dtt200u.c b/drivers/media/usb/dvb-usb/dtt200u.c
index e6ee56b3a9dd..83a69df384f2 100644
--- a/drivers/media/usb/dvb-usb/dtt200u.c
+++ b/drivers/media/usb/dvb-usb/dtt200u.c
@@ -171,7 +171,7 @@ enum {
 	MIGLIA_WT220U_ZAP250_COLD,
 };
 
-static struct usb_device_id dtt200u_usb_table[] = {
+static const struct usb_device_id dtt200u_usb_table[] = {
 	DVB_USB_DEV(WIDEVIEW, WIDEVIEW_DTT200U_COLD),
 	DVB_USB_DEV(WIDEVIEW, WIDEVIEW_DTT200U_WARM),
 	DVB_USB_DEV(WIDEVIEW, WIDEVIEW_WT220U_COLD),
diff --git a/drivers/media/usb/dvb-usb/dtv5100.c b/drivers/media/usb/dvb-usb/dtv5100.c
index 56c9d521a34a..3d85c6f7f6ec 100644
--- a/drivers/media/usb/dvb-usb/dtv5100.c
+++ b/drivers/media/usb/dvb-usb/dtv5100.c
@@ -97,7 +97,7 @@ static u32 dtv5100_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm dtv5100_i2c_algo = {
+static const struct i2c_algorithm dtv5100_i2c_algo = {
 	.master_xfer   = dtv5100_i2c_xfer,
 	.functionality = dtv5100_i2c_func,
 };
@@ -166,7 +166,7 @@ enum {
 	AME_DTV5100,
 };
 
-static struct usb_device_id dtv5100_table[] = {
+static const struct usb_device_id dtv5100_table[] = {
 	DVB_USB_DEV(AME, AME_DTV5100),
 	{ }
 };
diff --git a/drivers/media/usb/dvb-usb/dvb-usb.h b/drivers/media/usb/dvb-usb/dvb-usb.h
index cbb0541d4dc1..550006a8d86f 100644
--- a/drivers/media/usb/dvb-usb/dvb-usb.h
+++ b/drivers/media/usb/dvb-usb/dvb-usb.h
@@ -73,8 +73,8 @@ struct dvb_usb_device_description {
 	const char *name;
 
 #define DVB_USB_ID_MAX_NUM 15
-	struct usb_device_id *cold_ids[DVB_USB_ID_MAX_NUM];
-	struct usb_device_id *warm_ids[DVB_USB_ID_MAX_NUM];
+	const struct usb_device_id *cold_ids[DVB_USB_ID_MAX_NUM];
+	const struct usb_device_id *warm_ids[DVB_USB_ID_MAX_NUM];
 };
 
 static inline u8 rc5_custom(struct rc_map_table *key)
@@ -309,7 +309,7 @@ struct dvb_usb_device_properties {
 		struct dvb_rc core;
 	} rc;
 
-	struct i2c_algorithm *i2c_algo;
+	const struct i2c_algorithm *i2c_algo;
 
 	int generic_bulk_ctrl_endpoint;
 	int generic_bulk_ctrl_endpoint_response;
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index b3bb1805829a..4fecf2f965e9 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -36,7 +36,6 @@
 /* Max transfer size done by I2C transfer functions */
 #define MAX_XFER_SIZE  64
 
-
 #define DW210X_READ_MSG 0
 #define DW210X_WRITE_MSG 1
 
@@ -53,10 +52,10 @@
 #define DW2102_FIRMWARE "dvb-usb-dw2102.fw"
 #define DW2104_FIRMWARE "dvb-usb-dw2104.fw"
 #define DW3101_FIRMWARE "dvb-usb-dw3101.fw"
-#define S630_FIRMWARE   "dvb-usb-s630.fw"
-#define S660_FIRMWARE   "dvb-usb-s660.fw"
-#define P1100_FIRMWARE  "dvb-usb-p1100.fw"
-#define P7500_FIRMWARE  "dvb-usb-p7500.fw"
+#define S630_FIRMWARE	"dvb-usb-s630.fw"
+#define S660_FIRMWARE	"dvb-usb-s660.fw"
+#define P1100_FIRMWARE	"dvb-usb-p1100.fw"
+#define P7500_FIRMWARE	"dvb-usb-p7500.fw"
 
 #define	err_str "did not find the firmware file '%s'. You can use <kernel_dir>/scripts/get_dvb_firmware to get the firmware"
 
@@ -87,7 +86,7 @@ MODULE_PARM_DESC(demod, "demod to probe (1=cx24116 2=stv0903+stv6110 4=stv0903+s
 DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 static int dw210x_op_rw(struct usb_device *dev, u8 request, u16 value,
-			u16 index, u8 * data, u16 len, int flags)
+			u16 index, u8 *data, u16 len, int flags)
 {
 	int ret;
 	u8 *u8buf;
@@ -99,11 +98,10 @@ static int dw210x_op_rw(struct usb_device *dev, u8 request, u16 value,
 	if (!u8buf)
 		return -ENOMEM;
 
-
 	if (flags == DW210X_WRITE_MSG)
 		memcpy(u8buf, data, len);
 	ret = usb_control_msg(dev, pipe, request, request_type | USB_TYPE_VENDOR,
-				value, index , u8buf, len, 2000);
+			      value, index, u8buf, len, 2000);
 
 	if (flags == DW210X_READ_MSG)
 		memcpy(data, u8buf, len);
@@ -114,7 +112,7 @@ static int dw210x_op_rw(struct usb_device *dev, u8 request, u16 value,
 
 /* I2C */
 static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
-		int num)
+			       int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	int i = 0;
@@ -136,7 +134,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		value = msg[0].buf[0];/* register */
 		for (i = 0; i < msg[1].len; i++) {
 			dw210x_op_rw(d->udev, 0xb5, value + i, 0,
-					buf6, 2, DW210X_READ_MSG);
+				     buf6, 2, DW210X_READ_MSG);
 			msg[1].buf[i] = buf6[0];
 		}
 		break;
@@ -152,7 +150,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			buf6[1] = msg[0].buf[0];
 			buf6[2] = msg[0].buf[1];
 			dw210x_op_rw(d->udev, 0xb2, 0, 0,
-					buf6, 3, DW210X_WRITE_MSG);
+				     buf6, 3, DW210X_WRITE_MSG);
 			break;
 		case 0x60:
 			if (msg[0].flags == 0) {
@@ -169,7 +167,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				buf6[5] = msg[0].buf[2];
 				buf6[6] = msg[0].buf[3];
 				dw210x_op_rw(d->udev, 0xb2, 0, 0,
-						buf6, 7, DW210X_WRITE_MSG);
+					     buf6, 7, DW210X_WRITE_MSG);
 			} else {
 				if (msg[0].len < 1) {
 					num = -EOPNOTSUPP;
@@ -177,7 +175,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				}
 			/* read from tuner */
 				dw210x_op_rw(d->udev, 0xb5, 0, 0,
-						buf6, 1, DW210X_READ_MSG);
+					     buf6, 1, DW210X_READ_MSG);
 				msg[0].buf[0] = buf6[0];
 			}
 			break;
@@ -187,7 +185,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				break;
 			}
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					buf6, 2, DW210X_READ_MSG);
+				     buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
 			break;
@@ -199,7 +197,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
 			dw210x_op_rw(d->udev, 0xb2, 0, 0,
-					buf6, 2, DW210X_WRITE_MSG);
+				     buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
 
@@ -211,7 +209,7 @@ static int dw2102_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 }
 
 static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
-						struct i2c_msg msg[], int num)
+				     struct i2c_msg msg[], int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	u8 buf6[] = {0, 0, 0, 0, 0, 0, 0};
@@ -242,10 +240,10 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 		buf6[1] = msg[0].len;
 		buf6[2] = msg[0].buf[0];
 		dw210x_op_rw(d->udev, 0xc2, 0, 0,
-				buf6, msg[0].len + 2, DW210X_WRITE_MSG);
+			     buf6, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* read si2109 register */
 		dw210x_op_rw(d->udev, 0xc3, 0xd0, 0,
-				buf6, msg[1].len + 2, DW210X_READ_MSG);
+			     buf6, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, buf6 + 2, msg[1].len);
 
 		break;
@@ -264,11 +262,11 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 			buf6[1] = msg[0].len;
 			memcpy(buf6 + 2, msg[0].buf, msg[0].len);
 			dw210x_op_rw(d->udev, 0xc2, 0, 0, buf6,
-					msg[0].len + 2, DW210X_WRITE_MSG);
+				     msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		case(DW2102_RC_QUERY):
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					buf6, 2, DW210X_READ_MSG);
+				     buf6, 2, DW210X_READ_MSG);
 			msg[0].buf[0] = buf6[0];
 			msg[0].buf[1] = buf6[1];
 			break;
@@ -276,7 +274,7 @@ static int dw2102_serit_i2c_transfer(struct i2c_adapter *adap,
 			buf6[0] = 0x30;
 			buf6[1] = msg[0].buf[0];
 			dw210x_op_rw(d->udev, 0xb2, 0, 0,
-					buf6, 2, DW210X_WRITE_MSG);
+				     buf6, 2, DW210X_WRITE_MSG);
 			break;
 		}
 		break;
@@ -320,10 +318,10 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
 		dw210x_op_rw(d->udev, 0xc2, 0, 0,
-				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
+			     obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		dw210x_op_rw(d->udev, 0xc3, 0xd1 , 0,
-				ibuf, msg[1].len + 2, DW210X_READ_MSG);
+		dw210x_op_rw(d->udev, 0xc3, 0xd1, 0,
+			     ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
 		break;
@@ -345,7 +343,7 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
 			dw210x_op_rw(d->udev, 0xc2, 0, 0,
-					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
+				     obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case 0x61: {
@@ -363,22 +361,24 @@ static int dw2102_earda_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg ms
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
 			dw210x_op_rw(d->udev, 0xc2, 0, 0,
-					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
+				     obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
+
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					ibuf, 2, DW210X_READ_MSG);
-			memcpy(msg[0].buf, ibuf , 2);
+				     ibuf, 2, DW210X_READ_MSG);
+			memcpy(msg[0].buf, ibuf, 2);
 			break;
 		}
 		case(DW2102_VOLTAGE_CTRL): {
 			u8 obuf[2];
+
 			obuf[0] = 0x30;
 			obuf[1] = msg[0].buf[0];
 			dw210x_op_rw(d->udev, 0xb2, 0, 0,
-					obuf, 2, DW210X_WRITE_MSG);
+				     obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
 		}
@@ -406,23 +406,26 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 		switch (msg[j].addr) {
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
+
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					ibuf, 2, DW210X_READ_MSG);
-			memcpy(msg[j].buf, ibuf , 2);
+				     ibuf, 2, DW210X_READ_MSG);
+			memcpy(msg[j].buf, ibuf, 2);
 			break;
 		}
 		case(DW2102_VOLTAGE_CTRL): {
 			u8 obuf[2];
+
 			obuf[0] = 0x30;
 			obuf[1] = msg[j].buf[0];
 			dw210x_op_rw(d->udev, 0xb2, 0, 0,
-					obuf, 2, DW210X_WRITE_MSG);
+				     obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
-		/*case 0x55: cx24116
-		case 0x6a: stv0903
-		case 0x68: ds3000, stv0903
-		case 0x60: ts2020, stv6110, stb6100 */
+		/* case 0x55: cx24116
+		 * case 0x6a: stv0903
+		 * case 0x68: ds3000, stv0903
+		 * case 0x60: ts2020, stv6110, stb6100
+		 */
 		default: {
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
@@ -436,17 +439,16 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				}
 
 				dw210x_op_rw(d->udev, 0xc3,
-						(msg[j].addr << 1) + 1, 0,
-						ibuf, msg[j].len + 2,
-						DW210X_READ_MSG);
+					     (msg[j].addr << 1) + 1, 0,
+					     ibuf, msg[j].len + 2,
+					     DW210X_READ_MSG);
 				memcpy(msg[j].buf, ibuf + 2, msg[j].len);
 				mdelay(10);
-			} else if (((msg[j].buf[0] == 0xb0) &&
-						(msg[j].addr == 0x68)) ||
-						((msg[j].buf[0] == 0xf7) &&
-						(msg[j].addr == 0x55))) {
+			} else if (((msg[j].buf[0] == 0xb0) && (msg[j].addr == 0x68)) ||
+				   ((msg[j].buf[0] == 0xf7) && (msg[j].addr == 0x55))) {
 				/* write firmware */
 				u8 obuf[19];
+
 				obuf[0] = msg[j].addr << 1;
 				obuf[1] = (msg[j].len > 15 ? 17 : msg[j].len);
 				obuf[2] = msg[j].buf[0];
@@ -454,10 +456,10 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				i = 1;
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
-							(len > 16 ? 16 : len));
+					       (len > 16 ? 16 : len));
 					dw210x_op_rw(d->udev, 0xc2, 0, 0,
-						obuf, (len > 16 ? 16 : len) + 3,
-						DW210X_WRITE_MSG);
+						     obuf, (len > 16 ? 16 : len) + 3,
+						     DW210X_WRITE_MSG);
 					i += 16;
 					len -= 16;
 				} while (len > 0);
@@ -476,13 +478,12 @@ static int dw2104_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[], i
 				obuf[1] = msg[j].len;
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
 				dw210x_op_rw(d->udev, 0xc2, 0, 0,
-						obuf, msg[j].len + 2,
-						DW210X_WRITE_MSG);
+					     obuf, msg[j].len + 2,
+					     DW210X_WRITE_MSG);
 			}
 			break;
 		}
 		}
-
 	}
 	ret = num;
 
@@ -492,7 +493,7 @@ unlock:
 }
 
 static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
-								int num)
+			       int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	int ret;
@@ -525,10 +526,10 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		obuf[1] = msg[0].len;
 		obuf[2] = msg[0].buf[0];
 		dw210x_op_rw(d->udev, 0xc2, 0, 0,
-				obuf, msg[0].len + 2, DW210X_WRITE_MSG);
+			     obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 		/* second read registers */
-		dw210x_op_rw(d->udev, 0xc3, 0x19 , 0,
-				ibuf, msg[1].len + 2, DW210X_READ_MSG);
+		dw210x_op_rw(d->udev, 0xc3, 0x19, 0,
+			     ibuf, msg[1].len + 2, DW210X_READ_MSG);
 		memcpy(msg[1].buf, ibuf + 2, msg[1].len);
 
 		break;
@@ -550,14 +551,15 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			obuf[1] = msg[0].len;
 			memcpy(obuf + 2, msg[0].buf, msg[0].len);
 			dw210x_op_rw(d->udev, 0xc2, 0, 0,
-					obuf, msg[0].len + 2, DW210X_WRITE_MSG);
+				     obuf, msg[0].len + 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case(DW2102_RC_QUERY): {
 			u8 ibuf[2];
+
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					ibuf, 2, DW210X_READ_MSG);
-			memcpy(msg[0].buf, ibuf , 2);
+				     ibuf, 2, DW210X_READ_MSG);
+			memcpy(msg[0].buf, ibuf, 2);
 			break;
 		}
 		}
@@ -567,7 +569,7 @@ static int dw3101_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 	for (i = 0; i < num; i++) {
 		deb_xfer("%02x:%02x: %s ", i, msg[i].addr,
-				msg[i].flags == 0 ? ">>>" : "<<<");
+			 msg[i].flags == 0 ? ">>>" : "<<<");
 		debug_dump(msg[i].buf, msg[i].len, deb_xfer);
 	}
 	ret = num;
@@ -578,7 +580,7 @@ unlock:
 }
 
 static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
-								int num)
+			     int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct usb_device *udev;
@@ -594,8 +596,9 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		switch (msg[j].addr) {
 		case (DW2102_RC_QUERY): {
 			u8 ibuf[5];
+
 			dw210x_op_rw(d->udev, 0xb8, 0, 0,
-					ibuf, 5, DW210X_READ_MSG);
+				     ibuf, 5, DW210X_READ_MSG);
 			memcpy(msg[j].buf, ibuf + 3, 2);
 			break;
 		}
@@ -605,11 +608,11 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			obuf[0] = 1;
 			obuf[1] = msg[j].buf[1];/* off-on */
 			dw210x_op_rw(d->udev, 0x8a, 0, 0,
-					obuf, 2, DW210X_WRITE_MSG);
+				     obuf, 2, DW210X_WRITE_MSG);
 			obuf[0] = 3;
 			obuf[1] = msg[j].buf[0];/* 13v-18v */
 			dw210x_op_rw(d->udev, 0x8a, 0, 0,
-					obuf, 2, DW210X_WRITE_MSG);
+				     obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
 		case (DW2102_LED_CTRL): {
@@ -618,14 +621,15 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			obuf[0] = 5;
 			obuf[1] = msg[j].buf[0];
 			dw210x_op_rw(d->udev, 0x8a, 0, 0,
-					obuf, 2, DW210X_WRITE_MSG);
+				     obuf, 2, DW210X_WRITE_MSG);
 			break;
 		}
-		/*case 0x55: cx24116
-		case 0x6a: stv0903
-		case 0x68: ds3000, stv0903, rs2000
-		case 0x60: ts2020, stv6110, stb6100
-		case 0xa0: eeprom */
+		/* case 0x55: cx24116
+		 * case 0x6a: stv0903
+		 * case 0x68: ds3000, stv0903, rs2000
+		 * case 0x60: ts2020, stv6110, stb6100
+		 * case 0xa0: eeprom
+		 */
 		default: {
 			if (msg[j].flags == I2C_M_RD) {
 				/* read registers */
@@ -639,14 +643,14 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				}
 
 				dw210x_op_rw(d->udev, 0x91, 0, 0,
-						ibuf, msg[j].len,
+					     ibuf, msg[j].len,
 						DW210X_READ_MSG);
 				memcpy(msg[j].buf, ibuf, msg[j].len);
 				break;
-			} else if ((msg[j].buf[0] == 0xb0) &&
-						(msg[j].addr == 0x68)) {
+			} else if ((msg[j].buf[0] == 0xb0) && (msg[j].addr == 0x68)) {
 				/* write firmware */
 				u8 obuf[19];
+
 				obuf[0] = (msg[j].len > 16 ?
 						18 : msg[j].len + 1);
 				obuf[1] = msg[j].addr << 1;
@@ -655,10 +659,10 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				i = 1;
 				do {
 					memcpy(obuf + 3, msg[j].buf + i,
-							(len > 16 ? 16 : len));
+					       (len > 16 ? 16 : len));
 					dw210x_op_rw(d->udev, 0x80, 0, 0,
-						obuf, (len > 16 ? 16 : len) + 3,
-						DW210X_WRITE_MSG);
+						     obuf, (len > 16 ? 16 : len) + 3,
+						     DW210X_WRITE_MSG);
 					i += 16;
 					len -= 16;
 				} while (len > 0);
@@ -677,10 +681,9 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
 				dw210x_op_rw(d->udev,
-						le16_to_cpu(udev->descriptor.idProduct) ==
-						0x7500 ? 0x92 : 0x90, 0, 0,
-						obuf, msg[j].len + 2,
-						DW210X_WRITE_MSG);
+					     le16_to_cpu(udev->descriptor.idProduct) == 0x7500 ? 0x92 : 0x90,
+					     0, 0, obuf, msg[j].len + 2,
+					     DW210X_WRITE_MSG);
 				break;
 			} else {
 				/* write registers */
@@ -696,8 +699,8 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
 				dw210x_op_rw(d->udev, 0x80, 0, 0,
-						obuf, msg[j].len + 2,
-						DW210X_WRITE_MSG);
+					     obuf, msg[j].len + 2,
+					     DW210X_WRITE_MSG);
 				break;
 			}
 			break;
@@ -712,10 +715,11 @@ unlock:
 }
 
 static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
-								int num)
+			       int num)
 {
 	struct dvb_usb_device *d = i2c_get_adapdata(adap);
 	struct dw2102_state *state;
+	int j;
 
 	if (!d)
 		return -ENODEV;
@@ -729,77 +733,102 @@ static int su3000_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 		return -EAGAIN;
 	}
 
-	switch (num) {
-	case 1:
-		switch (msg[0].addr) {
+	j = 0;
+	while (j < num) {
+		switch (msg[j].addr) {
 		case SU3000_STREAM_CTRL:
-			state->data[0] = msg[0].buf[0] + 0x36;
+			state->data[0] = msg[j].buf[0] + 0x36;
 			state->data[1] = 3;
 			state->data[2] = 0;
 			if (dvb_usb_generic_rw(d, state->data, 3,
-					state->data, 0, 0) < 0)
+					       state->data, 0, 0) < 0)
 				err("i2c transfer failed.");
 			break;
 		case DW2102_RC_QUERY:
 			state->data[0] = 0x10;
 			if (dvb_usb_generic_rw(d, state->data, 1,
-					state->data, 2, 0) < 0)
+					       state->data, 2, 0) < 0)
 				err("i2c transfer failed.");
-			msg[0].buf[1] = state->data[0];
-			msg[0].buf[0] = state->data[1];
+			msg[j].buf[1] = state->data[0];
+			msg[j].buf[0] = state->data[1];
 			break;
 		default:
-			if (3 + msg[0].len > sizeof(state->data)) {
-				warn("i2c wr: len=%d is too big!\n",
-				     msg[0].len);
-				num = -EOPNOTSUPP;
+			/* if the current write msg is followed by a another
+			 * read msg to/from the same address
+			 */
+			if ((j + 1 < num) && (msg[j + 1].flags & I2C_M_RD) &&
+			    (msg[j].addr == msg[j + 1].addr)) {
+				/* join both i2c msgs to one usb read command */
+				if (4 + msg[j].len > sizeof(state->data)) {
+					warn("i2c combined wr/rd: write len=%d is too big!\n",
+					     msg[j].len);
+					num = -EOPNOTSUPP;
+					break;
+				}
+				if (1 + msg[j + 1].len > sizeof(state->data)) {
+					warn("i2c combined wr/rd: read len=%d is too big!\n",
+					    msg[j + 1].len);
+					num = -EOPNOTSUPP;
+					break;
+				}
+
+				state->data[0] = 0x09;
+				state->data[1] = msg[j].len;
+				state->data[2] = msg[j + 1].len;
+				state->data[3] = msg[j].addr;
+				memcpy(&state->data[4], msg[j].buf, msg[j].len);
+
+				if (dvb_usb_generic_rw(d, state->data, msg[j].len + 4,
+					state->data, msg[j + 1].len + 1, 0) < 0)
+					err("i2c transfer failed.");
+
+				memcpy(msg[j + 1].buf, &state->data[1], msg[j + 1].len);
+				j++;
 				break;
 			}
 
-			/* always i2c write*/
-			state->data[0] = 0x08;
-			state->data[1] = msg[0].addr;
-			state->data[2] = msg[0].len;
+			if (msg[j].flags & I2C_M_RD) {
+				/* single read */
+				if (4 + msg[j].len > sizeof(state->data)) {
+					warn("i2c rd: len=%d is too big!\n", msg[j].len);
+					num = -EOPNOTSUPP;
+					break;
+				}
 
-			memcpy(&state->data[3], msg[0].buf, msg[0].len);
+				state->data[0] = 0x09;
+				state->data[1] = 0;
+				state->data[2] = msg[j].len;
+				state->data[3] = msg[j].addr;
+				memcpy(&state->data[4], msg[j].buf, msg[j].len);
 
-			if (dvb_usb_generic_rw(d, state->data, msg[0].len + 3,
-						state->data, 1, 0) < 0)
-				err("i2c transfer failed.");
+				if (dvb_usb_generic_rw(d, state->data, 4,
+						       state->data, msg[j].len + 1, 0) < 0)
+					err("i2c transfer failed.");
 
-		}
-		break;
-	case 2:
-		/* always i2c read */
-		if (4 + msg[0].len > sizeof(state->data)) {
-			warn("i2c rd: len=%d is too big!\n",
-			     msg[0].len);
-			num = -EOPNOTSUPP;
-			break;
-		}
-		if (1 + msg[1].len > sizeof(state->data)) {
-			warn("i2c rd: len=%d is too big!\n",
-			     msg[1].len);
-			num = -EOPNOTSUPP;
-			break;
-		}
+				memcpy(msg[j].buf, &state->data[1], msg[j].len);
+				break;
+			}
 
-		state->data[0] = 0x09;
-		state->data[1] = msg[0].len;
-		state->data[2] = msg[1].len;
-		state->data[3] = msg[0].addr;
-		memcpy(&state->data[4], msg[0].buf, msg[0].len);
+			/* single write */
+			if (3 + msg[j].len > sizeof(state->data)) {
+				warn("i2c wr: len=%d is too big!\n", msg[j].len);
+				num = -EOPNOTSUPP;
+				break;
+			}
 
-		if (dvb_usb_generic_rw(d, state->data, msg[0].len + 4,
-					state->data, msg[1].len + 1, 0) < 0)
-			err("i2c transfer failed.");
+			state->data[0] = 0x08;
+			state->data[1] = msg[j].addr;
+			state->data[2] = msg[j].len;
 
-		memcpy(msg[1].buf, &state->data[1], msg[1].len);
-		break;
-	default:
-		warn("more than 2 i2c messages at a time is not handled yet.");
-		break;
-	}
+			memcpy(&state->data[3], msg[j].buf, msg[j].len);
+
+			if (dvb_usb_generic_rw(d, state->data, msg[j].len + 3,
+					       state->data, 1, 0) < 0)
+				err("i2c transfer failed.");
+		} // switch
+		j++;
+
+	} // while
 	mutex_unlock(&d->data_mutex);
 	mutex_unlock(&d->i2c_mutex);
 	return num;
@@ -810,37 +839,37 @@ static u32 dw210x_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm dw2102_i2c_algo = {
+static const struct i2c_algorithm dw2102_i2c_algo = {
 	.master_xfer = dw2102_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm dw2102_serit_i2c_algo = {
+static const struct i2c_algorithm dw2102_serit_i2c_algo = {
 	.master_xfer = dw2102_serit_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm dw2102_earda_i2c_algo = {
+static const struct i2c_algorithm dw2102_earda_i2c_algo = {
 	.master_xfer = dw2102_earda_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm dw2104_i2c_algo = {
+static const struct i2c_algorithm dw2104_i2c_algo = {
 	.master_xfer = dw2104_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm dw3101_i2c_algo = {
+static const struct i2c_algorithm dw3101_i2c_algo = {
 	.master_xfer = dw3101_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm s6x0_i2c_algo = {
+static const struct i2c_algorithm s6x0_i2c_algo = {
 	.master_xfer = s6x0_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
 
-static struct i2c_algorithm su3000_i2c_algo = {
+static const struct i2c_algorithm su3000_i2c_algo = {
 	.master_xfer = su3000_i2c_transfer,
 	.functionality = dw210x_i2c_func,
 };
@@ -852,11 +881,11 @@ static int dw210x_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
 	u8 eeprom[256], eepromline[16];
 
 	for (i = 0; i < 256; i++) {
-		if (dw210x_op_rw(d->udev, 0xb6, 0xa0 , i, ibuf, 2, DW210X_READ_MSG) < 0) {
+		if (dw210x_op_rw(d->udev, 0xb6, 0xa0, i, ibuf, 2, DW210X_READ_MSG) < 0) {
 			err("read eeprom failed.");
 			return -EIO;
 		} else {
-			eepromline[i%16] = ibuf[0];
+			eepromline[i % 16] = ibuf[0];
 			eeprom[i] = ibuf[0];
 		}
 		if ((i % 16) == 15) {
@@ -963,7 +992,6 @@ static int su3000_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
 			.flags = I2C_M_RD,
 			.buf = ibuf,
 			.len = 1,
-
 		}
 	};
 
@@ -983,8 +1011,6 @@ static int su3000_identify_state(struct usb_device *udev,
 				 const struct dvb_usb_device_description **desc,
 				 int *cold)
 {
-	info("%s", __func__);
-
 	*cold = 0;
 	return 0;
 }
@@ -1003,6 +1029,7 @@ static int dw210x_set_voltage(struct dvb_frontend *fe,
 	};
 
 	struct dvb_usb_adapter *udev_adap = fe->dvb->priv;
+
 	if (voltage == SEC_VOLTAGE_18)
 		msg.buf = command_18v;
 	else if (voltage == SEC_VOLTAGE_13)
@@ -1206,11 +1233,11 @@ static int dw2104_frontend_attach(struct dvb_usb_adapter *d)
 
 	if (demod_probe & 4) {
 		d->fe_adap[0].fe = dvb_attach(stv0900_attach, &dw2104a_stv0900_config,
-				&d->dev->i2c_adap, 0);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap, 0);
+		if (d->fe_adap[0].fe) {
 			if (dvb_attach(stb6100_attach, d->fe_adap[0].fe,
-					&dw2104a_stb6100_config,
-					&d->dev->i2c_adap)) {
+				       &dw2104a_stb6100_config,
+				       &d->dev->i2c_adap)) {
 				tuner_ops = &d->fe_adap[0].fe->ops.tuner_ops;
 				tuner_ops->set_frequency = stb6100_set_freq;
 				tuner_ops->get_frequency = stb6100_get_freq;
@@ -1225,11 +1252,11 @@ static int dw2104_frontend_attach(struct dvb_usb_adapter *d)
 
 	if (demod_probe & 2) {
 		d->fe_adap[0].fe = dvb_attach(stv0900_attach, &dw2104_stv0900_config,
-				&d->dev->i2c_adap, 0);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap, 0);
+		if (d->fe_adap[0].fe) {
 			if (dvb_attach(stv6110_attach, d->fe_adap[0].fe,
-					&dw2104_stv6110_config,
-					&d->dev->i2c_adap)) {
+				       &dw2104_stv6110_config,
+				       &d->dev->i2c_adap)) {
 				d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 				info("Attached STV0900+STV6110A!");
 				return 0;
@@ -1239,8 +1266,8 @@ static int dw2104_frontend_attach(struct dvb_usb_adapter *d)
 
 	if (demod_probe & 1) {
 		d->fe_adap[0].fe = dvb_attach(cx24116_attach, &dw2104_config,
-				&d->dev->i2c_adap);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap);
+		if (d->fe_adap[0].fe) {
 			d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 			info("Attached cx24116!");
 			return 0;
@@ -1248,10 +1275,10 @@ static int dw2104_frontend_attach(struct dvb_usb_adapter *d)
 	}
 
 	d->fe_adap[0].fe = dvb_attach(ds3000_attach, &dw2104_ds3000_config,
-			&d->dev->i2c_adap);
-	if (d->fe_adap[0].fe != NULL) {
+				      &d->dev->i2c_adap);
+	if (d->fe_adap[0].fe) {
 		dvb_attach(ts2020_attach, d->fe_adap[0].fe,
-			&dw2104_ts2020_config, &d->dev->i2c_adap);
+			   &dw2104_ts2020_config, &d->dev->i2c_adap);
 		d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 		info("Attached DS3000!");
 		return 0;
@@ -1269,8 +1296,8 @@ static int dw2102_frontend_attach(struct dvb_usb_adapter *d)
 	if (dw2102_properties.i2c_algo == &dw2102_serit_i2c_algo) {
 		/*dw2102_properties.adapter->tuner_attach = NULL;*/
 		d->fe_adap[0].fe = dvb_attach(si21xx_attach, &serit_sp1511lhb_config,
-					&d->dev->i2c_adap);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap);
+		if (d->fe_adap[0].fe) {
 			d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 			info("Attached si21xx!");
 			return 0;
@@ -1279,10 +1306,10 @@ static int dw2102_frontend_attach(struct dvb_usb_adapter *d)
 
 	if (dw2102_properties.i2c_algo == &dw2102_earda_i2c_algo) {
 		d->fe_adap[0].fe = dvb_attach(stv0288_attach, &earda_config,
-					&d->dev->i2c_adap);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap);
+		if (d->fe_adap[0].fe) {
 			if (dvb_attach(stb6000_attach, d->fe_adap[0].fe, 0x61,
-					&d->dev->i2c_adap)) {
+				       &d->dev->i2c_adap)) {
 				d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 				info("Attached stv0288!");
 				return 0;
@@ -1293,8 +1320,8 @@ static int dw2102_frontend_attach(struct dvb_usb_adapter *d)
 	if (dw2102_properties.i2c_algo == &dw2102_i2c_algo) {
 		/*dw2102_properties.adapter->tuner_attach = dw2102_tuner_attach;*/
 		d->fe_adap[0].fe = dvb_attach(stv0299_attach, &sharp_z0194a_config,
-					&d->dev->i2c_adap);
-		if (d->fe_adap[0].fe != NULL) {
+					      &d->dev->i2c_adap);
+		if (d->fe_adap[0].fe) {
 			d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 			info("Attached stv0299!");
 			return 0;
@@ -1306,8 +1333,8 @@ static int dw2102_frontend_attach(struct dvb_usb_adapter *d)
 static int dw3101_frontend_attach(struct dvb_usb_adapter *d)
 {
 	d->fe_adap[0].fe = dvb_attach(tda10023_attach, &dw3101_tda10023_config,
-				&d->dev->i2c_adap, 0x48);
-	if (d->fe_adap[0].fe != NULL) {
+				      &d->dev->i2c_adap, 0x48);
+	if (d->fe_adap[0].fe) {
 		info("Attached tda10023!");
 		return 0;
 	}
@@ -1317,10 +1344,10 @@ static int dw3101_frontend_attach(struct dvb_usb_adapter *d)
 static int zl100313_frontend_attach(struct dvb_usb_adapter *d)
 {
 	d->fe_adap[0].fe = dvb_attach(mt312_attach, &zl313_config,
-			&d->dev->i2c_adap);
-	if (d->fe_adap[0].fe != NULL) {
+				      &d->dev->i2c_adap);
+	if (d->fe_adap[0].fe) {
 		if (dvb_attach(zl10039_attach, d->fe_adap[0].fe, 0x60,
-				&d->dev->i2c_adap)) {
+			       &d->dev->i2c_adap)) {
 			d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
 			info("Attached zl100313+zl10039!");
 			return 0;
@@ -1335,12 +1362,12 @@ static int stv0288_frontend_attach(struct dvb_usb_adapter *d)
 	u8 obuf[] = {7, 1};
 
 	d->fe_adap[0].fe = dvb_attach(stv0288_attach, &earda_config,
-			&d->dev->i2c_adap);
+				      &d->dev->i2c_adap);
 
-	if (d->fe_adap[0].fe == NULL)
+	if (!d->fe_adap[0].fe)
 		return -EIO;
 
-	if (NULL == dvb_attach(stb6000_attach, d->fe_adap[0].fe, 0x61, &d->dev->i2c_adap))
+	if (dvb_attach(stb6000_attach, d->fe_adap[0].fe, 0x61, &d->dev->i2c_adap) == NULL)
 		return -EIO;
 
 	d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
@@ -1350,7 +1377,6 @@ static int stv0288_frontend_attach(struct dvb_usb_adapter *d)
 	info("Attached stv0288+stb6000!");
 
 	return 0;
-
 }
 
 static int ds3000_frontend_attach(struct dvb_usb_adapter *d)
@@ -1359,13 +1385,13 @@ static int ds3000_frontend_attach(struct dvb_usb_adapter *d)
 	u8 obuf[] = {7, 1};
 
 	d->fe_adap[0].fe = dvb_attach(ds3000_attach, &s660_ds3000_config,
-			&d->dev->i2c_adap);
+				      &d->dev->i2c_adap);
 
-	if (d->fe_adap[0].fe == NULL)
+	if (!d->fe_adap[0].fe)
 		return -EIO;
 
 	dvb_attach(ts2020_attach, d->fe_adap[0].fe, &s660_ts2020_config,
-		&d->dev->i2c_adap);
+		   &d->dev->i2c_adap);
 
 	st->old_set_voltage = d->fe_adap[0].fe->ops.set_voltage;
 	d->fe_adap[0].fe->ops.set_voltage = s660_set_voltage;
@@ -1382,8 +1408,8 @@ static int prof_7500_frontend_attach(struct dvb_usb_adapter *d)
 	u8 obuf[] = {7, 1};
 
 	d->fe_adap[0].fe = dvb_attach(stv0900_attach, &prof_7500_stv0900_config,
-					&d->dev->i2c_adap, 0);
-	if (d->fe_adap[0].fe == NULL)
+				      &d->dev->i2c_adap, 0);
+	if (!d->fe_adap[0].fe)
 		return -EIO;
 
 	d->fe_adap[0].fe->ops.set_voltage = dw210x_set_voltage;
@@ -1439,12 +1465,12 @@ static int su3000_frontend_attach(struct dvb_usb_adapter *adap)
 	mutex_unlock(&d->data_mutex);
 
 	adap->fe_adap[0].fe = dvb_attach(ds3000_attach, &su3000_ds3000_config,
-					&d->i2c_adap);
-	if (adap->fe_adap[0].fe == NULL)
+					 &d->i2c_adap);
+	if (!adap->fe_adap[0].fe)
 		return -EIO;
 
 	if (dvb_attach(ts2020_attach, adap->fe_adap[0].fe,
-				&dw2104_ts2020_config,
+		       &dw2104_ts2020_config,
 				&d->i2c_adap)) {
 		info("Attached DS3000/TS2020!");
 		return 0;
@@ -1499,10 +1525,10 @@ static int t220_frontend_attach(struct dvb_usb_adapter *adap)
 	mutex_unlock(&d->data_mutex);
 
 	adap->fe_adap[0].fe = dvb_attach(cxd2820r_attach, &cxd2820r_config,
-					&d->i2c_adap, NULL);
-	if (adap->fe_adap[0].fe != NULL) {
+					 &d->i2c_adap, NULL);
+	if (adap->fe_adap[0].fe) {
 		if (dvb_attach(tda18271_attach, adap->fe_adap[0].fe, 0x60,
-					&d->i2c_adap, &tda18271_config)) {
+			       &d->i2c_adap, &tda18271_config)) {
 			info("Attached TDA18271HD/CXD2820R!");
 			return 0;
 		}
@@ -1527,14 +1553,14 @@ static int m88rs2000_frontend_attach(struct dvb_usb_adapter *adap)
 	mutex_unlock(&d->data_mutex);
 
 	adap->fe_adap[0].fe = dvb_attach(m88rs2000_attach,
-					&s421_m88rs2000_config,
-					&d->i2c_adap);
+					 &s421_m88rs2000_config,
+					 &d->i2c_adap);
 
-	if (adap->fe_adap[0].fe == NULL)
+	if (!adap->fe_adap[0].fe)
 		return -EIO;
 
 	if (dvb_attach(ts2020_attach, adap->fe_adap[0].fe,
-				&dw2104_ts2020_config,
+		       &dw2104_ts2020_config,
 				&d->i2c_adap)) {
 		info("Attached RS2000/TS2020!");
 		return 0;
@@ -1701,14 +1727,14 @@ static int tt_s2_4600_frontend_attach(struct dvb_usb_adapter *adap)
 static int dw2102_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	dvb_attach(dvb_pll_attach, adap->fe_adap[0].fe, 0x60,
-		&adap->dev->i2c_adap, DVB_PLL_OPERA1);
+		   &adap->dev->i2c_adap, DVB_PLL_OPERA1);
 	return 0;
 }
 
 static int dw3101_tuner_attach(struct dvb_usb_adapter *adap)
 {
 	dvb_attach(dvb_pll_attach, adap->fe_adap[0].fe, 0x60,
-		&adap->dev->i2c_adap, DVB_PLL_TUA6034);
+		   &adap->dev->i2c_adap, DVB_PLL_TUA6034);
 
 	return 0;
 }
@@ -1726,7 +1752,7 @@ static int dw2102_rc_query(struct dvb_usb_device *d)
 	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
 		if (msg.buf[0] != 0xff) {
 			deb_rc("%s: rc code: %x, %x\n",
-					__func__, key[0], key[1]);
+			       __func__, key[0], key[1]);
 			rc_keydown(d->rc_dev, RC_PROTO_UNKNOWN, key[0], 0);
 		}
 	}
@@ -1747,7 +1773,7 @@ static int prof_rc_query(struct dvb_usb_device *d)
 	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
 		if (msg.buf[0] != 0xff) {
 			deb_rc("%s: rc code: %x, %x\n",
-					__func__, key[0], key[1]);
+			       __func__, key[0], key[1]);
 			rc_keydown(d->rc_dev, RC_PROTO_UNKNOWN, key[0] ^ 0xff,
 				   0);
 		}
@@ -1769,7 +1795,7 @@ static int su3000_rc_query(struct dvb_usb_device *d)
 	if (d->props.i2c_algo->master_xfer(&d->i2c_adap, &msg, 1) == 1) {
 		if (msg.buf[0] != 0xff) {
 			deb_rc("%s: rc code: %x, %x\n",
-					__func__, key[0], key[1]);
+			       __func__, key[0], key[1]);
 			rc_keydown(d->rc_dev, RC_PROTO_RC5,
 				   RC_SCANCODE_RC5(key[1], key[0]), 0);
 		}
@@ -1807,11 +1833,10 @@ enum dw2102_table_entry {
 	TECHNOTREND_CONNECT_S2_4600,
 	TEVII_S482_1,
 	TEVII_S482_2,
-	TERRATEC_CINERGY_S2_BOX,
 	TEVII_S662
 };
 
-static struct usb_device_id dw2102_table[] = {
+static const struct usb_device_id dw2102_table[] = {
 	DVB_USB_DEV(CYPRESS, CYPRESS_DW2102),
 	DVB_USB_DEV(CYPRESS, CYPRESS_DW2101),
 	DVB_USB_DEV(CYPRESS, CYPRESS_DW2104),
@@ -1840,7 +1865,6 @@ static struct usb_device_id dw2102_table[] = {
 	DVB_USB_DEV(TECHNOTREND, TECHNOTREND_CONNECT_S2_4600),
 	DVB_USB_DEV(TEVII, TEVII_S482_1),
 	DVB_USB_DEV(TEVII, TEVII_S482_2),
-	DVB_USB_DEV(TERRATEC, TERRATEC_CINERGY_S2_BOX),
 	DVB_USB_DEV(TEVII, TEVII_S662),
 	{ }
 };
@@ -1848,7 +1872,7 @@ static struct usb_device_id dw2102_table[] = {
 MODULE_DEVICE_TABLE(usb, dw2102_table);
 
 static int dw2102_load_firmware(struct usb_device *dev,
-			const struct firmware *frmwr)
+				const struct firmware *frmwr)
 {
 	u8 *b, *p;
 	int ret = 0, i;
@@ -1875,12 +1899,12 @@ static int dw2102_load_firmware(struct usb_device *dev,
 	dw210x_op_rw(dev, 0xa0, 0x7f92, 0, &reset, 1, DW210X_WRITE_MSG);
 	dw210x_op_rw(dev, 0xa0, 0xe600, 0, &reset, 1, DW210X_WRITE_MSG);
 
-	if (p != NULL) {
+	if (p) {
 		memcpy(p, fw->data, fw->size);
 		for (i = 0; i < fw->size; i += 0x40) {
-			b = (u8 *) p + i;
-			if (dw210x_op_rw(dev, 0xa0, i, 0, b , 0x40,
-					DW210X_WRITE_MSG) != 0x40) {
+			b = (u8 *)p + i;
+			if (dw210x_op_rw(dev, 0xa0, i, 0, b, 0x40,
+					 DW210X_WRITE_MSG) != 0x40) {
 				err("error while transferring firmware");
 				ret = -EINVAL;
 				break;
@@ -1906,50 +1930,49 @@ static int dw2102_load_firmware(struct usb_device *dev,
 		case USB_PID_CYPRESS_DW2104:
 			reset = 1;
 			dw210x_op_rw(dev, 0xc4, 0x0000, 0, &reset, 1,
-					DW210X_WRITE_MSG);
+				     DW210X_WRITE_MSG);
 			fallthrough;
 		case USB_PID_CYPRESS_DW3101:
 			reset = 0;
 			dw210x_op_rw(dev, 0xbf, 0x0040, 0, &reset, 0,
-					DW210X_WRITE_MSG);
+				     DW210X_WRITE_MSG);
 			break;
 		case USB_PID_TERRATEC_CINERGY_S:
 		case USB_PID_CYPRESS_DW2102:
 			dw210x_op_rw(dev, 0xbf, 0x0040, 0, &reset, 0,
-					DW210X_WRITE_MSG);
+				     DW210X_WRITE_MSG);
 			dw210x_op_rw(dev, 0xb9, 0x0000, 0, &reset16[0], 2,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			/* check STV0299 frontend  */
 			dw210x_op_rw(dev, 0xb5, 0, 0, &reset16[0], 2,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			if ((reset16[0] == 0xa1) || (reset16[0] == 0x80)) {
 				dw2102_properties.i2c_algo = &dw2102_i2c_algo;
 				dw2102_properties.adapter->fe[0].tuner_attach = &dw2102_tuner_attach;
 				break;
-			} else {
-				/* check STV0288 frontend  */
-				reset16[0] = 0xd0;
-				reset16[1] = 1;
-				reset16[2] = 0;
-				dw210x_op_rw(dev, 0xc2, 0, 0, &reset16[0], 3,
-						DW210X_WRITE_MSG);
-				dw210x_op_rw(dev, 0xc3, 0xd1, 0, &reset16[0], 3,
-						DW210X_READ_MSG);
-				if (reset16[2] == 0x11) {
-					dw2102_properties.i2c_algo = &dw2102_earda_i2c_algo;
-					break;
-				}
+			}
+			/* check STV0288 frontend  */
+			reset16[0] = 0xd0;
+			reset16[1] = 1;
+			reset16[2] = 0;
+			dw210x_op_rw(dev, 0xc2, 0, 0, &reset16[0], 3,
+				     DW210X_WRITE_MSG);
+			dw210x_op_rw(dev, 0xc3, 0xd1, 0, &reset16[0], 3,
+				     DW210X_READ_MSG);
+			if (reset16[2] == 0x11) {
+				dw2102_properties.i2c_algo = &dw2102_earda_i2c_algo;
+				break;
 			}
 			fallthrough;
 		case 0x2101:
 			dw210x_op_rw(dev, 0xbc, 0x0030, 0, &reset16[0], 2,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			dw210x_op_rw(dev, 0xba, 0x0000, 0, &reset16[0], 7,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			dw210x_op_rw(dev, 0xba, 0x0000, 0, &reset16[0], 7,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			dw210x_op_rw(dev, 0xb9, 0x0000, 0, &reset16[0], 2,
-					DW210X_READ_MSG);
+				     DW210X_READ_MSG);
 			break;
 		}
 
@@ -2551,7 +2574,7 @@ static struct dvb_usb_device_properties tt_s2_4600_properties = {
 			{ NULL },
 		},
 		{ "Terratec Cinergy S2 USB BOX",
-			{ &dw2102_table[TERRATEC_CINERGY_S2_BOX], NULL },
+			{ &dw2102_table[TERRATEC_CINERGY_S2_R4], NULL },
 			{ NULL },
 		},
 		{ "TeVii S662",
@@ -2562,18 +2585,18 @@ static struct dvb_usb_device_properties tt_s2_4600_properties = {
 };
 
 static int dw2102_probe(struct usb_interface *intf,
-		const struct usb_device_id *id)
+			const struct usb_device_id *id)
 {
 	if (!(dvb_usb_device_init(intf, &dw2102_properties,
-			          THIS_MODULE, NULL, adapter_nr) &&
+				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &dw2104_properties,
 				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &dw3101_properties,
-			          THIS_MODULE, NULL, adapter_nr) &&
+				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &s6x0_properties,
-			          THIS_MODULE, NULL, adapter_nr) &&
+				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &p1100_properties,
-			          THIS_MODULE, NULL, adapter_nr) &&
+				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &s660_properties,
 				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &p7500_properties,
@@ -2586,7 +2609,6 @@ static int dw2102_probe(struct usb_interface *intf,
 				  THIS_MODULE, NULL, adapter_nr) &&
 	      dvb_usb_device_init(intf, &tt_s2_4600_properties,
 				  THIS_MODULE, NULL, adapter_nr))) {
-
 		return 0;
 	}
 
diff --git a/drivers/media/usb/dvb-usb/gp8psk.c b/drivers/media/usb/dvb-usb/gp8psk.c
index 4cd21bb8805e..96a255500b38 100644
--- a/drivers/media/usb/dvb-usb/gp8psk.c
+++ b/drivers/media/usb/dvb-usb/gp8psk.c
@@ -319,7 +319,7 @@ enum {
 	GENPIX_SKYWALKER_CW3K,
 };
 
-static struct usb_device_id gp8psk_usb_table[] = {
+static const struct usb_device_id gp8psk_usb_table[] = {
 	DVB_USB_DEV(GENPIX, GENPIX_8PSK_REV_1_COLD),
 	DVB_USB_DEV(GENPIX, GENPIX_8PSK_REV_1_WARM),
 	DVB_USB_DEV(GENPIX, GENPIX_8PSK_REV_2),
diff --git a/drivers/media/usb/dvb-usb/m920x.c b/drivers/media/usb/dvb-usb/m920x.c
index c88a202daf5f..45337ba0a0a3 100644
--- a/drivers/media/usb/dvb-usb/m920x.c
+++ b/drivers/media/usb/dvb-usb/m920x.c
@@ -17,7 +17,7 @@
 
 #include <media/tuner.h>
 #include "tuner-simple.h"
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 /* debug */
 static int dvb_usb_m920x_debug;
@@ -319,7 +319,7 @@ static u32 m920x_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm m920x_i2c_algo = {
+static const struct i2c_algorithm m920x_i2c_algo = {
 	.master_xfer   = m920x_i2c_xfer,
 	.functionality = m920x_i2c_func,
 };
@@ -909,7 +909,7 @@ enum {
 	AZUREWAVE_TWINHAN_VP7049,
 };
 
-static struct usb_device_id m920x_table[] = {
+static const struct usb_device_id m920x_table[] = {
 	DVB_USB_DEV(MSI, MSI_MEGASKY580),
 	DVB_USB_DEV(ANUBIS_ELECTRONIC, ANUBIS_MSI_DIGI_VOX_MINI_II),
 	DVB_USB_DEV(ANUBIS_ELECTRONIC, ANUBIS_LIFEVIEW_TV_WALKER_TWIN_COLD),
diff --git a/drivers/media/usb/dvb-usb/nova-t-usb2.c b/drivers/media/usb/dvb-usb/nova-t-usb2.c
index 4782d0780913..2e5cbfacbeed 100644
--- a/drivers/media/usb/dvb-usb/nova-t-usb2.c
+++ b/drivers/media/usb/dvb-usb/nova-t-usb2.c
@@ -165,7 +165,7 @@ enum {
 	HAUPPAUGE_WINTV_NOVA_T_USB2_WARM,
 };
 
-static struct usb_device_id nova_t_table[] = {
+static const struct usb_device_id nova_t_table[] = {
 	DVB_USB_DEV(HAUPPAUGE, HAUPPAUGE_WINTV_NOVA_T_USB2_COLD),
 	DVB_USB_DEV(HAUPPAUGE, HAUPPAUGE_WINTV_NOVA_T_USB2_WARM),
 	{ }
diff --git a/drivers/media/usb/dvb-usb/opera1.c b/drivers/media/usb/dvb-usb/opera1.c
index d269f8bb2dee..3c79cc6848b4 100644
--- a/drivers/media/usb/dvb-usb/opera1.c
+++ b/drivers/media/usb/dvb-usb/opera1.c
@@ -32,10 +32,6 @@
 struct opera1_state {
 	u32 last_key_pressed;
 };
-struct rc_map_opera_table {
-	u32 keycode;
-	u32 event;
-};
 
 static int dvb_usb_opera1_debug;
 module_param_named(debug, dvb_usb_opera1_debug, int, 0644);
@@ -159,7 +155,7 @@ static u32 opera1_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm opera1_i2c_algo = {
+static const struct i2c_algorithm opera1_i2c_algo = {
 	.master_xfer = opera1_i2c_xfer,
 	.functionality = opera1_i2c_func,
 };
@@ -429,7 +425,7 @@ enum {
 	OPERA1_WARM,
 };
 
-static struct usb_device_id opera1_table[] = {
+static const struct usb_device_id opera1_table[] = {
 	DVB_USB_DEV(CYPRESS, CYPRESS_OPERA1_COLD),
 	DVB_USB_DEV(OPERA1, OPERA1_WARM),
 	{ }
diff --git a/drivers/media/usb/dvb-usb/pctv452e.c b/drivers/media/usb/dvb-usb/pctv452e.c
index 2aab49003493..5094de9a312e 100644
--- a/drivers/media/usb/dvb-usb/pctv452e.c
+++ b/drivers/media/usb/dvb-usb/pctv452e.c
@@ -906,14 +906,14 @@ static struct stb6100_config stb6100_config = {
 };
 
 
-static struct i2c_algorithm pctv452e_i2c_algo = {
+static const struct i2c_algorithm pctv452e_i2c_algo = {
 	.master_xfer   = pctv452e_i2c_xfer,
 	.functionality = pctv452e_i2c_func
 };
 
 static int pctv452e_frontend_attach(struct dvb_usb_adapter *a)
 {
-	struct usb_device_id *id;
+	const struct usb_device_id *id;
 
 	a->fe_adap[0].fe = dvb_attach(stb0899_attach, &stb0899_config,
 						&a->dev->i2c_adap);
@@ -959,7 +959,7 @@ enum {
 	TECHNOTREND_CONNECT_S2_3650_CI,
 };
 
-static struct usb_device_id pctv452e_usb_table[] = {
+static const struct usb_device_id pctv452e_usb_table[] = {
 	DVB_USB_DEV(PINNACLE, PINNACLE_PCTV_452E),
 	DVB_USB_DEV(TECHNOTREND, TECHNOTREND_CONNECT_S2_3600),
 	DVB_USB_DEV(TECHNOTREND, TECHNOTREND_CONNECT_S2_3650_CI),
diff --git a/drivers/media/usb/dvb-usb/technisat-usb2.c b/drivers/media/usb/dvb-usb/technisat-usb2.c
index df90c6c5f3b9..1e43aab2bc27 100644
--- a/drivers/media/usb/dvb-usb/technisat-usb2.c
+++ b/drivers/media/usb/dvb-usb/technisat-usb2.c
@@ -199,7 +199,7 @@ static u32 technisat_usb2_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm technisat_usb2_i2c_algo = {
+static const struct i2c_algorithm technisat_usb2_i2c_algo = {
 	.master_xfer   = technisat_usb2_i2c_xfer,
 	.functionality = technisat_usb2_i2c_func,
 };
@@ -693,7 +693,7 @@ enum {
 	TECHNISAT_USB2_DVB_S2,
 };
 
-static struct usb_device_id technisat_usb2_id_table[] = {
+static const struct usb_device_id technisat_usb2_id_table[] = {
 	DVB_USB_DEV(TECHNISAT, TECHNISAT_USB2_DVB_S2),
 	{ }
 };
diff --git a/drivers/media/usb/dvb-usb/ttusb2.c b/drivers/media/usb/dvb-usb/ttusb2.c
index 373ffa7f641e..acde6149d278 100644
--- a/drivers/media/usb/dvb-usb/ttusb2.c
+++ b/drivers/media/usb/dvb-usb/ttusb2.c
@@ -434,7 +434,7 @@ static u32 ttusb2_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-static struct i2c_algorithm ttusb2_i2c_algo = {
+static const struct i2c_algorithm ttusb2_i2c_algo = {
 	.master_xfer   = ttusb2_i2c_xfer,
 	.functionality = ttusb2_i2c_func,
 };
@@ -638,7 +638,7 @@ enum {
 	TECHNOTREND_CONNECT_S2400_8KEEPROM,
 };
 
-static struct usb_device_id ttusb2_table[] = {
+static const struct usb_device_id ttusb2_table[] = {
 	DVB_USB_DEV(PINNACLE, PINNACLE_PCTV_400E),
 	DVB_USB_DEV(PINNACLE, PINNACLE_PCTV_450E),
 	DVB_USB_DEV(TECHNOTREND, TECHNOTREND_CONNECT_S2400),
diff --git a/drivers/media/usb/dvb-usb/umt-010.c b/drivers/media/usb/dvb-usb/umt-010.c
index 464699b0b75b..8f23f92946d4 100644
--- a/drivers/media/usb/dvb-usb/umt-010.c
+++ b/drivers/media/usb/dvb-usb/umt-010.c
@@ -86,7 +86,7 @@ enum {
 	HANFTEK_UMT_010_WARM,
 };
 
-static struct usb_device_id umt_table[] = {
+static const struct usb_device_id umt_table[] = {
 	DVB_USB_DEV(HANFTEK, HANFTEK_UMT_010_COLD),
 	DVB_USB_DEV(HANFTEK, HANFTEK_UMT_010_WARM),
 	{ }
diff --git a/drivers/media/usb/dvb-usb/vp702x.c b/drivers/media/usb/dvb-usb/vp702x.c
index 5b6740cbd1d1..034b0652b9a1 100644
--- a/drivers/media/usb/dvb-usb/vp702x.c
+++ b/drivers/media/usb/dvb-usb/vp702x.c
@@ -375,7 +375,7 @@ enum {
 	VISIONPLUS_VP7020_WARM,
 };
 
-static struct usb_device_id vp702x_usb_table[] = {
+static const struct usb_device_id vp702x_usb_table[] = {
 	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_VP7021_COLD),
 //	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_VP7020_COLD),
 //	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_VP7020_WARM),
diff --git a/drivers/media/usb/dvb-usb/vp7045.c b/drivers/media/usb/dvb-usb/vp7045.c
index 1dc2b18d44d8..5224c3233f8c 100644
--- a/drivers/media/usb/dvb-usb/vp7045.c
+++ b/drivers/media/usb/dvb-usb/vp7045.c
@@ -179,7 +179,7 @@ enum {
 	VISIONPLUS_TINYUSB2_WARM,
 };
 
-static struct usb_device_id vp7045_usb_table[] = {
+static const struct usb_device_id vp7045_usb_table[] = {
 	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_VP7045_COLD),
 	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_VP7045_WARM),
 	DVB_USB_DEV(VISIONPLUS, VISIONPLUS_TINYUSB2_COLD),
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index bae76023cf71..a51cbcf429e1 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -555,6 +555,30 @@ static struct em28xx_reg_seq hauppauge_usb_quadhd_atsc_reg_seq[] = {
 };
 
 /*
+ * MyGica USB TV Box
+ * GPIO_1,0: 00=Composite audio
+ *           01=Tuner audio
+ *           10=Mute audio
+ *           11=FM radio? (if equipped)
+ * GPIO_2-6: Unused
+ * GPIO_7:   ??
+ */
+static const struct em28xx_reg_seq mygica_utv3_composite_audio_gpio[] = {
+	{EM2820_R08_GPIO_CTRL, 0xfc, 0xff, 0},
+	{ -1, -1, -1, -1},
+};
+
+static const struct em28xx_reg_seq mygica_utv3_tuner_audio_gpio[] = {
+	{EM2820_R08_GPIO_CTRL, 0xfd, 0xff, 0},
+	{ -1, -1, -1, -1},
+};
+
+static const struct em28xx_reg_seq mygica_utv3_suspend_gpio[] = {
+	{EM2820_R08_GPIO_CTRL, 0xfe, 0xff, 0},
+	{ -1, -1, -1, -1},
+};
+
+/*
  *  Button definitions
  */
 static const struct em28xx_button std_snapshot_button[] = {
@@ -2578,6 +2602,32 @@ const struct em28xx_board em28xx_boards[] = {
 		.tuner_gpio    = hauppauge_usb_quadhd_atsc_reg_seq,
 		.leds          = hauppauge_usb_quadhd_leds,
 	},
+	/*
+	 * eb1a:2860 MyGica UTV3 Analog USB2.0 TV Box
+	 * Empia EM2860, Philips SAA7113, NXP TDA9801T demod,
+	 * Tena TNF931D-DFDR1 tuner (contains NXP TDA6509A),
+	 * ST HCF4052 demux (switches audio to line out),
+	 * no audio over USB
+	 */
+	[EM2860_BOARD_MYGICA_UTV3] = {
+		.name         = "MyGica UTV3 Analog USB2.0 TV Box",
+		.xclk         = EM28XX_XCLK_IR_RC5_MODE | EM28XX_XCLK_FREQUENCY_12MHZ,
+		.tuner_type   = TUNER_TENA_TNF_931D_DFDR1,
+		.ir_codes     = RC_MAP_MYGICA_UTV3,
+		.decoder      = EM28XX_SAA711X,
+		.suspend_gpio = mygica_utv3_suspend_gpio,
+		.input        = { {
+			.type     = EM28XX_VMUX_COMPOSITE,
+			.vmux     = SAA7115_COMPOSITE0,
+			.amux     = EM28XX_AMUX_VIDEO,
+			.gpio     = mygica_utv3_composite_audio_gpio,
+		}, {
+			.type     = EM28XX_VMUX_TELEVISION,
+			.vmux     = SAA7115_COMPOSITE2,
+			.amux     = EM28XX_AMUX_VIDEO,
+			.gpio     = mygica_utv3_tuner_audio_gpio,
+		} },
+	},
 };
 EXPORT_SYMBOL_GPL(em28xx_boards);
 
@@ -2819,6 +2869,7 @@ static const struct em28xx_hash_table em28xx_eeprom_hash[] = {
 	{0x63f653bd, EM2870_BOARD_REDDO_DVB_C_USB_BOX, TUNER_ABSENT},
 	{0x4e913442, EM2882_BOARD_DIKOM_DK300, TUNER_XC2028},
 	{0x85dd871e, EM2882_BOARD_ZOLID_HYBRID_TV_STICK, TUNER_XC2028},
+	{0x8f597549, EM2860_BOARD_MYGICA_UTV3, TUNER_TENA_TNF_931D_DFDR1},
 };
 
 /* I2C devicelist hash table for devices with generic USB IDs */
@@ -2831,6 +2882,7 @@ static const struct em28xx_hash_table em28xx_i2c_hash[] = {
 	{0x4ba50080, EM2861_BOARD_GADMEI_UTV330PLUS, TUNER_TNF_5335MF},
 	{0x6b800080, EM2874_BOARD_LEADERSHIP_ISDBT, TUNER_ABSENT},
 	{0x27e10080, EM2882_BOARD_ZOLID_HYBRID_TV_STICK, TUNER_XC2028},
+	{0x840d0484, EM2860_BOARD_MYGICA_UTV3, TUNER_TENA_TNF_931D_DFDR1},
 };
 
 /* NOTE: introduce a separate hash table for devices with 16 bit eeproms */
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index 61d7bf701d57..29a7f3f19b56 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -416,8 +416,9 @@ int em28xx_audio_analog_set(struct em28xx *dev)
 	int ret, i;
 	u8 xclk;
 
+	/* Set GPIOs here for boards without audio */
 	if (dev->int_audio_type == EM28XX_INT_AUDIO_NONE)
-		return 0;
+		return em28xx_gpio_set(dev, INPUT(dev->ctl_input)->gpio);
 
 	/*
 	 * It is assumed that all devices use master volume for output.
diff --git a/drivers/media/usb/em28xx/em28xx-vbi.c b/drivers/media/usb/em28xx/em28xx-vbi.c
index b253c44c9724..8c5d95181223 100644
--- a/drivers/media/usb/em28xx/em28xx-vbi.c
+++ b/drivers/media/usb/em28xx/em28xx-vbi.c
@@ -84,6 +84,4 @@ const struct vb2_ops em28xx_vbi_qops = {
 	.buf_queue      = vbi_buffer_queue,
 	.start_streaming = em28xx_start_analog_streaming,
 	.stop_streaming = em28xx_stop_vbi_streaming,
-	.wait_prepare   = vb2_ops_wait_prepare,
-	.wait_finish    = vb2_ops_wait_finish,
 };
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 4aef584e21da..2dfa3242a7ab 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -264,7 +264,7 @@ static void em28xx_capture_area_set(struct em28xx *dev, u8 hstart, u8 vstart,
 	u8 overflow = (height >> 9 & 0x02) | (width >> 10 & 0x01);
 	/* NOTE: size limit: 2047x1023 = 2MPix */
 
-	em28xx_videodbg("capture area set to (%d,%d): %dx%d\n",
+	em28xx_videodbg("capture area set to (%u,%u)/%ux%u\n",
 			hstart, vstart,
 		       ((overflow & 2) << 9 | cwidth << 2),
 		       ((overflow & 1) << 10 | cheight << 2));
@@ -1229,8 +1229,6 @@ static const struct vb2_ops em28xx_video_qops = {
 	.buf_queue      = buffer_queue,
 	.start_streaming = em28xx_start_analog_streaming,
 	.stop_streaming = em28xx_stop_streaming,
-	.wait_prepare   = vb2_ops_wait_prepare,
-	.wait_finish    = vb2_ops_wait_finish,
 };
 
 static int em28xx_vb2_setup(struct em28xx *dev)
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index db18dd814a67..f3449c240d21 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -143,6 +143,7 @@
 #define EM28178_BOARD_PCTV_461E_V2                104
 #define EM2860_BOARD_MYGICA_IGRABBER              105
 #define EM2874_BOARD_HAUPPAUGE_USB_QUADHD         106
+#define EM2860_BOARD_MYGICA_UTV3                  107
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
diff --git a/drivers/media/usb/go7007/go7007-driver.c b/drivers/media/usb/go7007/go7007-driver.c
index eb03f98b2ef1..468406302cd5 100644
--- a/drivers/media/usb/go7007/go7007-driver.c
+++ b/drivers/media/usb/go7007/go7007-driver.c
@@ -736,4 +736,5 @@ void go7007_update_board(struct go7007 *go)
 }
 EXPORT_SYMBOL(go7007_update_board);
 
+MODULE_DESCRIPTION("WIS GO7007 MPEG encoder support");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/usb/go7007/go7007-fw.c b/drivers/media/usb/go7007/go7007-fw.c
index 018019ba47d4..86ce593e0c54 100644
--- a/drivers/media/usb/go7007/go7007-fw.c
+++ b/drivers/media/usb/go7007/go7007-fw.c
@@ -1289,8 +1289,8 @@ static int avsync_to_package(struct go7007 *go, __le16 *code, int space)
 		0xbf99,		(u16)((-adjratio) >> 16),
 		0xbf92,		0,
 		0xbf93,		0,
-		0xbff4,		f1 > f2 ? f1 : f2,
-		0xbff5,		f1 < f2 ? f1 : f2,
+		0xbff4,		max(f1, f2),
+		0xbff5,		min(f1, f2),
 		0xbff6,		f1 < f2 ? ratio : ratio + 1,
 		0xbff7,		f1 > f2 ? ratio : ratio + 1,
 		0xbff8,		0,
diff --git a/drivers/media/usb/go7007/go7007-i2c.c b/drivers/media/usb/go7007/go7007-i2c.c
index 2880370e45c8..f6ce28a4a768 100644
--- a/drivers/media/usb/go7007/go7007-i2c.c
+++ b/drivers/media/usb/go7007/go7007-i2c.c
@@ -33,7 +33,21 @@
 
 /* There is only one I2C port on the TW2804 that feeds all four GO7007 VIPs
  * on the Adlink PCI-MPG24, so access is shared between all of them. */
-static DEFINE_MUTEX(adlink_mpg24_i2c_lock);
+static DEFINE_MUTEX(adlink_mpg24_i2c_mutex);
+
+static inline void adlink_mpg24_i2c_lock(struct go7007 *go)
+{
+	/* Bridge the I2C port on this GO7007 to the shared bus */
+	mutex_lock(&adlink_mpg24_i2c_mutex);
+	go7007_write_addr(go, 0x3c82, 0x0020);
+}
+
+static inline void adlink_mpg24_i2c_unlock(struct go7007 *go)
+{
+	/* Isolate the I2C port on this GO7007 from the shared bus */
+	go7007_write_addr(go, 0x3c82, 0x0000);
+	mutex_unlock(&adlink_mpg24_i2c_mutex);
+}
 
 static int go7007_i2c_xfer(struct go7007 *go, u16 addr, int read,
 		u16 command, int flags, u8 *data)
@@ -56,11 +70,8 @@ static int go7007_i2c_xfer(struct go7007 *go, u16 addr, int read,
 
 	mutex_lock(&go->hw_lock);
 
-	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24) {
-		/* Bridge the I2C port on this GO7007 to the shared bus */
-		mutex_lock(&adlink_mpg24_i2c_lock);
-		go7007_write_addr(go, 0x3c82, 0x0020);
-	}
+	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24)
+		adlink_mpg24_i2c_lock(go);
 
 	/* Wait for I2C adapter to be ready */
 	for (i = 0; i < 10; ++i) {
@@ -116,11 +127,8 @@ static int go7007_i2c_xfer(struct go7007 *go, u16 addr, int read,
 	ret = 0;
 
 i2c_done:
-	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24) {
-		/* Isolate the I2C port on this GO7007 from the shared bus */
-		go7007_write_addr(go, 0x3c82, 0x0000);
-		mutex_unlock(&adlink_mpg24_i2c_lock);
-	}
+	if (go->board_id == GO7007_BOARDID_ADLINK_MPG24)
+		adlink_mpg24_i2c_unlock(go);
 	mutex_unlock(&go->hw_lock);
 	return ret;
 }
diff --git a/drivers/media/usb/go7007/go7007-usb.c b/drivers/media/usb/go7007/go7007-usb.c
index 762c13e49bfa..334cdde81a5c 100644
--- a/drivers/media/usb/go7007/go7007-usb.c
+++ b/drivers/media/usb/go7007/go7007-usb.c
@@ -1352,4 +1352,5 @@ static struct usb_driver go7007_usb_driver = {
 };
 
 module_usb_driver(go7007_usb_driver);
+MODULE_DESCRIPTION("WIS GO7007 USB support");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/usb/go7007/go7007-v4l2.c b/drivers/media/usb/go7007/go7007-v4l2.c
index 13256565b034..2087ffcb85a5 100644
--- a/drivers/media/usb/go7007/go7007-v4l2.c
+++ b/drivers/media/usb/go7007/go7007-v4l2.c
@@ -452,8 +452,6 @@ static const struct vb2_ops go7007_video_qops = {
 	.buf_finish     = go7007_buf_finish,
 	.start_streaming = go7007_start_streaming,
 	.stop_streaming = go7007_stop_streaming,
-	.wait_prepare   = vb2_ops_wait_prepare,
-	.wait_finish    = vb2_ops_wait_finish,
 };
 
 static int vidioc_g_parm(struct file *filp, void *priv,
diff --git a/drivers/media/usb/go7007/s2250-board.c b/drivers/media/usb/go7007/s2250-board.c
index db1fab96d529..a155b987282f 100644
--- a/drivers/media/usb/go7007/s2250-board.c
+++ b/drivers/media/usb/go7007/s2250-board.c
@@ -611,7 +611,7 @@ static void s2250_remove(struct i2c_client *client)
 }
 
 static const struct i2c_device_id s2250_id[] = {
-	{ "s2250", 0 },
+	{ "s2250" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, s2250_id);
diff --git a/drivers/media/usb/gspca/cpia1.c b/drivers/media/usb/gspca/cpia1.c
index 5f5fa851ca64..14aaf36cde6e 100644
--- a/drivers/media/usb/gspca/cpia1.c
+++ b/drivers/media/usb/gspca/cpia1.c
@@ -604,10 +604,8 @@ static int find_over_exposure(int brightness)
 	MaxAllowableOverExposure = FLICKER_MAX_EXPOSURE - brightness -
 				   FLICKER_BRIGHTNESS_CONSTANT;
 
-	if (MaxAllowableOverExposure < FLICKER_ALLOWABLE_OVER_EXPOSURE)
-		OverExposure = MaxAllowableOverExposure;
-	else
-		OverExposure = FLICKER_ALLOWABLE_OVER_EXPOSURE;
+	OverExposure = min(MaxAllowableOverExposure,
+			   FLICKER_ALLOWABLE_OVER_EXPOSURE);
 
 	return OverExposure;
 }
diff --git a/drivers/media/usb/gspca/gspca.c b/drivers/media/usb/gspca/gspca.c
index e8c8bdb9c40b..25edd2189654 100644
--- a/drivers/media/usb/gspca/gspca.c
+++ b/drivers/media/usb/gspca/gspca.c
@@ -1380,8 +1380,6 @@ static const struct vb2_ops gspca_qops = {
 	.buf_queue		= gspca_buffer_queue,
 	.start_streaming	= gspca_start_streaming,
 	.stop_streaming		= gspca_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static const struct v4l2_file_operations dev_fops = {
diff --git a/drivers/media/usb/gspca/ov534.c b/drivers/media/usb/gspca/ov534.c
index 8b6a57f170d0..bdff64a29a33 100644
--- a/drivers/media/usb/gspca/ov534.c
+++ b/drivers/media/usb/gspca/ov534.c
@@ -847,7 +847,7 @@ static void set_frame_rate(struct gspca_dev *gspca_dev)
 		r = rate_1;
 		i = ARRAY_SIZE(rate_1);
 	}
-	while (--i > 0) {
+	while (--i >= 0) {
 		if (sd->frame_rate >= r->fps)
 			break;
 		r++;
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
index 5a47dcbf1c8e..303b055fefea 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_hdcs.c
@@ -520,12 +520,13 @@ static int hdcs_init(struct sd *sd)
 static int hdcs_dump(struct sd *sd)
 {
 	u16 reg, val;
+	int err = 0;
 
 	pr_info("Dumping sensor registers:\n");
 
-	for (reg = HDCS_IDENT; reg <= HDCS_ROWEXPH; reg++) {
-		stv06xx_read_sensor(sd, reg, &val);
+	for (reg = HDCS_IDENT; reg <= HDCS_ROWEXPH && !err; reg++) {
+		err = stv06xx_read_sensor(sd, reg, &val);
 		pr_info("reg 0x%02x = 0x%02x\n", reg, val);
 	}
-	return 0;
+	return (err < 0) ? err : 0;
 }
diff --git a/drivers/media/usb/hackrf/hackrf.c b/drivers/media/usb/hackrf/hackrf.c
index 9c0ecd5f056c..0b50de8775a3 100644
--- a/drivers/media/usb/hackrf/hackrf.c
+++ b/drivers/media/usb/hackrf/hackrf.c
@@ -888,8 +888,6 @@ static const struct vb2_ops hackrf_vb2_ops = {
 	.buf_queue              = hackrf_buf_queue,
 	.start_streaming        = hackrf_start_streaming,
 	.stop_streaming         = hackrf_stop_streaming,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 static int hackrf_querycap(struct file *file, void *fh,
@@ -1398,6 +1396,7 @@ static int hackrf_probe(struct usb_interface *intf,
 	dev->rx_vb2_queue.drv_priv = dev;
 	dev->rx_vb2_queue.buf_struct_size = sizeof(struct hackrf_buffer);
 	dev->rx_vb2_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	dev->rx_vb2_queue.lock = &dev->vb_queue_lock;
 	ret = vb2_queue_init(&dev->rx_vb2_queue);
 	if (ret) {
 		dev_err(dev->dev, "Could not initialize rx vb2 queue\n");
@@ -1413,6 +1412,7 @@ static int hackrf_probe(struct usb_interface *intf,
 	dev->tx_vb2_queue.drv_priv = dev;
 	dev->tx_vb2_queue.buf_struct_size = sizeof(struct hackrf_buffer);
 	dev->tx_vb2_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	dev->tx_vb2_queue.lock = &dev->vb_queue_lock;
 	ret = vb2_queue_init(&dev->tx_vb2_queue);
 	if (ret) {
 		dev_err(dev->dev, "Could not initialize tx vb2 queue\n");
@@ -1474,7 +1474,6 @@ static int hackrf_probe(struct usb_interface *intf,
 	/* Init video_device structure for receiver */
 	dev->rx_vdev = hackrf_template;
 	dev->rx_vdev.queue = &dev->rx_vb2_queue;
-	dev->rx_vdev.queue->lock = &dev->vb_queue_lock;
 	dev->rx_vdev.v4l2_dev = &dev->v4l2_dev;
 	dev->rx_vdev.ctrl_handler = &dev->rx_ctrl_handler;
 	dev->rx_vdev.lock = &dev->v4l2_lock;
@@ -1494,7 +1493,6 @@ static int hackrf_probe(struct usb_interface *intf,
 	/* Init video_device structure for transmitter */
 	dev->tx_vdev = hackrf_template;
 	dev->tx_vdev.queue = &dev->tx_vb2_queue;
-	dev->tx_vdev.queue->lock = &dev->vb_queue_lock;
 	dev->tx_vdev.v4l2_dev = &dev->v4l2_dev;
 	dev->tx_vdev.ctrl_handler = &dev->tx_ctrl_handler;
 	dev->tx_vdev.lock = &dev->v4l2_lock;
diff --git a/drivers/media/usb/msi2500/msi2500.c b/drivers/media/usb/msi2500/msi2500.c
index 5138486abfa0..33099f39146a 100644
--- a/drivers/media/usb/msi2500/msi2500.c
+++ b/drivers/media/usb/msi2500/msi2500.c
@@ -883,8 +883,6 @@ static const struct vb2_ops msi2500_vb2_ops = {
 	.buf_queue              = msi2500_buf_queue,
 	.start_streaming        = msi2500_start_streaming,
 	.stop_streaming         = msi2500_stop_streaming,
-	.wait_prepare           = vb2_ops_wait_prepare,
-	.wait_finish            = vb2_ops_wait_finish,
 };
 
 static int msi2500_enum_fmt_sdr_cap(struct file *file, void *priv,
@@ -1199,6 +1197,7 @@ static int msi2500_probe(struct usb_interface *intf,
 	dev->vb_queue.ops = &msi2500_vb2_ops;
 	dev->vb_queue.mem_ops = &vb2_vmalloc_memops;
 	dev->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	dev->vb_queue.lock = &dev->vb_queue_lock;
 	ret = vb2_queue_init(&dev->vb_queue);
 	if (ret) {
 		dev_err(dev->dev, "Could not initialize vb2 queue\n");
@@ -1208,7 +1207,6 @@ static int msi2500_probe(struct usb_interface *intf,
 	/* Init video_device structure */
 	dev->vdev = msi2500_template;
 	dev->vdev.queue = &dev->vb_queue;
-	dev->vdev.queue->lock = &dev->vb_queue_lock;
 	video_set_drvdata(&dev->vdev, dev);
 
 	/* Register the v4l2_device structure */
@@ -1219,8 +1217,8 @@ static int msi2500_probe(struct usb_interface *intf,
 		goto err_free_mem;
 	}
 
-	/* SPI master adapter */
-	ctlr = spi_alloc_master(dev->dev, 0);
+	/* SPI host adapter */
+	ctlr = spi_alloc_host(dev->dev, 0);
 	if (ctlr == NULL) {
 		ret = -ENOMEM;
 		goto err_unregister_v4l2_dev;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-debugifc.c b/drivers/media/usb/pvrusb2/pvrusb2-debugifc.c
index 84cfb5ce8b8d..81d711269ab5 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-debugifc.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-debugifc.c
@@ -9,11 +9,6 @@
 #include "pvrusb2-hdw.h"
 #include "pvrusb2-debug.h"
 
-struct debugifc_mask_item {
-	const char *name;
-	unsigned long msk;
-};
-
 
 static unsigned int debugifc_count_whitespace(const char *buf,
 					      unsigned int count)
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-encoder.c b/drivers/media/usb/pvrusb2/pvrusb2-encoder.c
index c8102772344b..a5eabac1ec6e 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-encoder.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-encoder.c
@@ -257,7 +257,7 @@ rdData[0]);
 			ret = -EBUSY;
 		}
 		if (ret) {
-			del_timer_sync(&hdw->encoder_run_timer);
+			timer_delete_sync(&hdw->encoder_run_timer);
 			hdw->state_encoder_ok = 0;
 			pvr2_trace(PVR2_TRACE_STBITS,
 				   "State bit %s <-- %s",
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
index 29cc207194b9..f21c2806eb9f 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw.c
@@ -1527,7 +1527,7 @@ int pvr2_upload_firmware2(struct pvr2_hdw *hdw)
 
 	/* Encoder is about to be reset so note that as far as we're
 	   concerned now, the encoder has never been run. */
-	del_timer_sync(&hdw->encoder_run_timer);
+	timer_delete_sync(&hdw->encoder_run_timer);
 	if (hdw->state_encoder_runok) {
 		hdw->state_encoder_runok = 0;
 		trace_stbit("state_encoder_runok",hdw->state_encoder_runok);
@@ -3562,7 +3562,7 @@ struct hdw_timer {
 
 static void pvr2_ctl_timeout(struct timer_list *t)
 {
-	struct hdw_timer *timer = from_timer(timer, t, timer);
+	struct hdw_timer *timer = timer_container_of(timer, t, timer);
 	struct pvr2_hdw *hdw = timer->hdw;
 
 	if (hdw->ctl_write_pend_flag || hdw->ctl_read_pend_flag) {
@@ -3724,7 +3724,7 @@ status);
 	hdw->cmd_debug_state = 5;
 
 	/* Stop timer */
-	del_timer_sync(&timer.timer);
+	timer_delete_sync(&timer.timer);
 
 	hdw->cmd_debug_state = 6;
 	status = 0;
@@ -3806,7 +3806,7 @@ status);
 	if ((status < 0) && (!probe_fl)) {
 		pvr2_hdw_render_useless(hdw);
 	}
-	destroy_timer_on_stack(&timer.timer);
+	timer_destroy_on_stack(&timer.timer);
 
 	return status;
 }
@@ -4248,7 +4248,7 @@ static int state_eval_encoder_config(struct pvr2_hdw *hdw)
 		hdw->state_encoder_waitok = 0;
 		trace_stbit("state_encoder_waitok",hdw->state_encoder_waitok);
 		/* paranoia - solve race if timer just completed */
-		del_timer_sync(&hdw->encoder_wait_timer);
+		timer_delete_sync(&hdw->encoder_wait_timer);
 	} else {
 		if (!hdw->state_pathway_ok ||
 		    (hdw->pathway_state != PVR2_PATHWAY_ANALOG) ||
@@ -4261,7 +4261,7 @@ static int state_eval_encoder_config(struct pvr2_hdw *hdw)
 			   anything has happened that might have disturbed
 			   the encoder.  This should be a rare case. */
 			if (timer_pending(&hdw->encoder_wait_timer)) {
-				del_timer_sync(&hdw->encoder_wait_timer);
+				timer_delete_sync(&hdw->encoder_wait_timer);
 			}
 			if (hdw->state_encoder_waitok) {
 				/* Must clear the state - therefore we did
@@ -4399,7 +4399,7 @@ static int state_eval_encoder_run(struct pvr2_hdw *hdw)
 	if (hdw->state_encoder_run) {
 		if (!state_check_disable_encoder_run(hdw)) return 0;
 		if (hdw->state_encoder_ok) {
-			del_timer_sync(&hdw->encoder_run_timer);
+			timer_delete_sync(&hdw->encoder_run_timer);
 			if (pvr2_encoder_stop(hdw) < 0) return !0;
 		}
 		hdw->state_encoder_run = 0;
@@ -4421,7 +4421,7 @@ static int state_eval_encoder_run(struct pvr2_hdw *hdw)
 /* Timeout function for quiescent timer. */
 static void pvr2_hdw_quiescent_timeout(struct timer_list *t)
 {
-	struct pvr2_hdw *hdw = from_timer(hdw, t, quiescent_timer);
+	struct pvr2_hdw *hdw = timer_container_of(hdw, t, quiescent_timer);
 	hdw->state_decoder_quiescent = !0;
 	trace_stbit("state_decoder_quiescent",hdw->state_decoder_quiescent);
 	hdw->state_stale = !0;
@@ -4432,7 +4432,8 @@ static void pvr2_hdw_quiescent_timeout(struct timer_list *t)
 /* Timeout function for decoder stabilization timer. */
 static void pvr2_hdw_decoder_stabilization_timeout(struct timer_list *t)
 {
-	struct pvr2_hdw *hdw = from_timer(hdw, t, decoder_stabilization_timer);
+	struct pvr2_hdw *hdw = timer_container_of(hdw, t,
+						  decoder_stabilization_timer);
 	hdw->state_decoder_ready = !0;
 	trace_stbit("state_decoder_ready", hdw->state_decoder_ready);
 	hdw->state_stale = !0;
@@ -4443,7 +4444,7 @@ static void pvr2_hdw_decoder_stabilization_timeout(struct timer_list *t)
 /* Timeout function for encoder wait timer. */
 static void pvr2_hdw_encoder_wait_timeout(struct timer_list *t)
 {
-	struct pvr2_hdw *hdw = from_timer(hdw, t, encoder_wait_timer);
+	struct pvr2_hdw *hdw = timer_container_of(hdw, t, encoder_wait_timer);
 	hdw->state_encoder_waitok = !0;
 	trace_stbit("state_encoder_waitok",hdw->state_encoder_waitok);
 	hdw->state_stale = !0;
@@ -4454,7 +4455,7 @@ static void pvr2_hdw_encoder_wait_timeout(struct timer_list *t)
 /* Timeout function for encoder run timer. */
 static void pvr2_hdw_encoder_run_timeout(struct timer_list *t)
 {
-	struct pvr2_hdw *hdw = from_timer(hdw, t, encoder_run_timer);
+	struct pvr2_hdw *hdw = timer_container_of(hdw, t, encoder_run_timer);
 	if (!hdw->state_encoder_runok) {
 		hdw->state_encoder_runok = !0;
 		trace_stbit("state_encoder_runok",hdw->state_encoder_runok);
@@ -4479,11 +4480,11 @@ static int state_eval_decoder_run(struct pvr2_hdw *hdw)
 		hdw->state_decoder_quiescent = 0;
 		hdw->state_decoder_run = 0;
 		/* paranoia - solve race if timer(s) just completed */
-		del_timer_sync(&hdw->quiescent_timer);
+		timer_delete_sync(&hdw->quiescent_timer);
 		/* Kill the stabilization timer, in case we're killing the
 		   encoder before the previous stabilization interval has
 		   been properly timed. */
-		del_timer_sync(&hdw->decoder_stabilization_timer);
+		timer_delete_sync(&hdw->decoder_stabilization_timer);
 		hdw->state_decoder_ready = 0;
 	} else {
 		if (!hdw->state_decoder_quiescent) {
@@ -4517,7 +4518,7 @@ static int state_eval_decoder_run(struct pvr2_hdw *hdw)
 		    !hdw->state_pipeline_config ||
 		    !hdw->state_encoder_config ||
 		    !hdw->state_encoder_ok) return 0;
-		del_timer_sync(&hdw->quiescent_timer);
+		timer_delete_sync(&hdw->quiescent_timer);
 		if (hdw->flag_decoder_missed) return 0;
 		if (pvr2_decoder_enable(hdw,!0) < 0) return 0;
 		hdw->state_decoder_quiescent = 0;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-io.c b/drivers/media/usb/pvrusb2/pvrusb2-io.c
index 675dc7153e2b..28ffe7981f8c 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-io.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-io.c
@@ -335,8 +335,8 @@ static int pvr2_stream_buffer_count(struct pvr2_stream *sp, unsigned int cnt)
 		if (scnt < sp->buffer_slot_count) {
 			struct pvr2_buffer **nb = NULL;
 			if (scnt) {
-				nb = kmemdup(sp->buffers, scnt * sizeof(*nb),
-					     GFP_KERNEL);
+				nb = kmemdup_array(sp->buffers, scnt, sizeof(*nb),
+						   GFP_KERNEL);
 				if (!nb) return -ENOMEM;
 			}
 			kfree(sp->buffers);
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-std.c b/drivers/media/usb/pvrusb2/pvrusb2-std.c
index e7ab41401577..81c994e62241 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-std.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-std.c
@@ -212,173 +212,6 @@ unsigned int pvr2_std_id_to_str(char *bufPtr, unsigned int bufSize,
 }
 
 
-// Template data for possible enumerated video standards.  Here we group
-// standards which share common frame rates and resolution.
-static struct v4l2_standard generic_standards[] = {
-	{
-		.id             = (TSTD_B|TSTD_B1|
-				   TSTD_D|TSTD_D1|
-				   TSTD_G|
-				   TSTD_H|
-				   TSTD_I|
-				   TSTD_K|TSTD_K1|
-				   TSTD_L|
-				   V4L2_STD_SECAM_LC |
-				   TSTD_N|TSTD_Nc),
-		.frameperiod    =
-		{
-			.numerator  = 1,
-			.denominator= 25
-		},
-		.framelines     = 625,
-		.reserved       = {0,0,0,0}
-	}, {
-		.id             = (TSTD_M|
-				   V4L2_STD_NTSC_M_JP|
-				   V4L2_STD_NTSC_M_KR),
-		.frameperiod    =
-		{
-			.numerator  = 1001,
-			.denominator= 30000
-		},
-		.framelines     = 525,
-		.reserved       = {0,0,0,0}
-	}, { // This is a total wild guess
-		.id             = (TSTD_60),
-		.frameperiod    =
-		{
-			.numerator  = 1001,
-			.denominator= 30000
-		},
-		.framelines     = 525,
-		.reserved       = {0,0,0,0}
-	}, { // This is total wild guess
-		.id             = V4L2_STD_NTSC_443,
-		.frameperiod    =
-		{
-			.numerator  = 1001,
-			.denominator= 30000
-		},
-		.framelines     = 525,
-		.reserved       = {0,0,0,0}
-	}
-};
-
-static struct v4l2_standard *match_std(v4l2_std_id id)
-{
-	unsigned int idx;
-	for (idx = 0; idx < ARRAY_SIZE(generic_standards); idx++) {
-		if (generic_standards[idx].id & id) {
-			return generic_standards + idx;
-		}
-	}
-	return NULL;
-}
-
-static int pvr2_std_fill(struct v4l2_standard *std,v4l2_std_id id)
-{
-	struct v4l2_standard *template;
-	int idx;
-	unsigned int bcnt;
-	template = match_std(id);
-	if (!template) return 0;
-	idx = std->index;
-	memcpy(std,template,sizeof(*template));
-	std->index = idx;
-	std->id = id;
-	bcnt = pvr2_std_id_to_str(std->name,sizeof(std->name)-1,id);
-	std->name[bcnt] = 0;
-	pvr2_trace(PVR2_TRACE_STD,"Set up standard idx=%u name=%s",
-		   std->index,std->name);
-	return !0;
-}
-
-/* These are special cases of combined standards that we should enumerate
-   separately if the component pieces are present. */
-static v4l2_std_id std_mixes[] = {
-	V4L2_STD_PAL_B | V4L2_STD_PAL_G,
-	V4L2_STD_PAL_D | V4L2_STD_PAL_K,
-	V4L2_STD_SECAM_B | V4L2_STD_SECAM_G,
-	V4L2_STD_SECAM_D | V4L2_STD_SECAM_K,
-};
-
-struct v4l2_standard *pvr2_std_create_enum(unsigned int *countptr,
-					   v4l2_std_id id)
-{
-	unsigned int std_cnt = 0;
-	unsigned int idx,bcnt,idx2;
-	v4l2_std_id idmsk,cmsk,fmsk;
-	struct v4l2_standard *stddefs;
-
-	if (pvrusb2_debug & PVR2_TRACE_STD) {
-		char buf[100];
-		bcnt = pvr2_std_id_to_str(buf,sizeof(buf),id);
-		pvr2_trace(
-			PVR2_TRACE_STD,"Mapping standards mask=0x%x (%.*s)",
-			(int)id,bcnt,buf);
-	}
-
-	*countptr = 0;
-	std_cnt = 0;
-	fmsk = 0;
-	for (idmsk = 1, cmsk = id; cmsk; idmsk <<= 1) {
-		if (!(idmsk & cmsk)) continue;
-		cmsk &= ~idmsk;
-		if (match_std(idmsk)) {
-			std_cnt++;
-			continue;
-		}
-		fmsk |= idmsk;
-	}
-
-	for (idx2 = 0; idx2 < ARRAY_SIZE(std_mixes); idx2++) {
-		if ((id & std_mixes[idx2]) == std_mixes[idx2]) std_cnt++;
-	}
-
-	/* Don't complain about ATSC standard values */
-	fmsk &= ~CSTD_ATSC;
-
-	if (fmsk) {
-		char buf[100];
-		bcnt = pvr2_std_id_to_str(buf,sizeof(buf),fmsk);
-		pvr2_trace(
-			PVR2_TRACE_ERROR_LEGS,
-			"***WARNING*** Failed to classify the following standard(s): %.*s",
-			bcnt,buf);
-	}
-
-	pvr2_trace(PVR2_TRACE_STD,"Setting up %u unique standard(s)",
-		   std_cnt);
-	if (!std_cnt) return NULL; // paranoia
-
-	stddefs = kcalloc(std_cnt, sizeof(struct v4l2_standard),
-			  GFP_KERNEL);
-	if (!stddefs)
-		return NULL;
-
-	for (idx = 0; idx < std_cnt; idx++)
-		stddefs[idx].index = idx;
-
-	idx = 0;
-
-	/* Enumerate potential special cases */
-	for (idx2 = 0; (idx2 < ARRAY_SIZE(std_mixes)) && (idx < std_cnt);
-	     idx2++) {
-		if (!(id & std_mixes[idx2])) continue;
-		if (pvr2_std_fill(stddefs+idx,std_mixes[idx2])) idx++;
-	}
-	/* Now enumerate individual pieces */
-	for (idmsk = 1, cmsk = id; cmsk && (idx < std_cnt); idmsk <<= 1) {
-		if (!(idmsk & cmsk)) continue;
-		cmsk &= ~idmsk;
-		if (!pvr2_std_fill(stddefs+idx,idmsk)) continue;
-		idx++;
-	}
-
-	*countptr = std_cnt;
-	return stddefs;
-}
-
 v4l2_std_id pvr2_std_get_usable(void)
 {
 	return CSTD_ALL;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-std.h b/drivers/media/usb/pvrusb2/pvrusb2-std.h
index d8b4c6dc72fe..74b05ecb9708 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-std.h
+++ b/drivers/media/usb/pvrusb2/pvrusb2-std.h
@@ -23,12 +23,6 @@ int pvr2_std_str_to_id(v4l2_std_id *idPtr,const char *bufPtr,
 unsigned int pvr2_std_id_to_str(char *bufPtr, unsigned int bufSize,
 				v4l2_std_id id);
 
-// Create an array of suitable v4l2_standard structures given a bit mask of
-// video standards to support.  The array is allocated from the heap, and
-// the number of elements is returned in the first argument.
-struct v4l2_standard *pvr2_std_create_enum(unsigned int *countptr,
-					   v4l2_std_id id);
-
 // Return mask of which video standard bits are valid
 v4l2_std_id pvr2_std_get_usable(void);
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
index d608b793fa84..ad38e1240541 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
@@ -497,8 +497,8 @@ static int pvr2_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
 	return pvr2_hdw_set_streaming(hdw, 0);
 }
 
-static int pvr2_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *vc)
+static int pvr2_query_ext_ctrl(struct file *file, void *priv,
+			       struct v4l2_query_ext_ctrl *vc)
 {
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
@@ -521,13 +521,16 @@ static int pvr2_queryctrl(struct file *file, void *priv,
 	}
 
 	pvr2_trace(PVR2_TRACE_V4LIOCTL,
-			"QUERYCTRL id=0x%x mapping name=%s (%s)",
+			"QUERYEXTCTRL id=0x%x mapping name=%s (%s)",
 			vc->id, pvr2_ctrl_get_name(cptr),
 			pvr2_ctrl_get_desc(cptr));
 	strscpy(vc->name, pvr2_ctrl_get_desc(cptr), sizeof(vc->name));
 	vc->flags = pvr2_ctrl_get_v4lflags(cptr);
 	pvr2_ctrl_get_def(cptr, &val);
 	vc->default_value = val;
+	vc->nr_of_dims = 0;
+	vc->elems = 1;
+	vc->elem_size = 4;
 	switch (pvr2_ctrl_get_type(cptr)) {
 	case pvr2_ctl_enum:
 		vc->type = V4L2_CTRL_TYPE_MENU;
@@ -549,7 +552,7 @@ static int pvr2_queryctrl(struct file *file, void *priv,
 		break;
 	default:
 		pvr2_trace(PVR2_TRACE_V4LIOCTL,
-				"QUERYCTRL id=0x%x name=%s not mappable",
+				"QUERYEXTCTRL id=0x%x name=%s not mappable",
 				vc->id, pvr2_ctrl_get_name(cptr));
 		return -EINVAL;
 	}
@@ -571,31 +574,6 @@ static int pvr2_querymenu(struct file *file, void *priv, struct v4l2_querymenu *
 	return ret;
 }
 
-static int pvr2_g_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
-{
-	struct pvr2_v4l2_fh *fh = file->private_data;
-	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
-	int val = 0;
-	int ret;
-
-	ret = pvr2_ctrl_get_value(pvr2_hdw_get_ctrl_v4l(hdw, vc->id),
-			&val);
-	vc->value = val;
-	return ret;
-}
-
-static int pvr2_s_ctrl(struct file *file, void *priv, struct v4l2_control *vc)
-{
-	struct pvr2_v4l2_fh *fh = file->private_data;
-	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
-	int ret;
-
-	ret = pvr2_ctrl_set_value(pvr2_hdw_get_ctrl_v4l(hdw, vc->id),
-			vc->value);
-	pvr2_hdw_commit_ctl(hdw);
-	return ret;
-}
-
 static int pvr2_g_ext_ctrls(struct file *file, void *priv,
 					struct v4l2_ext_controls *ctls)
 {
@@ -812,10 +790,8 @@ static const struct v4l2_ioctl_ops pvr2_ioctl_ops = {
 	.vidioc_try_fmt_vid_cap		    = pvr2_try_fmt_vid_cap,
 	.vidioc_streamon		    = pvr2_streamon,
 	.vidioc_streamoff		    = pvr2_streamoff,
-	.vidioc_queryctrl		    = pvr2_queryctrl,
+	.vidioc_query_ext_ctrl		    = pvr2_query_ext_ctrl,
 	.vidioc_querymenu		    = pvr2_querymenu,
-	.vidioc_g_ctrl			    = pvr2_g_ctrl,
-	.vidioc_s_ctrl			    = pvr2_s_ctrl,
 	.vidioc_g_ext_ctrls		    = pvr2_g_ext_ctrls,
 	.vidioc_s_ext_ctrls		    = pvr2_s_ext_ctrls,
 	.vidioc_try_ext_ctrls		    = pvr2_try_ext_ctrls,
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index e342199711d3..c6e5d031f068 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -56,7 +56,6 @@
 #endif
 #include <linux/vmalloc.h>
 #include <asm/io.h>
-#include <linux/kernel.h>		/* simple_strtol() */
 
 #include "pwc.h"
 #include "pwc-kiara.h"
@@ -760,8 +759,6 @@ static const struct vb2_ops pwc_vb_queue_ops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 /***************************************************************************/
@@ -1054,6 +1051,7 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->vb_queue.ops = &pwc_vb_queue_ops;
 	pdev->vb_queue.mem_ops = &vb2_vmalloc_memops;
 	pdev->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	pdev->vb_queue.lock = &pdev->vb_queue_lock;
 	rc = vb2_queue_init(&pdev->vb_queue);
 	if (rc < 0) {
 		PWC_ERROR("Oops, could not initialize vb2 queue.\n");
@@ -1064,7 +1062,6 @@ static int usb_pwc_probe(struct usb_interface *intf, const struct usb_device_id
 	pdev->vdev = pwc_template;
 	strscpy(pdev->vdev.name, name, sizeof(pdev->vdev.name));
 	pdev->vdev.queue = &pdev->vb_queue;
-	pdev->vdev.queue->lock = &pdev->vb_queue_lock;
 	video_set_drvdata(&pdev->vdev, pdev);
 
 	pdev->release = le16_to_cpu(udev->descriptor.bcdDevice);
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 8e1de1e8bd12..8332f2c5aed7 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -247,7 +247,7 @@ struct s2255_vc {
 struct s2255_dev {
 	struct s2255_vc         vc[MAX_CHANNELS];
 	struct v4l2_device      v4l2_dev;
-	atomic_t                num_channels;
+	refcount_t		num_channels;
 	int			frames;
 	struct mutex		lock;	/* channels[].vdev.lock */
 	struct mutex		cmdlock; /* protects cmdbuf */
@@ -471,7 +471,7 @@ static void s2255_reset_dsppower(struct s2255_dev *dev)
  */
 static void s2255_timer(struct timer_list *t)
 {
-	struct s2255_dev *dev = from_timer(dev, t, timer);
+	struct s2255_dev *dev = timer_container_of(dev, t, timer);
 	struct s2255_fw *data = dev->fw_data;
 	if (usb_submit_urb(data->fw_urb, GFP_ATOMIC) < 0) {
 		pr_err("s2255: can't submit urb\n");
@@ -704,8 +704,6 @@ static const struct vb2_ops s2255_video_qops = {
 	.buf_queue = buffer_queue,
 	.start_streaming = start_streaming,
 	.stop_streaming = stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int vidioc_querycap(struct file *file, void *priv,
@@ -1550,11 +1548,11 @@ static void s2255_video_device_release(struct video_device *vdev)
 		container_of(vdev, struct s2255_vc, vdev);
 
 	dprintk(dev, 4, "%s, chnls: %d\n", __func__,
-		atomic_read(&dev->num_channels));
+		refcount_read(&dev->num_channels));
 
 	v4l2_ctrl_handler_free(&vc->hdl);
 
-	if (atomic_dec_and_test(&dev->num_channels))
+	if (refcount_dec_and_test(&dev->num_channels))
 		s2255_destroy(dev);
 	return;
 }
@@ -1659,7 +1657,7 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 				"failed to register video device!\n");
 			break;
 		}
-		atomic_inc(&dev->num_channels);
+		refcount_inc(&dev->num_channels);
 		v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
 			  video_device_node_name(&vc->vdev));
 
@@ -1667,11 +1665,11 @@ static int s2255_probe_v4l(struct s2255_dev *dev)
 	pr_info("Sensoray 2255 V4L driver Revision: %s\n",
 		S2255_VERSION);
 	/* if no channels registered, return error and probe will fail*/
-	if (atomic_read(&dev->num_channels) == 0) {
+	if (refcount_read(&dev->num_channels) == 0) {
 		v4l2_device_unregister(&dev->v4l2_dev);
 		return ret;
 	}
-	if (atomic_read(&dev->num_channels) != MAX_CHANNELS)
+	if (refcount_read(&dev->num_channels) != MAX_CHANNELS)
 		pr_warn("s2255: Not all channels available.\n");
 	return 0;
 }
@@ -2221,7 +2219,7 @@ static int s2255_probe(struct usb_interface *interface,
 		goto errorFWDATA1;
 	}
 
-	atomic_set(&dev->num_channels, 0);
+	refcount_set(&dev->num_channels, 0);
 	dev->pid = id->idProduct;
 	dev->fw_data = kzalloc(sizeof(struct s2255_fw), GFP_KERNEL);
 	if (!dev->fw_data)
@@ -2341,12 +2339,12 @@ static void s2255_disconnect(struct usb_interface *interface)
 {
 	struct s2255_dev *dev = to_s2255_dev(usb_get_intfdata(interface));
 	int i;
-	int channels = atomic_read(&dev->num_channels);
+	int channels = refcount_read(&dev->num_channels);
 	mutex_lock(&dev->lock);
 	v4l2_device_disconnect(&dev->v4l2_dev);
 	mutex_unlock(&dev->lock);
 	/*see comments in the uvc_driver.c usb disconnect function */
-	atomic_inc(&dev->num_channels);
+	refcount_inc(&dev->num_channels);
 	/* unregister each video device. */
 	for (i = 0; i < channels; i++)
 		video_unregister_device(&dev->vc[i].vdev);
@@ -2359,7 +2357,7 @@ static void s2255_disconnect(struct usb_interface *interface)
 		dev->vc[i].vidstatus_ready = 1;
 		wake_up(&dev->vc[i].wait_vidstatus);
 	}
-	if (atomic_dec_and_test(&dev->num_channels))
+	if (refcount_dec_and_test(&dev->num_channels))
 		s2255_destroy(dev);
 	dev_info(&interface->dev, "%s\n", __func__);
 }
diff --git a/drivers/media/usb/siano/smsusb.c b/drivers/media/usb/siano/smsusb.c
index 723510520d09..2c8179a84991 100644
--- a/drivers/media/usb/siano/smsusb.c
+++ b/drivers/media/usb/siano/smsusb.c
@@ -40,7 +40,7 @@ struct smsusb_urb_t {
 	struct smscore_buffer_t *cb;
 	struct smsusb_device_t *dev;
 
-	struct urb urb;
+	struct urb *urb;
 
 	/* For the bottom half */
 	struct work_struct wq;
@@ -160,7 +160,7 @@ static int smsusb_submit_urb(struct smsusb_device_t *dev,
 	}
 
 	usb_fill_bulk_urb(
-		&surb->urb,
+		surb->urb,
 		dev->udev,
 		usb_rcvbulkpipe(dev->udev, dev->in_ep),
 		surb->cb->p,
@@ -168,9 +168,9 @@ static int smsusb_submit_urb(struct smsusb_device_t *dev,
 		smsusb_onresponse,
 		surb
 	);
-	surb->urb.transfer_flags |= URB_FREE_BUFFER;
+	surb->urb->transfer_flags |= URB_FREE_BUFFER;
 
-	return usb_submit_urb(&surb->urb, GFP_ATOMIC);
+	return usb_submit_urb(surb->urb, GFP_ATOMIC);
 }
 
 static void smsusb_stop_streaming(struct smsusb_device_t *dev)
@@ -178,7 +178,7 @@ static void smsusb_stop_streaming(struct smsusb_device_t *dev)
 	int i;
 
 	for (i = 0; i < MAX_URBS; i++) {
-		usb_kill_urb(&dev->surbs[i].urb);
+		usb_kill_urb(dev->surbs[i].urb);
 		if (dev->surbs[i].wq.func)
 			cancel_work_sync(&dev->surbs[i].wq);
 
@@ -338,6 +338,8 @@ static void smsusb_term_device(struct usb_interface *intf)
 	struct smsusb_device_t *dev = usb_get_intfdata(intf);
 
 	if (dev) {
+		int i;
+
 		dev->state = SMSUSB_DISCONNECTED;
 
 		smsusb_stop_streaming(dev);
@@ -346,6 +348,9 @@ static void smsusb_term_device(struct usb_interface *intf)
 		if (dev->coredev)
 			smscore_unregister_device(dev->coredev);
 
+		for (i = 0; i < MAX_URBS; i++)
+			usb_free_urb(dev->surbs[i].urb);
+
 		pr_debug("device 0x%p destroyed\n", dev);
 		kfree(dev);
 	}
@@ -463,7 +468,9 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 	/* initialize urbs */
 	for (i = 0; i < MAX_URBS; i++) {
 		dev->surbs[i].dev = dev;
-		usb_init_urb(&dev->surbs[i].urb);
+		dev->surbs[i].urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!dev->surbs[i].urb)
+			goto err_unregister_device;
 	}
 
 	pr_debug("smsusb_start_streaming(...).\n");
@@ -486,6 +493,7 @@ static int smsusb_init_device(struct usb_interface *intf, int board_id)
 	return rc;
 
 err_unregister_device:
+	/* smsusb_term_device() frees any allocated urb. */
 	smsusb_term_device(intf);
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
 	media_device_unregister(mdev);
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index a1f785a5ffd8..5ba3d9c4b3fb 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -734,8 +734,6 @@ static const struct vb2_ops stk1160_video_qops = {
 	.buf_queue		= buffer_queue,
 	.start_streaming	= start_streaming,
 	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
 };
 
 static const struct video_device v4l_template = {
diff --git a/drivers/media/usb/stk1160/stk1160-video.c b/drivers/media/usb/stk1160/stk1160-video.c
index 366f0e4a5dc0..9cbd957ecc90 100644
--- a/drivers/media/usb/stk1160/stk1160-video.c
+++ b/drivers/media/usb/stk1160/stk1160-video.c
@@ -99,7 +99,7 @@ void stk1160_buffer_done(struct stk1160 *dev)
 static inline
 void stk1160_copy_video(struct stk1160 *dev, u8 *src, int len)
 {
-	int linesdone, lineoff, lencopy;
+	int linesdone, lineoff, lencopy, offset;
 	int bytesperline = dev->width * 2;
 	struct stk1160_buffer *buf = dev->isoc_ctl.buf;
 	u8 *dst = buf->mem;
@@ -130,17 +130,19 @@ void stk1160_copy_video(struct stk1160 *dev, u8 *src, int len)
 	dst += linesdone * bytesperline * 2 + lineoff;
 
 	/* Copy the remaining of current line */
-	if (remain < (bytesperline - lineoff))
-		lencopy = remain;
-	else
-		lencopy = bytesperline - lineoff;
+	lencopy = min(remain, bytesperline - lineoff);
 
 	/*
 	 * Check if we have enough space left in the buffer.
 	 * In that case, we force loop exit after copy.
 	 */
-	if (lencopy > buf->bytesused - buf->length) {
-		lencopy = buf->bytesused - buf->length;
+	offset = dst - (u8 *)buf->mem;
+	if (offset > buf->length) {
+		dev_warn_ratelimited(dev->dev, "out of bounds offset\n");
+		return;
+	}
+	if (lencopy > buf->length - offset) {
+		lencopy = buf->length - offset;
 		remain = lencopy;
 	}
 
@@ -173,17 +175,19 @@ void stk1160_copy_video(struct stk1160 *dev, u8 *src, int len)
 		src += lencopy;
 
 		/* Copy one line at a time */
-		if (remain < bytesperline)
-			lencopy = remain;
-		else
-			lencopy = bytesperline;
+		lencopy = min(remain, bytesperline);
 
 		/*
 		 * Check if we have enough space left in the buffer.
 		 * In that case, we force loop exit after copy.
 		 */
-		if (lencopy > buf->bytesused - buf->length) {
-			lencopy = buf->bytesused - buf->length;
+		offset = dst - (u8 *)buf->mem;
+		if (offset > buf->length) {
+			dev_warn_ratelimited(dev->dev, "offset out of bounds\n");
+			return;
+		}
+		if (lencopy > buf->length - offset) {
+			lencopy = buf->length - offset;
 			remain = lencopy;
 		}
 
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index 79faa2560613..b4575fe89c95 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -19,6 +19,7 @@
 #include <linux/input.h>
 
 #include <linux/mutex.h>
+#include <linux/workqueue.h>
 
 #include <media/dmxdev.h>
 #include <media/dvb_demux.h>
@@ -139,7 +140,7 @@ struct ttusb_dec {
 	int			v_pes_postbytes;
 
 	struct list_head	urb_frame_list;
-	struct tasklet_struct	urb_tasklet;
+	struct work_struct	urb_bh_work;
 	spinlock_t		urb_frame_list_lock;
 
 	struct dvb_demux_filter	*audio_filter;
@@ -766,9 +767,9 @@ static void ttusb_dec_process_urb_frame(struct ttusb_dec *dec, u8 *b,
 	}
 }
 
-static void ttusb_dec_process_urb_frame_list(struct tasklet_struct *t)
+static void ttusb_dec_process_urb_frame_list(struct work_struct *t)
 {
-	struct ttusb_dec *dec = from_tasklet(dec, t, urb_tasklet);
+	struct ttusb_dec *dec = from_work(dec, t, urb_bh_work);
 	struct list_head *item;
 	struct urb_frame *frame;
 	unsigned long flags;
@@ -822,7 +823,7 @@ static void ttusb_dec_process_urb(struct urb *urb)
 				spin_unlock_irqrestore(&dec->urb_frame_list_lock,
 						       flags);
 
-				tasklet_schedule(&dec->urb_tasklet);
+				queue_work(system_bh_wq, &dec->urb_bh_work);
 			}
 		}
 	} else {
@@ -1198,11 +1199,11 @@ static int ttusb_dec_alloc_iso_urbs(struct ttusb_dec *dec)
 	return 0;
 }
 
-static void ttusb_dec_init_tasklet(struct ttusb_dec *dec)
+static void ttusb_dec_init_bh_work(struct ttusb_dec *dec)
 {
 	spin_lock_init(&dec->urb_frame_list_lock);
 	INIT_LIST_HEAD(&dec->urb_frame_list);
-	tasklet_setup(&dec->urb_tasklet, ttusb_dec_process_urb_frame_list);
+	INIT_WORK(&dec->urb_bh_work, ttusb_dec_process_urb_frame_list);
 }
 
 static int ttusb_init_rc( struct ttusb_dec *dec)
@@ -1588,12 +1589,12 @@ static void ttusb_dec_exit_usb(struct ttusb_dec *dec)
 	ttusb_dec_free_iso_urbs(dec);
 }
 
-static void ttusb_dec_exit_tasklet(struct ttusb_dec *dec)
+static void ttusb_dec_exit_bh_work(struct ttusb_dec *dec)
 {
 	struct list_head *item;
 	struct urb_frame *frame;
 
-	tasklet_kill(&dec->urb_tasklet);
+	cancel_work_sync(&dec->urb_bh_work);
 
 	while ((item = dec->urb_frame_list.next) != &dec->urb_frame_list) {
 		frame = list_entry(item, struct urb_frame, urb_frame_list);
@@ -1703,7 +1704,7 @@ static int ttusb_dec_probe(struct usb_interface *intf,
 
 	ttusb_dec_init_v_pes(dec);
 	ttusb_dec_init_filters(dec);
-	ttusb_dec_init_tasklet(dec);
+	ttusb_dec_init_bh_work(dec);
 
 	dec->active = 1;
 
@@ -1729,7 +1730,7 @@ static void ttusb_dec_disconnect(struct usb_interface *intf)
 	dprintk("%s\n", __func__);
 
 	if (dec->active) {
-		ttusb_dec_exit_tasklet(dec);
+		ttusb_dec_exit_bh_work(dec);
 		ttusb_dec_exit_filters(dec);
 		if(enable_rc)
 			ttusb_dec_exit_rc(dec);
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index 702f1c8bd2ab..be22a9697197 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -780,8 +780,6 @@ static const struct vb2_ops usbtv_vb2_ops = {
 	.buf_queue = usbtv_buf_queue,
 	.start_streaming = usbtv_start_streaming,
 	.stop_streaming = usbtv_stop_streaming,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 };
 
 static int usbtv_s_ctrl(struct v4l2_ctrl *ctrl)
diff --git a/drivers/media/usb/uvc/uvc_ctrl.c b/drivers/media/usb/uvc/uvc_ctrl.c
index e59a463c2761..44b6513c5264 100644
--- a/drivers/media/usb/uvc/uvc_ctrl.c
+++ b/drivers/media/usb/uvc/uvc_ctrl.c
@@ -358,6 +358,24 @@ static const struct uvc_control_info uvc_ctrls[] = {
 		.flags		= UVC_CTRL_FLAG_GET_CUR
 				| UVC_CTRL_FLAG_AUTO_UPDATE,
 	},
+	/*
+	 * UVC_CTRL_FLAG_AUTO_UPDATE is needed because the RoI may get updated
+	 * by sensors.
+	 * "This RoI should be the same as specified in most recent SET_CUR
+	 * except in the case where the ‘Auto Detect and Track’ and/or
+	 * ‘Image Stabilization’ bit have been set."
+	 * 4.2.2.1.20 Digital Region of Interest (ROI) Control
+	 */
+	{
+		.entity		= UVC_GUID_UVC_CAMERA,
+		.selector	= UVC_CT_REGION_OF_INTEREST_CONTROL,
+		.index		= 21,
+		.size		= 10,
+		.flags		= UVC_CTRL_FLAG_SET_CUR | UVC_CTRL_FLAG_GET_CUR
+				| UVC_CTRL_FLAG_GET_MIN | UVC_CTRL_FLAG_GET_MAX
+				| UVC_CTRL_FLAG_GET_DEF
+				| UVC_CTRL_FLAG_AUTO_UPDATE,
+	},
 };
 
 static const u32 uvc_control_classes[] = {
@@ -367,6 +385,27 @@ static const u32 uvc_control_classes[] = {
 
 static const int exposure_auto_mapping[] = { 2, 1, 4, 8 };
 
+static bool uvc_ctrl_mapping_is_compound(struct uvc_control_mapping *mapping)
+{
+	return mapping->v4l2_type >= V4L2_CTRL_COMPOUND_TYPES;
+}
+
+static s32 uvc_mapping_get_s32(struct uvc_control_mapping *mapping,
+			       u8 query, const void *data_in)
+{
+	s32 data_out = 0;
+
+	mapping->get(mapping, query, data_in, sizeof(data_out), &data_out);
+
+	return data_out;
+}
+
+static void uvc_mapping_set_s32(struct uvc_control_mapping *mapping,
+				s32 data_in, void *data_out)
+{
+	mapping->set(mapping, sizeof(data_in), &data_in, data_out);
+}
+
 /*
  * This function translates the V4L2 menu index @idx, as exposed to userspace as
  * the V4L2 control value, to the corresponding UVC control value used by the
@@ -405,58 +444,219 @@ uvc_mapping_get_menu_name(const struct uvc_control_mapping *mapping, u32 idx)
 	return v4l2_ctrl_get_menu(mapping->id)[idx];
 }
 
-static s32 uvc_ctrl_get_zoom(struct uvc_control_mapping *mapping,
-	u8 query, const u8 *data)
+static int uvc_ctrl_get_zoom(struct uvc_control_mapping *mapping, u8 query,
+			     const void *uvc_in, size_t v4l2_size,
+			     void *v4l2_out)
 {
-	s8 zoom = (s8)data[0];
+	u8 value = ((u8 *)uvc_in)[2];
+	s8 sign = ((s8 *)uvc_in)[0];
+	s32 *out = v4l2_out;
+
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
 
 	switch (query) {
 	case UVC_GET_CUR:
-		return (zoom == 0) ? 0 : (zoom > 0 ? data[2] : -data[2]);
+		*out = (sign == 0) ? 0 : (sign > 0 ? value : -value);
+		return 0;
 
 	case UVC_GET_MIN:
 	case UVC_GET_MAX:
 	case UVC_GET_RES:
 	case UVC_GET_DEF:
 	default:
-		return data[2];
+		*out = value;
+		return 0;
 	}
 }
 
-static void uvc_ctrl_set_zoom(struct uvc_control_mapping *mapping,
-	s32 value, u8 *data)
+static int uvc_ctrl_set_zoom(struct uvc_control_mapping *mapping,
+			     size_t v4l2_size, const void *v4l2_in,
+			     void *uvc_out)
 {
-	data[0] = value == 0 ? 0 : (value > 0) ? 1 : 0xff;
-	data[2] = min((int)abs(value), 0xff);
+	u8 *out = uvc_out;
+	s32 value;
+
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
+
+	value = *(u32 *)v4l2_in;
+	out[0] = value == 0 ? 0 : (value > 0) ? 1 : 0xff;
+	out[2] = min_t(int, abs(value), 0xff);
+
+	return 0;
 }
 
-static s32 uvc_ctrl_get_rel_speed(struct uvc_control_mapping *mapping,
-	u8 query, const u8 *data)
+static int uvc_ctrl_get_rel_speed(struct uvc_control_mapping *mapping,
+				  u8 query, const void *uvc_in,
+				  size_t v4l2_size, void *v4l2_out)
 {
 	unsigned int first = mapping->offset / 8;
-	s8 rel = (s8)data[first];
+	u8 value = ((u8 *)uvc_in)[first + 1];
+	s8 sign = ((s8 *)uvc_in)[first];
+	s32 *out = v4l2_out;
+
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
 
 	switch (query) {
 	case UVC_GET_CUR:
-		return (rel == 0) ? 0 : (rel > 0 ? data[first+1]
-						 : -data[first+1]);
+		*out = (sign == 0) ? 0 : (sign > 0 ? value : -value);
+		return 0;
 	case UVC_GET_MIN:
-		return -data[first+1];
+		*out = -value;
+		return 0;
 	case UVC_GET_MAX:
 	case UVC_GET_RES:
 	case UVC_GET_DEF:
 	default:
-		return data[first+1];
+		*out = value;
+		return 0;
 	}
 }
 
-static void uvc_ctrl_set_rel_speed(struct uvc_control_mapping *mapping,
-	s32 value, u8 *data)
+static int uvc_ctrl_set_rel_speed(struct uvc_control_mapping *mapping,
+				  size_t v4l2_size, const void *v4l2_in,
+				  void *uvc_out)
 {
 	unsigned int first = mapping->offset / 8;
+	u8 *out = uvc_out;
+	s32 value;
+
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
 
-	data[first] = value == 0 ? 0 : (value > 0) ? 1 : 0xff;
-	data[first+1] = min_t(int, abs(value), 0xff);
+	value = *(u32 *)v4l2_in;
+	out[first] = value == 0 ? 0 : (value > 0) ? 1 : 0xff;
+	out[first + 1] = min_t(int, abs(value), 0xff);
+
+	return 0;
+}
+
+static const struct uvc_control_mapping uvc_ctrl_power_line_mapping_limited = {
+	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
+	.entity		= UVC_GUID_UVC_PROCESSING,
+	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
+	.size		= 2,
+	.offset		= 0,
+	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
+	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
+	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
+				  V4L2_CID_POWER_LINE_FREQUENCY_50HZ),
+};
+
+static const struct uvc_control_mapping uvc_ctrl_power_line_mapping_uvc11 = {
+	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
+	.entity		= UVC_GUID_UVC_PROCESSING,
+	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
+	.size		= 2,
+	.offset		= 0,
+	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
+	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
+	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
+				  V4L2_CID_POWER_LINE_FREQUENCY_DISABLED),
+};
+
+static const struct uvc_control_mapping uvc_ctrl_power_line_mapping_uvc15 = {
+	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
+	.entity		= UVC_GUID_UVC_PROCESSING,
+	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
+	.size		= 2,
+	.offset		= 0,
+	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
+	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
+	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_AUTO,
+				  V4L2_CID_POWER_LINE_FREQUENCY_DISABLED),
+};
+
+static const struct uvc_control_mapping *uvc_ctrl_filter_plf_mapping(
+	struct uvc_video_chain *chain, struct uvc_control *ctrl)
+{
+	const struct uvc_control_mapping *out_mapping =
+					&uvc_ctrl_power_line_mapping_uvc11;
+	u8 *buf __free(kfree) = NULL;
+	u8 init_val;
+	int ret;
+
+	buf = kmalloc(sizeof(*buf), GFP_KERNEL);
+	if (!buf)
+		return NULL;
+
+	/* Save the current PLF value, so we can restore it. */
+	ret = uvc_query_ctrl(chain->dev, UVC_GET_CUR, ctrl->entity->id,
+			     chain->dev->intfnum, ctrl->info.selector,
+			     buf, sizeof(*buf));
+	/* If we cannot read the control skip it. */
+	if (ret)
+		return NULL;
+	init_val = *buf;
+
+	/* If PLF value cannot be set to off, it is limited. */
+	*buf = V4L2_CID_POWER_LINE_FREQUENCY_DISABLED;
+	ret = uvc_query_ctrl(chain->dev, UVC_SET_CUR, ctrl->entity->id,
+			     chain->dev->intfnum, ctrl->info.selector,
+			     buf, sizeof(*buf));
+	if (ret)
+		return &uvc_ctrl_power_line_mapping_limited;
+
+	/* UVC 1.1 does not define auto, we can exit. */
+	if (chain->dev->uvc_version < 0x150)
+		goto end;
+
+	/* Check if the device supports auto. */
+	*buf = V4L2_CID_POWER_LINE_FREQUENCY_AUTO;
+	ret = uvc_query_ctrl(chain->dev, UVC_SET_CUR, ctrl->entity->id,
+			     chain->dev->intfnum, ctrl->info.selector,
+			     buf, sizeof(*buf));
+	if (!ret)
+		out_mapping = &uvc_ctrl_power_line_mapping_uvc15;
+
+end:
+	/* Restore initial value and add mapping. */
+	*buf = init_val;
+	uvc_query_ctrl(chain->dev, UVC_SET_CUR, ctrl->entity->id,
+		       chain->dev->intfnum, ctrl->info.selector,
+		       buf, sizeof(*buf));
+
+	return out_mapping;
+}
+
+static int uvc_get_rect(struct uvc_control_mapping *mapping, u8 query,
+			const void *uvc_in, size_t v4l2_size, void *v4l2_out)
+{
+	const struct uvc_rect *uvc_rect = uvc_in;
+	struct v4l2_rect *v4l2_rect = v4l2_out;
+
+	if (WARN_ON(v4l2_size != sizeof(struct v4l2_rect)))
+		return -EINVAL;
+
+	if (uvc_rect->left > uvc_rect->right ||
+	    uvc_rect->top > uvc_rect->bottom)
+		return -EIO;
+
+	v4l2_rect->top = uvc_rect->top;
+	v4l2_rect->left = uvc_rect->left;
+	v4l2_rect->height = uvc_rect->bottom - uvc_rect->top + 1;
+	v4l2_rect->width = uvc_rect->right - uvc_rect->left + 1;
+
+	return 0;
+}
+
+static int uvc_set_rect(struct uvc_control_mapping *mapping, size_t v4l2_size,
+			const void *v4l2_in, void *uvc_out)
+{
+	struct uvc_rect *uvc_rect = uvc_out;
+	const struct v4l2_rect *v4l2_rect = v4l2_in;
+
+	if (WARN_ON(v4l2_size != sizeof(struct v4l2_rect)))
+		return -EINVAL;
+
+	uvc_rect->top = min(0xffff, v4l2_rect->top);
+	uvc_rect->left = min(0xffff, v4l2_rect->left);
+	uvc_rect->bottom = min(0xffff, v4l2_rect->top + v4l2_rect->height - 1);
+	uvc_rect->right = min(0xffff, v4l2_rect->left + v4l2_rect->width - 1);
+
+	return 0;
 }
 
 static const struct uvc_control_mapping uvc_ctrl_mappings[] = {
@@ -748,52 +948,33 @@ static const struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.v4l2_type	= V4L2_CTRL_TYPE_BOOLEAN,
 		.data_type	= UVC_CTRL_DATA_TYPE_BOOLEAN,
 	},
-};
-
-const struct uvc_control_mapping uvc_ctrl_power_line_mapping_limited = {
-	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
-	.entity		= UVC_GUID_UVC_PROCESSING,
-	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
-	.size		= 2,
-	.offset		= 0,
-	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
-	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
-	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
-				  V4L2_CID_POWER_LINE_FREQUENCY_50HZ),
-};
-
-const struct uvc_control_mapping uvc_ctrl_power_line_mapping_uvc11 = {
-	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
-	.entity		= UVC_GUID_UVC_PROCESSING,
-	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
-	.size		= 2,
-	.offset		= 0,
-	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
-	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
-	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_60HZ,
-				  V4L2_CID_POWER_LINE_FREQUENCY_DISABLED),
-};
-
-static const struct uvc_control_mapping *uvc_ctrl_mappings_uvc11[] = {
-	&uvc_ctrl_power_line_mapping_uvc11,
-	NULL, /* Sentinel */
-};
-
-static const struct uvc_control_mapping uvc_ctrl_power_line_mapping_uvc15 = {
-	.id		= V4L2_CID_POWER_LINE_FREQUENCY,
-	.entity		= UVC_GUID_UVC_PROCESSING,
-	.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
-	.size		= 2,
-	.offset		= 0,
-	.v4l2_type	= V4L2_CTRL_TYPE_MENU,
-	.data_type	= UVC_CTRL_DATA_TYPE_ENUM,
-	.menu_mask	= GENMASK(V4L2_CID_POWER_LINE_FREQUENCY_AUTO,
-				  V4L2_CID_POWER_LINE_FREQUENCY_DISABLED),
-};
-
-static const struct uvc_control_mapping *uvc_ctrl_mappings_uvc15[] = {
-	&uvc_ctrl_power_line_mapping_uvc15,
-	NULL, /* Sentinel */
+	{
+		.entity		= UVC_GUID_UVC_PROCESSING,
+		.selector	= UVC_PU_POWER_LINE_FREQUENCY_CONTROL,
+		.filter_mapping	= uvc_ctrl_filter_plf_mapping,
+	},
+	{
+		.id		= V4L2_CID_UVC_REGION_OF_INTEREST_RECT,
+		.entity		= UVC_GUID_UVC_CAMERA,
+		.selector	= UVC_CT_REGION_OF_INTEREST_CONTROL,
+		.size		= sizeof(struct uvc_rect) * 8,
+		.offset		= 0,
+		.v4l2_type	= V4L2_CTRL_TYPE_RECT,
+		.data_type	= UVC_CTRL_DATA_TYPE_RECT,
+		.get		= uvc_get_rect,
+		.set		= uvc_set_rect,
+		.name		= "Region of Interest Rectangle",
+	},
+	{
+		.id		= V4L2_CID_UVC_REGION_OF_INTEREST_AUTO,
+		.entity		= UVC_GUID_UVC_CAMERA,
+		.selector	= UVC_CT_REGION_OF_INTEREST_CONTROL,
+		.size		= 16,
+		.offset		= 64,
+		.v4l2_type	= V4L2_CTRL_TYPE_BITMASK,
+		.data_type	= UVC_CTRL_DATA_TYPE_BITMASK,
+		.name		= "Region of Interest Auto Ctrls",
+	},
 };
 
 /* ------------------------------------------------------------------------
@@ -815,20 +996,45 @@ static inline void uvc_clear_bit(u8 *data, int bit)
 	data[bit >> 3] &= ~(1 << (bit & 7));
 }
 
+static s32 uvc_menu_to_v4l2_menu(struct uvc_control_mapping *mapping, s32 val)
+{
+	unsigned int i;
+
+	for (i = 0; BIT(i) <= mapping->menu_mask; ++i) {
+		u32 menu_value;
+
+		if (!test_bit(i, &mapping->menu_mask))
+			continue;
+
+		menu_value = uvc_mapping_get_menu_value(mapping, i);
+
+		if (menu_value == val)
+			return i;
+	}
+
+	return val;
+}
+
 /*
  * Extract the bit string specified by mapping->offset and mapping->size
  * from the little-endian data stored at 'data' and return the result as
  * a signed 32bit integer. Sign extension will be performed if the mapping
  * references a signed data type.
  */
-static s32 uvc_get_le_value(struct uvc_control_mapping *mapping,
-	u8 query, const u8 *data)
+static int uvc_get_le_value(struct uvc_control_mapping *mapping,
+			    u8 query, const void *uvc_in, size_t v4l2_size,
+			    void *v4l2_out)
 {
-	int bits = mapping->size;
 	int offset = mapping->offset;
+	int bits = mapping->size;
+	const u8 *data = uvc_in;
+	s32 *out = v4l2_out;
 	s32 value = 0;
 	u8 mask;
 
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
+
 	data += offset / 8;
 	offset &= 7;
 	mask = ((1LL << bits) - 1) << offset;
@@ -836,7 +1042,7 @@ static s32 uvc_get_le_value(struct uvc_control_mapping *mapping,
 	while (1) {
 		u8 byte = *data & mask;
 		value |= offset > 0 ? (byte >> offset) : (byte << (-offset));
-		bits -= 8 - (offset > 0 ? offset : 0);
+		bits -= 8 - max(offset, 0);
 		if (bits <= 0)
 			break;
 
@@ -849,28 +1055,58 @@ static s32 uvc_get_le_value(struct uvc_control_mapping *mapping,
 	if (mapping->data_type == UVC_CTRL_DATA_TYPE_SIGNED)
 		value |= -(value & (1 << (mapping->size - 1)));
 
-	return value;
+	/* If it is a menu, convert from uvc to v4l2. */
+	if (mapping->v4l2_type != V4L2_CTRL_TYPE_MENU) {
+		*out = value;
+		return 0;
+	}
+
+	switch (query) {
+	case UVC_GET_CUR:
+	case UVC_GET_DEF:
+		*out = uvc_menu_to_v4l2_menu(mapping, value);
+		return 0;
+	}
+
+	*out = value;
+	return 0;
 }
 
 /*
  * Set the bit string specified by mapping->offset and mapping->size
  * in the little-endian data stored at 'data' to the value 'value'.
  */
-static void uvc_set_le_value(struct uvc_control_mapping *mapping,
-	s32 value, u8 *data)
+static int uvc_set_le_value(struct uvc_control_mapping *mapping,
+			    size_t v4l2_size, const void *v4l2_in,
+			    void *uvc_out)
 {
-	int bits = mapping->size;
 	int offset = mapping->offset;
+	int bits = mapping->size;
+	u8 *data = uvc_out;
+	s32 value;
 	u8 mask;
 
-	/*
-	 * According to the v4l2 spec, writing any value to a button control
-	 * should result in the action belonging to the button control being
-	 * triggered. UVC devices however want to see a 1 written -> override
-	 * value.
-	 */
-	if (mapping->v4l2_type == V4L2_CTRL_TYPE_BUTTON)
+	if (WARN_ON(v4l2_size != sizeof(s32)))
+		return -EINVAL;
+
+	value = *(s32 *)v4l2_in;
+
+	switch (mapping->v4l2_type) {
+	case V4L2_CTRL_TYPE_MENU:
+		value = uvc_mapping_get_menu_value(mapping, value);
+		break;
+	case V4L2_CTRL_TYPE_BUTTON:
+		/*
+		 * According to the v4l2 spec, writing any value to a button
+		 * control should result in the action belonging to the button
+		 * control being triggered. UVC devices however want to see a 1
+		 * written -> override value.
+		 */
 		value = -1;
+		break;
+	default:
+		break;
+	}
 
 	data += offset / 8;
 	offset &= 7;
@@ -882,6 +1118,8 @@ static void uvc_set_le_value(struct uvc_control_mapping *mapping,
 		bits -= 8 - offset;
 		offset = 0;
 	}
+
+	return 0;
 }
 
 /* ------------------------------------------------------------------------
@@ -900,7 +1138,7 @@ static int uvc_entity_match_guid(const struct uvc_entity *entity,
 
 static void __uvc_find_control(struct uvc_entity *entity, u32 v4l2_id,
 	struct uvc_control_mapping **mapping, struct uvc_control **control,
-	int next)
+	int next, int next_compound)
 {
 	struct uvc_control *ctrl;
 	struct uvc_control_mapping *map;
@@ -915,14 +1153,16 @@ static void __uvc_find_control(struct uvc_entity *entity, u32 v4l2_id,
 			continue;
 
 		list_for_each_entry(map, &ctrl->info.mappings, list) {
-			if ((map->id == v4l2_id) && !next) {
+			if (map->id == v4l2_id && !next && !next_compound) {
 				*control = ctrl;
 				*mapping = map;
 				return;
 			}
 
 			if ((*mapping == NULL || (*mapping)->id > map->id) &&
-			    (map->id > v4l2_id) && next) {
+			    (map->id > v4l2_id) &&
+			    (uvc_ctrl_mapping_is_compound(map) ?
+			     next_compound : next)) {
 				*control = ctrl;
 				*mapping = map;
 			}
@@ -936,6 +1176,7 @@ static struct uvc_control *uvc_find_control(struct uvc_video_chain *chain,
 	struct uvc_control *ctrl = NULL;
 	struct uvc_entity *entity;
 	int next = v4l2_id & V4L2_CTRL_FLAG_NEXT_CTRL;
+	int next_compound = v4l2_id & V4L2_CTRL_FLAG_NEXT_COMPOUND;
 
 	*mapping = NULL;
 
@@ -944,12 +1185,13 @@ static struct uvc_control *uvc_find_control(struct uvc_video_chain *chain,
 
 	/* Find the control. */
 	list_for_each_entry(entity, &chain->entities, chain) {
-		__uvc_find_control(entity, v4l2_id, mapping, &ctrl, next);
-		if (ctrl && !next)
+		__uvc_find_control(entity, v4l2_id, mapping, &ctrl, next,
+				   next_compound);
+		if (ctrl && !next && !next_compound)
 			return ctrl;
 	}
 
-	if (ctrl == NULL && !next)
+	if (!ctrl && !next && !next_compound)
 		uvc_dbg(chain->dev, CONTROL, "Control 0x%08x not found\n",
 			v4l2_id);
 
@@ -1013,32 +1255,6 @@ static int uvc_ctrl_populate_cache(struct uvc_video_chain *chain,
 	return 0;
 }
 
-static s32 __uvc_ctrl_get_value(struct uvc_control_mapping *mapping,
-				const u8 *data)
-{
-	s32 value = mapping->get(mapping, UVC_GET_CUR, data);
-
-	if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
-		unsigned int i;
-
-		for (i = 0; BIT(i) <= mapping->menu_mask; ++i) {
-			u32 menu_value;
-
-			if (!test_bit(i, &mapping->menu_mask))
-				continue;
-
-			menu_value = uvc_mapping_get_menu_value(mapping, i);
-
-			if (menu_value == value) {
-				value = i;
-				break;
-			}
-		}
-	}
-
-	return value;
-}
-
 static int __uvc_ctrl_load_cur(struct uvc_video_chain *chain,
 			       struct uvc_control *ctrl)
 {
@@ -1089,8 +1305,8 @@ static int __uvc_ctrl_get(struct uvc_video_chain *chain,
 	if (ret < 0)
 		return ret;
 
-	*value = __uvc_ctrl_get_value(mapping,
-				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
+	*value = uvc_mapping_get_s32(mapping, UVC_GET_CUR,
+				     uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
 
 	return 0;
 }
@@ -1098,7 +1314,8 @@ static int __uvc_ctrl_get(struct uvc_video_chain *chain,
 static int __uvc_query_v4l2_class(struct uvc_video_chain *chain, u32 req_id,
 				  u32 found_id)
 {
-	bool find_next = req_id & V4L2_CTRL_FLAG_NEXT_CTRL;
+	bool find_next = req_id &
+		(V4L2_CTRL_FLAG_NEXT_CTRL | V4L2_CTRL_FLAG_NEXT_COMPOUND);
 	unsigned int i;
 
 	req_id &= V4L2_CTRL_ID_MASK;
@@ -1120,7 +1337,8 @@ static int __uvc_query_v4l2_class(struct uvc_video_chain *chain, u32 req_id,
 }
 
 static int uvc_query_v4l2_class(struct uvc_video_chain *chain, u32 req_id,
-				u32 found_id, struct v4l2_queryctrl *v4l2_ctrl)
+				u32 found_id,
+				struct v4l2_query_ext_ctrl *v4l2_ctrl)
 {
 	int idx;
 
@@ -1138,6 +1356,37 @@ static int uvc_query_v4l2_class(struct uvc_video_chain *chain, u32 req_id,
 	return 0;
 }
 
+static bool uvc_ctrl_is_readable(u32 which, struct uvc_control *ctrl,
+				 struct uvc_control_mapping *mapping)
+{
+	if (which == V4L2_CTRL_WHICH_CUR_VAL)
+		return !!(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR);
+
+	if (which == V4L2_CTRL_WHICH_DEF_VAL)
+		return !!(ctrl->info.flags & UVC_CTRL_FLAG_GET_DEF);
+
+	/* Types with implicit boundaries. */
+	switch (mapping->v4l2_type) {
+	case V4L2_CTRL_TYPE_MENU:
+	case V4L2_CTRL_TYPE_BOOLEAN:
+	case V4L2_CTRL_TYPE_BUTTON:
+		return true;
+	case V4L2_CTRL_TYPE_BITMASK:
+		return (ctrl->info.flags & UVC_CTRL_FLAG_GET_RES) ||
+			(ctrl->info.flags & UVC_CTRL_FLAG_GET_MAX);
+	default:
+		break;
+	}
+
+	if (which == V4L2_CTRL_WHICH_MIN_VAL)
+		return !!(ctrl->info.flags & UVC_CTRL_FLAG_GET_MIN);
+
+	if (which == V4L2_CTRL_WHICH_MAX_VAL)
+		return !!(ctrl->info.flags & UVC_CTRL_FLAG_GET_MAX);
+
+	return false;
+}
+
 /*
  * Check if control @v4l2_id can be accessed by the given control @ioctl
  * (VIDIOC_G_EXT_CTRLS, VIDIOC_TRY_EXT_CTRLS or VIDIOC_S_EXT_CTRLS).
@@ -1156,7 +1405,6 @@ int uvc_ctrl_is_accessible(struct uvc_video_chain *chain, u32 v4l2_id,
 	struct uvc_control *master_ctrl = NULL;
 	struct uvc_control_mapping *mapping;
 	struct uvc_control *ctrl;
-	bool read = ioctl == VIDIOC_G_EXT_CTRLS;
 	s32 val;
 	int ret;
 	int i;
@@ -1168,10 +1416,10 @@ int uvc_ctrl_is_accessible(struct uvc_video_chain *chain, u32 v4l2_id,
 	if (!ctrl)
 		return -EINVAL;
 
-	if (!(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR) && read)
-		return -EACCES;
+	if (ioctl == VIDIOC_G_EXT_CTRLS)
+		return uvc_ctrl_is_readable(ctrls->which, ctrl, mapping);
 
-	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR) && !read)
+	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
 		return -EACCES;
 
 	if (ioctl != VIDIOC_S_EXT_CTRLS || !mapping->master_id)
@@ -1188,10 +1436,12 @@ int uvc_ctrl_is_accessible(struct uvc_video_chain *chain, u32 v4l2_id,
 	}
 
 	__uvc_find_control(ctrl->entity, mapping->master_id, &master_map,
-			   &master_ctrl, 0);
+			   &master_ctrl, 0, 0);
 
 	if (!master_ctrl || !(master_ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR))
 		return 0;
+	if (WARN_ON(uvc_ctrl_mapping_is_compound(master_map)))
+		return -EIO;
 
 	ret = __uvc_ctrl_get(chain, master_ctrl, master_map, &val);
 	if (ret >= 0 && val != mapping->master_manual)
@@ -1223,50 +1473,21 @@ static u32 uvc_get_ctrl_bitmap(struct uvc_control *ctrl,
 	 * as supported.
 	 */
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_RES)
-		return mapping->get(mapping, UVC_GET_RES,
-				    uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
+		return uvc_mapping_get_s32(mapping, UVC_GET_RES,
+					   uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_MAX)
-		return mapping->get(mapping, UVC_GET_MAX,
-				    uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
+		return uvc_mapping_get_s32(mapping, UVC_GET_MAX,
+					   uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
 
 	return ~0;
 }
 
-static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
-	struct uvc_control *ctrl,
-	struct uvc_control_mapping *mapping,
-	struct v4l2_queryctrl *v4l2_ctrl)
+static int __uvc_queryctrl_boundaries(struct uvc_video_chain *chain,
+				      struct uvc_control *ctrl,
+				      struct uvc_control_mapping *mapping,
+				      struct v4l2_query_ext_ctrl *v4l2_ctrl)
 {
-	struct uvc_control_mapping *master_map = NULL;
-	struct uvc_control *master_ctrl = NULL;
-	unsigned int i;
-
-	memset(v4l2_ctrl, 0, sizeof(*v4l2_ctrl));
-	v4l2_ctrl->id = mapping->id;
-	v4l2_ctrl->type = mapping->v4l2_type;
-	strscpy(v4l2_ctrl->name, uvc_map_get_name(mapping),
-		sizeof(v4l2_ctrl->name));
-	v4l2_ctrl->flags = 0;
-
-	if (!(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR))
-		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
-	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
-		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
-
-	if (mapping->master_id)
-		__uvc_find_control(ctrl->entity, mapping->master_id,
-				   &master_map, &master_ctrl, 0);
-	if (master_ctrl && (master_ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR)) {
-		s32 val;
-		int ret = __uvc_ctrl_get(chain, master_ctrl, master_map, &val);
-		if (ret < 0)
-			return ret;
-
-		if (val != mapping->master_manual)
-				v4l2_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
-	}
-
 	if (!ctrl->cached) {
 		int ret = uvc_ctrl_populate_cache(chain, ctrl);
 		if (ret < 0)
@@ -1274,8 +1495,8 @@ static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 	}
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_DEF) {
-		v4l2_ctrl->default_value = mapping->get(mapping, UVC_GET_DEF,
-				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_DEF));
+		v4l2_ctrl->default_value = uvc_mapping_get_s32(mapping,
+				UVC_GET_DEF, uvc_ctrl_data(ctrl, UVC_CTRL_DATA_DEF));
 	}
 
 	switch (mapping->v4l2_type) {
@@ -1283,21 +1504,6 @@ static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 		v4l2_ctrl->minimum = ffs(mapping->menu_mask) - 1;
 		v4l2_ctrl->maximum = fls(mapping->menu_mask) - 1;
 		v4l2_ctrl->step = 1;
-
-		for (i = 0; BIT(i) <= mapping->menu_mask; ++i) {
-			u32 menu_value;
-
-			if (!test_bit(i, &mapping->menu_mask))
-				continue;
-
-			menu_value = uvc_mapping_get_menu_value(mapping, i);
-
-			if (menu_value == v4l2_ctrl->default_value) {
-				v4l2_ctrl->default_value = i;
-				break;
-			}
-		}
-
 		return 0;
 
 	case V4L2_CTRL_TYPE_BOOLEAN:
@@ -1323,22 +1529,95 @@ static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
 	}
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_MIN)
-		v4l2_ctrl->minimum = mapping->get(mapping, UVC_GET_MIN,
-				     uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MIN));
+		v4l2_ctrl->minimum = uvc_mapping_get_s32(mapping, UVC_GET_MIN,
+				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MIN));
+	else
+		v4l2_ctrl->minimum = 0;
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_MAX)
-		v4l2_ctrl->maximum = mapping->get(mapping, UVC_GET_MAX,
-				     uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
+		v4l2_ctrl->maximum = uvc_mapping_get_s32(mapping, UVC_GET_MAX,
+				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
+	else
+		v4l2_ctrl->maximum = 0;
 
 	if (ctrl->info.flags & UVC_CTRL_FLAG_GET_RES)
-		v4l2_ctrl->step = mapping->get(mapping, UVC_GET_RES,
-				  uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
+		v4l2_ctrl->step = uvc_mapping_get_s32(mapping, UVC_GET_RES,
+				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
+	else
+		v4l2_ctrl->step = 0;
 
 	return 0;
 }
 
+static size_t uvc_mapping_v4l2_size(struct uvc_control_mapping *mapping)
+{
+	if (mapping->v4l2_type == V4L2_CTRL_TYPE_RECT)
+		return sizeof(struct v4l2_rect);
+
+	if (uvc_ctrl_mapping_is_compound(mapping))
+		return DIV_ROUND_UP(mapping->size, 8);
+
+	return sizeof(s32);
+}
+
+static int __uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
+				 struct uvc_control *ctrl,
+				 struct uvc_control_mapping *mapping,
+				 struct v4l2_query_ext_ctrl *v4l2_ctrl)
+{
+	struct uvc_control_mapping *master_map = NULL;
+	struct uvc_control *master_ctrl = NULL;
+
+	memset(v4l2_ctrl, 0, sizeof(*v4l2_ctrl));
+	v4l2_ctrl->id = mapping->id;
+	v4l2_ctrl->type = mapping->v4l2_type;
+	strscpy(v4l2_ctrl->name, uvc_map_get_name(mapping),
+		sizeof(v4l2_ctrl->name));
+	v4l2_ctrl->flags = 0;
+
+	if (!(ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR))
+		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_WRITE_ONLY;
+	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
+		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
+	if ((ctrl->info.flags & UVC_CTRL_FLAG_GET_MAX) &&
+	    (ctrl->info.flags & UVC_CTRL_FLAG_GET_MIN))
+		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_HAS_WHICH_MIN_MAX;
+
+	if (mapping->master_id)
+		__uvc_find_control(ctrl->entity, mapping->master_id,
+				   &master_map, &master_ctrl, 0, 0);
+	if (master_ctrl && (master_ctrl->info.flags & UVC_CTRL_FLAG_GET_CUR)) {
+		s32 val;
+		int ret;
+
+		if (WARN_ON(uvc_ctrl_mapping_is_compound(master_map)))
+			return -EIO;
+
+		ret = __uvc_ctrl_get(chain, master_ctrl, master_map, &val);
+		if (ret < 0)
+			return ret;
+
+		if (val != mapping->master_manual)
+			v4l2_ctrl->flags |= V4L2_CTRL_FLAG_INACTIVE;
+	}
+
+	v4l2_ctrl->elem_size = uvc_mapping_v4l2_size(mapping);
+	v4l2_ctrl->elems = 1;
+
+	if (v4l2_ctrl->type >= V4L2_CTRL_COMPOUND_TYPES) {
+		v4l2_ctrl->flags |= V4L2_CTRL_FLAG_HAS_PAYLOAD;
+		v4l2_ctrl->default_value = 0;
+		v4l2_ctrl->minimum = 0;
+		v4l2_ctrl->maximum = 0;
+		v4l2_ctrl->step = 0;
+		return 0;
+	}
+
+	return __uvc_queryctrl_boundaries(chain, ctrl, mapping, v4l2_ctrl);
+}
+
 int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
-	struct v4l2_queryctrl *v4l2_ctrl)
+			struct v4l2_query_ext_ctrl *v4l2_ctrl)
 {
 	struct uvc_control *ctrl;
 	struct uvc_control_mapping *mapping;
@@ -1464,7 +1743,7 @@ static void uvc_ctrl_fill_event(struct uvc_video_chain *chain,
 	struct uvc_control_mapping *mapping,
 	s32 value, u32 changes)
 {
-	struct v4l2_queryctrl v4l2_ctrl;
+	struct v4l2_query_ext_ctrl v4l2_ctrl;
 
 	__uvc_query_v4l2_ctrl(chain, ctrl, mapping, &v4l2_ctrl);
 
@@ -1522,16 +1801,62 @@ static void uvc_ctrl_send_slave_event(struct uvc_video_chain *chain,
 	u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
 	s32 val = 0;
 
-	__uvc_find_control(master->entity, slave_id, &mapping, &ctrl, 0);
+	__uvc_find_control(master->entity, slave_id, &mapping, &ctrl, 0, 0);
 	if (ctrl == NULL)
 		return;
 
-	if (__uvc_ctrl_get(chain, ctrl, mapping, &val) == 0)
+	if (uvc_ctrl_mapping_is_compound(mapping) ||
+	    __uvc_ctrl_get(chain, ctrl, mapping, &val) == 0)
 		changes |= V4L2_EVENT_CTRL_CH_VALUE;
 
 	uvc_ctrl_send_event(chain, handle, ctrl, mapping, val, changes);
 }
 
+static int uvc_ctrl_set_handle(struct uvc_fh *handle, struct uvc_control *ctrl,
+			       struct uvc_fh *new_handle)
+{
+	lockdep_assert_held(&handle->chain->ctrl_mutex);
+
+	if (new_handle) {
+		int ret;
+
+		if (ctrl->handle)
+			dev_warn_ratelimited(&handle->stream->dev->udev->dev,
+					     "UVC non compliance: Setting an async control with a pending operation.");
+
+		if (new_handle == ctrl->handle)
+			return 0;
+
+		if (ctrl->handle) {
+			WARN_ON(!ctrl->handle->pending_async_ctrls);
+			if (ctrl->handle->pending_async_ctrls)
+				ctrl->handle->pending_async_ctrls--;
+			ctrl->handle = new_handle;
+			handle->pending_async_ctrls++;
+			return 0;
+		}
+
+		ret = uvc_pm_get(handle->chain->dev);
+		if (ret)
+			return ret;
+
+		ctrl->handle = new_handle;
+		handle->pending_async_ctrls++;
+		return 0;
+	}
+
+	/* Cannot clear the handle for a control not owned by us.*/
+	if (WARN_ON(ctrl->handle != handle))
+		return -EINVAL;
+
+	ctrl->handle = NULL;
+	if (WARN_ON(!handle->pending_async_ctrls))
+		return -EINVAL;
+	handle->pending_async_ctrls--;
+	uvc_pm_put(handle->chain->dev);
+	return 0;
+}
+
 void uvc_ctrl_status_event(struct uvc_video_chain *chain,
 			   struct uvc_control *ctrl, const u8 *data)
 {
@@ -1541,11 +1866,20 @@ void uvc_ctrl_status_event(struct uvc_video_chain *chain,
 
 	mutex_lock(&chain->ctrl_mutex);
 
+	/* Flush the control cache, the data might have changed. */
+	ctrl->loaded = 0;
+
 	handle = ctrl->handle;
-	ctrl->handle = NULL;
+	if (handle)
+		uvc_ctrl_set_handle(handle, ctrl, NULL);
 
 	list_for_each_entry(mapping, &ctrl->info.mappings, list) {
-		s32 value = __uvc_ctrl_get_value(mapping, data);
+		s32 value;
+
+		if (uvc_ctrl_mapping_is_compound(mapping))
+			value = 0;
+		else
+			value = uvc_mapping_get_s32(mapping, UVC_GET_CUR, data);
 
 		/*
 		 * handle may be NULL here if the device sends auto-update
@@ -1593,10 +1927,8 @@ bool uvc_ctrl_status_event_async(struct urb *urb, struct uvc_video_chain *chain,
 	struct uvc_device *dev = chain->dev;
 	struct uvc_ctrl_work *w = &dev->async_ctrl;
 
-	if (list_empty(&ctrl->info.mappings)) {
-		ctrl->handle = NULL;
+	if (list_empty(&ctrl->info.mappings))
 		return false;
-	}
 
 	w->data = data;
 	w->urb = urb;
@@ -1622,16 +1954,22 @@ static bool uvc_ctrl_xctrls_has_control(const struct v4l2_ext_control *xctrls,
 }
 
 static void uvc_ctrl_send_events(struct uvc_fh *handle,
-	const struct v4l2_ext_control *xctrls, unsigned int xctrls_count)
+				 struct uvc_entity *entity,
+				 const struct v4l2_ext_control *xctrls,
+				 unsigned int xctrls_count)
 {
 	struct uvc_control_mapping *mapping;
 	struct uvc_control *ctrl;
-	u32 changes = V4L2_EVENT_CTRL_CH_VALUE;
 	unsigned int i;
 	unsigned int j;
 
 	for (i = 0; i < xctrls_count; ++i) {
+		u32 changes = V4L2_EVENT_CTRL_CH_VALUE;
+		s32 value;
+
 		ctrl = uvc_find_control(handle->chain, xctrls[i].id, &mapping);
+		if (ctrl->entity != entity)
+			continue;
 
 		if (ctrl->info.flags & UVC_CTRL_FLAG_ASYNCHRONOUS)
 			/* Notification will be sent from an Interrupt event. */
@@ -1655,6 +1993,10 @@ static void uvc_ctrl_send_events(struct uvc_fh *handle,
 						  slave_id);
 		}
 
+		if (uvc_ctrl_mapping_is_compound(mapping))
+			value = 0;
+		else
+			value = xctrls[i].value;
 		/*
 		 * If the master is being modified in the same transaction
 		 * flags may change too.
@@ -1665,7 +2007,7 @@ static void uvc_ctrl_send_events(struct uvc_fh *handle,
 			changes |= V4L2_EVENT_CTRL_CH_FLAGS;
 
 		uvc_ctrl_send_event(handle->chain, handle, ctrl, mapping,
-				    xctrls[i].value, changes);
+				    value, changes);
 	}
 }
 
@@ -1697,7 +2039,8 @@ static int uvc_ctrl_add_event(struct v4l2_subscribed_event *sev, unsigned elems)
 		u32 changes = V4L2_EVENT_CTRL_CH_FLAGS;
 		s32 val = 0;
 
-		if (__uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
+		if (uvc_ctrl_mapping_is_compound(mapping) ||
+		    __uvc_ctrl_get(handle->chain, ctrl, mapping, &val) == 0)
 			changes |= V4L2_EVENT_CTRL_CH_VALUE;
 
 		uvc_ctrl_fill_event(handle->chain, &ev, ctrl, mapping, val,
@@ -1763,12 +2106,20 @@ int uvc_ctrl_begin(struct uvc_video_chain *chain)
 	return mutex_lock_interruptible(&chain->ctrl_mutex) ? -ERESTARTSYS : 0;
 }
 
+/*
+ * Returns the number of uvc controls that have been correctly set, or a
+ * negative number if there has been an error.
+ */
 static int uvc_ctrl_commit_entity(struct uvc_device *dev,
-	struct uvc_entity *entity, int rollback, struct uvc_control **err_ctrl)
+				  struct uvc_fh *handle,
+				  struct uvc_entity *entity,
+				  int rollback,
+				  struct uvc_control **err_ctrl)
 {
+	unsigned int processed_ctrls = 0;
 	struct uvc_control *ctrl;
 	unsigned int i;
-	int ret;
+	int ret = 0;
 
 	if (entity == NULL)
 		return 0;
@@ -1797,8 +2148,9 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 				dev->intfnum, ctrl->info.selector,
 				uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
 				ctrl->info.size);
-		else
-			ret = 0;
+
+		if (!ret)
+			processed_ctrls++;
 
 		if (rollback || ret < 0)
 			memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
@@ -1807,14 +2159,25 @@ static int uvc_ctrl_commit_entity(struct uvc_device *dev,
 
 		ctrl->dirty = 0;
 
-		if (ret < 0) {
+		if (!rollback && handle && !ret &&
+		    ctrl->info.flags & UVC_CTRL_FLAG_ASYNCHRONOUS)
+			ret = uvc_ctrl_set_handle(handle, ctrl, handle);
+
+		if (ret < 0 && !rollback) {
 			if (err_ctrl)
 				*err_ctrl = ctrl;
-			return ret;
+			/*
+			 * If we fail to set a control, we need to rollback
+			 * the next ones.
+			 */
+			rollback = 1;
 		}
 	}
 
-	return 0;
+	if (ret)
+		return ret;
+
+	return processed_ctrls;
 }
 
 static int uvc_ctrl_find_ctrl_idx(struct uvc_entity *entity,
@@ -1830,7 +2193,7 @@ static int uvc_ctrl_find_ctrl_idx(struct uvc_entity *entity,
 
 	for (i = 0; i < ctrls->count; i++) {
 		__uvc_find_control(entity, ctrls->controls[i].id, &mapping,
-				   &ctrl_found, 0);
+				   &ctrl_found, 0, 0);
 		if (uvc_control == ctrl_found)
 			return i;
 	}
@@ -1844,28 +2207,151 @@ int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
 	struct uvc_video_chain *chain = handle->chain;
 	struct uvc_control *err_ctrl;
 	struct uvc_entity *entity;
-	int ret = 0;
+	int ret_out = 0;
+	int ret;
 
 	/* Find the control. */
 	list_for_each_entry(entity, &chain->entities, chain) {
-		ret = uvc_ctrl_commit_entity(chain->dev, entity, rollback,
-					     &err_ctrl);
-		if (ret < 0)
-			goto done;
+		ret = uvc_ctrl_commit_entity(chain->dev, handle, entity,
+					     rollback, &err_ctrl);
+		if (ret < 0) {
+			if (ctrls)
+				ctrls->error_idx =
+					uvc_ctrl_find_ctrl_idx(entity, ctrls,
+							       err_ctrl);
+			/*
+			 * When we fail to commit an entity, we need to
+			 * restore the UVC_CTRL_DATA_BACKUP for all the
+			 * controls in the other entities, otherwise our cache
+			 * and the hardware will be out of sync.
+			 */
+			rollback = 1;
+
+			ret_out = ret;
+		} else if (ret > 0 && !rollback) {
+			uvc_ctrl_send_events(handle, entity,
+					     ctrls->controls, ctrls->count);
+		}
 	}
 
-	if (!rollback)
-		uvc_ctrl_send_events(handle, ctrls->controls, ctrls->count);
-done:
-	if (ret < 0 && ctrls)
-		ctrls->error_idx = uvc_ctrl_find_ctrl_idx(entity, ctrls,
-							  err_ctrl);
 	mutex_unlock(&chain->ctrl_mutex);
-	return ret;
+	return ret_out;
+}
+
+static int uvc_mapping_get_xctrl_compound(struct uvc_video_chain *chain,
+					  struct uvc_control *ctrl,
+					  struct uvc_control_mapping *mapping,
+					  u32 which,
+					  struct v4l2_ext_control *xctrl)
+{
+	u8 *data __free(kfree) = NULL;
+	size_t size;
+	u8 query;
+	int ret;
+	int id;
+
+	switch (which) {
+	case V4L2_CTRL_WHICH_CUR_VAL:
+		id = UVC_CTRL_DATA_CURRENT;
+		query = UVC_GET_CUR;
+		break;
+	case V4L2_CTRL_WHICH_MIN_VAL:
+		id = UVC_CTRL_DATA_MIN;
+		query = UVC_GET_MIN;
+		break;
+	case V4L2_CTRL_WHICH_MAX_VAL:
+		id = UVC_CTRL_DATA_MAX;
+		query = UVC_GET_MAX;
+		break;
+	case V4L2_CTRL_WHICH_DEF_VAL:
+		id = UVC_CTRL_DATA_DEF;
+		query = UVC_GET_DEF;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	size = uvc_mapping_v4l2_size(mapping);
+	if (xctrl->size < size) {
+		xctrl->size = size;
+		return -ENOSPC;
+	}
+
+	data = kmalloc(size, GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	if (which == V4L2_CTRL_WHICH_CUR_VAL)
+		ret = __uvc_ctrl_load_cur(chain, ctrl);
+	else
+		ret = uvc_ctrl_populate_cache(chain, ctrl);
+
+	if (ret < 0)
+		return ret;
+
+	ret = mapping->get(mapping, query, uvc_ctrl_data(ctrl, id), size, data);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * v4l2_ext_control does not have enough room to fit a compound control.
+	 * Instead, the value is in the user memory at xctrl->ptr. The v4l2
+	 * ioctl helper does not copy it for us.
+	 */
+	return copy_to_user(xctrl->ptr, data, size) ? -EFAULT : 0;
+}
+
+static int uvc_mapping_get_xctrl_std(struct uvc_video_chain *chain,
+				     struct uvc_control *ctrl,
+				     struct uvc_control_mapping *mapping,
+				     u32 which, struct v4l2_ext_control *xctrl)
+{
+	struct v4l2_query_ext_ctrl qec;
+	int ret;
+
+	switch (which) {
+	case V4L2_CTRL_WHICH_CUR_VAL:
+		return __uvc_ctrl_get(chain, ctrl, mapping, &xctrl->value);
+	case V4L2_CTRL_WHICH_DEF_VAL:
+	case V4L2_CTRL_WHICH_MIN_VAL:
+	case V4L2_CTRL_WHICH_MAX_VAL:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ret = __uvc_queryctrl_boundaries(chain, ctrl, mapping, &qec);
+	if (ret < 0)
+		return ret;
+
+	switch (which) {
+	case V4L2_CTRL_WHICH_DEF_VAL:
+		xctrl->value = qec.default_value;
+		break;
+	case V4L2_CTRL_WHICH_MIN_VAL:
+		xctrl->value = qec.minimum;
+		break;
+	case V4L2_CTRL_WHICH_MAX_VAL:
+		xctrl->value = qec.maximum;
+		break;
+	}
+
+	return 0;
 }
 
-int uvc_ctrl_get(struct uvc_video_chain *chain,
-	struct v4l2_ext_control *xctrl)
+static int uvc_mapping_get_xctrl(struct uvc_video_chain *chain,
+				 struct uvc_control *ctrl,
+				 struct uvc_control_mapping *mapping,
+				 u32 which, struct v4l2_ext_control *xctrl)
+{
+	if (uvc_ctrl_mapping_is_compound(mapping))
+		return uvc_mapping_get_xctrl_compound(chain, ctrl, mapping,
+						      which, xctrl);
+	return uvc_mapping_get_xctrl_std(chain, ctrl, mapping, which, xctrl);
+}
+
+int uvc_ctrl_get(struct uvc_video_chain *chain, u32 which,
+		 struct v4l2_ext_control *xctrl)
 {
 	struct uvc_control *ctrl;
 	struct uvc_control_mapping *mapping;
@@ -1874,34 +2360,23 @@ int uvc_ctrl_get(struct uvc_video_chain *chain,
 		return -EACCES;
 
 	ctrl = uvc_find_control(chain, xctrl->id, &mapping);
-	if (ctrl == NULL)
+	if (!ctrl)
 		return -EINVAL;
 
-	return __uvc_ctrl_get(chain, ctrl, mapping, &xctrl->value);
+	return uvc_mapping_get_xctrl(chain, ctrl, mapping, which, xctrl);
 }
 
-int uvc_ctrl_set(struct uvc_fh *handle,
-	struct v4l2_ext_control *xctrl)
+static int uvc_ctrl_clamp(struct uvc_video_chain *chain,
+			  struct uvc_control *ctrl,
+			  struct uvc_control_mapping *mapping,
+			  s32 *value_in_out)
 {
-	struct uvc_video_chain *chain = handle->chain;
-	struct uvc_control *ctrl;
-	struct uvc_control_mapping *mapping;
-	s32 value;
+	s32 value = *value_in_out;
 	u32 step;
 	s32 min;
 	s32 max;
 	int ret;
 
-	if (__uvc_query_v4l2_class(chain, xctrl->id, 0) >= 0)
-		return -EACCES;
-
-	ctrl = uvc_find_control(chain, xctrl->id, &mapping);
-	if (ctrl == NULL)
-		return -EINVAL;
-	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
-		return -EACCES;
-
-	/* Clamp out of range values. */
 	switch (mapping->v4l2_type) {
 	case V4L2_CTRL_TYPE_INTEGER:
 		if (!ctrl->cached) {
@@ -1910,23 +2385,22 @@ int uvc_ctrl_set(struct uvc_fh *handle,
 				return ret;
 		}
 
-		min = mapping->get(mapping, UVC_GET_MIN,
-				   uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MIN));
-		max = mapping->get(mapping, UVC_GET_MAX,
-				   uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
-		step = mapping->get(mapping, UVC_GET_RES,
-				    uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
+		min = uvc_mapping_get_s32(mapping, UVC_GET_MIN,
+					  uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MIN));
+		max = uvc_mapping_get_s32(mapping, UVC_GET_MAX,
+					  uvc_ctrl_data(ctrl, UVC_CTRL_DATA_MAX));
+		step = uvc_mapping_get_s32(mapping, UVC_GET_RES,
+					   uvc_ctrl_data(ctrl, UVC_CTRL_DATA_RES));
 		if (step == 0)
 			step = 1;
 
-		xctrl->value = min + DIV_ROUND_CLOSEST((u32)(xctrl->value - min),
-							step) * step;
+		value = min + DIV_ROUND_CLOSEST((u32)(value - min), step) * step;
 		if (mapping->data_type == UVC_CTRL_DATA_TYPE_SIGNED)
-			xctrl->value = clamp(xctrl->value, min, max);
+			value = clamp(value, min, max);
 		else
-			xctrl->value = clamp_t(u32, xctrl->value, min, max);
-		value = xctrl->value;
-		break;
+			value = clamp_t(u32, value, min, max);
+		*value_in_out = value;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BITMASK:
 		if (!ctrl->cached) {
@@ -1935,47 +2409,102 @@ int uvc_ctrl_set(struct uvc_fh *handle,
 				return ret;
 		}
 
-		xctrl->value &= uvc_get_ctrl_bitmap(ctrl, mapping);
-		value = xctrl->value;
-		break;
+		value &= uvc_get_ctrl_bitmap(ctrl, mapping);
+		*value_in_out = value;
+		return 0;
 
 	case V4L2_CTRL_TYPE_BOOLEAN:
-		xctrl->value = clamp(xctrl->value, 0, 1);
-		value = xctrl->value;
-		break;
+		*value_in_out = clamp(value, 0, 1);
+		return 0;
 
 	case V4L2_CTRL_TYPE_MENU:
-		if (xctrl->value < (ffs(mapping->menu_mask) - 1) ||
-		    xctrl->value > (fls(mapping->menu_mask) - 1))
+		if (value < (ffs(mapping->menu_mask) - 1) ||
+		    value > (fls(mapping->menu_mask) - 1))
 			return -ERANGE;
 
-		if (!test_bit(xctrl->value, &mapping->menu_mask))
+		if (!test_bit(value, &mapping->menu_mask))
 			return -EINVAL;
 
-		value = uvc_mapping_get_menu_value(mapping, xctrl->value);
-
 		/*
 		 * Valid menu indices are reported by the GET_RES request for
 		 * UVC controls that support it.
 		 */
 		if (mapping->data_type == UVC_CTRL_DATA_TYPE_BITMASK) {
+			int val = uvc_mapping_get_menu_value(mapping, value);
 			if (!ctrl->cached) {
 				ret = uvc_ctrl_populate_cache(chain, ctrl);
 				if (ret < 0)
 					return ret;
 			}
 
-			if (!(uvc_get_ctrl_bitmap(ctrl, mapping) & value))
+			if (!(uvc_get_ctrl_bitmap(ctrl, mapping) & val))
 				return -EINVAL;
 		}
-
-		break;
+		return 0;
 
 	default:
-		value = xctrl->value;
-		break;
+		return 0;
 	}
 
+	return 0;
+}
+
+static int uvc_mapping_set_xctrl_compound(struct uvc_control *ctrl,
+					  struct uvc_control_mapping *mapping,
+					  struct v4l2_ext_control *xctrl)
+{
+	u8 *data __free(kfree) = NULL;
+	size_t size = uvc_mapping_v4l2_size(mapping);
+
+	if (xctrl->size != size)
+		return -EINVAL;
+
+	/*
+	 * v4l2_ext_control does not have enough room to fit a compound control.
+	 * Instead, the value is in the user memory at xctrl->ptr. The v4l2
+	 * ioctl helper does not copy it for us.
+	 */
+	data = memdup_user(xctrl->ptr, size);
+	if (IS_ERR(data))
+		return PTR_ERR(data);
+
+	return mapping->set(mapping, size, data,
+			    uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
+}
+
+static int uvc_mapping_set_xctrl(struct uvc_control *ctrl,
+				 struct uvc_control_mapping *mapping,
+				 struct v4l2_ext_control *xctrl)
+{
+	if (uvc_ctrl_mapping_is_compound(mapping))
+		return uvc_mapping_set_xctrl_compound(ctrl, mapping, xctrl);
+
+	uvc_mapping_set_s32(mapping, xctrl->value,
+			    uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
+	return 0;
+}
+
+int uvc_ctrl_set(struct uvc_fh *handle, struct v4l2_ext_control *xctrl)
+{
+	struct uvc_video_chain *chain = handle->chain;
+	struct uvc_control_mapping *mapping;
+	struct uvc_control *ctrl;
+	int ret;
+
+	lockdep_assert_held(&chain->ctrl_mutex);
+
+	if (__uvc_query_v4l2_class(chain, xctrl->id, 0) >= 0)
+		return -EACCES;
+
+	ctrl = uvc_find_control(chain, xctrl->id, &mapping);
+	if (!ctrl)
+		return -EINVAL;
+	if (!(ctrl->info.flags & UVC_CTRL_FLAG_SET_CUR))
+		return -EACCES;
+
+	ret = uvc_ctrl_clamp(chain, ctrl, mapping, &xctrl->value);
+	if (ret)
+		return ret;
 	/*
 	 * If the mapping doesn't span the whole UVC control, the current value
 	 * needs to be loaded from the device to perform the read-modify-write
@@ -1994,11 +2523,9 @@ int uvc_ctrl_set(struct uvc_fh *handle,
 		       ctrl->info.size);
 	}
 
-	mapping->set(mapping, value,
-		uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT));
-
-	if (ctrl->info.flags & UVC_CTRL_FLAG_ASYNCHRONOUS)
-		ctrl->handle = handle;
+	ret = uvc_mapping_set_xctrl(ctrl, mapping, xctrl);
+	if (ret)
+		return ret;
 
 	ctrl->dirty = 1;
 	ctrl->modified = 1;
@@ -2029,7 +2556,13 @@ static int uvc_ctrl_get_flags(struct uvc_device *dev,
 	else
 		ret = uvc_query_ctrl(dev, UVC_GET_INFO, ctrl->entity->id,
 				     dev->intfnum, info->selector, data, 1);
-	if (!ret)
+
+	if (!ret) {
+		info->flags &= ~(UVC_CTRL_FLAG_GET_CUR |
+				 UVC_CTRL_FLAG_SET_CUR |
+				 UVC_CTRL_FLAG_AUTO_UPDATE |
+				 UVC_CTRL_FLAG_ASYNCHRONOUS);
+
 		info->flags |= (data[0] & UVC_CONTROL_CAP_GET ?
 				UVC_CTRL_FLAG_GET_CUR : 0)
 			    |  (data[0] & UVC_CONTROL_CAP_SET ?
@@ -2038,6 +2571,7 @@ static int uvc_ctrl_get_flags(struct uvc_device *dev,
 				UVC_CTRL_FLAG_AUTO_UPDATE : 0)
 			    |  (data[0] & UVC_CONTROL_CAP_ASYNCHRONOUS ?
 				UVC_CTRL_FLAG_ASYNCHRONOUS : 0);
+	}
 
 	kfree(data);
 	return ret;
@@ -2165,7 +2699,7 @@ static int uvc_ctrl_init_xu_ctrl(struct uvc_device *dev,
 int uvc_xu_ctrl_query(struct uvc_video_chain *chain,
 	struct uvc_xu_control_query *xqry)
 {
-	struct uvc_entity *entity;
+	struct uvc_entity *entity, *iter;
 	struct uvc_control *ctrl;
 	unsigned int i;
 	bool found;
@@ -2175,16 +2709,16 @@ int uvc_xu_ctrl_query(struct uvc_video_chain *chain,
 	int ret;
 
 	/* Find the extension unit. */
-	found = false;
-	list_for_each_entry(entity, &chain->entities, chain) {
-		if (UVC_ENTITY_TYPE(entity) == UVC_VC_EXTENSION_UNIT &&
-		    entity->id == xqry->unit) {
-			found = true;
+	entity = NULL;
+	list_for_each_entry(iter, &chain->entities, chain) {
+		if (UVC_ENTITY_TYPE(iter) == UVC_VC_EXTENSION_UNIT &&
+		    iter->id == xqry->unit) {
+			entity = iter;
 			break;
 		}
 	}
 
-	if (!found) {
+	if (!entity) {
 		uvc_dbg(chain->dev, CONTROL, "Extension unit %u not found\n",
 			xqry->unit);
 		return -ENOENT;
@@ -2321,7 +2855,7 @@ int uvc_ctrl_restore_values(struct uvc_device *dev)
 			ctrl->dirty = 1;
 		}
 
-		ret = uvc_ctrl_commit_entity(dev, entity, 0, NULL);
+		ret = uvc_ctrl_commit_entity(dev, NULL, entity, 0, NULL);
 		if (ret < 0)
 			return ret;
 	}
@@ -2366,6 +2900,7 @@ static int __uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
 	struct uvc_control_mapping *map;
 	unsigned int size;
 	unsigned int i;
+	int ret;
 
 	/*
 	 * Most mappings come from static kernel data, and need to be duplicated.
@@ -2406,6 +2941,12 @@ static int __uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
 			goto err_nomem;
 	}
 
+	if (uvc_ctrl_mapping_is_compound(map))
+		if (WARN_ON(!map->set || !map->get)) {
+			ret = -EIO;
+			goto free_mem;
+		}
+
 	if (map->get == NULL)
 		map->get = uvc_get_le_value;
 	if (map->set == NULL)
@@ -2427,11 +2968,13 @@ static int __uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
 	return 0;
 
 err_nomem:
+	ret = -ENOMEM;
+free_mem:
 	kfree(map->menu_names);
 	kfree(map->menu_mapping);
 	kfree(map->name);
 	kfree(map);
-	return -ENOMEM;
+	return ret;
 }
 
 int uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
@@ -2589,7 +3132,6 @@ static void uvc_ctrl_prune_entity(struct uvc_device *dev,
 static void uvc_ctrl_init_ctrl(struct uvc_video_chain *chain,
 			       struct uvc_control *ctrl)
 {
-	const struct uvc_control_mapping **mappings;
 	unsigned int i;
 
 	/*
@@ -2621,50 +3163,22 @@ static void uvc_ctrl_init_ctrl(struct uvc_video_chain *chain,
 	if (!ctrl->initialized)
 		return;
 
-	/*
-	 * First check if the device provides a custom mapping for this control,
-	 * used to override standard mappings for non-conformant devices. Don't
-	 * process standard mappings if a custom mapping is found. This
-	 * mechanism doesn't support combining standard and custom mappings for
-	 * a single control.
-	 */
-	if (chain->dev->info->mappings) {
-		bool custom = false;
-
-		for (i = 0; chain->dev->info->mappings[i]; ++i) {
-			const struct uvc_control_mapping *mapping =
-				chain->dev->info->mappings[i];
-
-			if (uvc_entity_match_guid(ctrl->entity, mapping->entity) &&
-			    ctrl->info.selector == mapping->selector) {
-				__uvc_ctrl_add_mapping(chain, ctrl, mapping);
-				custom = true;
-			}
-		}
-
-		if (custom)
-			return;
-	}
-
-	/* Process common mappings next. */
+	/* Process common mappings. */
 	for (i = 0; i < ARRAY_SIZE(uvc_ctrl_mappings); ++i) {
 		const struct uvc_control_mapping *mapping = &uvc_ctrl_mappings[i];
 
-		if (uvc_entity_match_guid(ctrl->entity, mapping->entity) &&
-		    ctrl->info.selector == mapping->selector)
-			__uvc_ctrl_add_mapping(chain, ctrl, mapping);
-	}
-
-	/* Finally process version-specific mappings. */
-	mappings = chain->dev->uvc_version < 0x0150
-		 ? uvc_ctrl_mappings_uvc11 : uvc_ctrl_mappings_uvc15;
+		if (!uvc_entity_match_guid(ctrl->entity, mapping->entity) ||
+		    ctrl->info.selector != mapping->selector)
+			continue;
 
-	for (i = 0; mappings[i]; ++i) {
-		const struct uvc_control_mapping *mapping = mappings[i];
+		/* Let the device provide a custom mapping. */
+		if (mapping->filter_mapping) {
+			mapping = mapping->filter_mapping(chain, ctrl);
+			if (!mapping)
+				continue;
+		}
 
-		if (uvc_entity_match_guid(ctrl->entity, mapping->entity) &&
-		    ctrl->info.selector == mapping->selector)
-			__uvc_ctrl_add_mapping(chain, ctrl, mapping);
+		__uvc_ctrl_add_mapping(chain, ctrl, mapping);
 	}
 }
 
@@ -2743,6 +3257,31 @@ int uvc_ctrl_init_device(struct uvc_device *dev)
 	return 0;
 }
 
+void uvc_ctrl_cleanup_fh(struct uvc_fh *handle)
+{
+	struct uvc_entity *entity;
+	int i;
+
+	guard(mutex)(&handle->chain->ctrl_mutex);
+
+	if (!handle->pending_async_ctrls)
+		return;
+
+	list_for_each_entry(entity, &handle->chain->dev->entities, list) {
+		for (unsigned int i = 0; i < entity->ncontrols; ++i) {
+			if (entity->controls[i].handle != handle)
+				continue;
+			uvc_ctrl_set_handle(handle, &entity->controls[i], NULL);
+		}
+	}
+
+	if (!WARN_ON(handle->pending_async_ctrls))
+		return;
+
+	for (i = 0; i < handle->pending_async_ctrls; i++)
+		uvc_pm_put(handle->stream->dev);
+}
+
 /*
  * Cleanup device controls.
  */
diff --git a/drivers/media/usb/uvc/uvc_debugfs.c b/drivers/media/usb/uvc/uvc_debugfs.c
index 1a1258d4ffca..14fa41cb8148 100644
--- a/drivers/media/usb/uvc/uvc_debugfs.c
+++ b/drivers/media/usb/uvc/uvc_debugfs.c
@@ -59,7 +59,6 @@ static int uvc_debugfs_stats_release(struct inode *inode, struct file *file)
 static const struct file_operations uvc_debugfs_stats_fops = {
 	.owner = THIS_MODULE,
 	.open = uvc_debugfs_stats_open,
-	.llseek = no_llseek,
 	.read = uvc_debugfs_stats_read,
 	.release = uvc_debugfs_stats_release,
 };
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index bbd90123a4e7..da24a655ab68 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -14,11 +14,12 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/usb.h>
+#include <linux/usb/quirks.h>
 #include <linux/usb/uvc.h>
 #include <linux/videodev2.h>
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
@@ -31,11 +32,13 @@
 
 unsigned int uvc_clock_param = CLOCK_MONOTONIC;
 unsigned int uvc_hw_timestamps_param;
-unsigned int uvc_no_drop_param;
+unsigned int uvc_no_drop_param = 1;
 static unsigned int uvc_quirks_param = -1;
 unsigned int uvc_dbg_param;
 unsigned int uvc_timeout_param = UVC_CTRL_STREAMING_TIMEOUT;
 
+static struct usb_driver uvc_driver;
+
 /* ------------------------------------------------------------------------
  * Utility functions
  */
@@ -219,20 +222,127 @@ static struct uvc_streaming *uvc_stream_new(struct uvc_device *dev,
  * Descriptors parsing
  */
 
+static int uvc_parse_frame(struct uvc_device *dev,
+			   struct uvc_streaming *streaming,
+			   struct uvc_format *format, struct uvc_frame *frame,
+			   u32 **intervals, u8 ftype, int width_multiplier,
+			   const unsigned char *buffer, int buflen)
+{
+	struct usb_host_interface *alts = streaming->intf->cur_altsetting;
+	unsigned int maxIntervalIndex;
+	unsigned int interval;
+	unsigned int i, n;
+
+	if (ftype != UVC_VS_FRAME_FRAME_BASED)
+		n = buflen > 25 ? buffer[25] : 0;
+	else
+		n = buflen > 21 ? buffer[21] : 0;
+
+	n = n ? n : 3;
+
+	if (buflen < 26 + 4 * n) {
+		uvc_dbg(dev, DESCR,
+			"device %d videostreaming interface %d FRAME error\n",
+			dev->udev->devnum, alts->desc.bInterfaceNumber);
+		return -EINVAL;
+	}
+
+	frame->bFrameIndex = buffer[3];
+	frame->bmCapabilities = buffer[4];
+	frame->wWidth = get_unaligned_le16(&buffer[5]) * width_multiplier;
+	frame->wHeight = get_unaligned_le16(&buffer[7]);
+	frame->dwMinBitRate = get_unaligned_le32(&buffer[9]);
+	frame->dwMaxBitRate = get_unaligned_le32(&buffer[13]);
+	if (ftype != UVC_VS_FRAME_FRAME_BASED) {
+		frame->dwMaxVideoFrameBufferSize =
+			get_unaligned_le32(&buffer[17]);
+		frame->dwDefaultFrameInterval =
+			get_unaligned_le32(&buffer[21]);
+		frame->bFrameIntervalType = buffer[25];
+	} else {
+		frame->dwMaxVideoFrameBufferSize = 0;
+		frame->dwDefaultFrameInterval =
+			get_unaligned_le32(&buffer[17]);
+		frame->bFrameIntervalType = buffer[21];
+	}
+
+	/*
+	 * Copy the frame intervals.
+	 *
+	 * Some bogus devices report dwMinFrameInterval equal to
+	 * dwMaxFrameInterval and have dwFrameIntervalStep set to zero. Setting
+	 * all null intervals to 1 fixes the problem and some other divisions
+	 * by zero that could happen.
+	 */
+	frame->dwFrameInterval = *intervals;
+
+	for (i = 0; i < n; ++i) {
+		interval = get_unaligned_le32(&buffer[26 + 4 * i]);
+		(*intervals)[i] = interval ? interval : 1;
+	}
+
+	/*
+	 * Apply more fixes, quirks and workarounds to handle incorrect or
+	 * broken descriptors.
+	 */
+
+	/*
+	 * Several UVC chipsets screw up dwMaxVideoFrameBufferSize completely.
+	 * Observed behaviours range from setting the value to 1.1x the actual
+	 * frame size to hardwiring the 16 low bits to 0. This results in a
+	 * higher than necessary memory usage as well as a wrong image size
+	 * information. For uncompressed formats this can be fixed by computing
+	 * the value from the frame size.
+	 */
+	if (!(format->flags & UVC_FMT_FLAG_COMPRESSED))
+		frame->dwMaxVideoFrameBufferSize = format->bpp * frame->wWidth
+						 * frame->wHeight / 8;
+
+	/*
+	 * Clamp the default frame interval to the boundaries. A zero
+	 * bFrameIntervalType value indicates a continuous frame interval
+	 * range, with dwFrameInterval[0] storing the minimum value and
+	 * dwFrameInterval[1] storing the maximum value.
+	 */
+	maxIntervalIndex = frame->bFrameIntervalType ? n - 1 : 1;
+	frame->dwDefaultFrameInterval =
+		clamp(frame->dwDefaultFrameInterval,
+		      frame->dwFrameInterval[0],
+		      frame->dwFrameInterval[maxIntervalIndex]);
+
+	/*
+	 * Some devices report frame intervals that are not functional. If the
+	 * corresponding quirk is set, restrict operation to the first interval
+	 * only.
+	 */
+	if (dev->quirks & UVC_QUIRK_RESTRICT_FRAME_RATE) {
+		frame->bFrameIntervalType = 1;
+		(*intervals)[0] = frame->dwDefaultFrameInterval;
+	}
+
+	uvc_dbg(dev, DESCR, "- %ux%u (%u.%u fps)\n",
+		frame->wWidth, frame->wHeight,
+		10000000 / frame->dwDefaultFrameInterval,
+		(100000000 / frame->dwDefaultFrameInterval) % 10);
+
+	*intervals += n;
+
+	return buffer[0];
+}
+
 static int uvc_parse_format(struct uvc_device *dev,
 	struct uvc_streaming *streaming, struct uvc_format *format,
 	struct uvc_frame *frames, u32 **intervals, const unsigned char *buffer,
 	int buflen)
 {
-	struct usb_interface *intf = streaming->intf;
-	struct usb_host_interface *alts = intf->cur_altsetting;
+	struct usb_host_interface *alts = streaming->intf->cur_altsetting;
 	const struct uvc_format_desc *fmtdesc;
 	struct uvc_frame *frame;
 	const unsigned char *start = buffer;
 	unsigned int width_multiplier = 1;
-	unsigned int interval;
 	unsigned int i, n;
 	u8 ftype;
+	int ret;
 
 	format->type = buffer[2];
 	format->index = buffer[3];
@@ -370,111 +480,19 @@ static int uvc_parse_format(struct uvc_device *dev,
 	 * Parse the frame descriptors. Only uncompressed, MJPEG and frame
 	 * based formats have frame descriptors.
 	 */
-	while (buflen > 2 && buffer[1] == USB_DT_CS_INTERFACE &&
-	       buffer[2] == ftype) {
-		unsigned int maxIntervalIndex;
-
-		frame = &frames[format->nframes];
-		if (ftype != UVC_VS_FRAME_FRAME_BASED)
-			n = buflen > 25 ? buffer[25] : 0;
-		else
-			n = buflen > 21 ? buffer[21] : 0;
-
-		n = n ? n : 3;
-
-		if (buflen < 26 + 4*n) {
-			uvc_dbg(dev, DESCR,
-				"device %d videostreaming interface %d FRAME error\n",
-				dev->udev->devnum,
-				alts->desc.bInterfaceNumber);
-			return -EINVAL;
-		}
-
-		frame->bFrameIndex = buffer[3];
-		frame->bmCapabilities = buffer[4];
-		frame->wWidth = get_unaligned_le16(&buffer[5])
-			      * width_multiplier;
-		frame->wHeight = get_unaligned_le16(&buffer[7]);
-		frame->dwMinBitRate = get_unaligned_le32(&buffer[9]);
-		frame->dwMaxBitRate = get_unaligned_le32(&buffer[13]);
-		if (ftype != UVC_VS_FRAME_FRAME_BASED) {
-			frame->dwMaxVideoFrameBufferSize =
-				get_unaligned_le32(&buffer[17]);
-			frame->dwDefaultFrameInterval =
-				get_unaligned_le32(&buffer[21]);
-			frame->bFrameIntervalType = buffer[25];
-		} else {
-			frame->dwMaxVideoFrameBufferSize = 0;
-			frame->dwDefaultFrameInterval =
-				get_unaligned_le32(&buffer[17]);
-			frame->bFrameIntervalType = buffer[21];
-		}
-
-		/*
-		 * Copy the frame intervals.
-		 *
-		 * Some bogus devices report dwMinFrameInterval equal to
-		 * dwMaxFrameInterval and have dwFrameIntervalStep set to
-		 * zero. Setting all null intervals to 1 fixes the problem and
-		 * some other divisions by zero that could happen.
-		 */
-		frame->dwFrameInterval = *intervals;
-
-		for (i = 0; i < n; ++i) {
-			interval = get_unaligned_le32(&buffer[26+4*i]);
-			(*intervals)[i] = interval ? interval : 1;
-		}
-
-		/*
-		 * Apply more fixes, quirks and workarounds to handle incorrect
-		 * or broken descriptors.
-		 */
-
-		/*
-		 * Several UVC chipsets screw up dwMaxVideoFrameBufferSize
-		 * completely. Observed behaviours range from setting the
-		 * value to 1.1x the actual frame size to hardwiring the
-		 * 16 low bits to 0. This results in a higher than necessary
-		 * memory usage as well as a wrong image size information. For
-		 * uncompressed formats this can be fixed by computing the
-		 * value from the frame size.
-		 */
-		if (!(format->flags & UVC_FMT_FLAG_COMPRESSED))
-			frame->dwMaxVideoFrameBufferSize = format->bpp
-				* frame->wWidth * frame->wHeight / 8;
-
-		/*
-		 * Clamp the default frame interval to the boundaries. A zero
-		 * bFrameIntervalType value indicates a continuous frame
-		 * interval range, with dwFrameInterval[0] storing the minimum
-		 * value and dwFrameInterval[1] storing the maximum value.
-		 */
-		maxIntervalIndex = frame->bFrameIntervalType ? n - 1 : 1;
-		frame->dwDefaultFrameInterval =
-			clamp(frame->dwDefaultFrameInterval,
-			      frame->dwFrameInterval[0],
-			      frame->dwFrameInterval[maxIntervalIndex]);
-
-		/*
-		 * Some devices report frame intervals that are not functional.
-		 * If the corresponding quirk is set, restrict operation to the
-		 * first interval only.
-		 */
-		if (dev->quirks & UVC_QUIRK_RESTRICT_FRAME_RATE) {
-			frame->bFrameIntervalType = 1;
-			(*intervals)[0] = frame->dwDefaultFrameInterval;
+	if (ftype) {
+		while (buflen > 2 && buffer[1] == USB_DT_CS_INTERFACE &&
+		       buffer[2] == ftype) {
+			frame = &frames[format->nframes];
+			ret = uvc_parse_frame(dev, streaming, format, frame,
+					      intervals, ftype, width_multiplier,
+					      buffer, buflen);
+			if (ret < 0)
+				return ret;
+			format->nframes++;
+			buflen -= ret;
+			buffer += ret;
 		}
-
-		uvc_dbg(dev, DESCR, "- %ux%u (%u.%u fps)\n",
-			frame->wWidth, frame->wHeight,
-			10000000 / frame->dwDefaultFrameInterval,
-			(100000000 / frame->dwDefaultFrameInterval) % 10);
-
-		format->nframes++;
-		*intervals += n;
-
-		buflen -= buffer[0];
-		buffer += buffer[0];
 	}
 
 	if (buflen > 2 && buffer[1] == USB_DT_CS_INTERFACE &&
@@ -530,7 +548,7 @@ static int uvc_parse_streaming(struct uvc_device *dev,
 		return -EINVAL;
 	}
 
-	if (usb_driver_claim_interface(&uvc_driver.driver, intf, dev)) {
+	if (usb_driver_claim_interface(&uvc_driver, intf, dev)) {
 		uvc_dbg(dev, DESCR,
 			"device %d interface %d is already claimed\n",
 			dev->udev->devnum,
@@ -540,7 +558,7 @@ static int uvc_parse_streaming(struct uvc_device *dev,
 
 	streaming = uvc_stream_new(dev, intf);
 	if (streaming == NULL) {
-		usb_driver_release_interface(&uvc_driver.driver, intf);
+		usb_driver_release_interface(&uvc_driver, intf);
 		return -ENOMEM;
 	}
 
@@ -686,16 +704,26 @@ static int uvc_parse_streaming(struct uvc_device *dev,
 		goto error;
 	}
 
-	size = nformats * sizeof(*format) + nframes * sizeof(*frame)
+	/*
+	 * Allocate memory for the formats, the frames and the intervals,
+	 * plus any required padding to guarantee that everything has the
+	 * correct alignment.
+	 */
+	size = nformats * sizeof(*format);
+	size = ALIGN(size, __alignof__(*frame)) + nframes * sizeof(*frame);
+	size = ALIGN(size, __alignof__(*interval))
 	     + nintervals * sizeof(*interval);
+
 	format = kzalloc(size, GFP_KERNEL);
-	if (format == NULL) {
+	if (!format) {
 		ret = -ENOMEM;
 		goto error;
 	}
 
-	frame = (struct uvc_frame *)&format[nformats];
-	interval = (u32 *)&frame[nframes];
+	frame = (void *)format + nformats * sizeof(*format);
+	frame = PTR_ALIGN(frame, __alignof__(*frame));
+	interval = (void *)frame + nframes * sizeof(*frame);
+	interval = PTR_ALIGN(interval, __alignof__(*interval));
 
 	streaming->formats = format;
 	streaming->nformats = 0;
@@ -753,7 +781,7 @@ static int uvc_parse_streaming(struct uvc_device *dev,
 	return 0;
 
 error:
-	usb_driver_release_interface(&uvc_driver.driver, intf);
+	usb_driver_release_interface(&uvc_driver, intf);
 	uvc_stream_delete(streaming);
 	return ret;
 }
@@ -1269,14 +1297,19 @@ static int uvc_gpio_parse(struct uvc_device *dev)
 	struct gpio_desc *gpio_privacy;
 	int irq;
 
-	gpio_privacy = devm_gpiod_get_optional(&dev->udev->dev, "privacy",
+	gpio_privacy = devm_gpiod_get_optional(&dev->intf->dev, "privacy",
 					       GPIOD_IN);
-	if (IS_ERR_OR_NULL(gpio_privacy))
-		return PTR_ERR_OR_ZERO(gpio_privacy);
+	if (!gpio_privacy)
+		return 0;
+
+	if (IS_ERR(gpio_privacy))
+		return dev_err_probe(&dev->intf->dev,
+				     PTR_ERR(gpio_privacy),
+				     "Can't get privacy GPIO\n");
 
 	irq = gpiod_to_irq(gpio_privacy);
 	if (irq < 0)
-		return dev_err_probe(&dev->udev->dev, irq,
+		return dev_err_probe(&dev->intf->dev, irq,
 				     "No IRQ for privacy GPIO\n");
 
 	unit = uvc_alloc_entity(UVC_EXT_GPIO_UNIT, UVC_EXT_GPIO_UNIT_ID, 0, 1);
@@ -1302,15 +1335,27 @@ static int uvc_gpio_parse(struct uvc_device *dev)
 static int uvc_gpio_init_irq(struct uvc_device *dev)
 {
 	struct uvc_entity *unit = dev->gpio_unit;
+	int ret;
 
 	if (!unit || unit->gpio.irq < 0)
 		return 0;
 
-	return devm_request_threaded_irq(&dev->udev->dev, unit->gpio.irq, NULL,
-					 uvc_gpio_irq,
-					 IRQF_ONESHOT | IRQF_TRIGGER_FALLING |
-					 IRQF_TRIGGER_RISING,
-					 "uvc_privacy_gpio", dev);
+	ret = request_threaded_irq(unit->gpio.irq, NULL, uvc_gpio_irq,
+				   IRQF_ONESHOT | IRQF_TRIGGER_FALLING |
+				   IRQF_TRIGGER_RISING,
+				   "uvc_privacy_gpio", dev);
+
+	unit->gpio.initialized = !ret;
+
+	return ret;
+}
+
+static void uvc_gpio_deinit(struct uvc_device *dev)
+{
+	if (!dev->gpio_unit || !dev->gpio_unit->gpio.initialized)
+		return;
+
+	free_irq(dev->gpio_unit->gpio.irq, dev);
 }
 
 /* ------------------------------------------------------------------------
@@ -1884,8 +1929,7 @@ static void uvc_delete(struct kref *kref)
 		struct uvc_streaming *streaming;
 
 		streaming = list_entry(p, struct uvc_streaming, list);
-		usb_driver_release_interface(&uvc_driver.driver,
-			streaming->intf);
+		usb_driver_release_interface(&uvc_driver, streaming->intf);
 		uvc_stream_delete(streaming);
 	}
 
@@ -1907,12 +1951,44 @@ static void uvc_unregister_video(struct uvc_device *dev)
 {
 	struct uvc_streaming *stream;
 
+	uvc_gpio_deinit(dev);
+
 	list_for_each_entry(stream, &dev->streams, list) {
+		/* Nothing to do here, continue. */
 		if (!video_is_registered(&stream->vdev))
 			continue;
 
+		/*
+		 * For stream->vdev we follow the same logic as:
+		 * vb2_video_unregister_device().
+		 */
+
+		/* 1. Take a reference to vdev */
+		get_device(&stream->vdev.dev);
+
+		/* 2. Ensure that no new ioctls can be called. */
 		video_unregister_device(&stream->vdev);
-		video_unregister_device(&stream->meta.vdev);
+
+		/* 3. Wait for old ioctls to finish. */
+		mutex_lock(&stream->mutex);
+
+		/* 4. Stop streaming. */
+		uvc_queue_release(&stream->queue);
+
+		mutex_unlock(&stream->mutex);
+
+		put_device(&stream->vdev.dev);
+
+		/*
+		 * For stream->meta.vdev we can directly call:
+		 * vb2_video_unregister_device().
+		 */
+		vb2_video_unregister_device(&stream->meta.vdev);
+
+		/*
+		 * Now both vdevs are not streaming and all the ioctls will
+		 * return -ENODEV.
+		 */
 
 		uvc_debugfs_cleanup_stream(stream);
 	}
@@ -1938,7 +2014,7 @@ int uvc_register_video_device(struct uvc_device *dev,
 	int ret;
 
 	/* Initialize the video buffers queue. */
-	ret = uvc_queue_init(queue, type, !uvc_no_drop_param);
+	ret = uvc_queue_init(queue, type);
 	if (ret)
 		return ret;
 
@@ -2105,7 +2181,6 @@ static int uvc_probe(struct usb_interface *intf,
 	INIT_LIST_HEAD(&dev->streams);
 	kref_init(&dev->ref);
 	atomic_set(&dev->nmappings, 0);
-	mutex_init(&dev->lock);
 
 	dev->udev = usb_get_dev(udev);
 	dev->intf = usb_get_intf(intf);
@@ -2162,16 +2237,17 @@ static int uvc_probe(struct usb_interface *intf,
 #endif
 
 	/* Parse the Video Class control descriptor. */
-	if (uvc_parse_control(dev) < 0) {
+	ret = uvc_parse_control(dev);
+	if (ret < 0) {
+		ret = -ENODEV;
 		uvc_dbg(dev, PROBE, "Unable to parse UVC descriptors\n");
 		goto error;
 	}
 
 	/* Parse the associated GPIOs. */
-	if (uvc_gpio_parse(dev) < 0) {
-		uvc_dbg(dev, PROBE, "Unable to parse UVC GPIOs\n");
+	ret = uvc_gpio_parse(dev);
+	if (ret < 0)
 		goto error;
-	}
 
 	dev_info(&dev->udev->dev, "Found UVC %u.%02x device %s (%04x:%04x)\n",
 		 dev->uvc_version >> 8, dev->uvc_version & 0xff,
@@ -2194,24 +2270,32 @@ static int uvc_probe(struct usb_interface *intf,
 	}
 
 	/* Register the V4L2 device. */
-	if (v4l2_device_register(&intf->dev, &dev->vdev) < 0)
+	ret = v4l2_device_register(&intf->dev, &dev->vdev);
+	if (ret < 0)
 		goto error;
 
 	/* Scan the device for video chains. */
-	if (uvc_scan_device(dev) < 0)
+	if (uvc_scan_device(dev) < 0) {
+		ret = -ENODEV;
 		goto error;
+	}
 
 	/* Initialize controls. */
-	if (uvc_ctrl_init_device(dev) < 0)
+	if (uvc_ctrl_init_device(dev) < 0) {
+		ret = -ENODEV;
 		goto error;
+	}
 
 	/* Register video device nodes. */
-	if (uvc_register_chains(dev) < 0)
+	if (uvc_register_chains(dev) < 0) {
+		ret = -ENODEV;
 		goto error;
+	}
 
 #ifdef CONFIG_MEDIA_CONTROLLER
 	/* Register the media device node */
-	if (media_device_register(&dev->mdev) < 0)
+	ret = media_device_register(&dev->mdev);
+	if (ret < 0)
 		goto error;
 #endif
 	/* Save our data pointer in the interface data. */
@@ -2232,14 +2316,20 @@ static int uvc_probe(struct usb_interface *intf,
 		goto error;
 	}
 
+	if (dev->quirks & UVC_QUIRK_NO_RESET_RESUME)
+		udev->quirks &= ~USB_QUIRK_RESET_RESUME;
+
+	if (!(dev->quirks & UVC_QUIRK_DISABLE_AUTOSUSPEND))
+		usb_enable_autosuspend(udev);
+
 	uvc_dbg(dev, PROBE, "UVC device initialized\n");
-	usb_enable_autosuspend(udev);
+
 	return 0;
 
 error:
 	uvc_unregister_video(dev);
 	kref_put(&dev->ref, uvc_delete);
-	return -ENODEV;
+	return ret;
 }
 
 static void uvc_disconnect(struct usb_interface *intf)
@@ -2271,10 +2361,7 @@ static int uvc_suspend(struct usb_interface *intf, pm_message_t message)
 	/* Controls are cached on the fly so they don't need to be saved. */
 	if (intf->cur_altsetting->desc.bInterfaceSubClass ==
 	    UVC_SC_VIDEOCONTROL) {
-		mutex_lock(&dev->lock);
-		if (dev->users)
-			uvc_status_stop(dev);
-		mutex_unlock(&dev->lock);
+		uvc_status_suspend(dev);
 		return 0;
 	}
 
@@ -2305,12 +2392,7 @@ static int __uvc_resume(struct usb_interface *intf, int reset)
 				return ret;
 		}
 
-		mutex_lock(&dev->lock);
-		if (dev->users)
-			ret = uvc_status_start(dev, GFP_NOIO);
-		mutex_unlock(&dev->lock);
-
-		return ret;
+		return uvc_status_resume(dev);
 	}
 
 	list_for_each_entry(stream, &dev->streams, list) {
@@ -2370,8 +2452,25 @@ module_param_call(clock, uvc_clock_param_set, uvc_clock_param_get,
 MODULE_PARM_DESC(clock, "Video buffers timestamp clock");
 module_param_named(hwtimestamps, uvc_hw_timestamps_param, uint, 0644);
 MODULE_PARM_DESC(hwtimestamps, "Use hardware timestamps");
-module_param_named(nodrop, uvc_no_drop_param, uint, 0644);
+
+static int param_set_nodrop(const char *val, const struct kernel_param *kp)
+{
+	pr_warn_once("uvcvideo: "
+		     DEPRECATED
+		     "nodrop parameter will be eventually removed.\n");
+	return param_set_bool(val, kp);
+}
+
+static const struct kernel_param_ops param_ops_nodrop = {
+	.set = param_set_nodrop,
+	.get = param_get_uint,
+};
+
+param_check_uint(nodrop, &uvc_no_drop_param);
+module_param_cb(nodrop, &param_ops_nodrop, &uvc_no_drop_param, 0644);
+__MODULE_PARM_TYPE(nodrop, "uint");
 MODULE_PARM_DESC(nodrop, "Don't drop incomplete frames");
+
 module_param_named(quirks, uvc_quirks_param, uint, 0644);
 MODULE_PARM_DESC(quirks, "Forced device quirks");
 module_param_named(trace, uvc_dbg_param, uint, 0644);
@@ -2383,20 +2482,6 @@ MODULE_PARM_DESC(timeout, "Streaming control requests timeout");
  * Driver initialization and cleanup
  */
 
-static const struct uvc_device_info uvc_ctrl_power_line_limited = {
-	.mappings = (const struct uvc_control_mapping *[]) {
-		&uvc_ctrl_power_line_mapping_limited,
-		NULL, /* Sentinel */
-	},
-};
-
-static const struct uvc_device_info uvc_ctrl_power_line_uvc11 = {
-	.mappings = (const struct uvc_control_mapping *[]) {
-		&uvc_ctrl_power_line_mapping_uvc11,
-		NULL, /* Sentinel */
-	},
-};
-
 static const struct uvc_device_info uvc_quirk_probe_minmax = {
 	.quirks = UVC_QUIRK_PROBE_MINMAX,
 };
@@ -2425,35 +2510,32 @@ static const struct uvc_device_info uvc_quirk_force_y8 = {
  * The Logitech cameras listed below have their interface class set to
  * VENDOR_SPEC because they don't announce themselves as UVC devices, even
  * though they are compliant.
+ *
+ * Sort these by vendor/product ID.
  */
 static const struct usb_device_id uvc_ids[] = {
-	/* Quanta USB2.0 HD UVC Webcam */
-	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
-				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x0408,
-	  .idProduct		= 0x3090,
-	  .bInterfaceClass	= USB_CLASS_VIDEO,
-	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Quanta USB2.0 HD UVC Webcam */
+	/* Quanta ACER HD User Facing */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
 	  .idVendor		= 0x0408,
-	  .idProduct		= 0x4030,
+	  .idProduct		= 0x4033,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Quanta USB2.0 HD UVC Webcam */
+	  .bInterfaceProtocol	= UVC_PC_PROTOCOL_15,
+	  .driver_info		= (kernel_ulong_t)&(const struct uvc_device_info){
+		.uvc_version = 0x010a,
+	  } },
+	/* Quanta ACER HD User Facing */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
 	  .idVendor		= 0x0408,
-	  .idProduct		= 0x4034,
+	  .idProduct		= 0x4035,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= UVC_PC_PROTOCOL_15,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
+	  .driver_info		= (kernel_ulong_t)&(const struct uvc_device_info){
+		.uvc_version = 0x010a,
+	  } },
 	/* LogiLink Wireless Webcam */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -2573,52 +2655,53 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_RESTORE_CTRLS_ON_INIT) },
-	/* Chicony CNF7129 (Asus EEE 100HE) */
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_RESTORE_CTRLS_ON_INIT
+					       | UVC_QUIRK_INVALID_DEVICE_SOF) },
+	/* Logitech HD Pro Webcam C922 */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x04f2,
-	  .idProduct		= 0xb071,
+	  .idVendor		= 0x046d,
+	  .idProduct		= 0x085c,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_RESTRICT_FRAME_RATE) },
-	/* Chicony EasyCamera */
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_INVALID_DEVICE_SOF) },
+	/* Logitech Rally Bar Huddle */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x04f2,
-	  .idProduct		= 0xb5eb,
+	  .idVendor		= 0x046d,
+	  .idProduct		= 0x087c,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Chicony Electronics Co., Ltd Integrated Camera */
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_NO_RESET_RESUME) },
+	/* Logitech Rally Bar */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x04f2,
-	  .idProduct		= 0xb67c,
+	  .idVendor		= 0x046d,
+	  .idProduct		= 0x089b,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= UVC_PC_PROTOCOL_15,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_uvc11 },
-	/* Chicony EasyCamera */
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_NO_RESET_RESUME) },
+	/* Logitech Rally Bar Mini */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x04f2,
-	  .idProduct		= 0xb6ba,
+	  .idVendor		= 0x046d,
+	  .idProduct		= 0x08d3,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Chicony EasyCamera */
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_NO_RESET_RESUME) },
+	/* Chicony CNF7129 (Asus EEE 100HE) */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
 	  .idVendor		= 0x04f2,
-	  .idProduct		= 0xb746,
+	  .idProduct		= 0xb071,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_RESTRICT_FRAME_RATE) },
 	/* Alcor Micro AU3820 (Future Boy PC USB Webcam) */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -2764,6 +2847,15 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= (kernel_ulong_t)&uvc_quirk_probe_minmax },
+	/* Sonix Technology Co. Ltd. - 292A IPC AR0330 */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x0c45,
+	  .idProduct		= 0x6366,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_MJPEG_NO_EOF) },
 	/* MT6227 */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -2792,6 +2884,15 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= (kernel_ulong_t)&uvc_quirk_probe_minmax },
+	/* Kurokesu C1 PRO */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x16d0,
+	  .idProduct		= 0x0ed1,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_MJPEG_NO_EOF) },
 	/* Syntek (HP Spartan) */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -2976,6 +3077,24 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_PROBE_MINMAX
 					| UVC_QUIRK_IGNORE_SELECTOR_UNIT) },
+	/* Actions Microelectronics Co. Display capture-UVC05 */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x1de1,
+	  .idProduct		= 0xf105,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_DISABLE_AUTOSUSPEND) },
+	/* NXP Semiconductors IR VIDEO */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x1fc9,
+	  .idProduct		= 0x009b,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= (kernel_ulong_t)&uvc_quirk_probe_minmax },
 	/* Oculus VR Positional Tracker DK2 */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -3003,51 +3122,15 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_FORCE_BPP) },
-	/* SunplusIT Inc HD Camera */
+	/* Insta360 Link */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x2b7e,
-	  .idProduct		= 0xb752,
-	  .bInterfaceClass	= USB_CLASS_VIDEO,
-	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= UVC_PC_PROTOCOL_15,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_uvc11 },
-	/* Lenovo Integrated Camera */
-	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
-				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x30c9,
-	  .idProduct		= 0x0093,
-	  .bInterfaceClass	= USB_CLASS_VIDEO,
-	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= UVC_PC_PROTOCOL_15,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_uvc11 },
-	/* Sonix Technology USB 2.0 Camera */
-	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
-				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x3277,
-	  .idProduct		= 0x0072,
+	  .idVendor		= 0x2e1a,
+	  .idProduct		= 0x4c01,
 	  .bInterfaceClass	= USB_CLASS_VIDEO,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Acer EasyCamera */
-	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
-				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x5986,
-	  .idProduct		= 0x1172,
-	  .bInterfaceClass	= USB_CLASS_VIDEO,
-	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
-	/* Acer EasyCamera */
-	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
-				| USB_DEVICE_ID_MATCH_INT_INFO,
-	  .idVendor		= 0x5986,
-	  .idProduct		= 0x1180,
-	  .bInterfaceClass	= USB_CLASS_VIDEO,
-	  .bInterfaceSubClass	= 1,
-	  .bInterfaceProtocol	= 0,
-	  .driver_info		= (kernel_ulong_t)&uvc_ctrl_power_line_limited },
+	  .driver_info		= UVC_INFO_QUIRK(UVC_QUIRK_DISABLE_AUTOSUSPEND) },
 	/* Intel D410/ASR depth camera */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -3120,6 +3203,15 @@ static const struct usb_device_id uvc_ids[] = {
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= UVC_INFO_META(V4L2_META_FMT_D4XX) },
+	/* Intel D421 Depth Module */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x8086,
+	  .idProduct		= 0x1155,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_INFO_META(V4L2_META_FMT_D4XX) },
 	/* Generic USB Video Class */
 	{ USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) },
 	{ USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_15) },
@@ -3128,17 +3220,15 @@ static const struct usb_device_id uvc_ids[] = {
 
 MODULE_DEVICE_TABLE(usb, uvc_ids);
 
-struct uvc_driver uvc_driver = {
-	.driver = {
-		.name		= "uvcvideo",
-		.probe		= uvc_probe,
-		.disconnect	= uvc_disconnect,
-		.suspend	= uvc_suspend,
-		.resume		= uvc_resume,
-		.reset_resume	= uvc_reset_resume,
-		.id_table	= uvc_ids,
-		.supports_autosuspend = 1,
-	},
+static struct usb_driver uvc_driver = {
+	.name		= "uvcvideo",
+	.probe		= uvc_probe,
+	.disconnect	= uvc_disconnect,
+	.suspend	= uvc_suspend,
+	.resume		= uvc_resume,
+	.reset_resume	= uvc_reset_resume,
+	.id_table	= uvc_ids,
+	.supports_autosuspend = 1,
 };
 
 static int __init uvc_init(void)
@@ -3147,7 +3237,7 @@ static int __init uvc_init(void)
 
 	uvc_debugfs_init();
 
-	ret = usb_register(&uvc_driver.driver);
+	ret = usb_register(&uvc_driver);
 	if (ret < 0) {
 		uvc_debugfs_cleanup();
 		return ret;
@@ -3158,7 +3248,7 @@ static int __init uvc_init(void)
 
 static void __exit uvc_cleanup(void)
 {
-	usb_deregister(&uvc_driver.driver);
+	usb_deregister(&uvc_driver);
 	uvc_debugfs_cleanup();
 }
 
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index 16fa17bbd15e..2ee142621042 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -197,8 +197,6 @@ static const struct vb2_ops uvc_queue_qops = {
 	.buf_prepare = uvc_buffer_prepare,
 	.buf_queue = uvc_buffer_queue,
 	.buf_finish = uvc_buffer_finish,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.start_streaming = uvc_start_streaming,
 	.stop_streaming = uvc_stop_streaming,
 };
@@ -207,13 +205,10 @@ static const struct vb2_ops uvc_meta_queue_qops = {
 	.queue_setup = uvc_queue_setup,
 	.buf_prepare = uvc_buffer_prepare,
 	.buf_queue = uvc_buffer_queue,
-	.wait_prepare = vb2_ops_wait_prepare,
-	.wait_finish = vb2_ops_wait_finish,
 	.stop_streaming = uvc_stop_streaming,
 };
 
-int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
-		    int drop_corrupted)
+int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type)
 {
 	int ret;
 
@@ -243,7 +238,6 @@ int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
 	mutex_init(&queue->mutex);
 	spin_lock_init(&queue->irqlock);
 	INIT_LIST_HEAD(&queue->irqqueue);
-	queue->flags = drop_corrupted ? UVC_QUEUE_DROP_CORRUPTED : 0;
 
 	return 0;
 }
@@ -476,14 +470,15 @@ static void uvc_queue_buffer_complete(struct kref *ref)
 	struct vb2_buffer *vb = &buf->buf.vb2_buf;
 	struct uvc_video_queue *queue = vb2_get_drv_priv(vb->vb2_queue);
 
-	if ((queue->flags & UVC_QUEUE_DROP_CORRUPTED) && buf->error) {
+	if (buf->error && !uvc_no_drop_param) {
 		uvc_queue_buffer_requeue(queue, buf);
 		return;
 	}
 
 	buf->state = buf->error ? UVC_BUF_STATE_ERROR : UVC_BUF_STATE_DONE;
 	vb2_set_plane_payload(&buf->buf.vb2_buf, 0, buf->bytesused);
-	vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
+	vb2_buffer_done(&buf->buf.vb2_buf, buf->error ? VB2_BUF_STATE_ERROR :
+							VB2_BUF_STATE_DONE);
 }
 
 /*
diff --git a/drivers/media/usb/uvc/uvc_status.c b/drivers/media/usb/uvc/uvc_status.c
index a78a88c710e2..ee01dce4b783 100644
--- a/drivers/media/usb/uvc/uvc_status.c
+++ b/drivers/media/usb/uvc/uvc_status.c
@@ -257,11 +257,11 @@ int uvc_status_init(struct uvc_device *dev)
 	unsigned int pipe;
 	int interval;
 
+	mutex_init(&dev->status_lock);
+
 	if (ep == NULL)
 		return 0;
 
-	uvc_input_init(dev);
-
 	dev->status = kzalloc(sizeof(*dev->status), GFP_KERNEL);
 	if (!dev->status)
 		return -ENOMEM;
@@ -269,6 +269,7 @@ int uvc_status_init(struct uvc_device *dev)
 	dev->int_urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!dev->int_urb) {
 		kfree(dev->status);
+		dev->status = NULL;
 		return -ENOMEM;
 	}
 
@@ -287,12 +288,17 @@ int uvc_status_init(struct uvc_device *dev)
 		dev->status, sizeof(*dev->status), uvc_status_complete,
 		dev, interval);
 
+	uvc_input_init(dev);
+
 	return 0;
 }
 
 void uvc_status_unregister(struct uvc_device *dev)
 {
-	usb_kill_urb(dev->int_urb);
+	if (!dev->status)
+		return;
+
+	uvc_status_suspend(dev);
 	uvc_input_unregister(dev);
 }
 
@@ -302,18 +308,25 @@ void uvc_status_cleanup(struct uvc_device *dev)
 	kfree(dev->status);
 }
 
-int uvc_status_start(struct uvc_device *dev, gfp_t flags)
+static int uvc_status_start(struct uvc_device *dev, gfp_t flags)
 {
-	if (dev->int_urb == NULL)
+	lockdep_assert_held(&dev->status_lock);
+
+	if (!dev->int_urb)
 		return 0;
 
 	return usb_submit_urb(dev->int_urb, flags);
 }
 
-void uvc_status_stop(struct uvc_device *dev)
+static void uvc_status_stop(struct uvc_device *dev)
 {
 	struct uvc_ctrl_work *w = &dev->async_ctrl;
 
+	lockdep_assert_held(&dev->status_lock);
+
+	if (!dev->int_urb)
+		return;
+
 	/*
 	 * Prevent the asynchronous control handler from requeing the URB. The
 	 * barrier is needed so the flush_status change is visible to other
@@ -350,3 +363,49 @@ void uvc_status_stop(struct uvc_device *dev)
 	 */
 	smp_store_release(&dev->flush_status, false);
 }
+
+int uvc_status_resume(struct uvc_device *dev)
+{
+	guard(mutex)(&dev->status_lock);
+
+	if (dev->status_users)
+		return uvc_status_start(dev, GFP_NOIO);
+
+	return 0;
+}
+
+void uvc_status_suspend(struct uvc_device *dev)
+{
+	guard(mutex)(&dev->status_lock);
+
+	if (dev->status_users)
+		uvc_status_stop(dev);
+}
+
+int uvc_status_get(struct uvc_device *dev)
+{
+	int ret;
+
+	guard(mutex)(&dev->status_lock);
+
+	if (!dev->status_users) {
+		ret = uvc_status_start(dev, GFP_KERNEL);
+		if (ret)
+			return ret;
+	}
+
+	dev->status_users++;
+
+	return 0;
+}
+
+void uvc_status_put(struct uvc_device *dev)
+{
+	guard(mutex)(&dev->status_lock);
+
+	if (dev->status_users == 1)
+		uvc_status_stop(dev);
+	WARN_ON(!dev->status_users);
+	if (dev->status_users)
+		dev->status_users--;
+}
diff --git a/drivers/media/usb/uvc/uvc_v4l2.c b/drivers/media/usb/uvc/uvc_v4l2.c
index f4988f03640a..668a4e9d772c 100644
--- a/drivers/media/usb/uvc/uvc_v4l2.c
+++ b/drivers/media/usb/uvc/uvc_v4l2.c
@@ -26,6 +26,29 @@
 
 #include "uvcvideo.h"
 
+int uvc_pm_get(struct uvc_device *dev)
+{
+	int ret;
+
+	ret = usb_autopm_get_interface(dev->intf);
+	if (ret)
+		return ret;
+
+	ret = uvc_status_get(dev);
+	if (ret)
+		usb_autopm_put_interface(dev->intf);
+
+	return ret;
+}
+
+void uvc_pm_put(struct uvc_device *dev)
+{
+	uvc_status_put(dev);
+	usb_autopm_put_interface(dev->intf);
+}
+
+static int uvc_acquire_privileges(struct uvc_fh *handle);
+
 static int uvc_control_add_xu_mapping(struct uvc_video_chain *chain,
 				      struct uvc_control_mapping *map,
 				      const struct uvc_xu_control_mapping *xmap)
@@ -106,6 +129,12 @@ static int uvc_ioctl_xu_ctrl_map(struct uvc_video_chain *chain,
 	struct uvc_control_mapping *map;
 	int ret;
 
+	if (xmap->data_type > UVC_CTRL_DATA_TYPE_BITMASK) {
+		uvc_dbg(chain->dev, CONTROL,
+			"Unsupported UVC data type %u\n", xmap->data_type);
+		return -EINVAL;
+	}
+
 	map = kzalloc(sizeof(*map), GFP_KERNEL);
 	if (map == NULL)
 		return -ENOMEM;
@@ -361,9 +390,11 @@ static int uvc_v4l2_try_format(struct uvc_streaming *stream,
 	return ret;
 }
 
-static int uvc_v4l2_get_format(struct uvc_streaming *stream,
-	struct v4l2_format *fmt)
+static int uvc_ioctl_g_fmt(struct file *file, void *fh,
+			   struct v4l2_format *fmt)
 {
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
 	const struct uvc_format *format;
 	const struct uvc_frame *frame;
 	int ret = 0;
@@ -395,14 +426,20 @@ done:
 	return ret;
 }
 
-static int uvc_v4l2_set_format(struct uvc_streaming *stream,
-	struct v4l2_format *fmt)
+static int uvc_ioctl_s_fmt(struct file *file, void *fh,
+			   struct v4l2_format *fmt)
 {
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
 	struct uvc_streaming_control probe;
 	const struct uvc_format *format;
 	const struct uvc_frame *frame;
 	int ret;
 
+	ret = uvc_acquire_privileges(handle);
+	if (ret < 0)
+		return ret;
+
 	if (fmt->type != stream->type)
 		return -EINVAL;
 
@@ -426,10 +463,12 @@ done:
 	return ret;
 }
 
-static int uvc_v4l2_get_streamparm(struct uvc_streaming *stream,
-		struct v4l2_streamparm *parm)
+static int uvc_ioctl_g_parm(struct file *file, void *fh,
+			    struct v4l2_streamparm *parm)
 {
 	u32 numerator, denominator;
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
 
 	if (parm->type != stream->type)
 		return -EINVAL;
@@ -461,9 +500,11 @@ static int uvc_v4l2_get_streamparm(struct uvc_streaming *stream,
 	return 0;
 }
 
-static int uvc_v4l2_set_streamparm(struct uvc_streaming *stream,
-		struct v4l2_streamparm *parm)
+static int uvc_ioctl_s_parm(struct file *file, void *fh,
+			    struct v4l2_streamparm *parm)
 {
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
 	struct uvc_streaming_control probe;
 	struct v4l2_fract timeperframe;
 	const struct uvc_format *format;
@@ -472,6 +513,10 @@ static int uvc_v4l2_set_streamparm(struct uvc_streaming *stream,
 	unsigned int i;
 	int ret;
 
+	ret = uvc_acquire_privileges(handle);
+	if (ret < 0)
+		return ret;
+
 	if (parm->type != stream->type)
 		return -EINVAL;
 
@@ -573,6 +618,7 @@ static int uvc_v4l2_set_streamparm(struct uvc_streaming *stream,
  * - VIDIOC_S_INPUT
  * - VIDIOC_S_PARM
  * - VIDIOC_S_FMT
+ * - VIDIOC_CREATE_BUFS
  * - VIDIOC_REQBUFS
  */
 static int uvc_acquire_privileges(struct uvc_fh *handle)
@@ -612,35 +658,14 @@ static int uvc_v4l2_open(struct file *file)
 {
 	struct uvc_streaming *stream;
 	struct uvc_fh *handle;
-	int ret = 0;
 
 	stream = video_drvdata(file);
 	uvc_dbg(stream->dev, CALLS, "%s\n", __func__);
 
-	ret = usb_autopm_get_interface(stream->dev->intf);
-	if (ret < 0)
-		return ret;
-
 	/* Create the device handle. */
 	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
-	if (handle == NULL) {
-		usb_autopm_put_interface(stream->dev->intf);
+	if (!handle)
 		return -ENOMEM;
-	}
-
-	mutex_lock(&stream->dev->lock);
-	if (stream->dev->users == 0) {
-		ret = uvc_status_start(stream->dev, GFP_KERNEL);
-		if (ret < 0) {
-			mutex_unlock(&stream->dev->lock);
-			usb_autopm_put_interface(stream->dev->intf);
-			kfree(handle);
-			return ret;
-		}
-	}
-
-	stream->dev->users++;
-	mutex_unlock(&stream->dev->lock);
 
 	v4l2_fh_init(&handle->vfh, &stream->vdev);
 	v4l2_fh_add(&handle->vfh);
@@ -659,10 +684,15 @@ static int uvc_v4l2_release(struct file *file)
 
 	uvc_dbg(stream->dev, CALLS, "%s\n", __func__);
 
+	uvc_ctrl_cleanup_fh(handle);
+
 	/* Only free resources if this is a privileged handle. */
 	if (uvc_has_privileges(handle))
 		uvc_queue_release(&stream->queue);
 
+	if (handle->is_streaming)
+		uvc_pm_put(stream->dev);
+
 	/* Release the file handle. */
 	uvc_dismiss_privileges(handle);
 	v4l2_fh_del(&handle->vfh);
@@ -670,12 +700,6 @@ static int uvc_v4l2_release(struct file *file)
 	kfree(handle);
 	file->private_data = NULL;
 
-	mutex_lock(&stream->dev->lock);
-	if (--stream->dev->users == 0)
-		uvc_status_stop(stream->dev);
-	mutex_unlock(&stream->dev->lock);
-
-	usb_autopm_put_interface(stream->dev->intf);
 	return 0;
 }
 
@@ -695,11 +719,13 @@ static int uvc_ioctl_querycap(struct file *file, void *fh,
 	return 0;
 }
 
-static int uvc_ioctl_enum_fmt(struct uvc_streaming *stream,
+static int uvc_ioctl_enum_fmt(struct file *file, void *fh,
 			      struct v4l2_fmtdesc *fmt)
 {
-	const struct uvc_format *format;
+	struct uvc_fh *handle = fh;
+	struct uvc_streaming *stream = handle->stream;
 	enum v4l2_buf_type type = fmt->type;
+	const struct uvc_format *format;
 	u32 index = fmt->index;
 
 	if (fmt->type != stream->type || fmt->index >= stream->nformats)
@@ -717,82 +743,8 @@ static int uvc_ioctl_enum_fmt(struct uvc_streaming *stream,
 	return 0;
 }
 
-static int uvc_ioctl_enum_fmt_vid_cap(struct file *file, void *fh,
-				      struct v4l2_fmtdesc *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-
-	return uvc_ioctl_enum_fmt(stream, fmt);
-}
-
-static int uvc_ioctl_enum_fmt_vid_out(struct file *file, void *fh,
-				      struct v4l2_fmtdesc *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-
-	return uvc_ioctl_enum_fmt(stream, fmt);
-}
-
-static int uvc_ioctl_g_fmt_vid_cap(struct file *file, void *fh,
-				   struct v4l2_format *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-
-	return uvc_v4l2_get_format(stream, fmt);
-}
-
-static int uvc_ioctl_g_fmt_vid_out(struct file *file, void *fh,
-				   struct v4l2_format *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-
-	return uvc_v4l2_get_format(stream, fmt);
-}
-
-static int uvc_ioctl_s_fmt_vid_cap(struct file *file, void *fh,
-				   struct v4l2_format *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-	int ret;
-
-	ret = uvc_acquire_privileges(handle);
-	if (ret < 0)
-		return ret;
-
-	return uvc_v4l2_set_format(stream, fmt);
-}
-
-static int uvc_ioctl_s_fmt_vid_out(struct file *file, void *fh,
-				   struct v4l2_format *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-	int ret;
-
-	ret = uvc_acquire_privileges(handle);
-	if (ret < 0)
-		return ret;
-
-	return uvc_v4l2_set_format(stream, fmt);
-}
-
-static int uvc_ioctl_try_fmt_vid_cap(struct file *file, void *fh,
-				     struct v4l2_format *fmt)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-	struct uvc_streaming_control probe;
-
-	return uvc_v4l2_try_format(stream, fmt, &probe, NULL, NULL);
-}
-
-static int uvc_ioctl_try_fmt_vid_out(struct file *file, void *fh,
-				     struct v4l2_format *fmt)
+static int uvc_ioctl_try_fmt(struct file *file, void *fh,
+			     struct v4l2_format *fmt)
 {
 	struct uvc_fh *handle = fh;
 	struct uvc_streaming *stream = handle->stream;
@@ -896,11 +848,23 @@ static int uvc_ioctl_streamon(struct file *file, void *fh,
 	if (!uvc_has_privileges(handle))
 		return -EBUSY;
 
-	mutex_lock(&stream->mutex);
+	guard(mutex)(&stream->mutex);
+
+	if (handle->is_streaming)
+		return 0;
+
 	ret = uvc_queue_streamon(&stream->queue, type);
-	mutex_unlock(&stream->mutex);
+	if (ret)
+		return ret;
 
-	return ret;
+	ret = uvc_pm_get(stream->dev);
+	if (ret) {
+		uvc_queue_streamoff(&stream->queue, type);
+		return ret;
+	}
+	handle->is_streaming = true;
+
+	return 0;
 }
 
 static int uvc_ioctl_streamoff(struct file *file, void *fh,
@@ -912,9 +876,13 @@ static int uvc_ioctl_streamoff(struct file *file, void *fh,
 	if (!uvc_has_privileges(handle))
 		return -EBUSY;
 
-	mutex_lock(&stream->mutex);
+	guard(mutex)(&stream->mutex);
+
 	uvc_queue_streamoff(&stream->queue, type);
-	mutex_unlock(&stream->mutex);
+	if (handle->is_streaming) {
+		handle->is_streaming = false;
+		uvc_pm_put(stream->dev);
+	}
 
 	return 0;
 }
@@ -1024,42 +992,13 @@ static int uvc_ioctl_s_input(struct file *file, void *fh, unsigned int input)
 	return ret;
 }
 
-static int uvc_ioctl_queryctrl(struct file *file, void *fh,
-			       struct v4l2_queryctrl *qc)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_video_chain *chain = handle->chain;
-
-	return uvc_query_v4l2_ctrl(chain, qc);
-}
-
 static int uvc_ioctl_query_ext_ctrl(struct file *file, void *fh,
 				    struct v4l2_query_ext_ctrl *qec)
 {
 	struct uvc_fh *handle = fh;
 	struct uvc_video_chain *chain = handle->chain;
-	struct v4l2_queryctrl qc = { qec->id };
-	int ret;
-
-	ret = uvc_query_v4l2_ctrl(chain, &qc);
-	if (ret)
-		return ret;
 
-	qec->id = qc.id;
-	qec->type = qc.type;
-	strscpy(qec->name, qc.name, sizeof(qec->name));
-	qec->minimum = qc.minimum;
-	qec->maximum = qc.maximum;
-	qec->step = qc.step;
-	qec->default_value = qc.default_value;
-	qec->flags = qc.flags;
-	qec->elem_size = 4;
-	qec->elems = 1;
-	qec->nr_of_dims = 0;
-	memset(qec->dims, 0, sizeof(qec->dims));
-	memset(qec->reserved, 0, sizeof(qec->reserved));
-
-	return 0;
+	return uvc_query_v4l2_ctrl(chain, qec);
 }
 
 static int uvc_ctrl_check_access(struct uvc_video_chain *chain,
@@ -1088,34 +1027,33 @@ static int uvc_ioctl_g_ext_ctrls(struct file *file, void *fh,
 	struct uvc_video_chain *chain = handle->chain;
 	struct v4l2_ext_control *ctrl = ctrls->controls;
 	unsigned int i;
+	u32 which;
 	int ret;
 
+	if (!ctrls->count)
+		return 0;
+
+	switch (ctrls->which) {
+	case V4L2_CTRL_WHICH_DEF_VAL:
+	case V4L2_CTRL_WHICH_CUR_VAL:
+	case V4L2_CTRL_WHICH_MAX_VAL:
+	case V4L2_CTRL_WHICH_MIN_VAL:
+		which = ctrls->which;
+		break;
+	default:
+		which = V4L2_CTRL_WHICH_CUR_VAL;
+	}
+
 	ret = uvc_ctrl_check_access(chain, ctrls, VIDIOC_G_EXT_CTRLS);
 	if (ret < 0)
 		return ret;
 
-	if (ctrls->which == V4L2_CTRL_WHICH_DEF_VAL) {
-		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
-			struct v4l2_queryctrl qc = { .id = ctrl->id };
-
-			ret = uvc_query_v4l2_ctrl(chain, &qc);
-			if (ret < 0) {
-				ctrls->error_idx = i;
-				return ret;
-			}
-
-			ctrl->value = qc.default_value;
-		}
-
-		return 0;
-	}
-
 	ret = uvc_ctrl_begin(chain);
 	if (ret < 0)
 		return ret;
 
 	for (i = 0; i < ctrls->count; ++ctrl, ++i) {
-		ret = uvc_ctrl_get(chain, ctrl);
+		ret = uvc_ctrl_get(chain, which, ctrl);
 		if (ret < 0) {
 			uvc_ctrl_rollback(handle);
 			ctrls->error_idx = i;
@@ -1137,6 +1075,9 @@ static int uvc_ioctl_s_try_ext_ctrls(struct uvc_fh *handle,
 	unsigned int i;
 	int ret;
 
+	if (!ctrls->count)
+		return 0;
+
 	ret = uvc_ctrl_check_access(chain, ctrls, ioctl);
 	if (ret < 0)
 		return ret;
@@ -1222,29 +1163,6 @@ static int uvc_ioctl_g_selection(struct file *file, void *fh,
 	return 0;
 }
 
-static int uvc_ioctl_g_parm(struct file *file, void *fh,
-			    struct v4l2_streamparm *parm)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-
-	return uvc_v4l2_get_streamparm(stream, parm);
-}
-
-static int uvc_ioctl_s_parm(struct file *file, void *fh,
-			    struct v4l2_streamparm *parm)
-{
-	struct uvc_fh *handle = fh;
-	struct uvc_streaming *stream = handle->stream;
-	int ret;
-
-	ret = uvc_acquire_privileges(handle);
-	if (ret < 0)
-		return ret;
-
-	return uvc_v4l2_set_streamparm(stream, parm);
-}
-
 static int uvc_ioctl_enum_framesizes(struct file *file, void *fh,
 				     struct v4l2_frmsizeenum *fsize)
 {
@@ -1463,9 +1381,11 @@ static int uvc_v4l2_put_xu_query(const struct uvc_xu_control_query *kp,
 #define UVCIOC_CTRL_MAP32	_IOWR('u', 0x20, struct uvc_xu_control_mapping32)
 #define UVCIOC_CTRL_QUERY32	_IOWR('u', 0x21, struct uvc_xu_control_query32)
 
+DEFINE_FREE(uvc_pm_put, struct uvc_device *, if (_T) uvc_pm_put(_T))
 static long uvc_v4l2_compat_ioctl32(struct file *file,
 		     unsigned int cmd, unsigned long arg)
 {
+	struct uvc_device *uvc_device __free(uvc_pm_put) = NULL;
 	struct uvc_fh *handle = file->private_data;
 	union {
 		struct uvc_xu_control_mapping xmap;
@@ -1474,6 +1394,12 @@ static long uvc_v4l2_compat_ioctl32(struct file *file,
 	void __user *up = compat_ptr(arg);
 	long ret;
 
+	ret = uvc_pm_get(handle->stream->dev);
+	if (ret)
+		return ret;
+
+	uvc_device = handle->stream->dev;
+
 	switch (cmd) {
 	case UVCIOC_CTRL_MAP32:
 		ret = uvc_v4l2_get_xu_mapping(&karg.xmap, up);
@@ -1508,6 +1434,42 @@ static long uvc_v4l2_compat_ioctl32(struct file *file,
 }
 #endif
 
+static long uvc_v4l2_unlocked_ioctl(struct file *file,
+				    unsigned int cmd, unsigned long arg)
+{
+	struct uvc_fh *handle = file->private_data;
+	int ret;
+
+	/* The following IOCTLs do not need to turn on the camera. */
+	switch (cmd) {
+	case VIDIOC_CREATE_BUFS:
+	case VIDIOC_DQBUF:
+	case VIDIOC_ENUM_FMT:
+	case VIDIOC_ENUM_FRAMEINTERVALS:
+	case VIDIOC_ENUM_FRAMESIZES:
+	case VIDIOC_ENUMINPUT:
+	case VIDIOC_EXPBUF:
+	case VIDIOC_G_FMT:
+	case VIDIOC_G_PARM:
+	case VIDIOC_G_SELECTION:
+	case VIDIOC_QBUF:
+	case VIDIOC_QUERYCAP:
+	case VIDIOC_REQBUFS:
+	case VIDIOC_SUBSCRIBE_EVENT:
+	case VIDIOC_UNSUBSCRIBE_EVENT:
+		return video_ioctl2(file, cmd, arg);
+	}
+
+	ret = uvc_pm_get(handle->stream->dev);
+	if (ret)
+		return ret;
+
+	ret = video_ioctl2(file, cmd, arg);
+
+	uvc_pm_put(handle->stream->dev);
+	return ret;
+}
+
 static ssize_t uvc_v4l2_read(struct file *file, char __user *data,
 		    size_t count, loff_t *ppos)
 {
@@ -1553,15 +1515,17 @@ static unsigned long uvc_v4l2_get_unmapped_area(struct file *file,
 #endif
 
 const struct v4l2_ioctl_ops uvc_ioctl_ops = {
+	.vidioc_g_fmt_vid_cap = uvc_ioctl_g_fmt,
+	.vidioc_g_fmt_vid_out = uvc_ioctl_g_fmt,
+	.vidioc_s_fmt_vid_cap = uvc_ioctl_s_fmt,
+	.vidioc_s_fmt_vid_out = uvc_ioctl_s_fmt,
+	.vidioc_g_parm = uvc_ioctl_g_parm,
+	.vidioc_s_parm = uvc_ioctl_s_parm,
 	.vidioc_querycap = uvc_ioctl_querycap,
-	.vidioc_enum_fmt_vid_cap = uvc_ioctl_enum_fmt_vid_cap,
-	.vidioc_enum_fmt_vid_out = uvc_ioctl_enum_fmt_vid_out,
-	.vidioc_g_fmt_vid_cap = uvc_ioctl_g_fmt_vid_cap,
-	.vidioc_g_fmt_vid_out = uvc_ioctl_g_fmt_vid_out,
-	.vidioc_s_fmt_vid_cap = uvc_ioctl_s_fmt_vid_cap,
-	.vidioc_s_fmt_vid_out = uvc_ioctl_s_fmt_vid_out,
-	.vidioc_try_fmt_vid_cap = uvc_ioctl_try_fmt_vid_cap,
-	.vidioc_try_fmt_vid_out = uvc_ioctl_try_fmt_vid_out,
+	.vidioc_enum_fmt_vid_cap = uvc_ioctl_enum_fmt,
+	.vidioc_enum_fmt_vid_out = uvc_ioctl_enum_fmt,
+	.vidioc_try_fmt_vid_cap = uvc_ioctl_try_fmt,
+	.vidioc_try_fmt_vid_out = uvc_ioctl_try_fmt,
 	.vidioc_reqbufs = uvc_ioctl_reqbufs,
 	.vidioc_querybuf = uvc_ioctl_querybuf,
 	.vidioc_qbuf = uvc_ioctl_qbuf,
@@ -1573,15 +1537,12 @@ const struct v4l2_ioctl_ops uvc_ioctl_ops = {
 	.vidioc_enum_input = uvc_ioctl_enum_input,
 	.vidioc_g_input = uvc_ioctl_g_input,
 	.vidioc_s_input = uvc_ioctl_s_input,
-	.vidioc_queryctrl = uvc_ioctl_queryctrl,
 	.vidioc_query_ext_ctrl = uvc_ioctl_query_ext_ctrl,
 	.vidioc_g_ext_ctrls = uvc_ioctl_g_ext_ctrls,
 	.vidioc_s_ext_ctrls = uvc_ioctl_s_ext_ctrls,
 	.vidioc_try_ext_ctrls = uvc_ioctl_try_ext_ctrls,
 	.vidioc_querymenu = uvc_ioctl_querymenu,
 	.vidioc_g_selection = uvc_ioctl_g_selection,
-	.vidioc_g_parm = uvc_ioctl_g_parm,
-	.vidioc_s_parm = uvc_ioctl_s_parm,
 	.vidioc_enum_framesizes = uvc_ioctl_enum_framesizes,
 	.vidioc_enum_frameintervals = uvc_ioctl_enum_frameintervals,
 	.vidioc_subscribe_event = uvc_ioctl_subscribe_event,
@@ -1593,7 +1554,7 @@ const struct v4l2_file_operations uvc_fops = {
 	.owner		= THIS_MODULE,
 	.open		= uvc_v4l2_open,
 	.release	= uvc_v4l2_release,
-	.unlocked_ioctl	= video_ioctl2,
+	.unlocked_ioctl	= uvc_v4l2_unlocked_ioctl,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl32	= uvc_v4l2_compat_ioctl32,
 #endif
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 7cbf4692bd87..e3567aeb0007 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -18,8 +18,9 @@
 #include <linux/vmalloc.h>
 #include <linux/wait.h>
 #include <linux/atomic.h>
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
+#include <media/jpeg.h>
 #include <media/v4l2-common.h>
 
 #include "uvcvideo.h"
@@ -79,6 +80,27 @@ int uvc_query_ctrl(struct uvc_device *dev, u8 query, u8 unit,
 	if (likely(ret == size))
 		return 0;
 
+	/*
+	 * Some devices return shorter USB control packets than expected if the
+	 * returned value can fit in less bytes. Zero all the bytes that the
+	 * device has not written.
+	 *
+	 * This quirk is applied to all controls, regardless of their data type.
+	 * Most controls are little-endian integers, in which case the missing
+	 * bytes become 0 MSBs. For other data types, a different heuristic
+	 * could be implemented if a device is found needing it.
+	 *
+	 * We exclude UVC_GET_INFO from the quirk. UVC_GET_LEN does not need
+	 * to be excluded because its size is always 1.
+	 */
+	if (ret > 0 && query != UVC_GET_INFO) {
+		memset(data + ret, 0, size - ret);
+		dev_warn_once(&dev->udev->dev,
+			      "UVC non compliance: %s control %u on unit %u returned %d bytes when we expected %u.\n",
+			      uvc_query_name(query), cs, unit, ret, size);
+		return 0;
+	}
+
 	if (ret != -EPIPE) {
 		dev_err(&dev->udev->dev,
 			"Failed to query (%s) UVC control %u on unit %u: %d (exp. %u).\n",
@@ -96,8 +118,12 @@ int uvc_query_ctrl(struct uvc_device *dev, u8 query, u8 unit,
 	error = *(u8 *)data;
 	*(u8 *)data = tmp;
 
-	if (ret != 1)
+	if (ret != 1) {
+		dev_err_ratelimited(&dev->udev->dev,
+				    "Failed to query (%s) UVC error code control %u on unit %u: %d (exp. 1).\n",
+				    uvc_query_name(query), cs, unit, ret);
 		return ret < 0 ? ret : -EPIPE;
+	}
 
 	uvc_dbg(dev, CONTROL, "Control error %u\n", error);
 
@@ -214,13 +240,13 @@ static void uvc_fixup_video_ctrl(struct uvc_streaming *stream,
 		 * Compute a bandwidth estimation by multiplying the frame
 		 * size by the number of video frames per second, divide the
 		 * result by the number of USB frames (or micro-frames for
-		 * high-speed devices) per second and add the UVC header size
-		 * (assumed to be 12 bytes long).
+		 * high- and super-speed devices) per second and add the UVC
+		 * header size (assumed to be 12 bytes long).
 		 */
 		bandwidth = frame->wWidth * frame->wHeight / 8 * format->bpp;
 		bandwidth *= 10000000 / interval + 1;
 		bandwidth /= 1000;
-		if (stream->dev->udev->speed == USB_SPEED_HIGH)
+		if (stream->dev->udev->speed >= USB_SPEED_HIGH)
 			bandwidth /= 8;
 		bandwidth += 12;
 
@@ -297,8 +323,9 @@ static int uvc_get_video_ctrl(struct uvc_streaming *stream,
 		goto out;
 	} else if (ret != size) {
 		dev_err(&stream->intf->dev,
-			"Failed to query (%u) UVC %s control : %d (exp. %u).\n",
-			query, probe ? "probe" : "commit", ret, size);
+			"Failed to query (%s) UVC %s control : %d (exp. %u).\n",
+			uvc_query_name(query), probe ? "probe" : "commit",
+			ret, size);
 		ret = (ret == -EPROTO) ? -EPROTO : -EIO;
 		goto out;
 	}
@@ -466,18 +493,49 @@ static inline ktime_t uvc_video_get_time(void)
 		return ktime_get_real();
 }
 
+static void uvc_video_clock_add_sample(struct uvc_clock *clock,
+				       const struct uvc_clock_sample *sample)
+{
+	unsigned long flags;
+
+	/*
+	 * If we write new data on the position where we had the last
+	 * overflow, remove the overflow pointer. There is no SOF overflow
+	 * in the whole circular buffer.
+	 */
+	if (clock->head == clock->last_sof_overflow)
+		clock->last_sof_overflow = -1;
+
+	spin_lock_irqsave(&clock->lock, flags);
+
+	if (clock->count > 0 && clock->last_sof > sample->dev_sof) {
+		/*
+		 * Remove data from the circular buffer that is older than the
+		 * last SOF overflow. We only support one SOF overflow per
+		 * circular buffer.
+		 */
+		if (clock->last_sof_overflow != -1)
+			clock->count = (clock->head - clock->last_sof_overflow
+					+ clock->size) % clock->size;
+		clock->last_sof_overflow = clock->head;
+	}
+
+	/* Add sample. */
+	clock->samples[clock->head] = *sample;
+	clock->head = (clock->head + 1) % clock->size;
+	clock->count = min(clock->count + 1, clock->size);
+
+	spin_unlock_irqrestore(&clock->lock, flags);
+}
+
 static void
 uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
 		       const u8 *data, int len)
 {
-	struct uvc_clock_sample *sample;
+	struct uvc_clock_sample sample;
 	unsigned int header_size;
 	bool has_pts = false;
 	bool has_scr = false;
-	unsigned long flags;
-	ktime_t time;
-	u16 host_sof;
-	u16 dev_sof;
 
 	switch (data[1] & (UVC_STREAM_PTS | UVC_STREAM_SCR)) {
 	case UVC_STREAM_PTS | UVC_STREAM_SCR:
@@ -522,14 +580,51 @@ uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
 	 * all the data packets of the same frame contains the same SOF. In that
 	 * case only the first one will match the host_sof.
 	 */
-	dev_sof = get_unaligned_le16(&data[header_size - 2]);
-	if (dev_sof == stream->clock.last_sof)
+	sample.dev_sof = get_unaligned_le16(&data[header_size - 2]);
+	if (sample.dev_sof == stream->clock.last_sof)
 		return;
 
-	stream->clock.last_sof = dev_sof;
+	sample.dev_stc = get_unaligned_le32(&data[header_size - 6]);
 
-	host_sof = usb_get_current_frame_number(stream->dev->udev);
-	time = uvc_video_get_time();
+	/*
+	 * STC (Source Time Clock) is the clock used by the camera. The UVC 1.5
+	 * standard states that it "must be captured when the first video data
+	 * of a video frame is put on the USB bus". This is generally understood
+	 * as requiring devices to clear the payload header's SCR bit before
+	 * the first packet containing video data.
+	 *
+	 * Most vendors follow that interpretation, but some (namely SunplusIT
+	 * on some devices) always set the `UVC_STREAM_SCR` bit, fill the SCR
+	 * field with 0's,and expect that the driver only processes the SCR if
+	 * there is data in the packet.
+	 *
+	 * Ignore all the hardware timestamp information if we haven't received
+	 * any data for this frame yet, the packet contains no data, and both
+	 * STC and SOF are zero. This heuristics should be safe on compliant
+	 * devices. This should be safe with compliant devices, as in the very
+	 * unlikely case where a UVC 1.1 device would send timing information
+	 * only before the first packet containing data, and both STC and SOF
+	 * happen to be zero for a particular frame, we would only miss one
+	 * clock sample from many and the clock recovery algorithm wouldn't
+	 * suffer from this condition.
+	 */
+	if (buf && buf->bytesused == 0 && len == header_size &&
+	    sample.dev_stc == 0 && sample.dev_sof == 0)
+		return;
+
+	sample.host_sof = usb_get_current_frame_number(stream->dev->udev);
+
+	/*
+	 * On some devices, like the Logitech C922, the device SOF does not run
+	 * at a stable rate of 1kHz. For those devices use the host SOF instead.
+	 * In the tests performed so far, this improves the timestamp precision.
+	 * This is probably explained by a small packet handling jitter from the
+	 * host, but the exact reason hasn't been fully determined.
+	 */
+	if (stream->dev->quirks & UVC_QUIRK_INVALID_DEVICE_SOF)
+		sample.dev_sof = sample.host_sof;
+
+	sample.host_time = uvc_video_get_time();
 
 	/*
 	 * The UVC specification allows device implementations that can't obtain
@@ -552,46 +647,29 @@ uvc_video_clock_decode(struct uvc_streaming *stream, struct uvc_buffer *buf,
 	 * the 8 LSBs of the delta are kept.
 	 */
 	if (stream->clock.sof_offset == (u16)-1) {
-		u16 delta_sof = (host_sof - dev_sof) & 255;
+		u16 delta_sof = (sample.host_sof - sample.dev_sof) & 255;
 		if (delta_sof >= 10)
 			stream->clock.sof_offset = delta_sof;
 		else
 			stream->clock.sof_offset = 0;
 	}
 
-	dev_sof = (dev_sof + stream->clock.sof_offset) & 2047;
-
-	spin_lock_irqsave(&stream->clock.lock, flags);
-
-	sample = &stream->clock.samples[stream->clock.head];
-	sample->dev_stc = get_unaligned_le32(&data[header_size - 6]);
-	sample->dev_sof = dev_sof;
-	sample->host_sof = host_sof;
-	sample->host_time = time;
-
-	/* Update the sliding window head and count. */
-	stream->clock.head = (stream->clock.head + 1) % stream->clock.size;
-
-	if (stream->clock.count < stream->clock.size)
-		stream->clock.count++;
-
-	spin_unlock_irqrestore(&stream->clock.lock, flags);
+	sample.dev_sof = (sample.dev_sof + stream->clock.sof_offset) & 2047;
+	uvc_video_clock_add_sample(&stream->clock, &sample);
+	stream->clock.last_sof = sample.dev_sof;
 }
 
-static void uvc_video_clock_reset(struct uvc_streaming *stream)
+static void uvc_video_clock_reset(struct uvc_clock *clock)
 {
-	struct uvc_clock *clock = &stream->clock;
-
 	clock->head = 0;
 	clock->count = 0;
 	clock->last_sof = -1;
+	clock->last_sof_overflow = -1;
 	clock->sof_offset = -1;
 }
 
-static int uvc_video_clock_init(struct uvc_streaming *stream)
+static int uvc_video_clock_init(struct uvc_clock *clock)
 {
-	struct uvc_clock *clock = &stream->clock;
-
 	spin_lock_init(&clock->lock);
 	clock->size = 32;
 
@@ -600,15 +678,15 @@ static int uvc_video_clock_init(struct uvc_streaming *stream)
 	if (clock->samples == NULL)
 		return -ENOMEM;
 
-	uvc_video_clock_reset(stream);
+	uvc_video_clock_reset(clock);
 
 	return 0;
 }
 
-static void uvc_video_clock_cleanup(struct uvc_streaming *stream)
+static void uvc_video_clock_cleanup(struct uvc_clock *clock)
 {
-	kfree(stream->clock.samples);
-	stream->clock.samples = NULL;
+	kfree(clock->samples);
+	clock->samples = NULL;
 }
 
 /*
@@ -709,11 +787,11 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 	unsigned long flags;
 	u64 timestamp;
 	u32 delta_stc;
-	u32 y1, y2;
+	u32 y1;
 	u32 x1, x2;
 	u32 mean;
 	u32 sof;
-	u64 y;
+	u64 y, y2;
 
 	if (!uvc_hw_timestamps_param)
 		return;
@@ -728,11 +806,11 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 
 	spin_lock_irqsave(&clock->lock, flags);
 
-	if (clock->count < clock->size)
+	if (clock->count < 2)
 		goto done;
 
-	first = &clock->samples[clock->head];
-	last = &clock->samples[(clock->head - 1) % clock->size];
+	first = &clock->samples[(clock->head - clock->count + clock->size) % clock->size];
+	last = &clock->samples[(clock->head - 1 + clock->size) % clock->size];
 
 	/* First step, PTS to SOF conversion. */
 	delta_stc = buf->pts - (1UL << 31);
@@ -746,6 +824,18 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 	if (y2 < y1)
 		y2 += 2048 << 16;
 
+	/*
+	 * Have at least 1/4 of a second of timestamps before we
+	 * try to do any calculation. Otherwise we do not have enough
+	 * precision. This value was determined by running Android CTS
+	 * on different devices.
+	 *
+	 * dev_sof runs at 1KHz, and we have a fixed point precision of
+	 * 16 bits.
+	 */
+	if ((y2 - y1) < ((1000 / 4) << 16))
+		goto done;
+
 	y = (u64)(y2 - y1) * (1ULL << 31) + (u64)y1 * (u64)x2
 	  - (u64)y2 * (u64)x1;
 	y = div_u64(y, x2 - x1);
@@ -753,7 +843,7 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 	sof = y;
 
 	uvc_dbg(stream->dev, CLOCK,
-		"%s: PTS %u y %llu.%06llu SOF %u.%06llu (x1 %u x2 %u y1 %u y2 %u SOF offset %u)\n",
+		"%s: PTS %u y %llu.%06llu SOF %u.%06llu (x1 %u x2 %u y1 %u y2 %llu SOF offset %u)\n",
 		stream->dev->name, buf->pts,
 		y >> 16, div_u64((y & 0xffff) * 1000000, 65536),
 		sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536),
@@ -768,7 +858,7 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 		goto done;
 
 	y1 = NSEC_PER_SEC;
-	y2 = (u32)ktime_to_ns(ktime_sub(last->host_time, first->host_time)) + y1;
+	y2 = ktime_to_ns(ktime_sub(last->host_time, first->host_time)) + y1;
 
 	/*
 	 * Interpolated and host SOF timestamps can wrap around at slightly
@@ -789,7 +879,7 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 	timestamp = ktime_to_ns(first->host_time) + y - y1;
 
 	uvc_dbg(stream->dev, CLOCK,
-		"%s: SOF %u.%06llu y %llu ts %llu buf ts %llu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %u)\n",
+		"%s: SOF %u.%06llu y %llu ts %llu buf ts %llu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %llu)\n",
 		stream->dev->name,
 		sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536),
 		y, timestamp, vbuf->vb2_buf.timestamp,
@@ -1053,6 +1143,7 @@ static void uvc_video_stats_stop(struct uvc_streaming *stream)
 static int uvc_video_decode_start(struct uvc_streaming *stream,
 		struct uvc_buffer *buf, const u8 *data, int len)
 {
+	u8 header_len;
 	u8 fid;
 
 	/*
@@ -1066,6 +1157,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
 		return -EINVAL;
 	}
 
+	header_len = data[0];
 	fid = data[1] & UVC_STREAM_FID;
 
 	/*
@@ -1147,9 +1239,31 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
 		return -EAGAIN;
 	}
 
+	/*
+	 * Some cameras, when running two parallel streams (one MJPEG alongside
+	 * another non-MJPEG stream), are known to lose the EOF packet for a frame.
+	 * We can detect the end of a frame by checking for a new SOI marker, as
+	 * the SOI always lies on the packet boundary between two frames for
+	 * these devices.
+	 */
+	if (stream->dev->quirks & UVC_QUIRK_MJPEG_NO_EOF &&
+	    (stream->cur_format->fcc == V4L2_PIX_FMT_MJPEG ||
+	    stream->cur_format->fcc == V4L2_PIX_FMT_JPEG)) {
+		const u8 *packet = data + header_len;
+
+		if (len >= header_len + 2 &&
+		    packet[0] == 0xff && packet[1] == JPEG_MARKER_SOI &&
+		    buf->bytesused != 0) {
+			buf->state = UVC_BUF_STATE_READY;
+			buf->error = 1;
+			stream->last_fid ^= UVC_STREAM_FID;
+			return -EAGAIN;
+		}
+	}
+
 	stream->last_fid = fid;
 
-	return data[0];
+	return header_len;
 }
 
 static inline enum dma_data_direction uvc_stream_dir(
@@ -2071,7 +2185,7 @@ int uvc_video_resume(struct uvc_streaming *stream, int reset)
 
 	stream->frozen = 0;
 
-	uvc_video_clock_reset(stream);
+	uvc_video_clock_reset(&stream->clock);
 
 	if (!uvc_queue_streaming(&stream->queue))
 		return 0;
@@ -2220,7 +2334,7 @@ int uvc_video_start_streaming(struct uvc_streaming *stream)
 {
 	int ret;
 
-	ret = uvc_video_clock_init(stream);
+	ret = uvc_video_clock_init(&stream->clock);
 	if (ret < 0)
 		return ret;
 
@@ -2238,7 +2352,7 @@ int uvc_video_start_streaming(struct uvc_streaming *stream)
 error_video:
 	usb_set_interface(stream->dev->udev, stream->intfnum, 0);
 error_commit:
-	uvc_video_clock_cleanup(stream);
+	uvc_video_clock_cleanup(&stream->clock);
 
 	return ret;
 }
@@ -2266,5 +2380,5 @@ void uvc_video_stop_streaming(struct uvc_streaming *stream)
 		usb_clear_halt(stream->dev->udev, pipe);
 	}
 
-	uvc_video_clock_cleanup(stream);
+	uvc_video_clock_cleanup(&stream->clock);
 }
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index 6fb0a78b1b00..b9f8eb62ba1d 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -73,6 +73,10 @@
 #define UVC_QUIRK_FORCE_Y8		0x00000800
 #define UVC_QUIRK_FORCE_BPP		0x00001000
 #define UVC_QUIRK_WAKE_AUTOSUSPEND	0x00002000
+#define UVC_QUIRK_NO_RESET_RESUME	0x00004000
+#define UVC_QUIRK_DISABLE_AUTOSUSPEND	0x00008000
+#define UVC_QUIRK_INVALID_DEVICE_SOF	0x00010000
+#define UVC_QUIRK_MJPEG_NO_EOF		0x00020000
 
 /* Format flags */
 #define UVC_FMT_FLAG_COMPRESSED		0x00000001
@@ -84,7 +88,9 @@
 
 struct gpio_desc;
 struct sg_table;
+struct uvc_control;
 struct uvc_device;
+struct uvc_video_chain;
 
 /*
  * TODO: Put the most frequently accessed fields at the beginning of
@@ -110,7 +116,12 @@ struct uvc_control_mapping {
 	u8 entity[16];
 	u8 selector;
 
+	/*
+	 * Size of the control data in the payload of the UVC control GET and
+	 * SET requests, expressed in bits.
+	 */
 	u8 size;
+
 	u8 offset;
 	enum v4l2_ctrl_type v4l2_type;
 	u32 data_type;
@@ -123,10 +134,13 @@ struct uvc_control_mapping {
 	s32 master_manual;
 	u32 slave_ids[2];
 
-	s32 (*get)(struct uvc_control_mapping *mapping, u8 query,
-		   const u8 *data);
-	void (*set)(struct uvc_control_mapping *mapping, s32 value,
-		    u8 *data);
+	const struct uvc_control_mapping *(*filter_mapping)
+				(struct uvc_video_chain *chain,
+				struct uvc_control *ctrl);
+	int (*get)(struct uvc_control_mapping *mapping, u8 query,
+		   const void *uvc_in, size_t v4l2_size, void *v4l2_out);
+	int (*set)(struct uvc_control_mapping *mapping, size_t v4l2_size,
+		   const void *v4l2_in, void *uvc_out);
 };
 
 struct uvc_control {
@@ -226,6 +240,7 @@ struct uvc_entity {
 			u8  *bmControls;
 			struct gpio_desc *gpio_privacy;
 			int irq;
+			bool initialized;
 		} gpio;
 	};
 
@@ -308,7 +323,6 @@ struct uvc_buffer {
 };
 
 #define UVC_QUEUE_DISCONNECTED		(1 << 0)
-#define UVC_QUEUE_DROP_CORRUPTED	(1 << 1)
 
 struct uvc_video_queue {
 	struct vb2_queue queue;
@@ -329,7 +343,11 @@ struct uvc_video_chain {
 	struct uvc_entity *processing;		/* Processing unit */
 	struct uvc_entity *selector;		/* Selector unit */
 
-	struct mutex ctrl_mutex;		/* Protects ctrl.info */
+	struct mutex ctrl_mutex;		/*
+						 * Protects ctrl.info,
+						 * ctrl.handle and
+						 * uvc_fh.pending_async_ctrls
+						 */
 
 	struct v4l2_prio_state prio;		/* V4L2 priority state */
 	u32 caps;				/* V4L2 chain-wide caps */
@@ -498,6 +516,7 @@ struct uvc_streaming {
 		unsigned int head;
 		unsigned int count;
 		unsigned int size;
+		unsigned int last_sof_overflow;
 
 		u16 last_sof;
 		u16 sof_offset;
@@ -522,9 +541,15 @@ struct uvc_device_info {
 	u32	quirks;
 	u32	meta_format;
 	u16	uvc_version;
-	const struct uvc_control_mapping **mappings;
 };
 
+struct uvc_rect {
+	u16 top;
+	u16 left;
+	u16 bottom;
+	u16 right;
+} __packed;
+
 struct uvc_status_streaming {
 	u8	button;
 } __packed;
@@ -555,8 +580,6 @@ struct uvc_device {
 
 	const struct uvc_device_info *info;
 
-	struct mutex lock;		/* Protects users */
-	unsigned int users;
 	atomic_t nmappings;
 
 	/* Video control interface */
@@ -578,6 +601,8 @@ struct uvc_device {
 	struct usb_host_endpoint *int_ep;
 	struct urb *int_urb;
 	struct uvc_status *status;
+	struct mutex status_lock; /* Protects status_users */
+	unsigned int status_users;
 	bool flush_status;
 
 	struct input_dev *input;
@@ -604,10 +629,8 @@ struct uvc_fh {
 	struct uvc_video_chain *chain;
 	struct uvc_streaming *stream;
 	enum uvc_handle_state state;
-};
-
-struct uvc_driver {
-	struct usb_driver driver;
+	unsigned int pending_async_ctrls;
+	bool is_streaming;
 };
 
 /* ------------------------------------------------------------------------
@@ -660,14 +683,10 @@ do {									\
  * Internal functions.
  */
 
-/* Core driver */
-extern struct uvc_driver uvc_driver;
-
 struct uvc_entity *uvc_entity_by_id(struct uvc_device *dev, int id);
 
 /* Video buffers queue management. */
-int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type,
-		   int drop_corrupted);
+int uvc_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type);
 void uvc_queue_release(struct uvc_video_queue *queue);
 int uvc_request_buffers(struct uvc_video_queue *queue,
 			struct v4l2_requestbuffers *rb);
@@ -744,16 +763,20 @@ int uvc_register_video_device(struct uvc_device *dev,
 int uvc_status_init(struct uvc_device *dev);
 void uvc_status_unregister(struct uvc_device *dev);
 void uvc_status_cleanup(struct uvc_device *dev);
-int uvc_status_start(struct uvc_device *dev, gfp_t flags);
-void uvc_status_stop(struct uvc_device *dev);
+int uvc_status_resume(struct uvc_device *dev);
+void uvc_status_suspend(struct uvc_device *dev);
+int uvc_status_get(struct uvc_device *dev);
+void uvc_status_put(struct uvc_device *dev);
+
+/* PM */
+int uvc_pm_get(struct uvc_device *dev);
+void uvc_pm_put(struct uvc_device *dev);
 
 /* Controls */
-extern const struct uvc_control_mapping uvc_ctrl_power_line_mapping_limited;
-extern const struct uvc_control_mapping uvc_ctrl_power_line_mapping_uvc11;
 extern const struct v4l2_subscribed_event_ops uvc_ctrl_sub_ev_ops;
 
 int uvc_query_v4l2_ctrl(struct uvc_video_chain *chain,
-			struct v4l2_queryctrl *v4l2_ctrl);
+			struct v4l2_query_ext_ctrl *v4l2_ctrl);
 int uvc_query_v4l2_menu(struct uvc_video_chain *chain,
 			struct v4l2_querymenu *query_menu);
 
@@ -780,7 +803,8 @@ static inline int uvc_ctrl_rollback(struct uvc_fh *handle)
 	return __uvc_ctrl_commit(handle, 1, NULL);
 }
 
-int uvc_ctrl_get(struct uvc_video_chain *chain, struct v4l2_ext_control *xctrl);
+int uvc_ctrl_get(struct uvc_video_chain *chain, u32 which,
+		 struct v4l2_ext_control *xctrl);
 int uvc_ctrl_set(struct uvc_fh *handle, struct v4l2_ext_control *xctrl);
 int uvc_ctrl_is_accessible(struct uvc_video_chain *chain, u32 v4l2_id,
 			   const struct v4l2_ext_controls *ctrls,
@@ -789,6 +813,8 @@ int uvc_ctrl_is_accessible(struct uvc_video_chain *chain, u32 v4l2_id,
 int uvc_xu_ctrl_query(struct uvc_video_chain *chain,
 		      struct uvc_xu_control_query *xqry);
 
+void uvc_ctrl_cleanup_fh(struct uvc_fh *handle);
+
 /* Utility functions */
 struct usb_host_endpoint *uvc_find_endpoint(struct usb_host_interface *alts,
 					    u8 epaddr);
diff --git a/drivers/media/v4l2-core/v4l2-async.c b/drivers/media/v4l2-core/v4l2-async.c
index 3ec323bd528b..ee884a8221fb 100644
--- a/drivers/media/v4l2-core/v4l2-async.c
+++ b/drivers/media/v4l2-core/v4l2-async.c
@@ -316,19 +316,26 @@ v4l2_async_nf_try_all_subdevs(struct v4l2_async_notifier *notifier);
 static int v4l2_async_create_ancillary_links(struct v4l2_async_notifier *n,
 					     struct v4l2_subdev *sd)
 {
-	struct media_link *link = NULL;
-
 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
+	struct media_link *link;
 
 	if (sd->entity.function != MEDIA_ENT_F_LENS &&
 	    sd->entity.function != MEDIA_ENT_F_FLASH)
 		return 0;
 
-	link = media_create_ancillary_link(&n->sd->entity, &sd->entity);
+	if (!n->sd) {
+		dev_warn(notifier_dev(n),
+			 "not a sub-device notifier, not creating an ancillary link for %s!\n",
+			 dev_name(sd->dev));
+		return 0;
+	}
 
-#endif
+	link = media_create_ancillary_link(&n->sd->entity, &sd->entity);
 
 	return IS_ERR(link) ? PTR_ERR(link) : 0;
+#else
+	return 0;
+#endif
 }
 
 static int v4l2_async_match_notify(struct v4l2_async_notifier *notifier,
@@ -341,7 +348,7 @@ static int v4l2_async_match_notify(struct v4l2_async_notifier *notifier,
 	int ret;
 
 	if (list_empty(&sd->asc_list)) {
-		ret = v4l2_device_register_subdev(v4l2_dev, sd);
+		ret = __v4l2_device_register_subdev(v4l2_dev, sd, sd->owner);
 		if (ret < 0)
 			return ret;
 		registered = true;
@@ -563,6 +570,7 @@ void v4l2_async_nf_init(struct v4l2_async_notifier *notifier,
 {
 	INIT_LIST_HEAD(&notifier->waiting_list);
 	INIT_LIST_HEAD(&notifier->done_list);
+	INIT_LIST_HEAD(&notifier->notifier_entry);
 	notifier->v4l2_dev = v4l2_dev;
 }
 EXPORT_SYMBOL(v4l2_async_nf_init);
@@ -572,6 +580,7 @@ void v4l2_async_subdev_nf_init(struct v4l2_async_notifier *notifier,
 {
 	INIT_LIST_HEAD(&notifier->waiting_list);
 	INIT_LIST_HEAD(&notifier->done_list);
+	INIT_LIST_HEAD(&notifier->notifier_entry);
 	notifier->sd = sd;
 }
 EXPORT_SYMBOL_GPL(v4l2_async_subdev_nf_init);
@@ -618,16 +627,10 @@ err_unlock:
 
 int v4l2_async_nf_register(struct v4l2_async_notifier *notifier)
 {
-	int ret;
-
 	if (WARN_ON(!notifier->v4l2_dev == !notifier->sd))
 		return -EINVAL;
 
-	ret = __v4l2_async_nf_register(notifier);
-	if (ret)
-		notifier->v4l2_dev = NULL;
-
-	return ret;
+	return __v4l2_async_nf_register(notifier);
 }
 EXPORT_SYMBOL(v4l2_async_nf_register);
 
@@ -639,7 +642,7 @@ __v4l2_async_nf_unregister(struct v4l2_async_notifier *notifier)
 
 	v4l2_async_nf_unbind_all_subdevs(notifier);
 
-	list_del(&notifier->notifier_entry);
+	list_del_init(&notifier->notifier_entry);
 }
 
 void v4l2_async_nf_unregister(struct v4l2_async_notifier *notifier)
@@ -787,7 +790,7 @@ v4l2_async_connection_unique(struct v4l2_subdev *sd)
 }
 EXPORT_SYMBOL_GPL(v4l2_async_connection_unique);
 
-int v4l2_async_register_subdev(struct v4l2_subdev *sd)
+int __v4l2_async_register_subdev(struct v4l2_subdev *sd, struct module *module)
 {
 	struct v4l2_async_notifier *subdev_notifier;
 	struct v4l2_async_notifier *notifier;
@@ -811,6 +814,8 @@ int v4l2_async_register_subdev(struct v4l2_subdev *sd)
 		return -EINVAL;
 	}
 
+	sd->owner = module;
+
 	mutex_lock(&list_lock);
 
 	list_for_each_entry(notifier, &notifier_list, notifier_entry) {
@@ -853,9 +858,11 @@ err_unbind:
 
 	mutex_unlock(&list_lock);
 
+	sd->owner = NULL;
+
 	return ret;
 }
-EXPORT_SYMBOL(v4l2_async_register_subdev);
+EXPORT_SYMBOL(__v4l2_async_register_subdev);
 
 void v4l2_async_unregister_subdev(struct v4l2_subdev *sd)
 {
@@ -965,4 +972,5 @@ module_exit(v4l2_async_exit);
 MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
 MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
 MODULE_AUTHOR("Ezequiel Garcia <ezequiel@collabora.com>");
+MODULE_DESCRIPTION("V4L2 asynchronous subdevice registration API");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/v4l2-core/v4l2-cci.c b/drivers/media/v4l2-core/v4l2-cci.c
index ee3475bed37f..e9ecf4785946 100644
--- a/drivers/media/v4l2-core/v4l2-cci.c
+++ b/drivers/media/v4l2-core/v4l2-cci.c
@@ -12,7 +12,7 @@
 #include <linux/regmap.h>
 #include <linux/types.h>
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 
 #include <media/v4l2-cci.h>
 
@@ -23,6 +23,15 @@ int cci_read(struct regmap *map, u32 reg, u64 *val, int *err)
 	u8 buf[8];
 	int ret;
 
+	/*
+	 * TODO: Fix smatch. Assign *val to 0 here in order to avoid
+	 * failing a smatch check on caller when the caller proceeds to
+	 * read *val without initialising it on caller's side. *val is set
+	 * to a valid value whenever this function returns 0 but smatch
+	 * can't figure that out currently.
+	 */
+	*val = 0;
+
 	if (err && *err)
 		return *err;
 
diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index d34d210908d9..bd160a8c9efe 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -154,13 +154,18 @@ void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
 EXPORT_SYMBOL_GPL(v4l_bound_align_image);
 
 const void *
-__v4l2_find_nearest_size(const void *array, size_t array_size,
-			 size_t entry_size, size_t width_offset,
-			 size_t height_offset, s32 width, s32 height)
+__v4l2_find_nearest_size_conditional(const void *array, size_t array_size,
+				     size_t entry_size, size_t width_offset,
+				     size_t height_offset, s32 width,
+				     s32 height,
+				     bool (*func)(const void *array,
+						  size_t index,
+						  const void *context),
+				     const void *context)
 {
 	u32 error, min_error = U32_MAX;
 	const void *best = NULL;
-	unsigned int i;
+	size_t i;
 
 	if (!array)
 		return NULL;
@@ -169,6 +174,9 @@ __v4l2_find_nearest_size(const void *array, size_t array_size,
 		const u32 *entry_width = array + width_offset;
 		const u32 *entry_height = array + height_offset;
 
+		if (func && !func(array, i, context))
+			continue;
+
 		error = abs(*entry_width - width) + abs(*entry_height - height);
 		if (error > min_error)
 			continue;
@@ -181,7 +189,7 @@ __v4l2_find_nearest_size(const void *array, size_t array_size,
 
 	return best;
 }
-EXPORT_SYMBOL_GPL(__v4l2_find_nearest_size);
+EXPORT_SYMBOL_GPL(__v4l2_find_nearest_size_conditional);
 
 int v4l2_g_parm_cap(struct video_device *vdev,
 		    struct v4l2_subdev *sd, struct v4l2_streamparm *a)
@@ -250,9 +258,12 @@ const struct v4l2_format_info *v4l2_format_info(u32 format)
 		{ .format = V4L2_PIX_FMT_ABGR32,  .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_BGRA32,  .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_RGB565,  .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_RGB565X, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_RGB555,  .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_BGR666,  .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_BGR48_12, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 6, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_BGR48, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 6, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_RGB48, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 6, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_ABGR64_12, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 8, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_RGBA1010102, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_RGBX1010102, .pixel_enc = V4L2_PIXEL_ENC_RGB, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
@@ -263,7 +274,9 @@ const struct v4l2_format_info *v4l2_format_info(u32 format)
 		{ .format = V4L2_PIX_FMT_YVYU,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_UYVY,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_VYUY,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 2, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_Y210,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_Y212,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_Y216,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 4, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_YUV48_12, .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 1, .bpp = { 6, 0, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_MT2110T, .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 2, .comp_planes = 2, .bpp = { 5, 10, 0, 0 }, .bpp_div = { 4, 4, 1, 1 }, .hdiv = 2, .vdiv = 2,
 		  .block_w = { 16, 8, 0, 0 }, .block_h = { 32, 16, 0, 0 }},
@@ -273,8 +286,10 @@ const struct v4l2_format_info *v4l2_format_info(u32 format)
 		/* YUV planar formats */
 		{ .format = V4L2_PIX_FMT_NV12,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 2 },
 		{ .format = V4L2_PIX_FMT_NV21,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 2 },
+		{ .format = V4L2_PIX_FMT_NV15,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 5, 10, 0, 0 }, .bpp_div = { 4, 4, 1, 1 }, .hdiv = 2, .vdiv = 2 },
 		{ .format = V4L2_PIX_FMT_NV16,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_NV61,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
+		{ .format = V4L2_PIX_FMT_NV20,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 5, 10, 0, 0 }, .bpp_div = { 4, 4, 1, 1 }, .hdiv = 2, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_NV24,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_NV42,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 1, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 1, .vdiv = 1 },
 		{ .format = V4L2_PIX_FMT_P010,    .pixel_enc = V4L2_PIXEL_ENC_YUV, .mem_planes = 1, .comp_planes = 2, .bpp = { 2, 2, 0, 0 }, .bpp_div = { 1, 1, 1, 1 }, .hdiv = 2, .vdiv = 1 },
@@ -353,6 +368,34 @@ static inline unsigned int v4l2_format_block_height(const struct v4l2_format_inf
 	return info->block_h[plane];
 }
 
+static inline unsigned int v4l2_format_plane_stride(const struct v4l2_format_info *info, int plane,
+						    unsigned int width)
+{
+	unsigned int hdiv = plane ? info->hdiv : 1;
+	unsigned int aligned_width =
+		ALIGN(width, v4l2_format_block_width(info, plane));
+
+	return DIV_ROUND_UP(aligned_width, hdiv) *
+	       info->bpp[plane] / info->bpp_div[plane];
+}
+
+static inline unsigned int v4l2_format_plane_height(const struct v4l2_format_info *info, int plane,
+						    unsigned int height)
+{
+	unsigned int vdiv = plane ? info->vdiv : 1;
+	unsigned int aligned_height =
+		ALIGN(height, v4l2_format_block_height(info, plane));
+
+	return DIV_ROUND_UP(aligned_height, vdiv);
+}
+
+static inline unsigned int v4l2_format_plane_size(const struct v4l2_format_info *info, int plane,
+						  unsigned int width, unsigned int height)
+{
+	return v4l2_format_plane_stride(info, plane, width) *
+	       v4l2_format_plane_height(info, plane, height);
+}
+
 void v4l2_apply_frmsize_constraints(u32 *width, u32 *height,
 				    const struct v4l2_frmsize_stepwise *frmsize)
 {
@@ -388,37 +431,19 @@ int v4l2_fill_pixfmt_mp(struct v4l2_pix_format_mplane *pixfmt,
 
 	if (info->mem_planes == 1) {
 		plane = &pixfmt->plane_fmt[0];
-		plane->bytesperline = ALIGN(width, v4l2_format_block_width(info, 0)) * info->bpp[0] / info->bpp_div[0];
+		plane->bytesperline = v4l2_format_plane_stride(info, 0, width);
 		plane->sizeimage = 0;
 
-		for (i = 0; i < info->comp_planes; i++) {
-			unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
-			unsigned int vdiv = (i == 0) ? 1 : info->vdiv;
-			unsigned int aligned_width;
-			unsigned int aligned_height;
-
-			aligned_width = ALIGN(width, v4l2_format_block_width(info, i));
-			aligned_height = ALIGN(height, v4l2_format_block_height(info, i));
-
-			plane->sizeimage += info->bpp[i] *
-				DIV_ROUND_UP(aligned_width, hdiv) *
-				DIV_ROUND_UP(aligned_height, vdiv) / info->bpp_div[i];
-		}
+		for (i = 0; i < info->comp_planes; i++)
+			plane->sizeimage +=
+				v4l2_format_plane_size(info, i, width, height);
 	} else {
 		for (i = 0; i < info->comp_planes; i++) {
-			unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
-			unsigned int vdiv = (i == 0) ? 1 : info->vdiv;
-			unsigned int aligned_width;
-			unsigned int aligned_height;
-
-			aligned_width = ALIGN(width, v4l2_format_block_width(info, i));
-			aligned_height = ALIGN(height, v4l2_format_block_height(info, i));
-
 			plane = &pixfmt->plane_fmt[i];
 			plane->bytesperline =
-				info->bpp[i] * DIV_ROUND_UP(aligned_width, hdiv) / info->bpp_div[i];
-			plane->sizeimage =
-				plane->bytesperline * DIV_ROUND_UP(aligned_height, vdiv);
+				v4l2_format_plane_stride(info, i, width);
+			plane->sizeimage = plane->bytesperline *
+				v4l2_format_plane_height(info, i, height);
 		}
 	}
 	return 0;
@@ -442,28 +467,18 @@ int v4l2_fill_pixfmt(struct v4l2_pix_format *pixfmt, u32 pixelformat,
 	pixfmt->width = width;
 	pixfmt->height = height;
 	pixfmt->pixelformat = pixelformat;
-	pixfmt->bytesperline = ALIGN(width, v4l2_format_block_width(info, 0)) * info->bpp[0] / info->bpp_div[0];
+	pixfmt->bytesperline = v4l2_format_plane_stride(info, 0, width);
 	pixfmt->sizeimage = 0;
 
-	for (i = 0; i < info->comp_planes; i++) {
-		unsigned int hdiv = (i == 0) ? 1 : info->hdiv;
-		unsigned int vdiv = (i == 0) ? 1 : info->vdiv;
-		unsigned int aligned_width;
-		unsigned int aligned_height;
-
-		aligned_width = ALIGN(width, v4l2_format_block_width(info, i));
-		aligned_height = ALIGN(height, v4l2_format_block_height(info, i));
-
-		pixfmt->sizeimage += info->bpp[i] *
-			DIV_ROUND_UP(aligned_width, hdiv) *
-			DIV_ROUND_UP(aligned_height, vdiv) / info->bpp_div[i];
-	}
+	for (i = 0; i < info->comp_planes; i++)
+		pixfmt->sizeimage +=
+			v4l2_format_plane_size(info, i, width, height);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_fill_pixfmt);
 
-s64 v4l2_get_link_freq(struct v4l2_ctrl_handler *handler, unsigned int mul,
-		       unsigned int div)
+s64 __v4l2_get_link_freq_ctrl(struct v4l2_ctrl_handler *handler,
+			      unsigned int mul, unsigned int div)
 {
 	struct v4l2_ctrl *ctrl;
 	s64 freq;
@@ -498,7 +513,33 @@ s64 v4l2_get_link_freq(struct v4l2_ctrl_handler *handler, unsigned int mul,
 
 	return freq > 0 ? freq : -EINVAL;
 }
-EXPORT_SYMBOL_GPL(v4l2_get_link_freq);
+EXPORT_SYMBOL_GPL(__v4l2_get_link_freq_ctrl);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+s64 __v4l2_get_link_freq_pad(struct media_pad *pad, unsigned int mul,
+			     unsigned int div)
+{
+	struct v4l2_mbus_config mbus_config = {};
+	struct v4l2_subdev *sd;
+	int ret;
+
+	sd = media_entity_to_v4l2_subdev(pad->entity);
+	ret = v4l2_subdev_call(sd, pad, get_mbus_config, pad->index,
+			       &mbus_config);
+	if (ret < 0 && ret != -ENOIOCTLCMD)
+		return ret;
+
+	if (mbus_config.link_freq)
+		return mbus_config.link_freq;
+
+	/*
+	 * Fall back to using the link frequency control if the media bus config
+	 * doesn't provide a link frequency.
+	 */
+	return __v4l2_get_link_freq_ctrl(sd->ctrl_handler, mul, div);
+}
+EXPORT_SYMBOL_GPL(__v4l2_get_link_freq_pad);
+#endif /* CONFIG_MEDIA_CONTROLLER */
 
 /*
  * Simplify a fraction using a simple continued fraction decomposition. The
diff --git a/drivers/media/v4l2-core/v4l2-ctrls-api.c b/drivers/media/v4l2-core/v4l2-ctrls-api.c
index d9a422017bd9..d49a68b36c28 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls-api.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls-api.c
@@ -94,6 +94,22 @@ static int def_to_user(struct v4l2_ext_control *c, struct v4l2_ctrl *ctrl)
 	return ptr_to_user(c, ctrl, ctrl->p_new);
 }
 
+/* Helper function: copy the minimum control value back to the caller */
+static int min_to_user(struct v4l2_ext_control *c, struct v4l2_ctrl *ctrl)
+{
+	ctrl->type_ops->minimum(ctrl, 0, ctrl->p_new);
+
+	return ptr_to_user(c, ctrl, ctrl->p_new);
+}
+
+/* Helper function: copy the maximum control value back to the caller */
+static int max_to_user(struct v4l2_ext_control *c, struct v4l2_ctrl *ctrl)
+{
+	ctrl->type_ops->maximum(ctrl, 0, ctrl->p_new);
+
+	return ptr_to_user(c, ctrl, ctrl->p_new);
+}
+
 /* Helper function: copy the caller-provider value as the new control value */
 static int user_to_new(struct v4l2_ext_control *c, struct v4l2_ctrl *ctrl)
 {
@@ -229,8 +245,8 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 		cs->error_idx = i;
 
 		if (cs->which &&
-		    cs->which != V4L2_CTRL_WHICH_DEF_VAL &&
-		    cs->which != V4L2_CTRL_WHICH_REQUEST_VAL &&
+		    (cs->which < V4L2_CTRL_WHICH_DEF_VAL ||
+		     cs->which > V4L2_CTRL_WHICH_MAX_VAL) &&
 		    V4L2_CTRL_ID2WHICH(id) != cs->which) {
 			dprintk(vdev,
 				"invalid which 0x%x or control id 0x%x\n",
@@ -259,6 +275,15 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 			return -EINVAL;
 		}
 
+		if (!(ctrl->flags & V4L2_CTRL_FLAG_HAS_WHICH_MIN_MAX) &&
+		    (cs->which == V4L2_CTRL_WHICH_MIN_VAL ||
+		     cs->which == V4L2_CTRL_WHICH_MAX_VAL)) {
+			dprintk(vdev,
+				"invalid which 0x%x or control id 0x%x\n",
+				cs->which, id);
+			return -EINVAL;
+		}
+
 		if (ctrl->cluster[0]->ncontrols > 1)
 			have_clusters = true;
 		if (ctrl->cluster[0] != ctrl)
@@ -368,8 +393,8 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
  */
 static int class_check(struct v4l2_ctrl_handler *hdl, u32 which)
 {
-	if (which == 0 || which == V4L2_CTRL_WHICH_DEF_VAL ||
-	    which == V4L2_CTRL_WHICH_REQUEST_VAL)
+	if (which == 0 || (which >= V4L2_CTRL_WHICH_DEF_VAL &&
+			   which <= V4L2_CTRL_WHICH_MAX_VAL))
 		return 0;
 	return find_ref_lock(hdl, which | 1) ? 0 : -EINVAL;
 }
@@ -389,10 +414,12 @@ int v4l2_g_ext_ctrls_common(struct v4l2_ctrl_handler *hdl,
 	struct v4l2_ctrl_helper *helpers = helper;
 	int ret;
 	int i, j;
-	bool is_default, is_request;
+	bool is_default, is_request, is_min, is_max;
 
 	is_default = (cs->which == V4L2_CTRL_WHICH_DEF_VAL);
 	is_request = (cs->which == V4L2_CTRL_WHICH_REQUEST_VAL);
+	is_min = (cs->which == V4L2_CTRL_WHICH_MIN_VAL);
+	is_max = (cs->which == V4L2_CTRL_WHICH_MAX_VAL);
 
 	cs->error_idx = cs->count;
 	cs->which = V4L2_CTRL_ID2WHICH(cs->which);
@@ -432,13 +459,14 @@ int v4l2_g_ext_ctrls_common(struct v4l2_ctrl_handler *hdl,
 
 		/*
 		 * g_volatile_ctrl will update the new control values.
-		 * This makes no sense for V4L2_CTRL_WHICH_DEF_VAL and
+		 * This makes no sense for V4L2_CTRL_WHICH_DEF_VAL,
+		 * V4L2_CTRL_WHICH_MIN_VAL, V4L2_CTRL_WHICH_MAX_VAL and
 		 * V4L2_CTRL_WHICH_REQUEST_VAL. In the case of requests
 		 * it is v4l2_ctrl_request_complete() that copies the
 		 * volatile controls at the time of request completion
 		 * to the request, so you don't want to do that again.
 		 */
-		if (!is_default && !is_request &&
+		if (!is_default && !is_request && !is_min && !is_max &&
 		    ((master->flags & V4L2_CTRL_FLAG_VOLATILE) ||
 		    (master->has_volatiles && !is_cur_manual(master)))) {
 			for (j = 0; j < master->ncontrols; j++)
@@ -467,6 +495,10 @@ int v4l2_g_ext_ctrls_common(struct v4l2_ctrl_handler *hdl,
 				ret = -ENOMEM;
 			else if (is_request && ref->p_req_valid)
 				ret = req_to_user(cs->controls + idx, ref);
+			else if (is_min)
+				ret = min_to_user(cs->controls + idx, ref->ctrl);
+			else if (is_max)
+				ret = max_to_user(cs->controls + idx, ref->ctrl);
 			else if (is_volatile)
 				ret = new_to_user(cs->controls + idx, ref->ctrl);
 			else
@@ -564,9 +596,11 @@ int try_set_ext_ctrls_common(struct v4l2_fh *fh,
 
 	cs->error_idx = cs->count;
 
-	/* Default value cannot be changed */
-	if (cs->which == V4L2_CTRL_WHICH_DEF_VAL) {
-		dprintk(vdev, "%s: cannot change default value\n",
+	/* Default/minimum/maximum values cannot be changed */
+	if (cs->which == V4L2_CTRL_WHICH_DEF_VAL ||
+	    cs->which == V4L2_CTRL_WHICH_MIN_VAL ||
+	    cs->which == V4L2_CTRL_WHICH_MAX_VAL) {
+		dprintk(vdev, "%s: cannot change default/min/max value\n",
 			video_device_node_name(vdev));
 		return -EINVAL;
 	}
@@ -753,9 +787,10 @@ static int get_ctrl(struct v4l2_ctrl *ctrl, struct v4l2_ext_control *c)
 		for (i = 0; i < master->ncontrols; i++)
 			cur_to_new(master->cluster[i]);
 		ret = call_op(master, g_volatile_ctrl);
-		new_to_user(c, ctrl);
+		if (!ret)
+			ret = new_to_user(c, ctrl);
 	} else {
-		cur_to_user(c, ctrl);
+		ret = cur_to_user(c, ctrl);
 	}
 	v4l2_ctrl_unlock(master);
 	return ret;
@@ -770,7 +805,10 @@ int v4l2_g_ctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_control *control)
 	if (!ctrl || !ctrl->is_int)
 		return -EINVAL;
 	ret = get_ctrl(ctrl, &c);
-	control->value = c.value;
+
+	if (!ret)
+		control->value = c.value;
+
 	return ret;
 }
 EXPORT_SYMBOL(v4l2_g_ctrl);
@@ -811,10 +849,11 @@ static int set_ctrl_lock(struct v4l2_fh *fh, struct v4l2_ctrl *ctrl,
 	int ret;
 
 	v4l2_ctrl_lock(ctrl);
-	user_to_new(c, ctrl);
-	ret = set_ctrl(fh, ctrl, 0);
+	ret = user_to_new(c, ctrl);
 	if (!ret)
-		cur_to_user(c, ctrl);
+		ret = set_ctrl(fh, ctrl, 0);
+	if (!ret)
+		ret = cur_to_user(c, ctrl);
 	v4l2_ctrl_unlock(ctrl);
 	return ret;
 }
@@ -1052,35 +1091,40 @@ int v4l2_query_ext_ctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_query_ext_ctr
 		if (id >= node2id(hdl->ctrl_refs.prev)) {
 			ref = NULL; /* Yes, so there is no next control */
 		} else if (ref) {
+			struct v4l2_ctrl_ref *pos = ref;
+
 			/*
 			 * We found a control with the given ID, so just get
 			 * the next valid one in the list.
 			 */
-			list_for_each_entry_continue(ref, &hdl->ctrl_refs, node) {
-				is_compound = ref->ctrl->is_array ||
-					ref->ctrl->type >= V4L2_CTRL_COMPOUND_TYPES;
-				if (id < ref->ctrl->id &&
-				    (is_compound & mask) == match)
+			ref = NULL;
+			list_for_each_entry_continue(pos, &hdl->ctrl_refs, node) {
+				is_compound = pos->ctrl->is_array ||
+					pos->ctrl->type >= V4L2_CTRL_COMPOUND_TYPES;
+				if (id < pos->ctrl->id &&
+				    (is_compound & mask) == match) {
+					ref = pos;
 					break;
+				}
 			}
-			if (&ref->node == &hdl->ctrl_refs)
-				ref = NULL;
 		} else {
+			struct v4l2_ctrl_ref *pos;
+
 			/*
 			 * No control with the given ID exists, so start
 			 * searching for the next largest ID. We know there
 			 * is one, otherwise the first 'if' above would have
 			 * been true.
 			 */
-			list_for_each_entry(ref, &hdl->ctrl_refs, node) {
-				is_compound = ref->ctrl->is_array ||
-					ref->ctrl->type >= V4L2_CTRL_COMPOUND_TYPES;
-				if (id < ref->ctrl->id &&
-				    (is_compound & mask) == match)
+			list_for_each_entry(pos, &hdl->ctrl_refs, node) {
+				is_compound = pos->ctrl->is_array ||
+					pos->ctrl->type >= V4L2_CTRL_COMPOUND_TYPES;
+				if (id < pos->ctrl->id &&
+				    (is_compound & mask) == match) {
+					ref = pos;
 					break;
+				}
 			}
-			if (&ref->node == &hdl->ctrl_refs)
-				ref = NULL;
 		}
 	}
 	mutex_unlock(hdl->lock);
@@ -1113,39 +1157,48 @@ int v4l2_query_ext_ctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_query_ext_ctr
 }
 EXPORT_SYMBOL(v4l2_query_ext_ctrl);
 
-/* Implement VIDIOC_QUERYCTRL */
-int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
+void v4l2_query_ext_ctrl_to_v4l2_queryctrl(struct v4l2_queryctrl *to,
+					   const struct v4l2_query_ext_ctrl *from)
 {
-	struct v4l2_query_ext_ctrl qec = { qc->id };
-	int rc;
+	to->id = from->id;
+	to->type = from->type;
+	to->flags = from->flags;
+	strscpy(to->name, from->name, sizeof(to->name));
 
-	rc = v4l2_query_ext_ctrl(hdl, &qec);
-	if (rc)
-		return rc;
-
-	qc->id = qec.id;
-	qc->type = qec.type;
-	qc->flags = qec.flags;
-	strscpy(qc->name, qec.name, sizeof(qc->name));
-	switch (qc->type) {
+	switch (from->type) {
 	case V4L2_CTRL_TYPE_INTEGER:
 	case V4L2_CTRL_TYPE_BOOLEAN:
 	case V4L2_CTRL_TYPE_MENU:
 	case V4L2_CTRL_TYPE_INTEGER_MENU:
 	case V4L2_CTRL_TYPE_STRING:
 	case V4L2_CTRL_TYPE_BITMASK:
-		qc->minimum = qec.minimum;
-		qc->maximum = qec.maximum;
-		qc->step = qec.step;
-		qc->default_value = qec.default_value;
+		to->minimum = from->minimum;
+		to->maximum = from->maximum;
+		to->step = from->step;
+		to->default_value = from->default_value;
 		break;
 	default:
-		qc->minimum = 0;
-		qc->maximum = 0;
-		qc->step = 0;
-		qc->default_value = 0;
+		to->minimum = 0;
+		to->maximum = 0;
+		to->step = 0;
+		to->default_value = 0;
 		break;
 	}
+}
+EXPORT_SYMBOL(v4l2_query_ext_ctrl_to_v4l2_queryctrl);
+
+/* Implement VIDIOC_QUERYCTRL */
+int v4l2_queryctrl(struct v4l2_ctrl_handler *hdl, struct v4l2_queryctrl *qc)
+{
+	struct v4l2_query_ext_ctrl qec = { qc->id };
+	int rc;
+
+	rc = v4l2_query_ext_ctrl(hdl, &qec);
+	if (rc)
+		return rc;
+
+	v4l2_query_ext_ctrl_to_v4l2_queryctrl(qc, &qec);
+
 	return 0;
 }
 EXPORT_SYMBOL(v4l2_queryctrl);
diff --git a/drivers/media/v4l2-core/v4l2-ctrls-core.c b/drivers/media/v4l2-core/v4l2-ctrls-core.c
index c4d995f32191..90d25329661e 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls-core.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls-core.c
@@ -182,29 +182,66 @@ static void std_init_compound(const struct v4l2_ctrl *ctrl, u32 idx,
 	}
 }
 
-void v4l2_ctrl_type_op_init(const struct v4l2_ctrl *ctrl, u32 from_idx,
-			    union v4l2_ctrl_ptr ptr)
+static void std_min_compound(const struct v4l2_ctrl *ctrl, u32 idx,
+			     union v4l2_ctrl_ptr ptr)
+{
+	void *p = ptr.p + idx * ctrl->elem_size;
+
+	if (ctrl->p_min.p_const)
+		memcpy(p, ctrl->p_min.p_const, ctrl->elem_size);
+	else
+		memset(p, 0, ctrl->elem_size);
+}
+
+static void std_max_compound(const struct v4l2_ctrl *ctrl, u32 idx,
+			     union v4l2_ctrl_ptr ptr)
+{
+	void *p = ptr.p + idx * ctrl->elem_size;
+
+	if (ctrl->p_max.p_const)
+		memcpy(p, ctrl->p_max.p_const, ctrl->elem_size);
+	else
+		memset(p, 0, ctrl->elem_size);
+}
+
+static void __v4l2_ctrl_type_op_init(const struct v4l2_ctrl *ctrl, u32 from_idx,
+				     u32 which, union v4l2_ctrl_ptr ptr)
 {
 	unsigned int i;
 	u32 tot_elems = ctrl->elems;
 	u32 elems = tot_elems - from_idx;
+	s64 value;
 
-	if (from_idx >= tot_elems)
+	switch (which) {
+	case V4L2_CTRL_WHICH_DEF_VAL:
+		value = ctrl->default_value;
+		break;
+	case V4L2_CTRL_WHICH_MAX_VAL:
+		value = ctrl->maximum;
+		break;
+	case V4L2_CTRL_WHICH_MIN_VAL:
+		value = ctrl->minimum;
+		break;
+	default:
 		return;
+	}
 
 	switch (ctrl->type) {
 	case V4L2_CTRL_TYPE_STRING:
+		if (which == V4L2_CTRL_WHICH_DEF_VAL)
+			value = ctrl->minimum;
+
 		for (i = from_idx; i < tot_elems; i++) {
 			unsigned int offset = i * ctrl->elem_size;
 
-			memset(ptr.p_char + offset, ' ', ctrl->minimum);
-			ptr.p_char[offset + ctrl->minimum] = '\0';
+			memset(ptr.p_char + offset, ' ', value);
+			ptr.p_char[offset + value] = '\0';
 		}
 		break;
 	case V4L2_CTRL_TYPE_INTEGER64:
-		if (ctrl->default_value) {
+		if (value) {
 			for (i = from_idx; i < tot_elems; i++)
-				ptr.p_s64[i] = ctrl->default_value;
+				ptr.p_s64[i] = value;
 		} else {
 			memset(ptr.p_s64 + from_idx, 0, elems * sizeof(s64));
 		}
@@ -214,9 +251,9 @@ void v4l2_ctrl_type_op_init(const struct v4l2_ctrl *ctrl, u32 from_idx,
 	case V4L2_CTRL_TYPE_MENU:
 	case V4L2_CTRL_TYPE_BITMASK:
 	case V4L2_CTRL_TYPE_BOOLEAN:
-		if (ctrl->default_value) {
+		if (value) {
 			for (i = from_idx; i < tot_elems; i++)
-				ptr.p_s32[i] = ctrl->default_value;
+				ptr.p_s32[i] = value;
 		} else {
 			memset(ptr.p_s32 + from_idx, 0, elems * sizeof(s32));
 		}
@@ -226,32 +263,61 @@ void v4l2_ctrl_type_op_init(const struct v4l2_ctrl *ctrl, u32 from_idx,
 		memset(ptr.p_s32 + from_idx, 0, elems * sizeof(s32));
 		break;
 	case V4L2_CTRL_TYPE_U8:
-		memset(ptr.p_u8 + from_idx, ctrl->default_value, elems);
+		memset(ptr.p_u8 + from_idx, value, elems);
 		break;
 	case V4L2_CTRL_TYPE_U16:
-		if (ctrl->default_value) {
+		if (value) {
 			for (i = from_idx; i < tot_elems; i++)
-				ptr.p_u16[i] = ctrl->default_value;
+				ptr.p_u16[i] = value;
 		} else {
 			memset(ptr.p_u16 + from_idx, 0, elems * sizeof(u16));
 		}
 		break;
 	case V4L2_CTRL_TYPE_U32:
-		if (ctrl->default_value) {
+		if (value) {
 			for (i = from_idx; i < tot_elems; i++)
-				ptr.p_u32[i] = ctrl->default_value;
+				ptr.p_u32[i] = value;
 		} else {
 			memset(ptr.p_u32 + from_idx, 0, elems * sizeof(u32));
 		}
 		break;
 	default:
-		for (i = from_idx; i < tot_elems; i++)
-			std_init_compound(ctrl, i, ptr);
+		for (i = from_idx; i < tot_elems; i++) {
+			switch (which) {
+			case V4L2_CTRL_WHICH_DEF_VAL:
+				std_init_compound(ctrl, i, ptr);
+				break;
+			case V4L2_CTRL_WHICH_MAX_VAL:
+				std_max_compound(ctrl, i, ptr);
+				break;
+			case V4L2_CTRL_WHICH_MIN_VAL:
+				std_min_compound(ctrl, i, ptr);
+				break;
+			}
+		}
 		break;
 	}
 }
+
+void v4l2_ctrl_type_op_init(const struct v4l2_ctrl *ctrl, u32 from_idx,
+			    union v4l2_ctrl_ptr ptr)
+{
+	__v4l2_ctrl_type_op_init(ctrl, from_idx, V4L2_CTRL_WHICH_DEF_VAL, ptr);
+}
 EXPORT_SYMBOL(v4l2_ctrl_type_op_init);
 
+static void v4l2_ctrl_type_op_minimum(const struct v4l2_ctrl *ctrl,
+				      u32 from_idx, union v4l2_ctrl_ptr ptr)
+{
+	__v4l2_ctrl_type_op_init(ctrl, from_idx, V4L2_CTRL_WHICH_MIN_VAL, ptr);
+}
+
+static void v4l2_ctrl_type_op_maximum(const struct v4l2_ctrl *ctrl,
+				      u32 from_idx, union v4l2_ctrl_ptr ptr)
+{
+	__v4l2_ctrl_type_op_init(ctrl, from_idx, V4L2_CTRL_WHICH_MAX_VAL, ptr);
+}
+
 void v4l2_ctrl_type_op_log(const struct v4l2_ctrl *ctrl)
 {
 	union v4l2_ctrl_ptr ptr = ctrl->p_cur;
@@ -295,6 +361,9 @@ void v4l2_ctrl_type_op_log(const struct v4l2_ctrl *ctrl)
 	case V4L2_CTRL_TYPE_U32:
 		pr_cont("%u", (unsigned)*ptr.p_u32);
 		break;
+	case V4L2_CTRL_TYPE_AREA:
+		pr_cont("%ux%u", ptr.p_area->width, ptr.p_area->height);
+		break;
 	case V4L2_CTRL_TYPE_H264_SPS:
 		pr_cont("H264_SPS");
 		break;
@@ -367,7 +436,11 @@ void v4l2_ctrl_type_op_log(const struct v4l2_ctrl *ctrl)
 	case V4L2_CTRL_TYPE_AV1_FILM_GRAIN:
 		pr_cont("AV1_FILM_GRAIN");
 		break;
-
+	case V4L2_CTRL_TYPE_RECT:
+		pr_cont("(%d,%d)/%ux%u",
+			ptr.p_rect->left, ptr.p_rect->top,
+			ptr.p_rect->width, ptr.p_rect->height);
+		break;
 	default:
 		pr_cont("unknown type %d", ctrl->type);
 		break;
@@ -812,6 +885,7 @@ static int std_validate_compound(const struct v4l2_ctrl *ctrl, u32 idx,
 	struct v4l2_ctrl_hdr10_mastering_display *p_hdr10_mastering;
 	struct v4l2_ctrl_hevc_decode_params *p_hevc_decode_params;
 	struct v4l2_area *area;
+	struct v4l2_rect *rect;
 	void *p = ptr.p + idx * ctrl->elem_size;
 	unsigned int i;
 
@@ -1169,6 +1243,12 @@ static int std_validate_compound(const struct v4l2_ctrl *ctrl, u32 idx,
 			return -EINVAL;
 		break;
 
+	case V4L2_CTRL_TYPE_RECT:
+		rect = p;
+		if (!rect->width || !rect->height)
+			return -EINVAL;
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -1282,6 +1362,8 @@ EXPORT_SYMBOL(v4l2_ctrl_type_op_validate);
 static const struct v4l2_ctrl_type_ops std_type_ops = {
 	.equal = v4l2_ctrl_type_op_equal,
 	.init = v4l2_ctrl_type_op_init,
+	.minimum = v4l2_ctrl_type_op_minimum,
+	.maximum = v4l2_ctrl_type_op_maximum,
 	.log = v4l2_ctrl_type_op_log,
 	.validate = v4l2_ctrl_type_op_validate,
 };
@@ -1754,7 +1836,10 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 			s64 min, s64 max, u64 step, s64 def,
 			const u32 dims[V4L2_CTRL_MAX_DIMS], u32 elem_size,
 			u32 flags, const char * const *qmenu,
-			const s64 *qmenu_int, const union v4l2_ctrl_ptr p_def,
+			const s64 *qmenu_int,
+			const union v4l2_ctrl_ptr p_def,
+			const union v4l2_ctrl_ptr p_min,
+			const union v4l2_ctrl_ptr p_max,
 			void *priv)
 {
 	struct v4l2_ctrl *ctrl;
@@ -1869,12 +1954,21 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 	case V4L2_CTRL_TYPE_AREA:
 		elem_size = sizeof(struct v4l2_area);
 		break;
+	case V4L2_CTRL_TYPE_RECT:
+		elem_size = sizeof(struct v4l2_rect);
+		break;
 	default:
 		if (type < V4L2_CTRL_COMPOUND_TYPES)
 			elem_size = sizeof(s32);
 		break;
 	}
 
+	if (type < V4L2_CTRL_COMPOUND_TYPES &&
+	    type != V4L2_CTRL_TYPE_BUTTON &&
+	    type != V4L2_CTRL_TYPE_CTRL_CLASS &&
+	    type != V4L2_CTRL_TYPE_STRING)
+		flags |= V4L2_CTRL_FLAG_HAS_WHICH_MIN_MAX;
+
 	/* Sanity checks */
 	if (id == 0 || name == NULL || !elem_size ||
 	    id >= V4L2_CID_PRIVATE_BASE ||
@@ -1883,6 +1977,7 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 		handler_set_err(hdl, -ERANGE);
 		return NULL;
 	}
+
 	err = check_range(type, min, max, step, def);
 	if (err) {
 		handler_set_err(hdl, err);
@@ -1924,6 +2019,10 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 
 	if (type >= V4L2_CTRL_COMPOUND_TYPES && p_def.p_const)
 		sz_extra += elem_size;
+	if (type >= V4L2_CTRL_COMPOUND_TYPES && p_min.p_const)
+		sz_extra += elem_size;
+	if (type >= V4L2_CTRL_COMPOUND_TYPES && p_max.p_const)
+		sz_extra += elem_size;
 
 	ctrl = kvzalloc(sizeof(*ctrl) + sz_extra, GFP_KERNEL);
 	if (ctrl == NULL) {
@@ -1989,6 +2088,22 @@ static struct v4l2_ctrl *v4l2_ctrl_new(struct v4l2_ctrl_handler *hdl,
 		memcpy(ctrl->p_def.p, p_def.p_const, elem_size);
 	}
 
+	if (flags & V4L2_CTRL_FLAG_HAS_WHICH_MIN_MAX) {
+		void *ptr = ctrl->p_def.p;
+
+		if (p_min.p_const) {
+			ptr += elem_size;
+			ctrl->p_min.p = ptr;
+			memcpy(ctrl->p_min.p, p_min.p_const, elem_size);
+		}
+
+		if (p_max.p_const) {
+			ptr += elem_size;
+			ctrl->p_max.p = ptr;
+			memcpy(ctrl->p_max.p, p_max.p_const, elem_size);
+		}
+	}
+
 	ctrl->type_ops->init(ctrl, 0, ctrl->p_cur);
 	cur_to_new(ctrl);
 
@@ -2039,7 +2154,8 @@ struct v4l2_ctrl *v4l2_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 			type, min, max,
 			is_menu ? cfg->menu_skip_mask : step, def,
 			cfg->dims, cfg->elem_size,
-			flags, qmenu, qmenu_int, cfg->p_def, priv);
+			flags, qmenu, qmenu_int, cfg->p_def, cfg->p_min,
+			cfg->p_max, priv);
 	if (ctrl)
 		ctrl->is_private = cfg->is_private;
 	return ctrl;
@@ -2064,7 +2180,8 @@ struct v4l2_ctrl *v4l2_ctrl_new_std(struct v4l2_ctrl_handler *hdl,
 	}
 	return v4l2_ctrl_new(hdl, ops, NULL, id, name, type,
 			     min, max, step, def, NULL, 0,
-			     flags, NULL, NULL, ptr_null, NULL);
+			     flags, NULL, NULL, ptr_null, ptr_null,
+			     ptr_null, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std);
 
@@ -2097,7 +2214,8 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu(struct v4l2_ctrl_handler *hdl,
 	}
 	return v4l2_ctrl_new(hdl, ops, NULL, id, name, type,
 			     0, max, mask, def, NULL, 0,
-			     flags, qmenu, qmenu_int, ptr_null, NULL);
+			     flags, qmenu, qmenu_int, ptr_null, ptr_null,
+			     ptr_null, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu);
 
@@ -2129,7 +2247,8 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_menu_items(struct v4l2_ctrl_handler *hdl,
 	}
 	return v4l2_ctrl_new(hdl, ops, NULL, id, name, type,
 			     0, max, mask, def, NULL, 0,
-			     flags, qmenu, NULL, ptr_null, NULL);
+			     flags, qmenu, NULL, ptr_null, ptr_null,
+			     ptr_null, NULL);
 
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_menu_items);
@@ -2137,7 +2256,9 @@ EXPORT_SYMBOL(v4l2_ctrl_new_std_menu_items);
 /* Helper function for standard compound controls */
 struct v4l2_ctrl *v4l2_ctrl_new_std_compound(struct v4l2_ctrl_handler *hdl,
 				const struct v4l2_ctrl_ops *ops, u32 id,
-				const union v4l2_ctrl_ptr p_def)
+				const union v4l2_ctrl_ptr p_def,
+				const union v4l2_ctrl_ptr p_min,
+				const union v4l2_ctrl_ptr p_max)
 {
 	const char *name;
 	enum v4l2_ctrl_type type;
@@ -2151,7 +2272,7 @@ struct v4l2_ctrl *v4l2_ctrl_new_std_compound(struct v4l2_ctrl_handler *hdl,
 	}
 	return v4l2_ctrl_new(hdl, ops, NULL, id, name, type,
 			     min, max, step, def, NULL, 0,
-			     flags, NULL, NULL, p_def, NULL);
+			     flags, NULL, NULL, p_def, p_min, p_max, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_std_compound);
 
@@ -2175,7 +2296,8 @@ struct v4l2_ctrl *v4l2_ctrl_new_int_menu(struct v4l2_ctrl_handler *hdl,
 	}
 	return v4l2_ctrl_new(hdl, ops, NULL, id, name, type,
 			     0, max, 0, def, NULL, 0,
-			     flags, NULL, qmenu_int, ptr_null, NULL);
+			     flags, NULL, qmenu_int, ptr_null, ptr_null,
+			     ptr_null, NULL);
 }
 EXPORT_SYMBOL(v4l2_ctrl_new_int_menu);
 
@@ -2504,8 +2626,7 @@ int v4l2_ctrl_handler_setup(struct v4l2_ctrl_handler *hdl)
 EXPORT_SYMBOL(v4l2_ctrl_handler_setup);
 
 /* Log the control name and value */
-static void log_ctrl(const struct v4l2_ctrl_handler *hdl,
-		     struct v4l2_ctrl *ctrl,
+static void log_ctrl(const struct v4l2_ctrl *ctrl,
 		     const char *prefix, const char *colon)
 {
 	if (ctrl->flags & (V4L2_CTRL_FLAG_DISABLED | V4L2_CTRL_FLAG_WRITE_ONLY))
@@ -2515,11 +2636,7 @@ static void log_ctrl(const struct v4l2_ctrl_handler *hdl,
 
 	pr_info("%s%s%s: ", prefix, colon, ctrl->name);
 
-	if (ctrl->handler != hdl)
-		v4l2_ctrl_lock(ctrl);
 	ctrl->type_ops->log(ctrl);
-	if (ctrl->handler != hdl)
-		v4l2_ctrl_unlock(ctrl);
 
 	if (ctrl->flags & (V4L2_CTRL_FLAG_INACTIVE |
 			   V4L2_CTRL_FLAG_GRABBED |
@@ -2538,7 +2655,7 @@ static void log_ctrl(const struct v4l2_ctrl_handler *hdl,
 void v4l2_ctrl_handler_log_status(struct v4l2_ctrl_handler *hdl,
 				  const char *prefix)
 {
-	struct v4l2_ctrl_ref *ref;
+	struct v4l2_ctrl *ctrl;
 	const char *colon = "";
 	int len;
 
@@ -2550,12 +2667,9 @@ void v4l2_ctrl_handler_log_status(struct v4l2_ctrl_handler *hdl,
 	if (len && prefix[len - 1] != ' ')
 		colon = ": ";
 	mutex_lock(hdl->lock);
-	list_for_each_entry(ref, &hdl->ctrl_refs, node) {
-		if (ref->from_other_dev ||
-		    (ref->ctrl->flags & V4L2_CTRL_FLAG_DISABLED))
-			continue;
-		log_ctrl(hdl, ref->ctrl, prefix, colon);
-	}
+	list_for_each_entry(ctrl, &hdl->ctrls, node)
+		if (!(ctrl->flags & V4L2_CTRL_FLAG_DISABLED))
+			log_ctrl(ctrl, prefix, colon);
 	mutex_unlock(hdl->lock);
 }
 EXPORT_SYMBOL(v4l2_ctrl_handler_log_status);
@@ -2564,6 +2678,9 @@ int v4l2_ctrl_new_fwnode_properties(struct v4l2_ctrl_handler *hdl,
 				    const struct v4l2_ctrl_ops *ctrl_ops,
 				    const struct v4l2_fwnode_device_properties *p)
 {
+	if (hdl->error)
+		return hdl->error;
+
 	if (p->orientation != V4L2_FWNODE_PROPERTY_UNSET) {
 		u32 orientation_ctrl;
 
diff --git a/drivers/media/v4l2-core/v4l2-ctrls-defs.c b/drivers/media/v4l2-core/v4l2-ctrls-defs.c
index 8696eb1cdd61..1ea52011247a 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls-defs.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls-defs.c
@@ -970,6 +970,7 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_MPEG_VIDEO_LTR_COUNT:			return "LTR Count";
 	case V4L2_CID_MPEG_VIDEO_FRAME_LTR_INDEX:		return "Frame LTR Index";
 	case V4L2_CID_MPEG_VIDEO_USE_LTR_FRAMES:		return "Use LTR Frames";
+	case V4L2_CID_MPEG_VIDEO_AVERAGE_QP:			return "Average QP Value";
 	case V4L2_CID_FWHT_I_FRAME_QP:				return "FWHT I-Frame QP Value";
 	case V4L2_CID_FWHT_P_FRAME_QP:				return "FWHT P-Frame QP Value";
 
@@ -1507,6 +1508,10 @@ void v4l2_ctrl_fill(u32 id, const char **name, enum v4l2_ctrl_type *type,
 		*max = 0xffffffffffffLL;
 		*step = 1;
 		break;
+	case V4L2_CID_MPEG_VIDEO_AVERAGE_QP:
+		*type = V4L2_CTRL_TYPE_INTEGER;
+		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
+		break;
 	case V4L2_CID_PIXEL_RATE:
 		*type = V4L2_CTRL_TYPE_INTEGER64;
 		*flags |= V4L2_CTRL_FLAG_READ_ONLY;
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index d13954bd31fd..c369235113d9 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -93,6 +93,8 @@ static struct attribute *video_device_attrs[] = {
 };
 ATTRIBUTE_GROUPS(video_device);
 
+static struct dentry *v4l2_debugfs_root_dir;
+
 /*
  *	Active devices
  */
@@ -227,7 +229,7 @@ static void v4l2_device_release(struct device *cd)
 		v4l2_device_put(v4l2_dev);
 }
 
-static struct class video_class = {
+static const struct class video_class = {
 	.name = VIDEO_NAME,
 	.dev_groups = video_device_groups,
 };
@@ -483,7 +485,6 @@ static const struct file_operations v4l2_fops = {
 #endif
 	.release = v4l2_release,
 	.poll = v4l2_poll,
-	.llseek = no_llseek,
 };
 
 /**
@@ -557,6 +558,7 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	bool is_tx = vdev->vfl_dir != VFL_DIR_RX;
 	bool is_io_mc = vdev->device_caps & V4L2_CAP_IO_MC;
 	bool has_streaming = vdev->device_caps & V4L2_CAP_STREAMING;
+	bool is_edid =  vdev->device_caps & V4L2_CAP_EDID;
 
 	bitmap_zero(valid_ioctls, BASE_VIDIOC_PRIVATE);
 
@@ -570,13 +572,13 @@ static void determine_valid_ioctls(struct video_device *vdev)
 	   and that can't be tested here. If the bit for these control ioctls
 	   is set, then the ioctl is valid. But if it is 0, then it can still
 	   be valid if the filehandle passed the control handler. */
-	if (vdev->ctrl_handler || ops->vidioc_queryctrl)
+	if (vdev->ctrl_handler || ops->vidioc_query_ext_ctrl)
 		__set_bit(_IOC_NR(VIDIOC_QUERYCTRL), valid_ioctls);
 	if (vdev->ctrl_handler || ops->vidioc_query_ext_ctrl)
 		__set_bit(_IOC_NR(VIDIOC_QUERY_EXT_CTRL), valid_ioctls);
-	if (vdev->ctrl_handler || ops->vidioc_g_ctrl || ops->vidioc_g_ext_ctrls)
+	if (vdev->ctrl_handler || ops->vidioc_g_ext_ctrls)
 		__set_bit(_IOC_NR(VIDIOC_G_CTRL), valid_ioctls);
-	if (vdev->ctrl_handler || ops->vidioc_s_ctrl || ops->vidioc_s_ext_ctrls)
+	if (vdev->ctrl_handler || ops->vidioc_s_ext_ctrls)
 		__set_bit(_IOC_NR(VIDIOC_S_CTRL), valid_ioctls);
 	if (vdev->ctrl_handler || ops->vidioc_g_ext_ctrls)
 		__set_bit(_IOC_NR(VIDIOC_G_EXT_CTRLS), valid_ioctls);
@@ -722,6 +724,9 @@ static void determine_valid_ioctls(struct video_device *vdev)
 		SET_VALID_IOCTL(ops, VIDIOC_PREPARE_BUF, vidioc_prepare_buf);
 		SET_VALID_IOCTL(ops, VIDIOC_STREAMON, vidioc_streamon);
 		SET_VALID_IOCTL(ops, VIDIOC_STREAMOFF, vidioc_streamoff);
+		/* VIDIOC_CREATE_BUFS support is mandatory to enable VIDIOC_REMOVE_BUFS */
+		if (ops->vidioc_create_bufs)
+			SET_VALID_IOCTL(ops, VIDIOC_REMOVE_BUFS, vidioc_remove_bufs);
 	}
 
 	if (is_vid || is_vbi || is_meta) {
@@ -781,6 +786,20 @@ static void determine_valid_ioctls(struct video_device *vdev)
 		SET_VALID_IOCTL(ops, VIDIOC_S_TUNER, vidioc_s_tuner);
 		SET_VALID_IOCTL(ops, VIDIOC_S_HW_FREQ_SEEK, vidioc_s_hw_freq_seek);
 	}
+	if (is_edid) {
+		SET_VALID_IOCTL(ops, VIDIOC_G_EDID, vidioc_g_edid);
+		if (is_tx) {
+			SET_VALID_IOCTL(ops, VIDIOC_G_OUTPUT, vidioc_g_output);
+			SET_VALID_IOCTL(ops, VIDIOC_S_OUTPUT, vidioc_s_output);
+			SET_VALID_IOCTL(ops, VIDIOC_ENUMOUTPUT, vidioc_enum_output);
+		}
+		if (is_rx) {
+			SET_VALID_IOCTL(ops, VIDIOC_ENUMINPUT, vidioc_enum_input);
+			SET_VALID_IOCTL(ops, VIDIOC_G_INPUT, vidioc_g_input);
+			SET_VALID_IOCTL(ops, VIDIOC_S_INPUT, vidioc_s_input);
+			SET_VALID_IOCTL(ops, VIDIOC_S_EDID, vidioc_s_edid);
+		}
+	}
 
 	bitmap_andnot(vdev->valid_ioctls, valid_ioctls, vdev->valid_ioctls,
 			BASE_VIDIOC_PRIVATE);
@@ -1035,28 +1054,31 @@ int __video_register_device(struct video_device *vdev,
 	vdev->dev.class = &video_class;
 	vdev->dev.devt = MKDEV(VIDEO_MAJOR, vdev->minor);
 	vdev->dev.parent = vdev->dev_parent;
+	vdev->dev.release = v4l2_device_release;
 	dev_set_name(&vdev->dev, "%s%d", name_base, vdev->num);
+
+	/* Increase v4l2_device refcount */
+	v4l2_device_get(vdev->v4l2_dev);
+
+	mutex_lock(&videodev_lock);
 	ret = device_register(&vdev->dev);
 	if (ret < 0) {
+		mutex_unlock(&videodev_lock);
 		pr_err("%s: device_register failed\n", __func__);
-		goto cleanup;
+		put_device(&vdev->dev);
+		return ret;
 	}
-	/* Register the release callback that will be called when the last
-	   reference to the device goes away. */
-	vdev->dev.release = v4l2_device_release;
 
 	if (nr != -1 && nr != vdev->num && warn_if_nr_in_use)
 		pr_warn("%s: requested %s%d, got %s\n", __func__,
 			name_base, nr, video_device_node_name(vdev));
 
-	/* Increase v4l2_device refcount */
-	v4l2_device_get(vdev->v4l2_dev);
-
 	/* Part 5: Register the entity. */
 	ret = video_register_media_controller(vdev);
 
 	/* Part 6: Activate this minor. The char device can now be used. */
 	set_bit(V4L2_FL_REGISTERED, &vdev->flags);
+	mutex_unlock(&videodev_lock);
 
 	return 0;
 
@@ -1098,6 +1120,16 @@ void video_unregister_device(struct video_device *vdev)
 }
 EXPORT_SYMBOL(video_unregister_device);
 
+#ifdef CONFIG_DEBUG_FS
+struct dentry *v4l2_debugfs_root(void)
+{
+	if (!v4l2_debugfs_root_dir)
+		v4l2_debugfs_root_dir = debugfs_create_dir("v4l2", NULL);
+	return v4l2_debugfs_root_dir;
+}
+EXPORT_SYMBOL_GPL(v4l2_debugfs_root);
+#endif
+
 #if defined(CONFIG_MEDIA_CONTROLLER)
 
 __must_check int video_device_pipeline_start(struct video_device *vdev,
@@ -1202,6 +1234,8 @@ static void __exit videodev_exit(void)
 
 	class_unregister(&video_class);
 	unregister_chrdev_region(dev, VIDEO_NUM_DEVICES);
+	debugfs_remove_recursive(v4l2_debugfs_root_dir);
+	v4l2_debugfs_root_dir = NULL;
 }
 
 subsys_initcall(videodev_init);
diff --git a/drivers/media/v4l2-core/v4l2-device.c b/drivers/media/v4l2-core/v4l2-device.c
index d2e58ae91f9b..5e537454f5cd 100644
--- a/drivers/media/v4l2-core/v4l2-device.c
+++ b/drivers/media/v4l2-core/v4l2-device.c
@@ -108,8 +108,8 @@ void v4l2_device_unregister(struct v4l2_device *v4l2_dev)
 }
 EXPORT_SYMBOL_GPL(v4l2_device_unregister);
 
-int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
-				struct v4l2_subdev *sd)
+int __v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
+				  struct v4l2_subdev *sd, struct module *module)
 {
 	int err;
 
@@ -125,9 +125,9 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 	 * try_module_get() such sub-device owners.
 	 */
 	sd->owner_v4l2_dev = v4l2_dev->dev && v4l2_dev->dev->driver &&
-		sd->owner == v4l2_dev->dev->driver->owner;
+		module == v4l2_dev->dev->driver->owner;
 
-	if (!sd->owner_v4l2_dev && !try_module_get(sd->owner))
+	if (!sd->owner_v4l2_dev && !try_module_get(module))
 		return -ENODEV;
 
 	sd->v4l2_dev = v4l2_dev;
@@ -152,6 +152,8 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 			goto error_unregister;
 	}
 
+	sd->owner = module;
+
 	spin_lock(&v4l2_dev->lock);
 	list_add_tail(&sd->list, &v4l2_dev->subdevs);
 	spin_unlock(&v4l2_dev->lock);
@@ -168,7 +170,7 @@ error_module:
 	sd->v4l2_dev = NULL;
 	return err;
 }
-EXPORT_SYMBOL_GPL(v4l2_device_register_subdev);
+EXPORT_SYMBOL_GPL(__v4l2_device_register_subdev);
 
 static void v4l2_subdev_release(struct v4l2_subdev *sd)
 {
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index 942d0005c55e..7710cb26bea0 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -481,25 +481,28 @@ EXPORT_SYMBOL_GPL(v4l2_calc_timeperframe);
  * @polarities - the horizontal and vertical polarities (same as struct
  *		v4l2_bt_timings polarities).
  * @interlaced - if this flag is true, it indicates interlaced format
- * @fmt - the resulting timings.
+ * @cap - the v4l2_dv_timings_cap capabilities.
+ * @timings - the resulting timings.
  *
  * This function will attempt to detect if the given values correspond to a
  * valid CVT format. If so, then it will return true, and fmt will be filled
  * in with the found CVT timings.
  */
-bool v4l2_detect_cvt(unsigned frame_height,
-		     unsigned hfreq,
-		     unsigned vsync,
-		     unsigned active_width,
+bool v4l2_detect_cvt(unsigned int frame_height,
+		     unsigned int hfreq,
+		     unsigned int vsync,
+		     unsigned int active_width,
 		     u32 polarities,
 		     bool interlaced,
-		     struct v4l2_dv_timings *fmt)
+		     const struct v4l2_dv_timings_cap *cap,
+		     struct v4l2_dv_timings *timings)
 {
-	int  v_fp, v_bp, h_fp, h_bp, hsync;
-	int  frame_width, image_height, image_width;
+	struct v4l2_dv_timings t = {};
+	int v_fp, v_bp, h_fp, h_bp, hsync;
+	int frame_width, image_height, image_width;
 	bool reduced_blanking;
 	bool rb_v2 = false;
-	unsigned pix_clk;
+	unsigned int pix_clk;
 
 	if (vsync < 4 || vsync > 8)
 		return false;
@@ -625,36 +628,39 @@ bool v4l2_detect_cvt(unsigned frame_height,
 		h_fp = h_blank - hsync - h_bp;
 	}
 
-	fmt->type = V4L2_DV_BT_656_1120;
-	fmt->bt.polarities = polarities;
-	fmt->bt.width = image_width;
-	fmt->bt.height = image_height;
-	fmt->bt.hfrontporch = h_fp;
-	fmt->bt.vfrontporch = v_fp;
-	fmt->bt.hsync = hsync;
-	fmt->bt.vsync = vsync;
-	fmt->bt.hbackporch = frame_width - image_width - h_fp - hsync;
+	t.type = V4L2_DV_BT_656_1120;
+	t.bt.polarities = polarities;
+	t.bt.width = image_width;
+	t.bt.height = image_height;
+	t.bt.hfrontporch = h_fp;
+	t.bt.vfrontporch = v_fp;
+	t.bt.hsync = hsync;
+	t.bt.vsync = vsync;
+	t.bt.hbackporch = frame_width - image_width - h_fp - hsync;
 
 	if (!interlaced) {
-		fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
-		fmt->bt.interlaced = V4L2_DV_PROGRESSIVE;
+		t.bt.vbackporch = frame_height - image_height - v_fp - vsync;
+		t.bt.interlaced = V4L2_DV_PROGRESSIVE;
 	} else {
-		fmt->bt.vbackporch = (frame_height - image_height - 2 * v_fp -
+		t.bt.vbackporch = (frame_height - image_height - 2 * v_fp -
 				      2 * vsync) / 2;
-		fmt->bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
-					2 * vsync - fmt->bt.vbackporch;
-		fmt->bt.il_vfrontporch = v_fp;
-		fmt->bt.il_vsync = vsync;
-		fmt->bt.flags |= V4L2_DV_FL_HALF_LINE;
-		fmt->bt.interlaced = V4L2_DV_INTERLACED;
+		t.bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
+					2 * vsync - t.bt.vbackporch;
+		t.bt.il_vfrontporch = v_fp;
+		t.bt.il_vsync = vsync;
+		t.bt.flags |= V4L2_DV_FL_HALF_LINE;
+		t.bt.interlaced = V4L2_DV_INTERLACED;
 	}
 
-	fmt->bt.pixelclock = pix_clk;
-	fmt->bt.standards = V4L2_DV_BT_STD_CVT;
+	t.bt.pixelclock = pix_clk;
+	t.bt.standards = V4L2_DV_BT_STD_CVT;
 
 	if (reduced_blanking)
-		fmt->bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
+		t.bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
 
+	if (!v4l2_valid_dv_timings(&t, cap, NULL, NULL))
+		return false;
+	*timings = t;
 	return true;
 }
 EXPORT_SYMBOL_GPL(v4l2_detect_cvt);
@@ -699,22 +705,25 @@ EXPORT_SYMBOL_GPL(v4l2_detect_cvt);
  *		image height, so it has to be passed explicitly. Usually
  *		the native screen aspect ratio is used for this. If it
  *		is not filled in correctly, then 16:9 will be assumed.
- * @fmt - the resulting timings.
+ * @cap - the v4l2_dv_timings_cap capabilities.
+ * @timings - the resulting timings.
  *
  * This function will attempt to detect if the given values correspond to a
  * valid GTF format. If so, then it will return true, and fmt will be filled
  * in with the found GTF timings.
  */
-bool v4l2_detect_gtf(unsigned frame_height,
-		unsigned hfreq,
-		unsigned vsync,
-		u32 polarities,
-		bool interlaced,
-		struct v4l2_fract aspect,
-		struct v4l2_dv_timings *fmt)
+bool v4l2_detect_gtf(unsigned int frame_height,
+		     unsigned int hfreq,
+		     unsigned int vsync,
+		     u32 polarities,
+		     bool interlaced,
+		     struct v4l2_fract aspect,
+		     const struct v4l2_dv_timings_cap *cap,
+		     struct v4l2_dv_timings *timings)
 {
+	struct v4l2_dv_timings t = {};
 	int pix_clk;
-	int  v_fp, v_bp, h_fp, hsync;
+	int v_fp, v_bp, h_fp, hsync;
 	int frame_width, image_height, image_width;
 	bool default_gtf;
 	int h_blank;
@@ -755,7 +764,7 @@ bool v4l2_detect_gtf(unsigned frame_height,
 		u64 num;
 		u32 den;
 
-		num = ((image_width * GTF_D_C_PRIME * (u64)hfreq) -
+		num = (((u64)image_width * GTF_D_C_PRIME * hfreq) -
 		      ((u64)image_width * GTF_D_M_PRIME * 1000));
 		den = (hfreq * (100 - GTF_D_C_PRIME) + GTF_D_M_PRIME * 1000) *
 		      (2 * GTF_CELL_GRAN);
@@ -765,7 +774,7 @@ bool v4l2_detect_gtf(unsigned frame_height,
 		u64 num;
 		u32 den;
 
-		num = ((image_width * GTF_S_C_PRIME * (u64)hfreq) -
+		num = (((u64)image_width * GTF_S_C_PRIME * hfreq) -
 		      ((u64)image_width * GTF_S_M_PRIME * 1000));
 		den = (hfreq * (100 - GTF_S_C_PRIME) + GTF_S_M_PRIME * 1000) *
 		      (2 * GTF_CELL_GRAN);
@@ -783,36 +792,39 @@ bool v4l2_detect_gtf(unsigned frame_height,
 
 	h_fp = h_blank / 2 - hsync;
 
-	fmt->type = V4L2_DV_BT_656_1120;
-	fmt->bt.polarities = polarities;
-	fmt->bt.width = image_width;
-	fmt->bt.height = image_height;
-	fmt->bt.hfrontporch = h_fp;
-	fmt->bt.vfrontporch = v_fp;
-	fmt->bt.hsync = hsync;
-	fmt->bt.vsync = vsync;
-	fmt->bt.hbackporch = frame_width - image_width - h_fp - hsync;
+	t.type = V4L2_DV_BT_656_1120;
+	t.bt.polarities = polarities;
+	t.bt.width = image_width;
+	t.bt.height = image_height;
+	t.bt.hfrontporch = h_fp;
+	t.bt.vfrontporch = v_fp;
+	t.bt.hsync = hsync;
+	t.bt.vsync = vsync;
+	t.bt.hbackporch = frame_width - image_width - h_fp - hsync;
 
 	if (!interlaced) {
-		fmt->bt.vbackporch = frame_height - image_height - v_fp - vsync;
-		fmt->bt.interlaced = V4L2_DV_PROGRESSIVE;
+		t.bt.vbackporch = frame_height - image_height - v_fp - vsync;
+		t.bt.interlaced = V4L2_DV_PROGRESSIVE;
 	} else {
-		fmt->bt.vbackporch = (frame_height - image_height - 2 * v_fp -
+		t.bt.vbackporch = (frame_height - image_height - 2 * v_fp -
 				      2 * vsync) / 2;
-		fmt->bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
-					2 * vsync - fmt->bt.vbackporch;
-		fmt->bt.il_vfrontporch = v_fp;
-		fmt->bt.il_vsync = vsync;
-		fmt->bt.flags |= V4L2_DV_FL_HALF_LINE;
-		fmt->bt.interlaced = V4L2_DV_INTERLACED;
+		t.bt.il_vbackporch = frame_height - image_height - 2 * v_fp -
+					2 * vsync - t.bt.vbackporch;
+		t.bt.il_vfrontporch = v_fp;
+		t.bt.il_vsync = vsync;
+		t.bt.flags |= V4L2_DV_FL_HALF_LINE;
+		t.bt.interlaced = V4L2_DV_INTERLACED;
 	}
 
-	fmt->bt.pixelclock = pix_clk;
-	fmt->bt.standards = V4L2_DV_BT_STD_GTF;
+	t.bt.pixelclock = pix_clk;
+	t.bt.standards = V4L2_DV_BT_STD_GTF;
 
 	if (!default_gtf)
-		fmt->bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
+		t.bt.flags |= V4L2_DV_FL_REDUCED_BLANKING;
 
+	if (!v4l2_valid_dv_timings(&t, cap, NULL, NULL))
+		return false;
+	*timings = t;
 	return true;
 }
 EXPORT_SYMBOL_GPL(v4l2_detect_gtf);
@@ -1006,6 +1018,42 @@ v4l2_hdmi_rx_colorimetry(const struct hdmi_avi_infoframe *avi,
 EXPORT_SYMBOL_GPL(v4l2_hdmi_rx_colorimetry);
 
 /**
+ * v4l2_num_edid_blocks() - return the number of EDID blocks
+ *
+ * @edid:	pointer to the EDID data
+ * @max_blocks:	maximum number of supported EDID blocks
+ *
+ * Return: the number of EDID blocks based on the contents of the EDID.
+ *	   This supports the HDMI Forum EDID Extension Override Data Block.
+ */
+unsigned int v4l2_num_edid_blocks(const u8 *edid, unsigned int max_blocks)
+{
+	unsigned int blocks;
+
+	if (!edid || !max_blocks)
+		return 0;
+
+	// The number of extension blocks is recorded at byte 126 of the
+	// first 128-byte block in the EDID.
+	//
+	// If there is an HDMI Forum EDID Extension Override Data Block
+	// present, then it is in bytes 4-6 of the first CTA-861 extension
+	// block of the EDID.
+	blocks = edid[126] + 1;
+	// Check for HDMI Forum EDID Extension Override Data Block
+	if (blocks >= 2 &&	// The EDID must be at least 2 blocks
+	    max_blocks >= 3 &&  // The caller supports at least 3 blocks
+	    edid[128] == 2 &&	// The first extension block is type CTA-861
+	    edid[133] == 0x78 && // Identifier for the EEODB
+	    (edid[132] & 0xe0) == 0xe0 && // Tag Code == 7
+	    (edid[132] & 0x1f) >= 2 &&	// Length >= 2
+	    edid[134] > 1)	// Number of extension blocks is sane
+		blocks = edid[134] + 1;
+	return blocks > max_blocks ? max_blocks : blocks;
+}
+EXPORT_SYMBOL_GPL(v4l2_num_edid_blocks);
+
+/**
  * v4l2_get_edid_phys_addr() - find and return the physical address
  *
  * @edid:	pointer to the EDID data
@@ -1154,3 +1202,70 @@ int v4l2_phys_addr_validate(u16 phys_addr, u16 *parent, u16 *port)
 	return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_phys_addr_validate);
+
+#ifdef CONFIG_DEBUG_FS
+
+#define DEBUGFS_FOPS(type, flag)					\
+static ssize_t								\
+infoframe_read_##type(struct file *filp,				\
+		      char __user *ubuf, size_t count, loff_t *ppos)	\
+{									\
+	struct v4l2_debugfs_if *infoframes = filp->private_data;	\
+									\
+	return infoframes->if_read((flag), infoframes->priv, filp,	\
+				   ubuf, count, ppos);			\
+}									\
+									\
+static const struct file_operations infoframe_##type##_fops = {		\
+	.owner   = THIS_MODULE,						\
+	.open    = simple_open,						\
+	.read    = infoframe_read_##type,				\
+}
+
+DEBUGFS_FOPS(avi, V4L2_DEBUGFS_IF_AVI);
+DEBUGFS_FOPS(audio, V4L2_DEBUGFS_IF_AUDIO);
+DEBUGFS_FOPS(spd, V4L2_DEBUGFS_IF_SPD);
+DEBUGFS_FOPS(hdmi, V4L2_DEBUGFS_IF_HDMI);
+
+struct v4l2_debugfs_if *v4l2_debugfs_if_alloc(struct dentry *root, u32 if_types,
+					      void *priv,
+					      v4l2_debugfs_if_read_t if_read)
+{
+	struct v4l2_debugfs_if *infoframes;
+
+	if (IS_ERR_OR_NULL(root) || !if_types || !if_read)
+		return NULL;
+
+	infoframes = kzalloc(sizeof(*infoframes), GFP_KERNEL);
+	if (!infoframes)
+		return NULL;
+
+	infoframes->if_dir = debugfs_create_dir("infoframes", root);
+	infoframes->priv = priv;
+	infoframes->if_read = if_read;
+	if (if_types & V4L2_DEBUGFS_IF_AVI)
+		debugfs_create_file("avi", 0400, infoframes->if_dir,
+				    infoframes, &infoframe_avi_fops);
+	if (if_types & V4L2_DEBUGFS_IF_AUDIO)
+		debugfs_create_file("audio", 0400, infoframes->if_dir,
+				    infoframes, &infoframe_audio_fops);
+	if (if_types & V4L2_DEBUGFS_IF_SPD)
+		debugfs_create_file("spd", 0400, infoframes->if_dir,
+				    infoframes, &infoframe_spd_fops);
+	if (if_types & V4L2_DEBUGFS_IF_HDMI)
+		debugfs_create_file("hdmi", 0400, infoframes->if_dir,
+				    infoframes, &infoframe_hdmi_fops);
+	return infoframes;
+}
+EXPORT_SYMBOL_GPL(v4l2_debugfs_if_alloc);
+
+void v4l2_debugfs_if_free(struct v4l2_debugfs_if *infoframes)
+{
+	if (infoframes) {
+		debugfs_remove_recursive(infoframes->if_dir);
+		kfree(infoframes);
+	}
+}
+EXPORT_SYMBOL_GPL(v4l2_debugfs_if_free);
+
+#endif
diff --git a/drivers/media/v4l2-core/v4l2-fwnode.c b/drivers/media/v4l2-core/v4l2-fwnode.c
index 89c7192148df..cb153ce42c45 100644
--- a/drivers/media/v4l2-core/v4l2-fwnode.c
+++ b/drivers/media/v4l2-core/v4l2-fwnode.c
@@ -127,7 +127,7 @@ static int v4l2_fwnode_endpoint_parse_csi2_bus(struct fwnode_handle *fwnode,
 {
 	struct v4l2_mbus_config_mipi_csi2 *bus = &vep->bus.mipi_csi2;
 	bool have_clk_lane = false, have_data_lanes = false,
-		have_lane_polarities = false;
+		have_lane_polarities = false, have_line_orders = false;
 	unsigned int flags = 0, lanes_used = 0;
 	u32 array[1 + V4L2_MBUS_CSI2_MAX_DATA_LANES];
 	u32 clock_lane = 0;
@@ -197,6 +197,17 @@ static int v4l2_fwnode_endpoint_parse_csi2_bus(struct fwnode_handle *fwnode,
 		have_lane_polarities = true;
 	}
 
+	rval = fwnode_property_count_u32(fwnode, "line-orders");
+	if (rval > 0) {
+		if (rval != num_data_lanes) {
+			pr_warn("invalid number of line-orders entries (need %u, got %u)\n",
+				num_data_lanes, rval);
+			return -EINVAL;
+		}
+
+		have_line_orders = true;
+	}
+
 	if (!fwnode_property_read_u32(fwnode, "clock-lanes", &v)) {
 		clock_lane = v;
 		pr_debug("clock lane position %u\n", v);
@@ -250,6 +261,36 @@ static int v4l2_fwnode_endpoint_parse_csi2_bus(struct fwnode_handle *fwnode,
 		} else {
 			pr_debug("no lane polarities defined, assuming not inverted\n");
 		}
+
+		if (have_line_orders) {
+			fwnode_property_read_u32_array(fwnode,
+						       "line-orders", array,
+						       num_data_lanes);
+
+			for (i = 0; i < num_data_lanes; i++) {
+				static const char * const orders[] = {
+					"ABC", "ACB", "BAC", "BCA", "CAB", "CBA"
+				};
+
+				if (array[i] >= ARRAY_SIZE(orders)) {
+					pr_warn("lane %u invalid line-order assuming ABC (got %u)\n",
+						i, array[i]);
+					bus->line_orders[i] =
+						V4L2_MBUS_CSI2_CPHY_LINE_ORDER_ABC;
+					continue;
+				}
+
+				bus->line_orders[i] = array[i];
+				pr_debug("lane %u line order %s", i,
+					 orders[array[i]]);
+			}
+		} else {
+			for (i = 0; i < num_data_lanes; i++)
+				bus->line_orders[i] =
+					V4L2_MBUS_CSI2_CPHY_LINE_ORDER_ABC;
+
+			pr_debug("no line orders defined, assuming ABC\n");
+		}
 	}
 
 	return 0;
@@ -1251,6 +1292,7 @@ out_cleanup:
 }
 EXPORT_SYMBOL_GPL(v4l2_async_register_subdev_sensor);
 
+MODULE_DESCRIPTION("V4L2 fwnode binding parsing library");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Sakari Ailus <sakari.ailus@linux.intel.com>");
 MODULE_AUTHOR("Sylwester Nawrocki <s.nawrocki@samsung.com>");
diff --git a/drivers/media/v4l2-core/v4l2-i2c.c b/drivers/media/v4l2-core/v4l2-i2c.c
index b4acca75644b..586c46544255 100644
--- a/drivers/media/v4l2-core/v4l2-i2c.c
+++ b/drivers/media/v4l2-core/v4l2-i2c.c
@@ -100,7 +100,7 @@ struct v4l2_subdev
 	 * Register with the v4l2_device which increases the module's
 	 * use count as well.
 	 */
-	if (v4l2_device_register_subdev(v4l2_dev, sd))
+	if (__v4l2_device_register_subdev(v4l2_dev, sd, sd->owner))
 		sd = NULL;
 	/* Decrease the module use count to match the first try_module_get. */
 	module_put(client->dev.driver->owner);
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 6e7b8b682d13..650dc1956f73 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -310,8 +310,8 @@ static void v4l_print_format(const void *arg, bool write_only)
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY:
 		win = &p->fmt.win;
-		pr_cont(", wxh=%dx%d, x,y=%d,%d, field=%s, chromakey=0x%08x, global_alpha=0x%02x\n",
-			win->w.width, win->w.height, win->w.left, win->w.top,
+		pr_cont(", (%d,%d)/%ux%u, field=%s, chromakey=0x%08x, global_alpha=0x%02x\n",
+			win->w.left, win->w.top, win->w.width, win->w.height,
 			prt_names(win->field, v4l2_field_names),
 			win->chromakey, win->global_alpha);
 		break;
@@ -343,8 +343,9 @@ static void v4l_print_format(const void *arg, bool write_only)
 	case V4L2_BUF_TYPE_META_OUTPUT:
 		meta = &p->fmt.meta;
 		pixelformat = meta->dataformat;
-		pr_cont(", dataformat=%p4cc, buffersize=%u\n",
-			&pixelformat, meta->buffersize);
+		pr_cont(", dataformat=%p4cc, buffersize=%u, width=%u, height=%u, bytesperline=%u\n",
+			&pixelformat, meta->buffersize, meta->width,
+			meta->height, meta->bytesperline);
 		break;
 	}
 }
@@ -483,12 +484,20 @@ static void v4l_print_create_buffers(const void *arg, bool write_only)
 {
 	const struct v4l2_create_buffers *p = arg;
 
-	pr_cont("index=%d, count=%d, memory=%s, capabilities=0x%08x, max num buffers=%u",
+	pr_cont("index=%d, count=%d, memory=%s, capabilities=0x%08x, max num buffers=%u, ",
 		p->index, p->count, prt_names(p->memory, v4l2_memory_names),
 		p->capabilities, p->max_num_buffers);
 	v4l_print_format(&p->format, write_only);
 }
 
+static void v4l_print_remove_buffers(const void *arg, bool write_only)
+{
+	const struct v4l2_remove_buffers *p = arg;
+
+	pr_cont("type=%s, index=%u, count=%u\n",
+		prt_names(p->type, v4l2_type_names), p->index, p->count);
+}
+
 static void v4l_print_streamparm(const void *arg, bool write_only)
 {
 	const struct v4l2_streamparm *p = arg;
@@ -580,12 +589,12 @@ static void v4l_print_cropcap(const void *arg, bool write_only)
 {
 	const struct v4l2_cropcap *p = arg;
 
-	pr_cont("type=%s, bounds wxh=%dx%d, x,y=%d,%d, defrect wxh=%dx%d, x,y=%d,%d, pixelaspect %d/%d\n",
+	pr_cont("type=%s, bounds (%d,%d)/%ux%u, defrect (%d,%d)/%ux%u, pixelaspect %d/%d\n",
 		prt_names(p->type, v4l2_type_names),
-		p->bounds.width, p->bounds.height,
 		p->bounds.left, p->bounds.top,
-		p->defrect.width, p->defrect.height,
+		p->bounds.width, p->bounds.height,
 		p->defrect.left, p->defrect.top,
+		p->defrect.width, p->defrect.height,
 		p->pixelaspect.numerator, p->pixelaspect.denominator);
 }
 
@@ -593,20 +602,20 @@ static void v4l_print_crop(const void *arg, bool write_only)
 {
 	const struct v4l2_crop *p = arg;
 
-	pr_cont("type=%s, wxh=%dx%d, x,y=%d,%d\n",
+	pr_cont("type=%s, crop=(%d,%d)/%ux%u\n",
 		prt_names(p->type, v4l2_type_names),
-		p->c.width, p->c.height,
-		p->c.left, p->c.top);
+		p->c.left, p->c.top,
+		p->c.width, p->c.height);
 }
 
 static void v4l_print_selection(const void *arg, bool write_only)
 {
 	const struct v4l2_selection *p = arg;
 
-	pr_cont("type=%s, target=%d, flags=0x%x, wxh=%dx%d, x,y=%d,%d\n",
+	pr_cont("type=%s, target=%d, flags=0x%x, rect=(%d,%d)/%ux%u\n",
 		prt_names(p->type, v4l2_type_names),
 		p->target, p->flags,
-		p->r.width, p->r.height, p->r.left, p->r.top);
+		p->r.left, p->r.top, p->r.width, p->r.height);
 }
 
 static void v4l_print_jpegcompression(const void *arg, bool write_only)
@@ -884,7 +893,9 @@ static bool check_ext_ctrls(struct v4l2_ext_controls *c, unsigned long ioctl)
 			return false;
 		break;
 	case V4L2_CTRL_WHICH_DEF_VAL:
-		/* Default value cannot be changed */
+	case V4L2_CTRL_WHICH_MIN_VAL:
+	case V4L2_CTRL_WHICH_MAX_VAL:
+		/* Default, minimum or maximum value cannot be changed */
 		if (ioctl == VIDIOC_S_EXT_CTRLS ||
 		    ioctl == VIDIOC_TRY_EXT_CTRLS) {
 			c->error_idx = c->count;
@@ -1298,6 +1309,8 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_PIX_FMT_RGBX1010102:	descr = "32-bit RGBX 10-10-10-2"; break;
 	case V4L2_PIX_FMT_RGBA1010102:	descr = "32-bit RGBA 10-10-10-2"; break;
 	case V4L2_PIX_FMT_ARGB2101010:	descr = "32-bit ARGB 2-10-10-10"; break;
+	case V4L2_PIX_FMT_BGR48:	descr = "48-bit BGR 16-16-16"; break;
+	case V4L2_PIX_FMT_RGB48:	descr = "48-bit RGB 16-16-16"; break;
 	case V4L2_PIX_FMT_BGR48_12:	descr = "12-bit Depth BGR"; break;
 	case V4L2_PIX_FMT_ABGR64_12:	descr = "12-bit Depth BGRA"; break;
 	case V4L2_PIX_FMT_GREY:		descr = "8-bit Greyscale"; break;
@@ -1312,8 +1325,11 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_PIX_FMT_Y10BPACK:	descr = "10-bit Greyscale (Packed)"; break;
 	case V4L2_PIX_FMT_Y10P:		descr = "10-bit Greyscale (MIPI Packed)"; break;
 	case V4L2_PIX_FMT_IPU3_Y10:	descr = "10-bit greyscale (IPU3 Packed)"; break;
+	case V4L2_PIX_FMT_Y12P:		descr = "12-bit Greyscale (MIPI Packed)"; break;
+	case V4L2_PIX_FMT_Y14P:		descr = "14-bit Greyscale (MIPI Packed)"; break;
 	case V4L2_PIX_FMT_Y8I:		descr = "Interleaved 8-bit Greyscale"; break;
 	case V4L2_PIX_FMT_Y12I:		descr = "Interleaved 12-bit Greyscale"; break;
+	case V4L2_PIX_FMT_Y16I:		descr = "Interleaved 16-bit Greyscale"; break;
 	case V4L2_PIX_FMT_Z16:		descr = "16-bit Depth"; break;
 	case V4L2_PIX_FMT_INZI:		descr = "Planar 10:16 Greyscale Depth"; break;
 	case V4L2_PIX_FMT_CNF4:		descr = "4-bit Depth Confidence (Packed)"; break;
@@ -1347,8 +1363,10 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_PIX_FMT_YUV48_12:	descr = "12-bit YUV 4:4:4 Packed"; break;
 	case V4L2_PIX_FMT_NV12:		descr = "Y/UV 4:2:0"; break;
 	case V4L2_PIX_FMT_NV21:		descr = "Y/VU 4:2:0"; break;
+	case V4L2_PIX_FMT_NV15:		descr = "10-bit Y/UV 4:2:0 (Packed)"; break;
 	case V4L2_PIX_FMT_NV16:		descr = "Y/UV 4:2:2"; break;
 	case V4L2_PIX_FMT_NV61:		descr = "Y/VU 4:2:2"; break;
+	case V4L2_PIX_FMT_NV20:		descr = "10-bit Y/UV 4:2:2 (Packed)"; break;
 	case V4L2_PIX_FMT_NV24:		descr = "Y/UV 4:4:4"; break;
 	case V4L2_PIX_FMT_NV42:		descr = "Y/VU 4:4:4"; break;
 	case V4L2_PIX_FMT_P010:		descr = "10-bit Y/UV 4:2:0"; break;
@@ -1445,6 +1463,9 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_META_FMT_VIVID:       descr = "Vivid Metadata"; break;
 	case V4L2_META_FMT_RK_ISP1_PARAMS:	descr = "Rockchip ISP1 3A Parameters"; break;
 	case V4L2_META_FMT_RK_ISP1_STAT_3A:	descr = "Rockchip ISP1 3A Statistics"; break;
+	case V4L2_META_FMT_RK_ISP1_EXT_PARAMS:	descr = "Rockchip ISP1 Ext 3A Params"; break;
+	case V4L2_META_FMT_C3ISP_PARAMS:	descr = "Amlogic C3 ISP Parameters"; break;
+	case V4L2_META_FMT_C3ISP_STATS:		descr = "Amlogic C3 ISP Statistics"; break;
 	case V4L2_PIX_FMT_NV12_8L128:	descr = "NV12 (8x128 Linear)"; break;
 	case V4L2_PIX_FMT_NV12M_8L128:	descr = "NV12M (8x128 Linear)"; break;
 	case V4L2_PIX_FMT_NV12_10BE_8L128:	descr = "10-bit NV12 (8x128 Linear, BE)"; break;
@@ -1452,6 +1473,16 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 	case V4L2_PIX_FMT_Y210:		descr = "10-bit YUYV Packed"; break;
 	case V4L2_PIX_FMT_Y212:		descr = "12-bit YUYV Packed"; break;
 	case V4L2_PIX_FMT_Y216:		descr = "16-bit YUYV Packed"; break;
+	case V4L2_META_FMT_RPI_BE_CFG:	descr = "RPi PiSP BE Config format"; break;
+	case V4L2_META_FMT_RPI_FE_CFG:  descr = "RPi PiSP FE Config format"; break;
+	case V4L2_META_FMT_RPI_FE_STATS: descr = "RPi PiSP FE Statistics format"; break;
+	case V4L2_META_FMT_GENERIC_8:	descr = "8-bit Generic Metadata"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_10:	descr = "8-bit Generic Meta, 10b CSI-2"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_12:	descr = "8-bit Generic Meta, 12b CSI-2"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_14:	descr = "8-bit Generic Meta, 14b CSI-2"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_16:	descr = "8-bit Generic Meta, 16b CSI-2"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_20:	descr = "8-bit Generic Meta, 20b CSI-2"; break;
+	case V4L2_META_FMT_GENERIC_CSI2_24:	descr = "8-bit Generic Meta, 24b CSI-2"; break;
 
 	default:
 		/* Compressed formats */
@@ -1511,6 +1542,16 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 		case V4L2_PIX_FMT_MT2110T:	descr = "Mediatek 10bit Tile Mode"; break;
 		case V4L2_PIX_FMT_MT2110R:	descr = "Mediatek 10bit Raster Mode"; break;
 		case V4L2_PIX_FMT_HEXTILE:	descr = "Hextile Compressed Format"; break;
+		case V4L2_PIX_FMT_PISP_COMP1_RGGB: descr = "PiSP 8b RGRG/GBGB mode1 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP1_GRBG: descr = "PiSP 8b GRGR/BGBG mode1 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP1_GBRG: descr = "PiSP 8b GBGB/RGRG mode1 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP1_BGGR: descr = "PiSP 8b BGBG/GRGR mode1 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP1_MONO: descr = "PiSP 8b monochrome mode1 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP2_RGGB: descr = "PiSP 8b RGRG/GBGB mode2 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP2_GRBG: descr = "PiSP 8b GRGR/BGBG mode2 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP2_GBRG: descr = "PiSP 8b GBGB/RGRG mode2 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP2_BGGR: descr = "PiSP 8b BGBG/GRGR mode2 compr"; break;
+		case V4L2_PIX_FMT_PISP_COMP2_MONO: descr = "PiSP 8b monochrome mode2 compr"; break;
 		default:
 			if (fmt->description[0])
 				return;
@@ -1522,6 +1563,22 @@ static void v4l_fill_fmtdesc(struct v4l2_fmtdesc *fmt)
 		}
 	}
 
+	if (fmt->type == V4L2_BUF_TYPE_META_CAPTURE) {
+		switch (fmt->pixelformat) {
+		case V4L2_META_FMT_GENERIC_8:
+		case V4L2_META_FMT_GENERIC_CSI2_10:
+		case V4L2_META_FMT_GENERIC_CSI2_12:
+		case V4L2_META_FMT_GENERIC_CSI2_14:
+		case V4L2_META_FMT_GENERIC_CSI2_16:
+		case V4L2_META_FMT_GENERIC_CSI2_20:
+		case V4L2_META_FMT_GENERIC_CSI2_24:
+			fmt->flags |= V4L2_FMT_FLAG_META_LINE_BASED;
+			break;
+		default:
+			fmt->flags &= ~V4L2_FMT_FLAG_META_LINE_BASED;
+		}
+	}
+
 	if (descr)
 		WARN_ON(strscpy(fmt->description, descr, sz) < 0);
 	fmt->flags |= flags;
@@ -2092,6 +2149,7 @@ static int v4l_overlay(const struct v4l2_ioctl_ops *ops,
 static int v4l_reqbufs(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_requestbuffers *p = arg;
 	int ret = check_fmt(file, p->type);
 
@@ -2100,6 +2158,10 @@ static int v4l_reqbufs(const struct v4l2_ioctl_ops *ops,
 
 	memset_after(p, 0, flags);
 
+	p->capabilities = 0;
+	if (is_valid_ioctl(vfd, VIDIOC_REMOVE_BUFS))
+		p->capabilities = V4L2_BUF_CAP_SUPPORTS_REMOVE_BUFS;
+
 	return ops->vidioc_reqbufs(file, fh, p);
 }
 
@@ -2133,6 +2195,7 @@ static int v4l_dqbuf(const struct v4l2_ioctl_ops *ops,
 static int v4l_create_bufs(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
+	struct video_device *vfd = video_devdata(file);
 	struct v4l2_create_buffers *create = arg;
 	int ret = check_fmt(file, create->format.type);
 
@@ -2143,6 +2206,10 @@ static int v4l_create_bufs(const struct v4l2_ioctl_ops *ops,
 
 	v4l_sanitize_format(&create->format);
 
+	create->capabilities = 0;
+	if (is_valid_ioctl(vfd, VIDIOC_REMOVE_BUFS))
+		create->capabilities = V4L2_BUF_CAP_SUPPORTS_REMOVE_BUFS;
+
 	ret = ops->vidioc_create_bufs(file, fh, create);
 
 	if (create->format.type == V4L2_BUF_TYPE_VIDEO_CAPTURE ||
@@ -2161,6 +2228,17 @@ static int v4l_prepare_buf(const struct v4l2_ioctl_ops *ops,
 	return ret ? ret : ops->vidioc_prepare_buf(file, fh, b);
 }
 
+static int v4l_remove_bufs(const struct v4l2_ioctl_ops *ops,
+			   struct file *file, void *fh, void *arg)
+{
+	struct v4l2_remove_buffers *remove = arg;
+
+	if (ops->vidioc_remove_bufs)
+		return ops->vidioc_remove_bufs(file, fh, remove);
+
+	return -ENOTTY;
+}
+
 static int v4l_g_parm(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
@@ -2212,17 +2290,26 @@ static int v4l_queryctrl(const struct v4l2_ioctl_ops *ops,
 				struct file *file, void *fh, void *arg)
 {
 	struct video_device *vfd = video_devdata(file);
+	struct v4l2_query_ext_ctrl qec = {};
 	struct v4l2_queryctrl *p = arg;
 	struct v4l2_fh *vfh =
 		test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags) ? fh : NULL;
+	int ret;
 
 	if (vfh && vfh->ctrl_handler)
 		return v4l2_queryctrl(vfh->ctrl_handler, p);
 	if (vfd->ctrl_handler)
 		return v4l2_queryctrl(vfd->ctrl_handler, p);
-	if (ops->vidioc_queryctrl)
-		return ops->vidioc_queryctrl(file, fh, p);
-	return -ENOTTY;
+	if (!ops->vidioc_query_ext_ctrl)
+		return -ENOTTY;
+
+	/* Simulate query_ext_ctr using query_ctrl. */
+	qec.id = p->id;
+	ret = ops->vidioc_query_ext_ctrl(file, fh, &qec);
+	if (ret)
+		return ret;
+	v4l2_query_ext_ctrl_to_v4l2_queryctrl(p, &qec);
+	return ret;
 }
 
 static int v4l_query_ext_ctrl(const struct v4l2_ioctl_ops *ops,
@@ -2273,8 +2360,6 @@ static int v4l_g_ctrl(const struct v4l2_ioctl_ops *ops,
 		return v4l2_g_ctrl(vfh->ctrl_handler, p);
 	if (vfd->ctrl_handler)
 		return v4l2_g_ctrl(vfd->ctrl_handler, p);
-	if (ops->vidioc_g_ctrl)
-		return ops->vidioc_g_ctrl(file, fh, p);
 	if (ops->vidioc_g_ext_ctrls == NULL)
 		return -ENOTTY;
 
@@ -2308,8 +2393,6 @@ static int v4l_s_ctrl(const struct v4l2_ioctl_ops *ops,
 		return v4l2_s_ctrl(vfh, vfh->ctrl_handler, p);
 	if (vfd->ctrl_handler)
 		return v4l2_s_ctrl(NULL, vfd->ctrl_handler, p);
-	if (ops->vidioc_s_ctrl)
-		return ops->vidioc_s_ctrl(file, fh, p);
 	if (ops->vidioc_s_ext_ctrls == NULL)
 		return -ENOTTY;
 
@@ -2754,8 +2837,7 @@ static int v4l_enum_freq_bands(const struct v4l2_ioctl_ops *ops,
 		p->capability = m.capability | V4L2_TUNER_CAP_FREQ_BANDS;
 		p->rangelow = m.rangelow;
 		p->rangehigh = m.rangehigh;
-		p->modulation = (type == V4L2_TUNER_RADIO) ?
-			V4L2_BAND_MODULATION_FM : V4L2_BAND_MODULATION_VSB;
+		p->modulation = V4L2_BAND_MODULATION_FM;
 		return 0;
 	}
 	return -ENOTTY;
@@ -2910,6 +2992,7 @@ static const struct v4l2_ioctl_info v4l2_ioctls[] = {
 	IOCTL_INFO(VIDIOC_ENUM_FREQ_BANDS, v4l_enum_freq_bands, v4l_print_freq_band, 0),
 	IOCTL_INFO(VIDIOC_DBG_G_CHIP_INFO, v4l_dbg_g_chip_info, v4l_print_dbg_chip_info, INFO_FL_CLEAR(v4l2_dbg_chip_info, match)),
 	IOCTL_INFO(VIDIOC_QUERY_EXT_CTRL, v4l_query_ext_ctrl, v4l_print_query_ext_ctrl, INFO_FL_CTRL | INFO_FL_CLEAR(v4l2_query_ext_ctrl, id)),
+	IOCTL_INFO(VIDIOC_REMOVE_BUFS, v4l_remove_bufs, v4l_print_remove_buffers, INFO_FL_PRIO | INFO_FL_QUEUE | INFO_FL_CLEAR(v4l2_remove_buffers, type)),
 };
 #define V4L2_IOCTLS ARRAY_SIZE(v4l2_ioctls)
 
@@ -3147,13 +3230,13 @@ static int check_array_args(unsigned int cmd, void *parg, size_t *array_size,
 	case VIDIOC_SUBDEV_S_ROUTING: {
 		struct v4l2_subdev_routing *routing = parg;
 
-		if (routing->num_routes > 256)
+		if (routing->len_routes > 256)
 			return -E2BIG;
 
 		*user_ptr = u64_to_user_ptr(routing->routes);
 		*kernel_ptr = (void **)&routing->routes;
 		*array_size = sizeof(struct v4l2_subdev_route)
-			    * routing->num_routes;
+			    * routing->len_routes;
 		ret = 1;
 		break;
 	}
@@ -3407,11 +3490,14 @@ video_usercopy(struct file *file, unsigned int orig_cmd, unsigned long arg,
 	 * FIXME: subdev IOCTLS are partially handled here and partially in
 	 * v4l2-subdev.c and the 'always_copy' flag can only be set for IOCTLS
 	 * defined here as part of the 'v4l2_ioctls' array. As
-	 * VIDIOC_SUBDEV_G_ROUTING needs to return results to applications even
-	 * in case of failure, but it is not defined here as part of the
+	 * VIDIOC_SUBDEV_[GS]_ROUTING needs to return results to applications
+	 * even in case of failure, but it is not defined here as part of the
 	 * 'v4l2_ioctls' array, insert an ad-hoc check to address that.
 	 */
-	if (err < 0 && !always_copy && cmd != VIDIOC_SUBDEV_G_ROUTING)
+	if (cmd == VIDIOC_SUBDEV_G_ROUTING || cmd == VIDIOC_SUBDEV_S_ROUTING)
+		always_copy = true;
+
+	if (err < 0 && !always_copy)
 		goto out;
 
 	if (has_array_args) {
diff --git a/drivers/media/v4l2-core/v4l2-jpeg.c b/drivers/media/v4l2-core/v4l2-jpeg.c
index 94435a7b6816..6e2647323522 100644
--- a/drivers/media/v4l2-core/v4l2-jpeg.c
+++ b/drivers/media/v4l2-core/v4l2-jpeg.c
@@ -9,7 +9,7 @@
  * [1] https://www.w3.org/Graphics/JPEG/itu-t81.pdf
  */
 
-#include <asm/unaligned.h>
+#include <linux/unaligned.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
@@ -52,6 +52,122 @@ MODULE_LICENSE("GPL");
 #define COM	0xfffe	/* comment */
 #define TEM	0xff01	/* temporary */
 
+/* Luma and chroma qp tables to achieve 50% compression quality
+ * This is as per example in Annex K.1 of ITU-T.81
+ */
+const u8 v4l2_jpeg_ref_table_luma_qt[V4L2_JPEG_PIXELS_IN_BLOCK] = {
+	16, 11, 10, 16, 24, 40, 51, 61,
+	12, 12, 14, 19, 26, 58, 60, 55,
+	14, 13, 16, 24, 40, 57, 69, 56,
+	14, 17, 22, 29, 51, 87, 80, 62,
+	18, 22, 37, 56, 68, 109, 103, 77,
+	24, 35, 55, 64, 81, 104, 113, 92,
+	49, 64, 78, 87, 103, 121, 120, 101,
+	72, 92, 95, 98, 112, 100, 103, 99
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_luma_qt);
+
+const u8 v4l2_jpeg_ref_table_chroma_qt[V4L2_JPEG_PIXELS_IN_BLOCK] = {
+	17, 18, 24, 47, 99, 99, 99, 99,
+	18, 21, 26, 66, 99, 99, 99, 99,
+	24, 26, 56, 99, 99, 99, 99, 99,
+	47, 66, 99, 99, 99, 99, 99, 99,
+	99, 99, 99, 99, 99, 99, 99, 99,
+	99, 99, 99, 99, 99, 99, 99, 99,
+	99, 99, 99, 99, 99, 99, 99, 99,
+	99, 99, 99, 99, 99, 99, 99, 99
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_chroma_qt);
+
+/* Zigzag scan pattern indexes */
+const u8 v4l2_jpeg_zigzag_scan_index[V4L2_JPEG_PIXELS_IN_BLOCK] = {
+	0,   1,  8, 16,  9,  2,  3, 10,
+	17, 24, 32, 25, 18, 11,  4,  5,
+	12, 19, 26, 33, 40, 48, 41, 34,
+	27, 20, 13,  6,  7, 14, 21, 28,
+	35, 42, 49, 56, 57, 50, 43, 36,
+	29, 22, 15, 23, 30, 37, 44, 51,
+	58, 59, 52, 45, 38, 31, 39, 46,
+	53, 60, 61, 54, 47, 55, 62, 63
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_zigzag_scan_index);
+
+/*
+ * Contains the data that needs to be sent in the marker segment of an
+ * interchange format JPEG stream or an abbreviated format table specification
+ * data stream. Specifies the huffman table used for encoding the luminance DC
+ * coefficient differences. The table represents Table K.3 of ITU-T.81
+ */
+const u8 v4l2_jpeg_ref_table_luma_dc_ht[V4L2_JPEG_REF_HT_DC_LEN] = {
+	0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_luma_dc_ht);
+
+/*
+ * Contains the data that needs to be sent in the marker segment of an
+ * interchange format JPEG stream or an abbreviated format table specification
+ * data stream. Specifies the huffman table used for encoding the luminance AC
+ * coefficients. The table represents Table K.5 of ITU-T.81
+ */
+const u8 v4l2_jpeg_ref_table_luma_ac_ht[V4L2_JPEG_REF_HT_AC_LEN] = {
+	0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05, 0x04, 0x04,
+	0x00, 0x00, 0x01, 0x7D, 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
+	0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32,
+	0x81, 0x91, 0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15, 0x52, 0xD1, 0xF0,
+	0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19, 0x1A,
+	0x25, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
+	0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55,
+	0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
+	0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85,
+	0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
+	0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2,
+	0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5,
+	0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8,
+	0xD9, 0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA,
+	0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_luma_ac_ht);
+
+/*
+ * Contains the data that needs to be sent in the marker segment of an interchange format JPEG
+ * stream or an abbreviated format table specification data stream.
+ * Specifies the huffman table used for encoding the chrominance DC coefficient differences.
+ * The table represents Table K.4 of ITU-T.81
+ */
+const u8 v4l2_jpeg_ref_table_chroma_dc_ht[V4L2_JPEG_REF_HT_DC_LEN] = {
+	0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_chroma_dc_ht);
+
+/*
+ * Contains the data that needs to be sent in the marker segment of an
+ * interchange format JPEG stream or an abbreviated format table specification
+ * data stream. Specifies the huffman table used for encoding the chrominance
+ * AC coefficients. The table represents Table K.6 of ITU-T.81
+ */
+const u8 v4l2_jpeg_ref_table_chroma_ac_ht[V4L2_JPEG_REF_HT_AC_LEN] = {
+	0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04,
+	0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21,
+	0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81,
+	0x08, 0x14, 0x42, 0x91, 0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52, 0xF0,
+	0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16, 0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17,
+	0x18, 0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x35, 0x36, 0x37, 0x38,
+	0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54,
+	0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
+	0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x82, 0x83,
+	0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96,
+	0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9,
+	0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3,
+	0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6,
+	0xD7, 0xD8, 0xD9, 0xDA, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9,
+	0xEA, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA
+};
+EXPORT_SYMBOL_GPL(v4l2_jpeg_ref_table_chroma_ac_ht);
+
 /**
  * struct jpeg_stream - JPEG byte stream
  * @curr: current position in stream
diff --git a/drivers/media/v4l2-core/v4l2-mc.c b/drivers/media/v4l2-core/v4l2-mc.c
index 4bb91359e3a9..937d358697e1 100644
--- a/drivers/media/v4l2-core/v4l2-mc.c
+++ b/drivers/media/v4l2-core/v4l2-mc.c
@@ -329,7 +329,7 @@ int v4l2_create_fwnode_links_to_pad(struct v4l2_subdev *src_sd,
 	if (!(sink->flags & MEDIA_PAD_FL_SINK))
 		return -EINVAL;
 
-	fwnode_graph_for_each_endpoint(dev_fwnode(src_sd->dev), endpoint) {
+	fwnode_graph_for_each_endpoint(src_sd->fwnode, endpoint) {
 		struct fwnode_handle *remote_ep;
 		int src_idx, sink_idx, ret;
 		struct media_pad *src;
diff --git a/drivers/media/v4l2-core/v4l2-mem2mem.c b/drivers/media/v4l2-core/v4l2-mem2mem.c
index 75517134a5e9..eb22d6172462 100644
--- a/drivers/media/v4l2-core/v4l2-mem2mem.c
+++ b/drivers/media/v4l2-core/v4l2-mem2mem.c
@@ -1386,6 +1386,21 @@ int v4l2_m2m_ioctl_create_bufs(struct file *file, void *priv,
 }
 EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_create_bufs);
 
+int v4l2_m2m_ioctl_remove_bufs(struct file *file, void *priv,
+			       struct v4l2_remove_buffers *remove)
+{
+	struct v4l2_fh *fh = file->private_data;
+	struct vb2_queue *q = v4l2_m2m_get_vq(fh->m2m_ctx, remove->type);
+
+	if (!q)
+		return -EINVAL;
+	if (q->type != remove->type)
+		return -EINVAL;
+
+	return vb2_core_remove_bufs(q, remove->index, remove->count);
+}
+EXPORT_SYMBOL_GPL(v4l2_m2m_ioctl_remove_bufs);
+
 int v4l2_m2m_ioctl_querybuf(struct file *file, void *priv,
 				struct v4l2_buffer *buf)
 {
diff --git a/drivers/media/v4l2-core/v4l2-spi.c b/drivers/media/v4l2-core/v4l2-spi.c
index a7092c3930d6..1baf8e63f19e 100644
--- a/drivers/media/v4l2-core/v4l2-spi.c
+++ b/drivers/media/v4l2-core/v4l2-spi.c
@@ -59,7 +59,7 @@ struct v4l2_subdev *v4l2_spi_new_subdev(struct v4l2_device *v4l2_dev,
 	 * Register with the v4l2_device which increases the module's
 	 * use count as well.
 	 */
-	if (v4l2_device_register_subdev(v4l2_dev, sd))
+	if (__v4l2_device_register_subdev(v4l2_dev, sd, sd->owner))
 		sd = NULL;
 
 	/* Decrease the module use count to match the first try_module_get. */
diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index 4c6198c48dd6..a3074f469b15 100644
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -148,6 +148,23 @@ static int subdev_close(struct file *file)
 }
 #endif /* CONFIG_VIDEO_V4L2_SUBDEV_API */
 
+static void v4l2_subdev_enable_privacy_led(struct v4l2_subdev *sd)
+{
+#if IS_REACHABLE(CONFIG_LEDS_CLASS)
+	if (!IS_ERR_OR_NULL(sd->privacy_led))
+		led_set_brightness(sd->privacy_led,
+				   sd->privacy_led->max_brightness);
+#endif
+}
+
+static void v4l2_subdev_disable_privacy_led(struct v4l2_subdev *sd)
+{
+#if IS_REACHABLE(CONFIG_LEDS_CLASS)
+	if (!IS_ERR_OR_NULL(sd->privacy_led))
+		led_set_brightness(sd->privacy_led, 0);
+#endif
+}
+
 static inline int check_which(u32 which)
 {
 	if (which != V4L2_SUBDEV_FORMAT_TRY &&
@@ -317,6 +334,11 @@ static int call_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 	unsigned int i;
 	int ret;
 
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
+		return -EOPNOTSUPP;
+#endif
+
 	memset(fd, 0, sizeof(*fd));
 
 	ret = sd->ops->pad->get_frame_desc(sd, pad, fd);
@@ -369,6 +391,36 @@ static int call_set_edid(struct v4l2_subdev *sd, struct v4l2_subdev_edid *edid)
 	return check_edid(sd, edid) ? : sd->ops->pad->set_edid(sd, edid);
 }
 
+static int call_s_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+			     struct v4l2_dv_timings *timings)
+{
+	if (!timings)
+		return -EINVAL;
+
+	return check_pad(sd, pad) ? :
+	       sd->ops->pad->s_dv_timings(sd, pad, timings);
+}
+
+static int call_g_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+			     struct v4l2_dv_timings *timings)
+{
+	if (!timings)
+		return -EINVAL;
+
+	return check_pad(sd, pad) ? :
+	       sd->ops->pad->g_dv_timings(sd, pad, timings);
+}
+
+static int call_query_dv_timings(struct v4l2_subdev *sd, unsigned int pad,
+				 struct v4l2_dv_timings *timings)
+{
+	if (!timings)
+		return -EINVAL;
+
+	return check_pad(sd, pad) ? :
+	       sd->ops->pad->query_dv_timings(sd, pad, timings);
+}
+
 static int call_dv_timings_cap(struct v4l2_subdev *sd,
 			       struct v4l2_dv_timings_cap *cap)
 {
@@ -392,6 +444,8 @@ static int call_enum_dv_timings(struct v4l2_subdev *sd,
 static int call_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 				struct v4l2_mbus_config *config)
 {
+	memset(config, 0, sizeof(*config));
+
 	return check_pad(sd, pad) ? :
 	       sd->ops->pad->get_mbus_config(sd, pad, config);
 }
@@ -404,23 +458,10 @@ static int call_s_stream(struct v4l2_subdev *sd, int enable)
 	 * The .s_stream() operation must never be called to start or stop an
 	 * already started or stopped subdev. Catch offenders but don't return
 	 * an error yet to avoid regressions.
-	 *
-	 * As .s_stream() is mutually exclusive with the .enable_streams() and
-	 * .disable_streams() operation, we can use the enabled_streams field
-	 * to store the subdev streaming state.
 	 */
-	if (WARN_ON(!!sd->enabled_streams == !!enable))
+	if (WARN_ON(sd->s_stream_enabled == !!enable))
 		return 0;
 
-#if IS_REACHABLE(CONFIG_LEDS_CLASS)
-	if (!IS_ERR_OR_NULL(sd->privacy_led)) {
-		if (enable)
-			led_set_brightness(sd->privacy_led,
-					   sd->privacy_led->max_brightness);
-		else
-			led_set_brightness(sd->privacy_led, 0);
-	}
-#endif
 	ret = sd->ops->video->s_stream(sd, enable);
 
 	if (!enable && ret < 0) {
@@ -428,8 +469,14 @@ static int call_s_stream(struct v4l2_subdev *sd, int enable)
 		ret = 0;
 	}
 
-	if (!ret)
-		sd->enabled_streams = enable ? BIT(0) : 0;
+	if (!ret) {
+		sd->s_stream_enabled = enable;
+
+		if (enable)
+			v4l2_subdev_enable_privacy_led(sd);
+		else
+			v4l2_subdev_disable_privacy_led(sd);
+	}
 
 	return ret;
 }
@@ -487,6 +534,9 @@ static const struct v4l2_subdev_pad_ops v4l2_subdev_call_pad_wrappers = {
 	.set_frame_interval	= call_set_frame_interval,
 	.get_edid		= call_get_edid,
 	.set_edid		= call_set_edid,
+	.s_dv_timings		= call_s_dv_timings,
+	.g_dv_timings		= call_g_dv_timings,
+	.query_dv_timings	= call_query_dv_timings,
 	.dv_timings_cap		= call_dv_timings_cap,
 	.enum_dv_timings	= call_enum_dv_timings,
 	.get_frame_desc		= call_get_frame_desc,
@@ -648,10 +698,25 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 		return v4l2_event_dequeue(vfh, arg, file->f_flags & O_NONBLOCK);
 
 	case VIDIOC_SUBSCRIBE_EVENT:
-		return v4l2_subdev_call(sd, core, subscribe_event, vfh, arg);
+		if (v4l2_subdev_has_op(sd, core, subscribe_event))
+			return v4l2_subdev_call(sd, core, subscribe_event,
+						vfh, arg);
+
+		if ((sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS) &&
+		    vfh->ctrl_handler)
+			return v4l2_ctrl_subdev_subscribe_event(sd, vfh, arg);
+
+		return -ENOIOCTLCMD;
 
 	case VIDIOC_UNSUBSCRIBE_EVENT:
-		return v4l2_subdev_call(sd, core, unsubscribe_event, vfh, arg);
+		if (v4l2_subdev_has_op(sd, core, unsubscribe_event))
+			return v4l2_subdev_call(sd, core, unsubscribe_event,
+						vfh, arg);
+
+		if (sd->flags & V4L2_SUBDEV_FL_HAS_EVENTS)
+			return v4l2_event_subdev_unsubscribe(sd, vfh, arg);
+
+		return -ENOIOCTLCMD;
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	case VIDIOC_DBG_G_REGISTER:
@@ -732,6 +797,7 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 		memset(&sel, 0, sizeof(sel));
 		sel.which = crop->which;
 		sel.pad = crop->pad;
+		sel.stream = crop->stream;
 		sel.target = V4L2_SEL_TGT_CROP;
 
 		rval = v4l2_subdev_call(
@@ -756,6 +822,7 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 		memset(&sel, 0, sizeof(sel));
 		sel.which = crop->which;
 		sel.pad = crop->pad;
+		sel.stream = crop->stream;
 		sel.target = V4L2_SEL_TGT_CROP;
 		sel.r = crop->rect;
 
@@ -873,16 +940,16 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 	}
 
 	case VIDIOC_SUBDEV_QUERY_DV_TIMINGS:
-		return v4l2_subdev_call(sd, video, query_dv_timings, arg);
+		return v4l2_subdev_call(sd, pad, query_dv_timings, 0, arg);
 
 	case VIDIOC_SUBDEV_G_DV_TIMINGS:
-		return v4l2_subdev_call(sd, video, g_dv_timings, arg);
+		return v4l2_subdev_call(sd, pad, g_dv_timings, 0, arg);
 
 	case VIDIOC_SUBDEV_S_DV_TIMINGS:
 		if (ro_subdev)
 			return -EPERM;
 
-		return v4l2_subdev_call(sd, video, s_dv_timings, arg);
+		return v4l2_subdev_call(sd, pad, s_dv_timings, 0, arg);
 
 	case VIDIOC_SUBDEV_G_STD:
 		return v4l2_subdev_call(sd, video, g_std, arg);
@@ -923,14 +990,10 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 
 		krouting = &state->routing;
 
-		if (routing->num_routes < krouting->num_routes) {
-			routing->num_routes = krouting->num_routes;
-			return -ENOSPC;
-		}
-
 		memcpy((struct v4l2_subdev_route *)(uintptr_t)routing->routes,
 		       krouting->routes,
-		       krouting->num_routes * sizeof(*krouting->routes));
+		       min(krouting->num_routes, routing->len_routes) *
+		       sizeof(*krouting->routes));
 		routing->num_routes = krouting->num_routes;
 
 		return 0;
@@ -952,6 +1015,9 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 		if (routing->which != V4L2_SUBDEV_FORMAT_TRY && ro_subdev)
 			return -EPERM;
 
+		if (routing->num_routes > routing->len_routes)
+			return -EINVAL;
+
 		memset(routing->reserved, 0, sizeof(routing->reserved));
 
 		for (i = 0; i < routing->num_routes; ++i) {
@@ -977,11 +1043,36 @@ static long subdev_do_ioctl(struct file *file, unsigned int cmd, void *arg,
 				return -EINVAL;
 		}
 
+		/*
+		 * If the driver doesn't support setting routing, just return
+		 * the routing table.
+		 */
+		if (!v4l2_subdev_has_op(sd, pad, set_routing)) {
+			memcpy((struct v4l2_subdev_route *)(uintptr_t)routing->routes,
+			       state->routing.routes,
+			       min(state->routing.num_routes, routing->len_routes) *
+			       sizeof(*state->routing.routes));
+			routing->num_routes = state->routing.num_routes;
+
+			return 0;
+		}
+
 		krouting.num_routes = routing->num_routes;
+		krouting.len_routes = routing->len_routes;
 		krouting.routes = routes;
 
-		return v4l2_subdev_call(sd, pad, set_routing, state,
+		rval = v4l2_subdev_call(sd, pad, set_routing, state,
 					routing->which, &krouting);
+		if (rval < 0)
+			return rval;
+
+		memcpy((struct v4l2_subdev_route *)(uintptr_t)routing->routes,
+		       state->routing.routes,
+		       min(state->routing.num_routes, routing->len_routes) *
+		       sizeof(*state->routing.routes));
+		routing->num_routes = state->routing.num_routes;
+
+		return 0;
 	}
 
 	case VIDIOC_SUBDEV_G_CLIENT_CAP: {
@@ -1200,14 +1291,6 @@ v4l2_subdev_link_validate_get_format(struct media_pad *pad, u32 stream,
 	struct v4l2_subdev *sd;
 	int ret;
 
-	if (!is_media_entity_v4l2_subdev(pad->entity)) {
-		WARN(pad->entity->function != MEDIA_ENT_F_IO_V4L,
-		     "Driver bug! Wrong media entity type 0x%08x, entity %s\n",
-		     pad->entity->function, pad->entity->name);
-
-		return -EINVAL;
-	}
-
 	sd = media_entity_to_v4l2_subdev(pad->entity);
 
 	fmt->which = V4L2_SUBDEV_FORMAT_ACTIVE;
@@ -1382,16 +1465,53 @@ int v4l2_subdev_link_validate(struct media_link *link)
 	bool states_locked;
 	int ret;
 
-	if (!is_media_entity_v4l2_subdev(link->sink->entity) ||
-	    !is_media_entity_v4l2_subdev(link->source->entity)) {
-		pr_warn_once("%s of link '%s':%u->'%s':%u is not a V4L2 sub-device, driver bug!\n",
-			     !is_media_entity_v4l2_subdev(link->sink->entity) ?
-			     "sink" : "source",
-			     link->source->entity->name, link->source->index,
-			     link->sink->entity->name, link->sink->index);
-		return 0;
+	/*
+	 * Links are validated in the context of the sink entity. Usage of this
+	 * helper on a sink that is not a subdev is a clear driver bug.
+	 */
+	if (WARN_ON_ONCE(!is_media_entity_v4l2_subdev(link->sink->entity)))
+		return -EINVAL;
+
+	/*
+	 * If the source is a video device, delegate link validation to it. This
+	 * allows usage of this helper for subdev connected to a video output
+	 * device, provided that the driver implement the video output device's
+	 * .link_validate() operation.
+	 */
+	if (is_media_entity_v4l2_video_device(link->source->entity)) {
+		struct media_entity *source = link->source->entity;
+
+		if (!source->ops || !source->ops->link_validate) {
+			/*
+			 * Many existing drivers do not implement the required
+			 * .link_validate() operation for their video devices.
+			 * Print a warning to get the drivers fixed, and return
+			 * 0 to avoid breaking userspace. This should
+			 * eventually be turned into a WARN_ON() when all
+			 * drivers will have been fixed.
+			 */
+			pr_warn_once("video device '%s' does not implement .link_validate(), driver bug!\n",
+				     source->name);
+			return 0;
+		}
+
+		/*
+		 * Avoid infinite loops in case a video device incorrectly uses
+		 * this helper function as its .link_validate() handler.
+		 */
+		if (WARN_ON(source->ops->link_validate == v4l2_subdev_link_validate))
+			return -EINVAL;
+
+		return source->ops->link_validate(link);
 	}
 
+	/*
+	 * If the source is still not a subdev, usage of this helper is a clear
+	 * driver bug.
+	 */
+	if (WARN_ON(!is_media_entity_v4l2_subdev(link->source->entity)))
+		return -EINVAL;
+
 	sink_sd = media_entity_to_v4l2_subdev(link->sink->entity);
 	source_sd = media_entity_to_v4l2_subdev(link->source->entity);
 
@@ -1400,17 +1520,13 @@ int v4l2_subdev_link_validate(struct media_link *link)
 
 	states_locked = sink_state && source_state;
 
-	if (states_locked) {
-		v4l2_subdev_lock_state(sink_state);
-		v4l2_subdev_lock_state(source_state);
-	}
+	if (states_locked)
+		v4l2_subdev_lock_states(sink_state, source_state);
 
 	ret = v4l2_subdev_link_validate_locked(link, states_locked);
 
-	if (states_locked) {
-		v4l2_subdev_unlock_state(sink_state);
-		v4l2_subdev_unlock_state(source_state);
-	}
+	if (states_locked)
+		v4l2_subdev_unlock_states(sink_state, source_state);
 
 	return ret;
 }
@@ -1519,6 +1635,36 @@ int __v4l2_subdev_init_finalize(struct v4l2_subdev *sd, const char *name,
 				struct lock_class_key *key)
 {
 	struct v4l2_subdev_state *state;
+	struct device *dev = sd->dev;
+	bool has_disable_streams;
+	bool has_enable_streams;
+	bool has_s_stream;
+
+	/* Check that the subdevice implements the required features */
+
+	has_s_stream = v4l2_subdev_has_op(sd, video, s_stream);
+	has_enable_streams = v4l2_subdev_has_op(sd, pad, enable_streams);
+	has_disable_streams = v4l2_subdev_has_op(sd, pad, disable_streams);
+
+	if (has_enable_streams != has_disable_streams) {
+		dev_err(dev,
+			"subdev '%s' must implement both or neither of .enable_streams() and .disable_streams()\n",
+			sd->name);
+		return -EINVAL;
+	}
+
+	if (sd->flags & V4L2_SUBDEV_FL_STREAMS) {
+		if (has_s_stream && !has_enable_streams) {
+			dev_err(dev,
+				"subdev '%s' must implement .enable/disable_streams()\n",
+				sd->name);
+
+			return -EINVAL;
+		}
+	}
+
+	if (sd->ctrl_handler)
+		sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
 
 	state = __v4l2_subdev_state_alloc(sd, name, key);
 	if (IS_ERR(state))
@@ -2063,43 +2209,55 @@ out:
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_routing_validate);
 
-static int v4l2_subdev_enable_streams_fallback(struct v4l2_subdev *sd, u32 pad,
-					       u64 streams_mask)
+static void v4l2_subdev_collect_streams(struct v4l2_subdev *sd,
+					struct v4l2_subdev_state *state,
+					u32 pad, u64 streams_mask,
+					u64 *found_streams,
+					u64 *enabled_streams)
 {
-	struct device *dev = sd->entity.graph_obj.mdev->dev;
-	unsigned int i;
-	int ret;
+	if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS)) {
+		*found_streams = BIT_ULL(0);
+		*enabled_streams =
+			(sd->enabled_pads & BIT_ULL(pad)) ? BIT_ULL(0) : 0;
+		return;
+	}
 
-	/*
-	 * The subdev doesn't implement pad-based stream enable, fall back
-	 * on the .s_stream() operation. This can only be done for subdevs that
-	 * have a single source pad, as sd->enabled_streams is global to the
-	 * subdev.
-	 */
-	if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
-		return -EOPNOTSUPP;
+	*found_streams = 0;
+	*enabled_streams = 0;
 
-	for (i = 0; i < sd->entity.num_pads; ++i) {
-		if (i != pad && sd->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE)
-			return -EOPNOTSUPP;
-	}
+	for (unsigned int i = 0; i < state->stream_configs.num_configs; ++i) {
+		const struct v4l2_subdev_stream_config *cfg =
+			&state->stream_configs.configs[i];
 
-	if (sd->enabled_streams & streams_mask) {
-		dev_dbg(dev, "set of streams %#llx already enabled on %s:%u\n",
-			streams_mask, sd->entity.name, pad);
-		return -EALREADY;
+		if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
+			continue;
+
+		*found_streams |= BIT_ULL(cfg->stream);
+		if (cfg->enabled)
+			*enabled_streams |= BIT_ULL(cfg->stream);
 	}
+}
 
-	/* Start streaming when the first streams are enabled. */
-	if (!sd->enabled_streams) {
-		ret = v4l2_subdev_call(sd, video, s_stream, 1);
-		if (ret)
-			return ret;
+static void v4l2_subdev_set_streams_enabled(struct v4l2_subdev *sd,
+					    struct v4l2_subdev_state *state,
+					    u32 pad, u64 streams_mask,
+					    bool enabled)
+{
+	if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS)) {
+		if (enabled)
+			sd->enabled_pads |= BIT_ULL(pad);
+		else
+			sd->enabled_pads &= ~BIT_ULL(pad);
+		return;
 	}
 
-	sd->enabled_streams |= streams_mask;
+	for (unsigned int i = 0; i < state->stream_configs.num_configs; ++i) {
+		struct v4l2_subdev_stream_config *cfg =
+			&state->stream_configs.configs[i];
 
-	return 0;
+		if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
+			cfg->enabled = enabled;
+	}
 }
 
 int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
@@ -2107,44 +2265,44 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
 {
 	struct device *dev = sd->entity.graph_obj.mdev->dev;
 	struct v4l2_subdev_state *state;
-	u64 found_streams = 0;
-	unsigned int i;
+	bool already_streaming;
+	u64 enabled_streams;
+	u64 found_streams;
+	bool use_s_stream;
 	int ret;
 
 	/* A few basic sanity checks first. */
 	if (pad >= sd->entity.num_pads)
 		return -EINVAL;
 
+	if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
+		return -EOPNOTSUPP;
+
+	/*
+	 * We use a 64-bit bitmask for tracking enabled pads, so only subdevices
+	 * with 64 pads or less can be supported.
+	 */
+	if (pad >= sizeof(sd->enabled_pads) * BITS_PER_BYTE)
+		return -EOPNOTSUPP;
+
 	if (!streams_mask)
 		return 0;
 
 	/* Fallback on .s_stream() if .enable_streams() isn't available. */
-	if (!sd->ops->pad || !sd->ops->pad->enable_streams)
-		return v4l2_subdev_enable_streams_fallback(sd, pad,
-							   streams_mask);
+	use_s_stream = !v4l2_subdev_has_op(sd, pad, enable_streams);
 
-	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (!use_s_stream)
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+	else
+		state = NULL;
 
 	/*
 	 * Verify that the requested streams exist and that they are not
 	 * already enabled.
 	 */
-	for (i = 0; i < state->stream_configs.num_configs; ++i) {
-		struct v4l2_subdev_stream_config *cfg =
-			&state->stream_configs.configs[i];
-
-		if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
-			continue;
 
-		found_streams |= BIT_ULL(cfg->stream);
-
-		if (cfg->enabled) {
-			dev_dbg(dev, "stream %u already enabled on %s:%u\n",
-				cfg->stream, sd->entity.name, pad);
-			ret = -EALREADY;
-			goto done;
-		}
-	}
+	v4l2_subdev_collect_streams(sd, state, pad, streams_mask,
+				    &found_streams, &enabled_streams);
 
 	if (found_streams != streams_mask) {
 		dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
@@ -2153,11 +2311,29 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
 		goto done;
 	}
 
+	if (enabled_streams) {
+		dev_dbg(dev, "streams 0x%llx already enabled on %s:%u\n",
+			enabled_streams, sd->entity.name, pad);
+		ret = -EALREADY;
+		goto done;
+	}
+
 	dev_dbg(dev, "enable streams %u:%#llx\n", pad, streams_mask);
 
-	/* Call the .enable_streams() operation. */
-	ret = v4l2_subdev_call(sd, pad, enable_streams, state, pad,
-			       streams_mask);
+	already_streaming = v4l2_subdev_is_streaming(sd);
+
+	if (!use_s_stream) {
+		/* Call the .enable_streams() operation. */
+		ret = v4l2_subdev_call(sd, pad, enable_streams, state, pad,
+				       streams_mask);
+	} else {
+		/* Start streaming when the first pad is enabled. */
+		if (!already_streaming)
+			ret = v4l2_subdev_call(sd, video, s_stream, 1);
+		else
+			ret = 0;
+	}
+
 	if (ret) {
 		dev_dbg(dev, "enable streams %u:%#llx failed: %d\n", pad,
 			streams_mask, ret);
@@ -2165,103 +2341,68 @@ int v4l2_subdev_enable_streams(struct v4l2_subdev *sd, u32 pad,
 	}
 
 	/* Mark the streams as enabled. */
-	for (i = 0; i < state->stream_configs.num_configs; ++i) {
-		struct v4l2_subdev_stream_config *cfg =
-			&state->stream_configs.configs[i];
+	v4l2_subdev_set_streams_enabled(sd, state, pad, streams_mask, true);
 
-		if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
-			cfg->enabled = true;
-	}
+	/*
+	 * TODO: When all the drivers have been changed to use
+	 * v4l2_subdev_enable_streams() and v4l2_subdev_disable_streams(),
+	 * instead of calling .s_stream() operation directly, we can remove
+	 * the privacy LED handling from call_s_stream() and do it here
+	 * for all cases.
+	 */
+	if (!use_s_stream && !already_streaming)
+		v4l2_subdev_enable_privacy_led(sd);
 
 done:
-	v4l2_subdev_unlock_state(state);
+	if (!use_s_stream)
+		v4l2_subdev_unlock_state(state);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_enable_streams);
 
-static int v4l2_subdev_disable_streams_fallback(struct v4l2_subdev *sd, u32 pad,
-						u64 streams_mask)
-{
-	struct device *dev = sd->entity.graph_obj.mdev->dev;
-	unsigned int i;
-	int ret;
-
-	/*
-	 * If the subdev doesn't implement pad-based stream enable, fall  back
-	 * on the .s_stream() operation. This can only be done for subdevs that
-	 * have a single source pad, as sd->enabled_streams is global to the
-	 * subdev.
-	 */
-	if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
-		return -EOPNOTSUPP;
-
-	for (i = 0; i < sd->entity.num_pads; ++i) {
-		if (i != pad && sd->entity.pads[i].flags & MEDIA_PAD_FL_SOURCE)
-			return -EOPNOTSUPP;
-	}
-
-	if ((sd->enabled_streams & streams_mask) != streams_mask) {
-		dev_dbg(dev, "set of streams %#llx already disabled on %s:%u\n",
-			streams_mask, sd->entity.name, pad);
-		return -EALREADY;
-	}
-
-	/* Stop streaming when the last streams are disabled. */
-	if (!(sd->enabled_streams & ~streams_mask)) {
-		ret = v4l2_subdev_call(sd, video, s_stream, 0);
-		if (ret)
-			return ret;
-	}
-
-	sd->enabled_streams &= ~streams_mask;
-
-	return 0;
-}
-
 int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
 				u64 streams_mask)
 {
 	struct device *dev = sd->entity.graph_obj.mdev->dev;
 	struct v4l2_subdev_state *state;
-	u64 found_streams = 0;
-	unsigned int i;
+	u64 enabled_streams;
+	u64 found_streams;
+	bool use_s_stream;
 	int ret;
 
 	/* A few basic sanity checks first. */
 	if (pad >= sd->entity.num_pads)
 		return -EINVAL;
 
+	if (!(sd->entity.pads[pad].flags & MEDIA_PAD_FL_SOURCE))
+		return -EOPNOTSUPP;
+
+	/*
+	 * We use a 64-bit bitmask for tracking enabled pads, so only subdevices
+	 * with 64 pads or less can be supported.
+	 */
+	if (pad >= sizeof(sd->enabled_pads) * BITS_PER_BYTE)
+		return -EOPNOTSUPP;
+
 	if (!streams_mask)
 		return 0;
 
 	/* Fallback on .s_stream() if .disable_streams() isn't available. */
-	if (!sd->ops->pad || !sd->ops->pad->disable_streams)
-		return v4l2_subdev_disable_streams_fallback(sd, pad,
-							    streams_mask);
+	use_s_stream = !v4l2_subdev_has_op(sd, pad, disable_streams);
 
-	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (!use_s_stream)
+		state = v4l2_subdev_lock_and_get_active_state(sd);
+	else
+		state = NULL;
 
 	/*
 	 * Verify that the requested streams exist and that they are not
 	 * already disabled.
 	 */
-	for (i = 0; i < state->stream_configs.num_configs; ++i) {
-		struct v4l2_subdev_stream_config *cfg =
-			&state->stream_configs.configs[i];
-
-		if (cfg->pad != pad || !(streams_mask & BIT_ULL(cfg->stream)))
-			continue;
 
-		found_streams |= BIT_ULL(cfg->stream);
-
-		if (!cfg->enabled) {
-			dev_dbg(dev, "stream %u already disabled on %s:%u\n",
-				cfg->stream, sd->entity.name, pad);
-			ret = -EALREADY;
-			goto done;
-		}
-	}
+	v4l2_subdev_collect_streams(sd, state, pad, streams_mask,
+				    &found_streams, &enabled_streams);
 
 	if (found_streams != streams_mask) {
 		dev_dbg(dev, "streams 0x%llx not found on %s:%u\n",
@@ -2270,28 +2411,43 @@ int v4l2_subdev_disable_streams(struct v4l2_subdev *sd, u32 pad,
 		goto done;
 	}
 
+	if (enabled_streams != streams_mask) {
+		dev_dbg(dev, "streams 0x%llx already disabled on %s:%u\n",
+			streams_mask & ~enabled_streams, sd->entity.name, pad);
+		ret = -EALREADY;
+		goto done;
+	}
+
 	dev_dbg(dev, "disable streams %u:%#llx\n", pad, streams_mask);
 
-	/* Call the .disable_streams() operation. */
-	ret = v4l2_subdev_call(sd, pad, disable_streams, state, pad,
-			       streams_mask);
+	if (!use_s_stream) {
+		/* Call the .disable_streams() operation. */
+		ret = v4l2_subdev_call(sd, pad, disable_streams, state, pad,
+				       streams_mask);
+	} else {
+		/* Stop streaming when the last streams are disabled. */
+
+		if (!(sd->enabled_pads & ~BIT_ULL(pad)))
+			ret = v4l2_subdev_call(sd, video, s_stream, 0);
+		else
+			ret = 0;
+	}
+
 	if (ret) {
 		dev_dbg(dev, "disable streams %u:%#llx failed: %d\n", pad,
 			streams_mask, ret);
 		goto done;
 	}
 
-	/* Mark the streams as disabled. */
-	for (i = 0; i < state->stream_configs.num_configs; ++i) {
-		struct v4l2_subdev_stream_config *cfg =
-			&state->stream_configs.configs[i];
-
-		if (cfg->pad == pad && (streams_mask & BIT_ULL(cfg->stream)))
-			cfg->enabled = false;
-	}
+	v4l2_subdev_set_streams_enabled(sd, state, pad, streams_mask, false);
 
 done:
-	v4l2_subdev_unlock_state(state);
+	if (!use_s_stream) {
+		if (!v4l2_subdev_is_streaming(sd))
+			v4l2_subdev_disable_privacy_led(sd);
+
+		v4l2_subdev_unlock_state(state);
+	}
 
 	return ret;
 }
@@ -2320,15 +2476,24 @@ int v4l2_subdev_s_stream_helper(struct v4l2_subdev *sd, int enable)
 	if (WARN_ON(pad_index == -1))
 		return -EINVAL;
 
-	/*
-	 * As there's a single source pad, just collect all the source streams.
-	 */
-	state = v4l2_subdev_lock_and_get_active_state(sd);
+	if (sd->flags & V4L2_SUBDEV_FL_STREAMS) {
+		/*
+		 * As there's a single source pad, just collect all the source
+		 * streams.
+		 */
+		state = v4l2_subdev_lock_and_get_active_state(sd);
 
-	for_each_active_route(&state->routing, route)
-		source_mask |= BIT_ULL(route->source_stream);
+		for_each_active_route(&state->routing, route)
+			source_mask |= BIT_ULL(route->source_stream);
 
-	v4l2_subdev_unlock_state(state);
+		v4l2_subdev_unlock_state(state);
+	} else {
+		/*
+		 * For non-streams subdevices, there's a single implicit stream
+		 * per pad.
+		 */
+		source_mask = BIT_ULL(0);
+	}
 
 	if (enable)
 		return v4l2_subdev_enable_streams(sd, pad_index, source_mask);
@@ -2370,6 +2535,31 @@ void v4l2_subdev_notify_event(struct v4l2_subdev *sd,
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_notify_event);
 
+bool v4l2_subdev_is_streaming(struct v4l2_subdev *sd)
+{
+	struct v4l2_subdev_state *state;
+
+	if (!v4l2_subdev_has_op(sd, pad, enable_streams))
+		return sd->s_stream_enabled;
+
+	if (!(sd->flags & V4L2_SUBDEV_FL_STREAMS))
+		return !!sd->enabled_pads;
+
+	state = v4l2_subdev_get_locked_active_state(sd);
+
+	for (unsigned int i = 0; i < state->stream_configs.num_configs; ++i) {
+		const struct v4l2_subdev_stream_config *cfg;
+
+		cfg = &state->stream_configs.configs[i];
+
+		if (cfg->enabled)
+			return true;
+	}
+
+	return false;
+}
+EXPORT_SYMBOL_GPL(v4l2_subdev_is_streaming);
+
 int v4l2_subdev_get_privacy_led(struct v4l2_subdev *sd)
 {
 #if IS_REACHABLE(CONFIG_LEDS_CLASS)